diff options
9 files changed, 298 insertions, 153 deletions
diff --git a/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj b/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj index 785945f2680..943479dc623 100644 --- a/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj +++ b/extern/bullet/BulletDynamics/BulletDynamics_vc8.vcproj @@ -163,6 +163,14 @@ > </File> <File + RelativePath=".\ConstraintSolver\HingeConstraint.cpp" + > + </File> + <File + RelativePath=".\ConstraintSolver\HingeConstraint.h" + > + </File> + <File RelativePath=".\ConstraintSolver\JacobianEntry.h" > </File> diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp index 4154dbf2caa..b24ec54b75d 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/HingeConstraint.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "HingeConstraint.h" #include "Dynamics/RigidBody.h" #include "Dynamics/MassProps.h" - +#include "SimdTransformUtil.h" HingeConstraint::HingeConstraint() @@ -27,8 +27,8 @@ HingeConstraint::HingeConstraint() HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB, SimdVector3& axisInA,SimdVector3& axisInB) :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_axisInA(m_axisInA), -m_axisInB(m_axisInB) +m_axisInA(axisInA), +m_axisInB(axisInB) { } @@ -36,9 +36,9 @@ m_axisInB(m_axisInB) HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA) :TypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_axisInA(m_axisInA), -//fixed axis in woeldspace -m_axisInB(rbA.getCenterOfMassTransform() * m_axisInA) +m_axisInA(axisInA), +//fixed axis in worldspace +m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA) { } @@ -66,9 +66,11 @@ void HingeConstraint::BuildJacobian() //calculate two perpendicular jointAxis, orthogonal to hingeAxis //these two jointAxis require equal angular velocities for both bodies - - SimdVector3 jointAxis0(0,0,1); - SimdVector3 jointAxis1(0,1,0); + //this is ununsed for now, it's a todo + SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + SimdVector3 jointAxis0; + SimdVector3 jointAxis1; + SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1); new (&m_jacAng[0]) JacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), @@ -87,17 +89,14 @@ void HingeConstraint::BuildJacobian() void HingeConstraint::SolveConstraint(SimdScalar timeStep) { + SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; - SimdVector3 normal(0,0,0); SimdScalar tau = 0.3f; SimdScalar damping = 1.f; -// SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); -// SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - for (int i=0;i<3;i++) { normal[i] = 1; @@ -105,24 +104,14 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); SimdVector3 vel = vel1 - vel2; - SimdScalar rel_vel; rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - */ - //positional error (zeroth order error) SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - SimdScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; SimdVector3 impulse_vector = normal * impulse; @@ -131,7 +120,44 @@ void HingeConstraint::SolveConstraint(SimdScalar timeStep) normal[i] = 0; } -//todo: do angular part + + ///solve angular part + + // get axes in world space + SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + + const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity(); + const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity(); + SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA); + SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB); + SimdVector3 velrel = angA-angB; + + //solve angular velocity correction + float relaxation = 1.f; + float len = velrel.length(); + if (len > 0.00001f) + { + SimdVector3 normal = velrel.normalized(); + float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) + + GetRigidBodyB().ComputeAngularImpulseDenominator(normal); + // scale for mass and relaxation + velrel *= (1.f/denom) * 0.9; + } + + //solve angular positional correction + SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep); + float len2 = angularError.length(); + if (len2>0.00001f) + { + SimdVector3 normal2 = angularError.normalized(); + float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) + + GetRigidBodyB().ComputeAngularImpulseDenominator(normal2); + angularError *= (1.f/denom2) * relaxation; + } + + m_rbA.applyTorqueImpulse(-velrel+angularError); + m_rbB.applyTorqueImpulse(velrel-angularError); } diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h index fae01513d10..8489778d824 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h @@ -180,6 +180,13 @@ public: } + inline float ComputeAngularImpulseDenominator(const SimdVector3& axis) const + { + SimdVector3 vec = axis * getInvInertiaTensorWorld(); + return axis.dot(vec); + } + + private: diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index f715387a189..e4d0e49d5e8 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -49,6 +49,9 @@ RaycastVehicle::VehicleTuning gTuning; #include "ConstraintSolver/ConstraintSolver.h" #include "ConstraintSolver/Point2PointConstraint.h" +#include "ConstraintSolver/HingeConstraint.h" + + //#include "BroadphaseCollision/QueryDispatcher.h" //#include "BroadphaseCollision/QueryBox.h" //todo: change this to allow dynamic registration of types! @@ -393,16 +396,16 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr //also remove constraint { - std::vector<Point2PointConstraint*>::iterator i; + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || - (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetUserConstraintId()); + removeConstraint(constraint->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -410,16 +413,16 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr } { - std::vector<Point2PointConstraint*>::iterator i; + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || - (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetUserConstraintId()); + removeConstraint(constraint->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -452,6 +455,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) if (!SimdFuzzyZero(timeStep)) { + +#define SPLIT_TIMESTEP 1 #ifdef SPLIT_TIMESTEP proceedDeltaTimeOneStep(0.5f*timeStep); proceedDeltaTimeOneStep(0.5f*timeStep); @@ -555,19 +560,17 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) int i; - int numPoint2Point = m_p2pConstraints.size(); + int numConstraints = m_constraints.size(); //point to point constraints - for (i=0;i< numPoint2Point ; i++ ) + for (i=0;i< numConstraints ; i++ ) { - Point2PointConstraint* p2p = m_p2pConstraints[i]; + TypedConstraint* constraint = m_constraints[i]; - p2p->BuildJacobian(); - p2p->SolveConstraint( timeStep ); + constraint->BuildJacobian(); + constraint->SolveConstraint( timeStep ); } - - } @@ -1036,7 +1039,11 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl SimdVector3 pivotInA(pivotX,pivotY,pivotZ); SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; - + SimdVector3 axisInA(axisX,axisY,axisZ); + SimdVector3 axisInB = rb1 ? + (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : + rb0->getCenterOfMassTransform().getBasis() * -axisInA; + switch (type) { case PHY_POINT2POINT_CONSTRAINT: @@ -1054,7 +1061,7 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl pivotInA); } - m_p2pConstraints.push_back(p2p); + m_constraints.push_back(p2p); p2p->SetUserConstraintId(gConstraintUid++); p2p->SetUserConstraintType(type); //64 bit systems can't cast pointer to int. could use size_t instead. @@ -1062,6 +1069,32 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + + case PHY_LINEHINGE_CONSTRAINT: + { + HingeConstraint* hinge = 0; + + if (rb1) + { + hinge = new HingeConstraint( + *rb0, + *rb1,pivotInA,pivotInB,axisInA,axisInB); + + + } else + { + hinge = new HingeConstraint(*rb0, + pivotInA,axisInA); + + } + + m_constraints.push_back(hinge); + hinge->SetUserConstraintId(gConstraintUid++); + hinge->SetUserConstraintType(type); + //64 bit systems can't cast pointer to int. could use size_t instead. + return hinge->GetUserConstraintId(); + break; + } #ifdef NEW_BULLET_VEHICLE_SUPPORT case PHY_VEHICLE_CONSTRAINT: @@ -1093,19 +1126,16 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl void CcdPhysicsEnvironment::removeConstraint(int constraintId) { - std::vector<Point2PointConstraint*>::iterator i; - - //std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), - // (Point2PointConstraint *)p2p); + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if (p2p->GetUserConstraintId() == constraintId) + TypedConstraint* constraint = (*i); + if (constraint->GetUserConstraintId() == constraintId) { - std::swap(*i, m_p2pConstraints.back()); - m_p2pConstraints.pop_back(); + std::swap(*i, m_constraints.back()); + m_constraints.pop_back(); break; } } @@ -1307,16 +1337,16 @@ const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const return GetDispatcher()->GetManifoldByIndexInternal(index); } -Point2PointConstraint* CcdPhysicsEnvironment::getPoint2PointConstraint(int constraintId) +TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId) { - int nump2p = m_p2pConstraints.size(); + int numConstraint = m_constraints.size(); int i; - for (i=0;i<nump2p;i++) + for (i=0;i<numConstraint;i++) { - Point2PointConstraint* p2p = m_p2pConstraints[i]; - if (p2p->GetUserConstraintId()==constraintId) + TypedConstraint* constraint = m_constraints[i]; + if (constraint->GetUserConstraintId()==constraintId) { - return p2p; + return constraint; } } return 0; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 37c78168ba6..311e74eaad8 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -8,13 +8,14 @@ class CcdPhysicsController; -class Point2PointConstraint; +class TypedConstraint; + class CollisionDispatcher; class Dispatcher; //#include "BroadphaseInterface.h" //switch on/off new vehicle support -//#define NEW_BULLET_VEHICLE_SUPPORT 1 +#define NEW_BULLET_VEHICLE_SUPPORT 1 #include "ConstraintSolver/ContactSolverInfo.h" @@ -96,7 +97,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment } #endif //NEW_BULLET_VEHICLE_SUPPORT - Point2PointConstraint* getPoint2PointConstraint(int constraintId); + TypedConstraint* getConstraintById(int constraintId); virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ); @@ -139,6 +140,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment const PersistentManifold* GetManifold(int index) const; + std::vector<TypedConstraint*> m_constraints; + private: @@ -146,7 +149,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment std::vector<CcdPhysicsController*> m_controllers; - std::vector<Point2PointConstraint*> m_p2pConstraints; + std::vector<WrapperVehicle*> m_wrapperVehicles; diff --git a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h index d85efa32829..8e60fdb126e 100644 --- a/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h +++ b/extern/bullet/Extras/PhysicsInterface/Common/PHY_DynamicTypes.h @@ -1,15 +1,18 @@ /* - * Copyright (c) 2001-2005 Erwin Coumans <phy@erwincoumans.com> - * - * Permission to use, copy, modify, distribute and sell this software - * and its documentation for any purpose is hereby granted without fee, - * provided that the above copyright notice appear in all copies and - * that both that copyright notice and this permission notice appear - * in supporting documentation. Erwin Coumans makes no - * representations about the suitability of this software for any - * purpose. It is provided "as is" without express or implied warranty. - * - */ +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + #ifndef __PHY_DYNAMIC_TYPES #define __PHY_DYNAMIC_TYPES diff --git a/extern/bullet/continuous_vc8.sln b/extern/bullet/continuous_vc8.sln index 38a29f14627..530552fd679 100644 --- a/extern/bullet/continuous_vc8.sln +++ b/extern/bullet/continuous_vc8.sln @@ -57,6 +57,15 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ConvexHullDistance", "Demos EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ConcaveDemo", "Demos\ConcaveDemo\ConcaveDemo_vc8.vcproj", "{F20F38CC-CFA7-40C7-81AE-945C8C09E473}" ProjectSection(ProjectDependencies) = postProject + {3427A9EF-FF84-4B3E-9AE5-C37CF44D7DA4} = {3427A9EF-FF84-4B3E-9AE5-C37CF44D7DA4} + {FFD3C64A-30E2-4BC7-BC8F-51818C320400} = {FFD3C64A-30E2-4BC7-BC8F-51818C320400} + {E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE} = {E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE} + {47328C1E-54A4-4065-B3DA-F88EE281D1E5} = {47328C1E-54A4-4065-B3DA-F88EE281D1E5} + {87D8C006-6DCC-4156-A03E-8CEA1B4C0580} = {87D8C006-6DCC-4156-A03E-8CEA1B4C0580} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ConstraintDemo", "Demos\ConstraintDemo\ConstraintDemo_vc8.vcproj", "{26C8AAF7-D504-4341-A882-7D082E9DED5F}" + ProjectSection(ProjectDependencies) = postProject {87D8C006-6DCC-4156-A03E-8CEA1B4C0580} = {87D8C006-6DCC-4156-A03E-8CEA1B4C0580} {47328C1E-54A4-4065-B3DA-F88EE281D1E5} = {47328C1E-54A4-4065-B3DA-F88EE281D1E5} {E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE} = {E3D57F36-36AC-4E9A-B0E0-2AE4510FCBFE} @@ -122,6 +131,10 @@ Global {F20F38CC-CFA7-40C7-81AE-945C8C09E473}.Debug|Win32.Build.0 = Debug|Win32 {F20F38CC-CFA7-40C7-81AE-945C8C09E473}.Release|Win32.ActiveCfg = Release|Win32 {F20F38CC-CFA7-40C7-81AE-945C8C09E473}.Release|Win32.Build.0 = Release|Win32 + {26C8AAF7-D504-4341-A882-7D082E9DED5F}.Debug|Win32.ActiveCfg = Debug|Win32 + {26C8AAF7-D504-4341-A882-7D082E9DED5F}.Debug|Win32.Build.0 = Debug|Win32 + {26C8AAF7-D504-4341-A882-7D082E9DED5F}.Release|Win32.ActiveCfg = Release|Win32 + {26C8AAF7-D504-4341-A882-7D082E9DED5F}.Release|Win32.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp index 2c4f98c5624..e4d0e49d5e8 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.cpp @@ -49,6 +49,9 @@ RaycastVehicle::VehicleTuning gTuning; #include "ConstraintSolver/ConstraintSolver.h" #include "ConstraintSolver/Point2PointConstraint.h" +#include "ConstraintSolver/HingeConstraint.h" + + //#include "BroadphaseCollision/QueryDispatcher.h" //#include "BroadphaseCollision/QueryBox.h" //todo: change this to allow dynamic registration of types! @@ -315,6 +318,7 @@ m_solverType(-1) //broadphase = new SimpleBroadphase(); } + setSolverType(1); m_collisionWorld = new CollisionWorld(dispatcher,broadphase); @@ -392,16 +396,16 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr //also remove constraint { - std::vector<Point2PointConstraint*>::iterator i; + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || - (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetUserConstraintId()); + removeConstraint(constraint->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -409,16 +413,16 @@ void CcdPhysicsEnvironment::removeCcdPhysicsController(CcdPhysicsController* ctr } { - std::vector<Point2PointConstraint*>::iterator i; + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if ((&p2p->GetRigidBodyA() == ctrl->GetRigidBody() || - (&p2p->GetRigidBodyB() == ctrl->GetRigidBody()))) + TypedConstraint* constraint = (*i); + if ((&constraint->GetRigidBodyA() == ctrl->GetRigidBody() || + (&constraint->GetRigidBodyB() == ctrl->GetRigidBody()))) { - removeConstraint(p2p->GetUserConstraintId()); + removeConstraint(constraint->GetUserConstraintId()); //only 1 constraint per constroller break; } @@ -451,9 +455,14 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) if (!SimdFuzzyZero(timeStep)) { - //Blender runs 30hertz, so subdivide so we get 60 hertz + +#define SPLIT_TIMESTEP 1 +#ifdef SPLIT_TIMESTEP proceedDeltaTimeOneStep(0.5f*timeStep); proceedDeltaTimeOneStep(0.5f*timeStep); +#else + proceedDeltaTimeOneStep(timeStep); +#endif } else { //todo: interpolate @@ -524,7 +533,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) DispatcherInfo dispatchInfo; dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = m_debugDrawer; scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g); @@ -542,7 +550,46 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) //contacts - struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback + //solve the regular constraints (point 2 point, hinge, etc) + + for (int g=0;g<numsubstep;g++) + { + // + // constraint solving + // + + + int i; + int numConstraints = m_constraints.size(); + + //point to point constraints + for (i=0;i< numConstraints ; i++ ) + { + TypedConstraint* constraint = m_constraints[i]; + + constraint->BuildJacobian(); + constraint->SolveConstraint( timeStep ); + + } + + + } + + //solve the vehicles + + #ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;i<numVehicles;i++) + { + WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; + RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); + vehicle->UpdateVehicle( timeStep); + } +#endif //NEW_BULLET_VEHICLE_SUPPORT + + + struct InplaceSolverIslandCallback : public CollisionDispatcher::IslandCallback { ContactSolverInfo& m_solverInfo; @@ -578,47 +625,10 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) m_solver, m_debugDrawer); + /// solve all the contact points and contact friction GetDispatcher()->BuildAndProcessIslands(numRigidBodies,&solverCallback); - for (int g=0;g<numsubstep;g++) - { - // - // constraint solving - // - - - int i; - int numPoint2Point = m_p2pConstraints.size(); - - //point to point constraints - for (i=0;i< numPoint2Point ; i++ ) - { - Point2PointConstraint* p2p = m_p2pConstraints[i]; - - p2p->BuildJacobian(); - p2p->SolveConstraint( timeStep ); - - } - - - - - } - - - #ifdef NEW_BULLET_VEHICLE_SUPPORT - //vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;i<numVehicles;i++) - { - WrapperVehicle* wrapperVehicle = m_wrapperVehicles[i]; - RaycastVehicle* vehicle = wrapperVehicle->GetVehicle(); - vehicle->UpdateVehicle( timeStep); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - { @@ -883,7 +893,7 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) { m_solver = new SimpleConstraintSolver(); - //printf("Iterative Impulse ConstraintSolver\n"); + break; } } @@ -893,7 +903,6 @@ void CcdPhysicsEnvironment::setSolverType(int solverType) if (m_solverType != solverType) { m_solver = new OdeConstraintSolver(); - //printf("Quickstep ConstraintSolver\n"); break; } @@ -944,13 +953,13 @@ void CcdPhysicsEnvironment::setGravity(float x,float y,float z) #ifdef NEW_BULLET_VEHICLE_SUPPORT -class BlenderVehicleRaycaster : public VehicleRaycaster +class DefaultVehicleRaycaster : public VehicleRaycaster { CcdPhysicsEnvironment* m_physEnv; PHY_IPhysicsController* m_chassis; public: - BlenderVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): + DefaultVehicleRaycaster(CcdPhysicsEnvironment* physEnv,PHY_IPhysicsController* chassis): m_physEnv(physEnv), m_chassis(chassis) { @@ -1030,7 +1039,11 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl SimdVector3 pivotInA(pivotX,pivotY,pivotZ); SimdVector3 pivotInB = rb1 ? rb1->getCenterOfMassTransform().inverse()(rb0->getCenterOfMassTransform()(pivotInA)) : pivotInA; - + SimdVector3 axisInA(axisX,axisY,axisZ); + SimdVector3 axisInB = rb1 ? + (rb1->getCenterOfMassTransform().getBasis().inverse()*(rb0->getCenterOfMassTransform().getBasis() * -axisInA)) : + rb0->getCenterOfMassTransform().getBasis() * -axisInA; + switch (type) { case PHY_POINT2POINT_CONSTRAINT: @@ -1048,7 +1061,7 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl pivotInA); } - m_p2pConstraints.push_back(p2p); + m_constraints.push_back(p2p); p2p->SetUserConstraintId(gConstraintUid++); p2p->SetUserConstraintType(type); //64 bit systems can't cast pointer to int. could use size_t instead. @@ -1056,13 +1069,39 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl break; } + + case PHY_LINEHINGE_CONSTRAINT: + { + HingeConstraint* hinge = 0; + + if (rb1) + { + hinge = new HingeConstraint( + *rb0, + *rb1,pivotInA,pivotInB,axisInA,axisInB); + + + } else + { + hinge = new HingeConstraint(*rb0, + pivotInA,axisInA); + + } + + m_constraints.push_back(hinge); + hinge->SetUserConstraintId(gConstraintUid++); + hinge->SetUserConstraintType(type); + //64 bit systems can't cast pointer to int. could use size_t instead. + return hinge->GetUserConstraintId(); + break; + } #ifdef NEW_BULLET_VEHICLE_SUPPORT case PHY_VEHICLE_CONSTRAINT: { RaycastVehicle::VehicleTuning* tuning = new RaycastVehicle::VehicleTuning(); RigidBody* chassis = rb0; - BlenderVehicleRaycaster* raycaster = new BlenderVehicleRaycaster(this,ctrl0); + DefaultVehicleRaycaster* raycaster = new DefaultVehicleRaycaster(this,ctrl0); RaycastVehicle* vehicle = new RaycastVehicle(*tuning,chassis,raycaster); WrapperVehicle* wrapperVehicle = new WrapperVehicle(vehicle,ctrl0); m_wrapperVehicles.push_back(wrapperVehicle); @@ -1087,19 +1126,16 @@ int CcdPhysicsEnvironment::createConstraint(class PHY_IPhysicsController* ctrl void CcdPhysicsEnvironment::removeConstraint(int constraintId) { - std::vector<Point2PointConstraint*>::iterator i; - - //std::find(m_p2pConstraints.begin(), m_p2pConstraints.end(), - // (Point2PointConstraint *)p2p); + std::vector<TypedConstraint*>::iterator i; - for (i=m_p2pConstraints.begin(); - !(i==m_p2pConstraints.end()); i++) + for (i=m_constraints.begin(); + !(i==m_constraints.end()); i++) { - Point2PointConstraint* p2p = (*i); - if (p2p->GetUserConstraintId() == constraintId) + TypedConstraint* constraint = (*i); + if (constraint->GetUserConstraintId() == constraintId) { - std::swap(*i, m_p2pConstraints.back()); - m_p2pConstraints.pop_back(); + std::swap(*i, m_constraints.back()); + m_constraints.pop_back(); break; } } @@ -1301,6 +1337,20 @@ const PersistentManifold* CcdPhysicsEnvironment::GetManifold(int index) const return GetDispatcher()->GetManifoldByIndexInternal(index); } +TypedConstraint* CcdPhysicsEnvironment::getConstraintById(int constraintId) +{ + int numConstraint = m_constraints.size(); + int i; + for (i=0;i<numConstraint;i++) + { + TypedConstraint* constraint = m_constraints[i]; + if (constraint->GetUserConstraintId()==constraintId) + { + return constraint; + } + } + return 0; +} #ifdef NEW_BULLET_VEHICLE_SUPPORT diff --git a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h index 46a240056fc..311e74eaad8 100644 --- a/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h +++ b/source/gameengine/Physics/Bullet/CcdPhysicsEnvironment.h @@ -8,7 +8,8 @@ class CcdPhysicsController; -class Point2PointConstraint; +class TypedConstraint; + class CollisionDispatcher; class Dispatcher; //#include "BroadphaseInterface.h" @@ -90,12 +91,14 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment //complex constraint for vehicles virtual PHY_IVehicle* getVehicleConstraint(int constraintId); #else - virtual PHY_IVehicle* getVehicleConstraint(int constraintId) + virtual class PHY_IVehicle* getVehicleConstraint(int constraintId) { return 0; } #endif //NEW_BULLET_VEHICLE_SUPPORT + TypedConstraint* getConstraintById(int constraintId); + virtual PHY_IPhysicsController* rayTest(PHY_IPhysicsController* ignoreClient, float fromX,float fromY,float fromZ, float toX,float toY,float toZ, float& hitX,float& hitY,float& hitZ,float& normalX,float& normalY,float& normalZ); @@ -137,6 +140,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment const PersistentManifold* GetManifold(int index) const; + std::vector<TypedConstraint*> m_constraints; + private: @@ -144,7 +149,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment std::vector<CcdPhysicsController*> m_controllers; - std::vector<Point2PointConstraint*> m_p2pConstraints; + std::vector<WrapperVehicle*> m_wrapperVehicles; |