From 210ab537ce34a31714d5073ff2e539a67aaea9af Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 4 Aug 2005 19:07:39 +0000 Subject: improved deactivation, exposed more tweakable parameters to python, fixed some copy/paste bug in inertia/inverse inertia. colors in deactivation debug --- extern/bullet/Bullet/Bullet3_vc8.vcproj | 17 ++- extern/bullet/Bullet/CollisionShapes/BoxShape.cpp | 38 +++++ extern/bullet/Bullet/CollisionShapes/BoxShape.h | 21 +-- .../Bullet/NarrowPhaseCollision/GjkPairDetector.h | 4 + .../NarrowPhaseCollision/PersistentManifold.cpp | 2 +- .../ConvexConcaveCollisionAlgorithm.cpp | 5 + .../CollisionDispatch/ConvexConvexAlgorithm.cpp | 40 +++++- .../CollisionDispatch/ConvexConvexAlgorithm.h | 8 +- .../CollisionDispatch/ToiContactDispatcher.cpp | 15 +- .../ConstraintSolver/OdeConstraintSolver.cpp | 30 ++-- .../BulletDynamics/ConstraintSolver/SorLcp.cpp | 26 ++-- .../BulletDynamics/Dynamics/ContactJoint.cpp | 16 ++- .../bullet/BulletDynamics/Dynamics/RigidBody.cpp | 13 +- extern/bullet/BulletDynamics/Dynamics/RigidBody.h | 6 + .../CcdPhysics/CcdPhysicsController.cpp | 49 ++++--- .../CcdPhysics/CcdPhysicsController.h | 8 +- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 159 +++++++++++++++++---- .../CcdPhysics/CcdPhysicsEnvironment.h | 15 ++ extern/bullet/LinearMath/SimdQuadWord.h | 2 +- 19 files changed, 349 insertions(+), 125 deletions(-) (limited to 'extern') diff --git a/extern/bullet/Bullet/Bullet3_vc8.vcproj b/extern/bullet/Bullet/Bullet3_vc8.vcproj index c2f3e70180f..125b4b6e0b1 100644 --- a/extern/bullet/Bullet/Bullet3_vc8.vcproj +++ b/extern/bullet/Bullet/Bullet3_vc8.vcproj @@ -4,7 +4,6 @@ Version="8.00" Name="Bullet3ContinuousCollision" ProjectGUID="{FFD3C64A-30E2-4BC7-BC8F-51818C320400}" - SignManifests="true" > - - - - @@ -437,6 +428,14 @@ RelativePath=".\CollisionShapes\TriangleCallback.h" > + + + + diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp index 12fbd895bb3..1cc62263603 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.cpp @@ -38,3 +38,41 @@ void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& } + + +void BoxShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) +{ + float margin = 0.f; + SimdVector3 halfExtents = GetHalfExtents(); + + SimdScalar lx=2.f*(halfExtents.x()); + SimdScalar ly=2.f*(halfExtents.y()); + SimdScalar lz=2.f*(halfExtents.z()); + + inertia[0] = mass/(12.0f) * (ly*ly + lz*lz); + inertia[1] = mass/(12.0f) * (lx*lx + lz*lz); + inertia[2] = mass/(12.0f) * (lx*lx + ly*ly); + + +// float radius = GetHalfExtents().length(); +// SimdScalar elem = 0.4f * mass * radius*radius; +// inertia[0] = inertia[1] = inertia[2] = elem; +return; +/* + float margin = GetMargin(); + SimdVector3 halfExtents = GetHalfExtents(); + + SimdScalar lx=2.f*(halfExtents.x()+margin); + SimdScalar ly=2.f*(halfExtents.y()+margin); + SimdScalar lz=2.f*(halfExtents.z()+margin); + const SimdScalar x2 = lx*lx; + const SimdScalar y2 = ly*ly; + const SimdScalar z2 = lz*lz; + const SimdScalar scaledmass = mass * 0.08333333f; + + inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2)); +*/ +// inertia.x() = scaledmass * (y2+z2); +// inertia.y() = scaledmass * (x2+z2); +// inertia.z() = scaledmass * (x2+y2); +} \ No newline at end of file diff --git a/extern/bullet/Bullet/CollisionShapes/BoxShape.h b/extern/bullet/Bullet/CollisionShapes/BoxShape.h index 755de78dffb..451ae2370b6 100644 --- a/extern/bullet/Bullet/CollisionShapes/BoxShape.h +++ b/extern/bullet/Bullet/CollisionShapes/BoxShape.h @@ -84,26 +84,7 @@ public: - virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) - { - float margin = GetMargin(); - SimdVector3 halfExtents = GetHalfExtents(); - - SimdScalar lx=2.f*(halfExtents.x()+margin); - SimdScalar ly=2.f*(halfExtents.y()+margin); - SimdScalar lz=2.f*(halfExtents.z()+margin); - const SimdScalar x2 = lx*lx; - const SimdScalar y2 = ly*ly; - const SimdScalar z2 = lz*lz; - const SimdScalar scaledmass = mass * 0.08333333f; - - inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2)); - -// inertia.x() = scaledmass * (y2+z2); -// inertia.y() = scaledmass * (x2+z2); -// inertia.z() = scaledmass * (x2+y2); - } - + virtual void CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia); virtual void GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const { diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h index ad9e41682bf..70ff3c6568b 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h +++ b/extern/bullet/Bullet/NarrowPhaseCollision/GjkPairDetector.h @@ -55,6 +55,10 @@ public: m_cachedSeparatingAxis = seperatingAxis; } + void SetPenetrationDepthSolver(ConvexPenetrationDepthSolver* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } }; #endif //GJK_PAIR_DETECTOR_H diff --git a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp index 16cd219903d..3c094d0bab8 100644 --- a/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp +++ b/extern/bullet/Bullet/NarrowPhaseCollision/PersistentManifold.cpp @@ -14,7 +14,7 @@ #include "SimdTransform.h" #include -float gContactBreakingTreshold = 0.02f; +float gContactBreakingTreshold = 0.002f; PersistentManifold::PersistentManifold() :m_body0(0), diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp index c430aedad7b..a6f6a4df5a4 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cpp @@ -139,6 +139,11 @@ void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,Broadp RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject; RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject; + //todo: move this in the dispatcher + if ((convexbody->GetActivationState() == 2) &&(concavebody->GetActivationState() == 2)) + return; + + TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape(); if (m_convex.IsConvexShape()) diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp index 1b4b9c61917..7be5ff4d145 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.cpp @@ -32,6 +32,8 @@ #include "NarrowPhaseCollision/VoronoiSimplexSolver.h" #include "CollisionShapes/SphereShape.h" +#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h" + #ifdef WIN32 void DrawRasterizerLine(const float* from,const float* to,int color); @@ -50,22 +52,22 @@ bool gForceBoxBox = false;//false;//true; bool gBoxBoxUseGjk = true;//true;//false; bool gDisableConvexCollision = false; - +bool gUseEpa = false; ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1) : CollisionAlgorithm(ci), -m_gjkPairDetector(0,0,&m_simplexSolver,&m_penetrationDepthSolver), +m_gjkPairDetector(0,0,&m_simplexSolver,0), m_box0(*proxy0), m_box1(*proxy1), m_collisionImpulse(0.f), m_ownManifold (false), m_manifoldPtr(mf), -m_lowLevelOfDetail(false) - +m_lowLevelOfDetail(false), +m_useEpa(gUseEpa) { + CheckPenetrationDepthSolver(); - RigidBody* body0 = (RigidBody*)m_box0.m_clientObject; RigidBody* body1 = (RigidBody*)m_box1.m_clientObject; @@ -127,6 +129,22 @@ public: }; +void ConvexConvexAlgorithm::CheckPenetrationDepthSolver() +{ +// if (m_useEpa != gUseEpa) + { + m_useEpa = gUseEpa; + if (m_useEpa) + { + //not distributed + //m_gjkPairDetector.SetPenetrationDepthSolver(new Solid3EpaPenetrationDepth); + } else + { + m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver); + } + } + +} bool extra = false; float gFriction = 0.5f; @@ -135,13 +153,19 @@ float gFriction = 0.5f; // void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount, bool useContinuous) { - + CheckPenetrationDepthSolver(); + // printf("ConvexConvexAlgorithm::ProcessCollision\n"); m_collisionImpulse = 0.f; RigidBody* body0 = (RigidBody*)m_box0.m_clientObject; RigidBody* body1 = (RigidBody*)m_box1.m_clientObject; + //todo: move this in the dispatcher + if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2)) + return; + + if (!m_manifoldPtr) return; @@ -188,6 +212,8 @@ bool disableCcd = false; float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,float timeStep,int stepCount) { + CheckPenetrationDepthSolver(); + m_collisionImpulse = 0.f; RigidBody* body0 = (RigidBody*)m_box0.m_clientObject; @@ -222,7 +248,7 @@ float ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,Broad //SubsimplexConvexCast ccd(&voronoiSimplex); //GjkConvexCast ccd(&voronoiSimplex); - ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,&m_penetrationDepthSolver); + ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,m_penetrationDepthSolver); if (disableCcd) return 1.f; diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h index d77d6465d6b..c136e459a32 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ConvexConvexAlgorithm.h @@ -16,18 +16,16 @@ #include "NarrowPhaseCollision/PersistentManifold.h" #include "BroadphaseCollision/BroadphaseProxy.h" #include "NarrowPhaseCollision/VoronoiSimplexSolver.h" -#include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h" - class ConvexPenetrationDepthSolver; ///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. class ConvexConvexAlgorithm : public CollisionAlgorithm { - //hardcoded penetration and simplex solver, its easy to make this flexible later - MinkowskiPenetrationDepthSolver m_penetrationDepthSolver; + ConvexPenetrationDepthSolver* m_penetrationDepthSolver; VoronoiSimplexSolver m_simplexSolver; GjkPairDetector m_gjkPairDetector; + bool m_useEpa; public: BroadphaseProxy m_box0; BroadphaseProxy m_box1; @@ -36,6 +34,8 @@ public: PersistentManifold* m_manifoldPtr; bool m_lowLevelOfDetail; + void CheckPenetrationDepthSolver(); + public: diff --git a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp index da72d3e0cad..64db510e97e 100644 --- a/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp +++ b/extern/bullet/BulletDynamics/CollisionDispatch/ToiContactDispatcher.cpp @@ -142,18 +142,21 @@ void ToiContactDispatcher::SolveConstraints(float timeStep, int numIterations,in for (i=0;iGetBody0())) + RigidBody* body0 = (RigidBody*)manifold->GetBody0(); + RigidBody* body1 = (RigidBody*)manifold->GetBody1(); + + if (body0) { - if ( ((RigidBody*)manifold->GetBody0())->GetActivationState() == ISLAND_SLEEPING) + if ( body0->GetActivationState() == ISLAND_SLEEPING) { - ((RigidBody*)manifold->GetBody0())->SetActivationState( WANTS_DEACTIVATION );//ACTIVE_TAG; + body0->SetActivationState( WANTS_DEACTIVATION); } } - if (((RigidBody*)manifold->GetBody1())) + if (body1) { - if ( ((RigidBody*)manifold->GetBody1())->GetActivationState() == ISLAND_SLEEPING) + if ( body1->GetActivationState() == ISLAND_SLEEPING) { - ((RigidBody*)manifold->GetBody1())->SetActivationState(WANTS_DEACTIVATION);//ACTIVE_TAG; + body1->SetActivationState(WANTS_DEACTIVATION); } } diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp index 4050e75f90f..0399fdb8a37 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp @@ -97,18 +97,24 @@ typedef SimdScalar dQuaternion[4]; void dRfromQ1 (dMatrix3 R, const dQuaternion q) { // q = (s,vx,vy,vz) - SimdScalar qq1 = 2*q[1]*q[1]; - SimdScalar qq2 = 2*q[2]*q[2]; - SimdScalar qq3 = 2*q[3]*q[3]; - _R(0,0) = 1 - qq2 - qq3; + SimdScalar qq1 = 2.f*q[1]*q[1]; + SimdScalar qq2 = 2.f*q[2]*q[2]; + SimdScalar qq3 = 2.f*q[3]*q[3]; + _R(0,0) = 1.f - qq2 - qq3; _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = 0.f; + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1 - qq1 - qq3; + _R(1,1) = 1.f - qq1 - qq3; _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = 0.f; + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); - _R(2,2) = 1 - qq1 - qq2; + _R(2,2) = 1.f - qq1 - qq2; + _R(2,3) = 0.f; + } @@ -131,8 +137,8 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num //convert data - body->m_facc.setValue(0,0,0); - body->m_tacc.setValue(0,0,0); + body->m_facc.setValue(0,0,0,0); + body->m_tacc.setValue(0,0,0,0); //are the indices the same ? for (i=0;i<4;i++) @@ -147,9 +153,9 @@ int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& num body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; - body->m_I[0+0*4] = body->getInvInertiaDiagLocal()[0]; - body->m_I[1+1*4] = body->getInvInertiaDiagLocal()[1]; - body->m_I[2+2*4] = body->getInvInertiaDiagLocal()[2]; + body->m_I[0+0*4] = 1.f/body->getInvInertiaDiagLocal()[0]; + body->m_I[1+1*4] = 1.f/body->getInvInertiaDiagLocal()[1]; + body->m_I[2+2*4] = 1.f/body->getInvInertiaDiagLocal()[2]; /* @@ -241,7 +247,7 @@ void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Join } assert (m_CurJoint < MAX_JOINTS_1); - if (manifold->GetContactPoint(i).GetDistance() < 0.f) + if (manifold->GetContactPoint(i).GetDistance() < 0.0f) { ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp index 678536cd905..4ceb000d111 100644 --- a/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp +++ b/extern/bullet/BulletDynamics/ConstraintSolver/SorLcp.cpp @@ -420,7 +420,7 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons } iMJ_ptr += 12; J_ptr += 12; - Ad[i] = sor_w / (sum + cfm[i]); + Ad[i] = sor_w / sum;//(sum + cfm[i]); } // scale J and b by Ad @@ -434,7 +434,8 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, RigidBody * cons } // scale Ad by CFM - for (i=0; i= 0) order[j++].index = i; + for (i=0; i= 0) + order[j++].index = i; dIASSERT (j==m); #endif @@ -629,17 +634,18 @@ void SolveInternal1 (float global_cfm, // compute inertia tensor in global frame dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); + // compute inverse inertia tensor in global frame dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); // compute rotational force dMULTIPLY0_331 (tmp,I+i*12,body[i]->getAngularVelocity()); - //dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). // joints with m=0 are inactive and are removed from the joints array // entirely, so that the code that follows does not consider them. @@ -742,9 +748,11 @@ void SolveInternal1 (float global_cfm, // put v/h + invM*fe into tmp1 for (i=0; igetInvMass(); - for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; + for (j=0; j<3; j++) + tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->getLinearVelocity()[j] * stepsize1; dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; + for (j=0; j<3; j++) + tmp1[i*6+3+j] += body[i]->getAngularVelocity()[j] * stepsize1; } // put J*tmp1 into rhs @@ -756,7 +764,7 @@ void SolveInternal1 (float global_cfm, // scale CFM for (i=0; ic[0] = k * depth; + info->cfm[0] = 0.f; + info->cfm[1] = 0.f; + info->cfm[2] = 0.f; // set LCP limits for normal info->lo[0] = 0; info->hi[0] = 1e30f;//dInfinity; - + info->lo[1] = 0; + info->hi[1] = 0.f; + info->lo[2] = 0.f; + info->hi[2] = 0.f; + #define DO_THE_FRICTION_2 #ifdef DO_THE_FRICTION_2 // now do jacobian for tangential forces @@ -151,7 +158,7 @@ void ContactJoint::GetInfo2(Info2 *info) c2[2] = ccc2[2]; - float friction = FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction(); + float friction = 30.f;//0.01f;//FRICTION_CONSTANT*m_body0->getFriction() * m_body1->getFriction(); // first friction direction if (m_numRows >= 2) @@ -182,13 +189,16 @@ void ContactJoint::GetInfo2(Info2 *info) info->lo[1] = -friction;//-j->contact.surface.mu; info->hi[1] = friction;//j->contact.surface.mu; - if (0)//j->contact.surface.mode & dContactApprox1_1) + if (1)//j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; // set slip (constraint force mixing) if (0)//j->contact.surface.mode & dContactSlip1) { // info->cfm[1] = j->contact.surface.slip1; + } else + { + info->cfm[1] = 0.f; } } diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp index d3b105dc7f6..c0927a96915 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.cpp @@ -4,12 +4,14 @@ #include "GEN_MinMax.h" #include -float gRigidBodyDamping = 0.5f; +float gLinearAirDamping = 1.f; + static int uniqueId = 0; RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution) : m_collisionShape(0), m_activationState1(1), + m_deactivationTime(0.f), m_hitFraction(1.f), m_gravity(0.0f, 0.0f, 0.0f), m_linearDamping(0.f), @@ -90,12 +92,13 @@ void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping) #include + void RigidBody::applyForces(SimdScalar step) { applyCentralForce(m_gravity); - m_linearVelocity *= GEN_clamped((1.f - step * m_linearDamping), 0.0f, 1.0f); + m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f); m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f); } @@ -150,7 +153,11 @@ SimdQuaternion RigidBody::getOrientation() const void RigidBody::setCenterOfMassTransform(const SimdTransform& xform) { m_worldTransform = xform; - + SimdQuaternion orn; +// m_worldTransform.getBasis().getRotation(orn); +// orn.normalize(); +// m_worldTransform.setBasis(SimdMatrix3x3(orn)); + // m_worldTransform.getBasis().getRotation(m_orn1); updateInertiaTensor(); } diff --git a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h index 4a61be786ca..4ff987f6eaa 100644 --- a/extern/bullet/BulletDynamics/Dynamics/RigidBody.h +++ b/extern/bullet/BulletDynamics/Dynamics/RigidBody.h @@ -9,6 +9,9 @@ class CollisionShape; struct MassProps; typedef SimdScalar dMatrix3[4*3]; +extern float gLinearAirDamping; +extern bool gUseEpa; + /// RigidBody class for RigidBody Dynamics /// @@ -170,7 +173,10 @@ public: dMatrix3 m_invI; int m_islandTag1;//temp int m_activationState1;//temp + float m_deactivationTime; + int m_odeTag; + float m_padding[1024]; SimdVector3 m_tacc;//temp SimdVector3 m_facc; SimdScalar m_hitFraction; //time of impact calculation diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp index 19c4632820c..f4634668296 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.cpp @@ -7,7 +7,11 @@ class BP_Proxy; -bool gEnableSleeping = false;//false;//true; +//'temporarily' global variables +float gDeactivationTime = 2.f; +float gLinearSleepingTreshold = 0.8f; +float gAngularSleepingTreshold = 1.0f; + #include "Dynamics/MassProps.h" SimdVector3 startVel(0,0,0);//-10000); @@ -15,8 +19,7 @@ CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci) { m_collisionDelay = 0; - m_sleepingCounter = 0; - + m_MotionState = ci.m_MotionState; @@ -190,36 +193,36 @@ void CcdPhysicsController::setNewClientInfo(void* clientinfo) } -#ifdef WIN32 -float gSleepingTreshold = 0.8f; -float gAngularSleepingTreshold = 1.f; -#else +void CcdPhysicsController::UpdateDeactivation(float timeStep) +{ + if ( (m_body->GetActivationState() == 2)) + return; + -float gSleepingTreshold = 0.8f; -float gAngularSleepingTreshold = 1.0f; -#endif + if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) && + (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold)) + { + m_body->m_deactivationTime += timeStep; + } else + { + m_body->m_deactivationTime=0.f; + m_body->SetActivationState(0); + } +} bool CcdPhysicsController::wantsSleeping() { - if (!gEnableSleeping) + //disable deactivation + if (gDeactivationTime == 0.f) return false; - - if ( (m_body->GetActivationState() == 3) || (m_body->GetActivationState() == 2)) + //2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION + if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3)) return true; - if ((m_body->getLinearVelocity().length2() < gSleepingTreshold*gSleepingTreshold) && - (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold)) - { - m_sleepingCounter++; - } else - { - m_sleepingCounter=0; - } - - if (m_sleepingCounter> 150) + if (m_body->m_deactivationTime> gDeactivationTime) { return true; } diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h index 235931ebab4..e0cba58cb53 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsController.h @@ -10,6 +10,11 @@ #include "SimdScalar.h" class CollisionShape; +extern float gDeactivationTime; +extern float gLinearSleepingTreshold; +extern float gAngularSleepingTreshold; + + struct CcdConstructionInfo { CcdConstructionInfo() @@ -47,7 +52,6 @@ class CcdPhysicsController : public PHY_IPhysicsController class PHY_IMotionState* m_MotionState; CollisionShape* m_collisionShape; - int m_sleepingCounter; public: int m_collisionDelay; @@ -121,6 +125,8 @@ class CcdPhysicsController : public PHY_IPhysicsController bool wantsSleeping(); + void UpdateDeactivation(float timeStep); + void SetAabb(const SimdVector3& aabbMin,const SimdVector3& aabbMax); diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 5b6db434f8b..aac699468d0 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -4,7 +4,7 @@ #include #include "SimdTransform.h" #include "Dynamics/RigidBody.h" -#include "BroadphaseCollision/BroadPhaseInterface.h" +#include "BroadphaseCollision/BroadphaseInterface.h" #include "BroadphaseCollision/SimpleBroadphase.h" #include "CollisionShapes/ConvexShape.h" @@ -33,8 +33,6 @@ bool useIslands = true; //#include "BroadphaseCollision/QueryBox.h" //todo: change this to allow dynamic registration of types! -unsigned long gNumIterations = 10; - #ifdef WIN32 void DrawRasterizerLine(const float* from,const float* to,int color); #endif @@ -88,14 +86,17 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV CcdPhysicsEnvironment::CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher,BroadphaseInterface* bp) :m_dispatcher(dispatcher), m_broadphase(bp), -m_scalingPropagated(false) +m_scalingPropagated(false), +m_numIterations(30), +m_ccdMode(0), +m_solverType(-1) { + if (!m_dispatcher) { - OdeConstraintSolver* solver = new OdeConstraintSolver(); - //SimpleConstraintSolver* solver= new SimpleConstraintSolver(); - m_dispatcher = new ToiContactDispatcher(solver); + setSolverType(0); } + if (!m_broadphase) { m_broadphase = new SimpleBroadphase(); @@ -290,7 +291,6 @@ void CcdPhysicsEnvironment::UpdateActivationState() } -bool gPredictCollision = false;//true;//false; /// Perform an integration step of duration 'timeStep'. @@ -346,7 +346,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) - int numsubstep = gNumIterations; + int numsubstep = m_numIterations; DispatcherInfo dispatchInfo; @@ -367,7 +367,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) //contacts - m_dispatcher->SolveConstraints(timeStep, gNumIterations ,numRigidBodies,m_debugDrawer); + m_dispatcher->SolveConstraints(timeStep, m_numIterations ,numRigidBodies,m_debugDrawer); for (int g=0;gGetActivationState()) + { + case ISLAND_SLEEPING: + { + color.setValue(0,1,0); + break; + } + case WANTS_DEACTIVATION: + { + color.setValue(0,0,1); + break; + } + case ACTIVE_TAG: + { + break; + } + + + }; DrawAabb(m_debugDrawer,minAabb,maxAabb,color); } #endif + scene->SetAabb(bp,minAabb,maxAabb); + + + } } @@ -454,7 +481,7 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) - if (gPredictCollision) + if (m_ccdMode == 3) { DispatcherInfo dispatchInfo; dispatchInfo.m_timeStep = timeStep; @@ -505,6 +532,9 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) { CcdPhysicsController* ctrl = (*i); RigidBody* body = ctrl->GetRigidBody(); + + ctrl->UpdateDeactivation(timeStep); + if (ctrl->wantsSleeping()) { @@ -543,26 +573,103 @@ bool CcdPhysicsEnvironment::proceedDeltaTime(double curTime,float timeStep) void CcdPhysicsEnvironment::setDebugMode(int debugMode) { - if (debugMode > 10) - { - if (m_dispatcher) - delete m_dispatcher; - - if (debugMode == 11) - { - SimpleConstraintSolver* solver= new SimpleConstraintSolver(); - m_dispatcher = new ToiContactDispatcher(solver); - } else - { - OdeConstraintSolver* solver = new OdeConstraintSolver(); - m_dispatcher = new ToiContactDispatcher(solver); - } - } if (m_debugDrawer){ m_debugDrawer->SetDebugMode(debugMode); } } +void CcdPhysicsEnvironment::setNumIterations(int numIter) +{ + m_numIterations = numIter; +} +void CcdPhysicsEnvironment::setDeactivationTime(float dTime) +{ + gDeactivationTime = dTime; +} +void CcdPhysicsEnvironment::setDeactivationLinearTreshold(float linTresh) +{ + gLinearSleepingTreshold = linTresh; +} +void CcdPhysicsEnvironment::setDeactivationAngularTreshold(float angTresh) +{ + gAngularSleepingTreshold = angTresh; +} +void CcdPhysicsEnvironment::setContactBreakingTreshold(float contactBreakingTreshold) +{ + gContactBreakingTreshold = contactBreakingTreshold; + +} + + +void CcdPhysicsEnvironment::setCcdMode(int ccdMode) +{ + m_ccdMode = ccdMode; +} + + +void CcdPhysicsEnvironment::setSolverSorConstant(float sor) +{ + m_dispatcher->SetSor(sor); +} + +void CcdPhysicsEnvironment::setSolverTau(float tau) +{ + m_dispatcher->SetTau(tau); +} +void CcdPhysicsEnvironment::setSolverDamping(float damping) +{ + m_dispatcher->SetDamping(damping); +} + + +void CcdPhysicsEnvironment::setLinearAirDamping(float damping) +{ + gLinearAirDamping = damping; +} + +void CcdPhysicsEnvironment::setUseEpa(bool epa) +{ + gUseEpa = epa; +} + +void CcdPhysicsEnvironment::setSolverType(int solverType) +{ + + switch (solverType) + { + case 1: + { + if (m_solverType != solverType) + { + if (m_dispatcher) + delete m_dispatcher; + + SimpleConstraintSolver* solver= new SimpleConstraintSolver(); + m_dispatcher = new ToiContactDispatcher(solver); + break; + } + } + + case 0: + default: + if (m_solverType != solverType) + { + if (m_dispatcher) + delete m_dispatcher; + + OdeConstraintSolver* solver= new OdeConstraintSolver(); + m_dispatcher = new ToiContactDispatcher(solver); + break; + } + + }; + + m_solverType = solverType ; +} + + + + void CcdPhysicsEnvironment::SyncMotionStates(float timeStep) { diff --git a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index be6f6c648c7..8bacbad8914 100644 --- a/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/extern/bullet/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -26,6 +26,9 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment SimdVector3 m_gravity; BroadphaseInterface* m_broadphase; IDebugDraw* m_debugDrawer; + int m_numIterations; + int m_ccdMode; + int m_solverType; public: CcdPhysicsEnvironment(ToiContactDispatcher* dispatcher=0, BroadphaseInterface* broadphase=0); @@ -43,6 +46,18 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment m_debugDrawer = debugDrawer; } + virtual void setNumIterations(int numIter); + virtual void setDeactivationTime(float dTime); + virtual void setDeactivationLinearTreshold(float linTresh) ; + virtual void setDeactivationAngularTreshold(float angTresh) ; + virtual void setContactBreakingTreshold(float contactBreakingTreshold) ; + virtual void setCcdMode(int ccdMode); + virtual void setSolverType(int solverType); + virtual void setSolverSorConstant(float sor); + virtual void setSolverTau(float tau); + virtual void setSolverDamping(float damping); + virtual void setLinearAirDamping(float damping); + virtual void setUseEpa(bool epa) ; virtual void beginFrame() {}; virtual void endFrame() {}; diff --git a/extern/bullet/LinearMath/SimdQuadWord.h b/extern/bullet/LinearMath/SimdQuadWord.h index 818b2e6007d..67bcf907693 100644 --- a/extern/bullet/LinearMath/SimdQuadWord.h +++ b/extern/bullet/LinearMath/SimdQuadWord.h @@ -99,7 +99,7 @@ class SimdQuadWord SIMD_FORCE_INLINE SimdQuadWord(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z) :m_x(x),m_y(y),m_z(z) //todo, remove this in release/simd ? - ,m_unusedW(1e30f) + ,m_unusedW(0.f) { } -- cgit v1.2.3