From dad6ef9045526b6b26f845151d3ea3b26edaf4a8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 29 Jul 2005 18:14:41 +0000 Subject: added physics-debugging didn't check every single file, so might have broken some gameengine stuff, will fix it this weekend! --- extern/bullet/Bullet/CollisionShapes/BoxShape.cpp | 13 +++- extern/bullet/Bullet/CollisionShapes/BoxShape.h | 5 +- .../Bullet/CollisionShapes/ConvexHullShape.cpp | 6 +- .../Bullet/NarrowPhaseCollision/CollisionMargin.h | 2 +- .../Bullet/NarrowPhaseCollision/ConvexCast.h | 12 +++- .../NarrowPhaseCollision/PersistentManifold.cpp | 4 ++ .../NarrowPhaseCollision/PersistentManifold.h | 9 ++- .../CollisionDispatch/ConvexConvexAlgorithm.cpp | 2 + .../CollisionDispatch/ToiContactDispatcher.cpp | 31 +++------ .../CollisionDispatch/ToiContactDispatcher.h | 6 +- .../ConstraintSolver/ConstraintSolver.h | 3 +- .../ConstraintSolver/ContactConstraint.cpp | 36 +++++++--- .../ConstraintSolver/OdeConstraintSolver.cpp | 32 +++++---- .../ConstraintSolver/OdeConstraintSolver.h | 17 ++++- .../ConstraintSolver/Point2PointConstraint.cpp | 2 +- .../ConstraintSolver/SimpleConstraintSolver.cpp | 12 +++- .../ConstraintSolver/SimpleConstraintSolver.h | 8 ++- .../BulletDynamics/Dynamics/ContactJoint.cpp | 14 ++-- .../bullet/BulletDynamics/Dynamics/RigidBody.cpp | 6 +- extern/bullet/BulletDynamics/Dynamics/RigidBody.h | 20 +++++- .../CcdPhysics/CcdPhysicsController.cpp | 2 +- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 77 +++++++++++++++------- .../CcdPhysics/CcdPhysicsEnvironment.h | 9 +-- extern/bullet/LinearMath/IDebugDraw.h | 42 ++++++++++++ 24 files changed, 256 insertions(+), 114 deletions(-) create mode 100644 extern/bullet/LinearMath/IDebugDraw.h (limited to 'extern') diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp index 4edd8cf9a12..0fa063a2e17 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp @@ -11,6 +11,14 @@ #include "BoxShape.h" +SimdVector3 BoxShape::GetHalfExtents() const +{ + SimdVector3 scalingCorrectedHalfExtents = m_boxHalfExtents1 * m_localScaling; + SimdVector3 marginCorrection (CONVEX_DISTANCE_MARGIN,CONVEX_DISTANCE_MARGIN,CONVEX_DISTANCE_MARGIN); + return (scalingCorrectedHalfExtents - marginCorrection).absolute(); +} + + void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const { @@ -25,8 +33,9 @@ void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& //todo: this is a quick fix, we need to enlarge the aabb dependent on several criteria - //extent += SimdVector3(.2f,.2f,.2f); - +// SimdVector3 extra(1,1,1); +// extent += extra; + aabbMin = center - extent; aabbMax = center + extent; diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h index 620ddd7e4f6..9d7806fc522 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h @@ -32,10 +32,7 @@ public: } - SimdVector3 GetHalfExtents() const{ return m_boxHalfExtents1 * m_localScaling;} - //const SimdVector3& GetHalfExtents() const{ return m_boxHalfExtents1;} - - + SimdVector3 GetHalfExtents() const; virtual int GetShapeType() const { return BOX_SHAPE_PROXYTYPE;} diff --git a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp index 0206324fa04..8e493ba5bc0 100644 --- a/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/ConvexHullShape.cpp @@ -118,13 +118,13 @@ void ConvexHullShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const int index0 = i%m_points.size(); int index1 = i/m_points.size(); - pa = m_points[index0]; - pb = m_points[index1]; + pa = m_points[index0]*m_localScaling; + pb = m_points[index1]*m_localScaling; } void ConvexHullShape::GetVertex(int i,SimdPoint3& vtx) const { - vtx = m_points[i]; + vtx = m_points[i]*m_localScaling; } int ConvexHullShape::GetNumPlanes() const diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h b/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h index 7e51dc7f2b3..41009dcd110 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/CollisionMargin.h @@ -3,7 +3,7 @@ //used by Gjk and some other algorithms -#define CONVEX_DISTANCE_MARGIN 0.05f// 0.1f//;//0.01f +#define CONVEX_DISTANCE_MARGIN 0.04f// 0.1f//;//0.01f diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h b/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h index f32887f6472..2e7fa0e8eea 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/ConvexCast.h @@ -16,6 +16,7 @@ #include #include class MinkowskiSumShape; +#include "IDebugDraw.h" /// ConvexCast is an interface for Casting class ConvexCast @@ -30,18 +31,23 @@ public: struct CastResult { //virtual bool addRayResult(const SimdVector3& normal,SimdScalar fraction) = 0; - + virtual void DebugDraw(SimdScalar fraction) {} virtual void DrawCoordSystem(const SimdTransform& trans) {} + CastResult() - :m_fraction(1e30f) + :m_fraction(1e30f), + m_debugDrawer(0) { } SimdVector3 m_normal; SimdScalar m_fraction; SimdTransform m_hitTransformA; - SimdTransform m_hitTransformB; + SimdTransform m_hitTransformB; + + IDebugDraw* m_debugDrawer; + }; diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp index 9085ffa0ef0..29b13f67b8f 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -111,6 +111,10 @@ void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint) ReplaceContactPoint(newPoint,insertIndex); } +float PersistentManifold::GetManifoldMargin() const +{ + return 0.02f; +} void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB) { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h index c1d0314069e..96f91d5900e 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.h @@ -37,6 +37,8 @@ class PersistentManifold /// sort cached points so most isolated points come first int SortCachedPoints(const ManifoldPoint& pt); + int FindContactPoint(const ManifoldPoint* unUsed, int numUnused,const ManifoldPoint& pt); + public: int m_index1; @@ -77,11 +79,8 @@ public: } /// todo: get this margin from the current physics / collision environment - inline float GetManifoldMargin() const - { - return 0.02f; - } - + float GetManifoldMargin() const; + int GetCacheEntry(const ManifoldPoint& newPoint) const; void AddManifoldPoint( const ManifoldPoint& newPoint); diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp index 42387ba271e..793c94804c0 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -175,6 +175,8 @@ m_collisionImpulse = 0.f; input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetManifoldMargin(); input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } + + input.m_maximumDistanceSquared = 1e30;// input.m_transformA = body0->getCenterOfMassTransform(); input.m_transformB = body1->getCenterOfMassTransform(); diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp index fab0d7e384d..ec566349bad 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp @@ -86,18 +86,15 @@ void ToiContactDispatcher::ReleaseManifold(PersistentManifold* manifold) // // todo: this is random access, it can be walked 'cache friendly'! // -void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,int numRigidBodies) +void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,int numRigidBodies,IDebugDraw* debugDrawer) { int i; - int numManifolds; for (int islandId=0;islandId islandmanifold; //int numSleeping = 0; @@ -116,16 +113,15 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in allSleeping = false; } - manifolds[numManifolds] = manifold; - numManifolds++; + islandmanifold.push_back(manifold); } } if (allSleeping) { //tag all as 'ISLAND_SLEEPING' - for (i=0;iGetBody0())) { ((RigidBody*)manifold->GetBody0())->SetActivationState( ISLAND_SLEEPING ); @@ -140,9 +136,9 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in { //tag all as 'ISLAND_SLEEPING' - for (i=0;iGetBody0())) { if ( ((RigidBody*)manifold->GetBody0())->GetActivationState() == ISLAND_SLEEPING) @@ -169,19 +165,8 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in info.m_tau = 0.4f; info.m_restitution = 0.1f;//m_restitution; - /* - int numManifolds = 0; - for (i=0;iGetBody0())); - //ASSERT(((RigidBody*)manifold->GetBody1())); - manifolds[i] = manifold; - numManifolds++; - } -*/ - m_solver->SolveGroup( manifolds, numManifolds,info ); + m_solver->SolveGroup( &islandmanifold[0], islandmanifold.size(),info,debugDrawer ); } } diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h index 818901cb901..97897a69dc4 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.h @@ -7,13 +7,13 @@ #include "BroadphaseCollision/BroadphaseProxy.h" class ConstraintSolver; +class IDebugDraw; //island management #define ACTIVE_TAG 1 #define ISLAND_SLEEPING 2 #define WANTS_DEACTIVATION 3 -#define MAX_MANIFOLDS 512 struct CollisionAlgorithmCreateFunc { @@ -40,8 +40,6 @@ class ToiContactDispatcher : public Dispatcher std::vector m_manifoldsPtr; -// PersistentManifold m_manifolds[MAX_MANIFOLDS]; -// int m_freeManifolds[MAX_MANIFOLDS]; UnionFind m_unionFind; ConstraintSolver* m_solver; @@ -86,7 +84,7 @@ public: // // todo: this is random access, it can be walked 'cache friendly'! // - virtual void SolveConstraints(float timeStep, int numIterations,int numRigidBodies); + virtual void SolveConstraints(float timeStep, int numIterations,int numRigidBodies,IDebugDraw* debugDrawer); CollisionAlgorithm* FindAlgorithm(BroadphaseProxy& proxy0,BroadphaseProxy& proxy1) diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h index e1512ae1756..84015929586 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ConstraintSolver.h @@ -16,6 +16,7 @@ class RigidBody; struct ContactSolverInfo; struct BroadphaseProxy; +class IDebugDraw; /// ConstraintSolver provides solver interface class ConstraintSolver @@ -25,7 +26,7 @@ public: virtual ~ConstraintSolver() {} - virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info) = 0; + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,class IDebugDraw* debugDrawer = 0) = 0; }; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp index e3603ccec67..a23d5c13f7f 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/ContactConstraint.cpp @@ -23,16 +23,29 @@ static SimdScalar ContactThreshold = -10.0f; float useGlobalSettingContacts = false;//true; SimdScalar contactDamping = 0.9f; -SimdScalar contactTau = .2f;//0.02f;//*0.02f; +SimdScalar contactTau = .02f;//0.02f;//*0.02f; SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution) { - return restitution; + return 0.f; // return restitution * GEN_min(1.0f, rel_vel / ContactThreshold); } +float MAX_FRICTION = 100.f; + +SimdScalar calculateCombinedFriction(RigidBody& body0,RigidBody& body1) +{ + SimdScalar friction = body0.getFriction() * body1.getFriction(); + if (friction < 0.f) + friction = 0.f; + if (friction > MAX_FRICTION) + friction = MAX_FRICTION; + return friction; + +} + void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1, RigidBody& body2, const SimdVector3& pos2, const SimdVector3& normal,float normalImpulse, @@ -51,6 +64,8 @@ void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1, SimdScalar rel_vel = normal.dot(vel); + float combinedFriction = calculateCombinedFriction(body1,body2); + #define PER_CONTACT_FRICTION #ifdef PER_CONTACT_FRICTION SimdVector3 lat_vel = vel - normal * rel_vel; @@ -63,7 +78,7 @@ void applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1, SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); SimdScalar frictionMaxImpulse = lat_rel_vel / (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); - SimdScalar frictionImpulse = (normalImpulse) * solverInfo.m_friction; + SimdScalar frictionImpulse = (normalImpulse) * combinedFriction; GEN_set_min(frictionImpulse,frictionMaxImpulse ); body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1); @@ -129,7 +144,6 @@ void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1, SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); - float contactDamping = 0.9f; impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv; //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; @@ -167,9 +181,11 @@ float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1, // if (rel_vel< 0.f)//-SIMD_EPSILON) // { - SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution); + float combinedRestitution = body1.getRestitution() * body2.getRestitution(); + + SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution); - SimdScalar timeCorrection = 1.f;///timeStep; + SimdScalar timeCorrection = 360.f*solverInfo.m_timeStep; float damping = solverInfo.m_damping ; float tau = solverInfo.m_tau; @@ -252,7 +268,9 @@ SimdScalar rel_vel; // if (rel_vel< 0.f)//-SIMD_EPSILON) // { - SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution); + float combinedRestitution = body1.getRestitution() * body2.getRestitution(); + + SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution); // SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass()); SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ; @@ -302,6 +320,8 @@ SimdScalar rel_vel; SimdVector3 lat_vel = vel - normal * rel_vel; SimdScalar lat_rel_vel = lat_vel.length(); + float combinedFriction = calculateCombinedFriction(body1,body2); + if (lat_rel_vel > SIMD_EPSILON) { lat_vel /= lat_rel_vel; @@ -310,7 +330,7 @@ SimdScalar rel_vel; friction_impulse = lat_rel_vel / (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); SimdScalar normal_impulse = (penetrationImpulse+ - velocityImpulse) * solverInfo.m_friction; + velocityImpulse) * combinedFriction; GEN_set_min(friction_impulse, normal_impulse); body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp index 8dc666d588f..37b3668bbf7 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp @@ -19,6 +19,8 @@ #include "Dynamics/BU_Joint.h" #include "Dynamics/ContactJoint.h" +#include "IDebugDraw.h" + #define USE_SOR_SOLVER #include "SorLcp.h" @@ -41,8 +43,7 @@ class BU_Joint; //see below -int gCurBody = 0; -int gCurJoint= 0; + int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, @@ -53,10 +54,10 @@ void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJo //iterative lcp and penalty method -float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal) +float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) { - gCurBody = 0; - gCurJoint = 0; + m_CurBody = 0; + m_CurJoint = 0; float cfm = 1e-5f; float erp = 0.2f; @@ -77,7 +78,7 @@ float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numM { body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); - ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1); + ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer); } } @@ -112,7 +113,7 @@ void dRfromQ1 (dMatrix3 R, const dQuaternion q) -int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) +int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) { if (!body || (body->getInvMass() == 0.f) ) { @@ -195,8 +196,8 @@ int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) static ContactJoint gJointArray[MAX_JOINTS_1]; -void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, - RigidBody** bodies,int _bodyId0,int _bodyId1) +void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer) { @@ -228,14 +229,21 @@ void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJo assert(bodyId0 >= 0); + SimdVector3 color(0,1,0); for (i=0;iDrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color); + debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color); + + } + assert (m_CurJoint < MAX_JOINTS_1); if (manifold->GetContactPoint(i).GetDistance() < 0.f) { - ContactJoint* cont = new (&gJointArray[gCurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); + ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); cont->node[0].joint = cont; cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h index 7ae8d0eaa49..354a444caa8 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.h @@ -13,15 +13,28 @@ #include "ConstraintSolver.h" +class RigidBody; +class BU_Joint; +/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework +/// It uses the the unmodified version of quickstep solver from the open dynamics project class OdeConstraintSolver : public ConstraintSolver { - +private: + + int m_CurBody; + int m_CurJoint; + + + int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); + void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer); + public: virtual ~OdeConstraintSolver() {} - virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info); + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info,IDebugDraw* debugDrawer = 0); }; diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp index 89b554cdad9..0c46b8fc47d 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/Point2PointConstraint.cpp @@ -13,7 +13,7 @@ #include "Dynamics/MassProps.h" -static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f); +static RigidBody s_fixed(MassProps(0,SimdVector3(0.f,0.f,0.f)),0.f,0.f,1.f,1.f); Point2PointConstraint::Point2PointConstraint(): m_rbA(s_fixed),m_rbB(s_fixed) diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp index 3243140b823..501b0a763e2 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.cpp @@ -16,6 +16,7 @@ #include "ContactSolverInfo.h" #include "Dynamics/BU_Joint.h" #include "Dynamics/ContactJoint.h" +#include "IDebugDraw.h" //debugging bool doApplyImpulse = true; @@ -28,7 +29,7 @@ bool useImpulseFriction = true;//true;//false; //iterative lcp and penalty method -float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal) +float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer) { ContactSolverInfo info = infoGlobal; @@ -41,7 +42,7 @@ float SimpleConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int n { for (int j=0;jGetBody0(); @@ -73,6 +74,7 @@ float SimpleConstraintSolver::Solve(PersistentManifold* manifoldPtr, const Conta { const int numpoints = manifoldPtr->GetNumContacts(); + SimdVector3 color(0,1,0); for (int i=0;iGetContactPoint(j); + + if (debugDrawer) + debugDrawer->DrawLine(cp.m_positionWorldOnA,cp.m_positionWorldOnB,color); + { diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h index 6bdcf3a2ac2..a85683b041a 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SimpleConstraintSolver.h @@ -12,17 +12,19 @@ #define SIMPLE_CONSTRAINT_SOLVER_H #include "ConstraintSolver.h" +class IDebugDraw; - +/// SimpleConstraintSolver uses a Propagation Method +/// Applies impulses for combined restitution and penetration recovery and to simulate friction class SimpleConstraintSolver : public ConstraintSolver { - float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter); + float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); public: virtual ~SimpleConstraintSolver() {} - virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info); + virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); }; diff --git a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp index 130a6b98402..7ac19a6d6a2 100644 --- a/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/ContactJoint.cpp @@ -15,7 +15,7 @@ m_body1(body1) int m_numRows = 3; -float gContactFrictionFactor = 30.5f;//100.f;//1e30f;//2.9f; +//float gContactFrictionFactor = 0.f;//12.f;//30.5f;//100.f;//1e30f;//2.9f; @@ -149,6 +149,9 @@ void ContactJoint::GetInfo2(Info2 *info) c2[0] = ccc2[0]; c2[1] = ccc2[1]; c2[2] = ccc2[2]; + + float friction = m_body0->getFriction() * m_body1->getFriction(); + // first friction direction if (m_numRows >= 2) { @@ -175,8 +178,9 @@ void ContactJoint::GetInfo2(Info2 *info) // mode //1e30f - info->lo[1] = -gContactFrictionFactor;//-j->contact.surface.mu; - info->hi[1] = gContactFrictionFactor;//j->contact.surface.mu; + + info->lo[1] = -friction;//-j->contact.surface.mu; + info->hi[1] = friction;//j->contact.surface.mu; if (0)//j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; @@ -210,8 +214,8 @@ void ContactJoint::GetInfo2(Info2 *info) //info->hi[2] = j->contact.surface.mu2; } else { - info->lo[2] = -gContactFrictionFactor; - info->hi[2] = gContactFrictionFactor; + info->lo[2] = -friction; + info->hi[2] = friction; } if (0)//j->contact.surface.mode & dContactApprox1_2) diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp index 10dccd3dd20..31d20930a10 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp @@ -7,7 +7,7 @@ float gRigidBodyDamping = 0.5f; static int uniqueId = 0; -RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping) +RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution) : m_collisionShape(0), m_activationState1(1), m_hitFraction(1.f), @@ -17,7 +17,9 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc m_totalForce(0.0f, 0.0f, 0.0f), m_totalTorque(0.0f, 0.0f, 0.0f), m_linearVelocity(0.0f, 0.0f, 0.0f), - m_angularVelocity(0.f,0.f,0.f) + m_angularVelocity(0.f,0.f,0.f), + m_restitution(restitution), + m_friction(friction) { m_debugBodyId = uniqueId++; diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h index ec7eb9a8678..527e37bc57d 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h @@ -15,7 +15,7 @@ typedef SimdScalar dMatrix3[4*3]; class RigidBody { public: - RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping); + RigidBody(const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution); void proceedToTransform(const SimdTransform& newTrans); @@ -123,6 +123,22 @@ public: int GetActivationState() const { return m_activationState1;} void SetActivationState(int newState); + void setRestitution(float rest) + { + m_restitution = rest; + } + float getRestitution() const + { + return m_restitution; + } + void setFriction(float frict) + { + m_friction = frict; + } + float getFriction() const + { + return m_friction; + } private: SimdTransform m_worldTransform; @@ -141,6 +157,8 @@ private: SimdScalar m_angularDamping; SimdScalar m_inverseMass; + SimdScalar m_friction; + SimdScalar m_restitution; CollisionShape* m_collisionShape; diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp index 376a75f40d3..8b423204bec 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp @@ -7,7 +7,7 @@ class BP_Proxy; -bool gEnableSleeping = true;//false;//true; +bool gEnableSleeping = false;//false;//true; #include "Dynamics/MassProps.h" SimdVector3 startVel(0,0,0);//-10000); diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 2a89cd0cf4a..885304983a6 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -14,6 +14,7 @@ #include "ConstraintSolver/OdeConstraintSolver.h" #include "ConstraintSolver/SimpleConstraintSolver.h" +#include "IDebugDraw.h" #include "CollisionDispatch/ToiContactDispatcher.h" @@ -32,7 +33,7 @@ bool useIslands = true; //#include "BroadphaseCollision/QueryBox.h" //todo: change this to allow dynamic registration of types! -unsigned long gNumIterations = 10; +unsigned long gNumIterations = 1; #ifdef WIN32 void DrawRasterizerLine(const float* from,const float* to,int color); @@ -47,6 +48,43 @@ void DrawRasterizerLine(const float* from,const float* to,int color); + +static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color) +{ + SimdVector3 halfExtents = (to-from)* 0.5f; + SimdVector3 center = (to+from) *0.5f; + int i,j; + + SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb; + for (i=0;i<4;i++) + { + for (j=0;j<3;j++) + { + pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pa+=center; + + int othercoord = j%3; + edgecoord[othercoord]*=-1.f; + pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], + edgecoord[2]*halfExtents[2]); + pb+=center; + + debugDrawer->DrawLine(pa,pb,color); + } + edgecoord = SimdVector3(-1.f,-1.f,-1.f); + if (i<3) + edgecoord[i]*=-1.f; + } + + +} + + + + + + CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp) :m_dispatcher(dispatcher), m_broadphase(bp), @@ -276,10 +314,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) //m_scalingPropagated = true; } -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.begin("integrate force"); -#endif //EXTRA_PHYSICS_PROFILE - { @@ -301,9 +335,6 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) } } -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.end("integrate force"); -#endif //EXTRA_PHYSICS_PROFILE BroadphaseInterface* scene = m_broadphase; @@ -321,33 +352,21 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) DispatcherInfo dispatchInfo; dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.begin("cd"); -#endif //EXTRA_PHYSICS_PROFILE scene->DispatchAllCollisionPairs(*m_dispatcher,dispatchInfo);///numsubstep,g); -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.end("cd"); -#endif //EXTRA_PHYSICS_PROFILE -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.begin("solver"); -#endif //EXTRA_PHYSICS_PROFILE int numRigidBodies = m_controllers.size(); UpdateActivationState(); //contacts - m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies); - -#ifdef EXTRA_PHYSICS_PROFILE - cpuProfile.end("solver"); -#endif //EXTRA_PHYSICS_PROFILE + + m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies,m_debugDrawer); for (int g=0;gGetCollisionShape(); + + + shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), body->getLinearVelocity(),body->getAngularVelocity(), timeStep,minAabb,maxAabb); + shapeinterface->GetAabb(body->getCenterOfMassTransform(), + minAabb,maxAabb); + BroadphaseProxy* bp = (BroadphaseProxy*) ctrl->m_broadphaseHandle; if (bp) @@ -413,8 +438,12 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) #ifdef WIN32 SimdVector3 color (1,0,0); - if (m_debugDrawer) - m_debugDrawer->DrawLine(minAabb,maxAabb,color); + if (0)//m_debugDrawer) + { + //draw aabb + + DrawAabb(m_debugDrawer,minAabb,maxAabb,color); + } #endif scene->SetAabb(bp,minAabb,maxAabb); } diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 88cf4b495cf..fc3786885b7 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -6,10 +6,6 @@ class CcdPhysicsController; #include "SimdVector3.h" -struct PHY_IPhysicsDebugDraw -{ - virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0; -}; class Point2PointConstraint; @@ -20,6 +16,7 @@ class Dispatcher; class Vehicle; class PersistentManifold; class BroadphaseInterface; +class IDebugDraw; /// Physics Environment takes care of stepping the simulation and is a container for physics entities. /// It stores rigidbodies,constraints, materials etc. @@ -28,7 +25,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment { SimdVector3 m_gravity; BroadphaseInterface* m_broadphase; - PHY_IPhysicsDebugDraw* m_debugDrawer; + IDebugDraw* m_debugDrawer; public: CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0); @@ -41,7 +38,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment /// Perform an integration step of duration 'timeStep'. - virtual void setDebugDrawer(PHY_IPhysicsDebugDraw* debugDrawer) + virtual void setDebugDrawer(IDebugDraw* debugDrawer) { m_debugDrawer = debugDrawer; } diff --git a/extern/bullet/LinearMath/IDebugDraw.h b/extern/bullet/LinearMath/IDebugDraw.h new file mode 100644 index 00000000000..b5cb24f74e5 --- /dev/null +++ b/extern/bullet/LinearMath/IDebugDraw.h @@ -0,0 +1,42 @@ +/* +Copyright (c) 2005 Gino van den Bergen / Erwin Coumans + +Permission is hereby granted, free of charge, to any person or organization +obtaining a copy of the software and accompanying documentation covered by +this license (the "Software") to use, reproduce, display, distribute, +execute, and transmit the Software, and to prepare derivative works of the +Software, and to permit third-parties to whom the Software is furnished to +do so, all subject to the following: + +The copyright notices in the Software and this entire statement, including +the above license grant, this restriction and the following disclaimer, +must be included in all copies of the Software, in whole or in part, and +all derivative works of the Software, unless such copies or derivative +works are solely in the form of machine-executable object code generated by +a source language processor. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT +SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE +FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +DEALINGS IN THE SOFTWARE. +*/ + + +#ifndef IDEBUG_DRAW__H +#define IDEBUG_DRAW__H + +#include "SimdVector3.h" + +class IDebugDraw +{ +public: + virtual void DrawLine(const SimdVector3& from,const SimdVector3& to,const SimdVector3& color)=0; + + virtual void SetDebugMode(int debugMode) =0; + +}; + +#endif //IDEBUG_DRAW__H \ No newline at end of file -- cgit v1.2.3