From 376be529bb932f867b01466592c33899ee5ec2c8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 6 Jul 2007 03:12:31 +0000 Subject: synchronized to latest version of Bullet. added btConeTwistConstraint (useful for ragdolls etc) --- .../BroadphaseCollision/btOverlappingPairCache.cpp | 3 +- .../BroadphaseCollision/btOverlappingPairCache.h | 28 +- .../BroadphaseCollision/btSimpleBroadphase.h | 2 + .../CollisionDispatch/btCollisionDispatcher.cpp | 2 + .../CollisionDispatch/btCollisionObject.cpp | 6 +- .../CollisionDispatch/btCollisionObject.h | 28 +- .../CollisionDispatch/btCollisionWorld.cpp | 62 +++-- .../CollisionDispatch/btCollisionWorld.h | 16 +- .../btCompoundCollisionAlgorithm.cpp | 12 +- .../CollisionShapes/btCompoundShape.h | 4 +- .../CollisionShapes/btHeightfieldTerrainShape.cpp | 298 +++++++++++++++++++-- .../CollisionShapes/btHeightfieldTerrainShape.h | 34 ++- .../CollisionShapes/btMinkowskiSumShape.cpp | 2 +- .../CollisionShapes/btMinkowskiSumShape.h | 6 +- .../CollisionShapes/btMultiSphereShape.h | 14 + .../btSubSimplexConvexCast.cpp | 2 +- .../NarrowPhaseCollision/btSubSimplexConvexCast.h | 6 +- .../ConstraintSolver/btConeTwistConstraint.cpp | 286 ++++++++++++++++++++ .../ConstraintSolver/btConeTwistConstraint.h | 123 +++++++++ .../ConstraintSolver/btHingeConstraint.cpp | 252 ++++++++++++++--- .../ConstraintSolver/btHingeConstraint.h | 66 ++++- .../btSequentialImpulseConstraintSolver.cpp | 4 + .../Dynamics/btDiscreteDynamicsWorld.cpp | 124 +++++++-- .../Dynamics/btDiscreteDynamicsWorld.h | 5 +- .../src/BulletDynamics/Dynamics/btDynamicsWorld.h | 2 +- .../src/BulletDynamics/Dynamics/btRigidBody.cpp | 34 ++- .../src/BulletDynamics/Dynamics/btRigidBody.h | 11 +- extern/bullet2/src/LinearMath/btMatrix3x3.h | 15 ++ extern/bullet2/src/LinearMath/btQuaternion.h | 31 +++ extern/bullet2/src/LinearMath/btQuickprof.h | 22 +- extern/bullet2/src/LinearMath/btScalar.h | 22 +- extern/bullet2/src/LinearMath/btTransform.h | 13 + extern/bullet2/src/btBulletDynamicsCommon.h | 1 + 33 files changed, 1372 insertions(+), 164 deletions(-) create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h (limited to 'extern') diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 4187a8f2970..60f0a41a9d7 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -24,7 +24,8 @@ subject to the following restrictions: int gOverlappingPairs = 0; btOverlappingPairCache::btOverlappingPairCache(): -m_blockedForChanges(false) +m_blockedForChanges(false), +m_overlapFilterCallback(0) //m_NumOverlapBroadphasePair(0) { } diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index e3442212171..a81fe3264df 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -26,13 +26,20 @@ subject to the following restrictions: struct btOverlapCallback { -virtual ~btOverlapCallback() -{ -} + virtual ~btOverlapCallback() + {} //return true for deletion of the pair virtual bool processOverlap(btBroadphasePair& pair) = 0; }; +struct btOverlapFilterCallback +{ + virtual ~btOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const = 0; +}; + ///btOverlappingPairCache maintains the objects with overlapping AABB ///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase class btOverlappingPairCache : public btBroadphaseInterface @@ -44,6 +51,8 @@ class btOverlappingPairCache : public btBroadphaseInterface //during the dispatch, check that user doesn't destroy/create proxy bool m_blockedForChanges; + //if set, use the callback instead of the built in filter in needBroadphaseCollision + btOverlapFilterCallback* m_overlapFilterCallback; public: btOverlappingPairCache(); @@ -67,6 +76,9 @@ class btOverlappingPairCache : public btBroadphaseInterface inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + bool collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); @@ -92,7 +104,17 @@ class btOverlappingPairCache : public btBroadphaseInterface return m_overlappingPairArray.size(); } + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } }; #endif //OVERLAPPING_PAIR_CACHE_H + diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 1f265f3dd6b..fb155e7047c 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -41,6 +41,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy class btSimpleBroadphase : public btOverlappingPairCache { +protected: + btSimpleBroadphaseProxy* m_proxies; int* m_freeProxies; int m_firstFreeProxy; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 198276d76df..b535fac6563 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -273,6 +273,8 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; + else if (!body0->checkCollideWith(body1)) + needsCollision = false; return needsCollision ; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 7b6f066275d..d4c0a4e8cb3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -24,11 +24,15 @@ btCollisionObject::btCollisionObject() m_userObjectPointer(0), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdSquareMotionThreshold(btScalar(0.)) + m_ccdSquareMotionThreshold(btScalar(0.)), + m_checkCollideWith(false) { } +btCollisionObject::~btCollisionObject() +{ +} void btCollisionObject::setActivationState(int newState) { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 93990235afe..9fb6a67c4a3 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -69,15 +69,23 @@ protected: void* m_internalOwner; ///time of impact calculation - btScalar m_hitFraction; + btScalar m_hitFraction; ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - btScalar m_ccdSquareMotionThreshold; + btScalar m_ccdSquareMotionThreshold; - char m_pad[8]; + /// If some object should have elaborate collision filtering by sub-classes + bool m_checkCollideWith; + + char m_pad[7]; + + virtual bool checkCollideWithOverride(btCollisionObject* co) + { + return true; + } public: @@ -118,6 +126,7 @@ public: btCollisionObject(); + virtual ~btCollisionObject(); void setCollisionShape(btCollisionShape* collisionShape) { @@ -159,7 +168,7 @@ public: return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(btScalar rest) + void setRestitution(btScalar rest) { m_restitution = rest; } @@ -322,6 +331,15 @@ public: m_userObjectPointer = userPointer; } + inline bool checkCollideWith(btCollisionObject* co) + { + if (m_checkCollideWith) + return checkCollideWithOverride(co); + + return true; + } + + } ; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 49f35c2cc90..b49036a5b50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -17,6 +17,8 @@ subject to the following restrictions: #include "btCollisionDispatcher.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" + #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" @@ -179,12 +181,27 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback) + RayResultCallback& resultCallback,short int collisionFilterMask) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); + objectQuerySingle(&pointShape,rayFromTrans,rayToTrans, + collisionObject, + collisionShape, + colObjWorldTransform, + resultCallback,collisionFilterMask); +} + +void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback,short int collisionFilterMask) +{ + + if (collisionShape->isConvex()) { btConvexCast::CastResult castResult; @@ -192,9 +209,9 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //GjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { @@ -292,11 +309,11 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); btTransform childWorldTrans = colObjWorldTransform * childTrans; - rayTestSingle(rayFromTrans,rayToTrans, + objectQuerySingle(castShape, rayFromTrans,rayToTrans, collisionObject, childCollisionShape, childWorldTrans, - resultCallback); + resultCallback, collisionFilterMask); } @@ -306,7 +323,7 @@ void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra } } -void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) +void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask) { @@ -323,21 +340,22 @@ void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& r for (i=0;iGetRigidcollisionObject(); - btVector3 collisionObjectAabbMin,collisionObjectAabbMax; - collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); - - btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing - btVector3 hitNormal; - if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) - { - rayTestSingle(rayFromTrans,rayToTrans, - collisionObject, - collisionObject->getCollisionShape(), - collisionObject->getWorldTransform(), - resultCallback); - + //only perform raycast if filterMask matches + if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); + + btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing + btVector3 hitNormal; + if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) + { + rayTestSingle(rayFromTrans,rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + resultCallback); + } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 0b0286c50f4..b6d80233ab7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -53,9 +53,9 @@ subject to the following restrictions: * Bullet has been designed in a modular way keeping dependencies to a minimum. The ConvexHullDistance demo demonstrates direct use of btGjkPairDetector. * * @section copyright Copyright - * Copyright (C) 2005-2006 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon + * Copyright (C) 2005-2007 Erwin Coumans, some contributions Copyright Gino van den Bergen, Christer Ericson, Simon Hobbs, Ricardo Padrela, F Richter(res), Stephane Redon * Special thanks to all visitors of the Bullet Physics forum, and in particular above contributors, Dave Eberle, Dirk Gregorius, Erin Catto, Dave Eberle, Adam Moravanszky, - * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren. + * Pierre Terdiman, Kenny Erleben, Russell Smith, Oliver Strunk, Jan Paul van Waveren, Marten Svanfeldt. * */ @@ -66,6 +66,7 @@ subject to the following restrictions: class btStackAlloc; class btCollisionShape; +class btConvexShape; class btBroadphaseInterface; #include "../../LinearMath/btVector3.h" #include "../../LinearMath/btTransform.h" @@ -208,7 +209,7 @@ public: /// rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. - void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback); + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback, short int collisionFilterMask=-1); /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. @@ -217,7 +218,14 @@ public: btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, - RayResultCallback& resultCallback); + RayResultCallback& resultCallback, short int collisionFilterMask=-1); + + /// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest. + static void objectQuerySingle(const btConvexShape* castShape, const btTransform& rayFromTrans,const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback, short int collisionFilterMask=-1); void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=1,short int collisionFilterMask=1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 2f96c2e13c5..92f4c8b28a6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -77,9 +77,9 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt btTransform orgTrans = colObj->getWorldTransform(); btCollisionShape* orgShape = colObj->getCollisionShape(); - btTransform childTrans = compoundShape->getChildTransform(i); - btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( newChildWorldTrans ); + const btTransform& childTrans = compoundShape->getChildTransform(i); + //btTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->setWorldTransform( orgTrans*childTrans ); //the contactpoint is still projected back using the original inverted worldtrans colObj->setCollisionShape( childShape ); m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut); @@ -119,9 +119,9 @@ btScalar btCompoundCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* btTransform orgTrans = colObj->getWorldTransform(); btCollisionShape* orgShape = colObj->getCollisionShape(); - btTransform childTrans = compoundShape->getChildTransform(i); - btTransform newChildWorldTrans = orgTrans*childTrans ; - colObj->setWorldTransform( newChildWorldTrans ); + const btTransform& childTrans = compoundShape->getChildTransform(i); + //btTransform newChildWorldTrans = orgTrans*childTrans ; + colObj->setWorldTransform( orgTrans*childTrans ); colObj->setCollisionShape( childShape ); btScalar frac = m_childCollisionAlgorithms[i]->calculateTimeOfImpact(colObj,otherObj,dispatchInfo,resultOut); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 2d384bd2180..86dc1f80947 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -58,11 +58,11 @@ public: return m_childShapes[index]; } - btTransform getChildTransform(int index) + btTransform& getChildTransform(int index) { return m_childTransforms[index]; } - const btTransform getChildTransform(int index) const + const btTransform& getChildTransform(int index) const { return m_childTransforms[index]; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 5a359c0cff1..94be43c88e2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -18,9 +18,66 @@ subject to the following restrictions: #include "LinearMath/btTransformUtil.h" -btHeightfieldTerrainShape::btHeightfieldTerrainShape() -:m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.)) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int width,int length,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +:m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)), +m_width(width), +m_length(length), +m_heightfieldDataUnknown(heightfieldData), +m_maxHeight(maxHeight), +m_upAxis(upAxis), +m_useFloatData(useFloatData), +m_flipQuadEdges(flipQuadEdges), +m_useDiamondSubdivision(false) { + + + btScalar quantizationMargin = 1.f; + + //enlarge the AABB to avoid division by zero when initializing the quantization values + btVector3 clampValue(quantizationMargin,quantizationMargin,quantizationMargin); + + btVector3 halfExtents(0,0,0); + + switch (m_upAxis) + { + case 0: + { + halfExtents.setValue( + m_maxHeight, + m_width, + m_length); + break; + } + case 1: + { + halfExtents.setValue( + m_width, + m_maxHeight, + m_length); + break; + }; + case 2: + { + halfExtents.setValue( + m_width, + m_length, + m_maxHeight + ); + break; + } + default: + { + //need to get valid m_upAxis + btAssert(0); + } + } + + halfExtents*= btScalar(0.5); + + m_localAabbMin = -halfExtents - clampValue; + m_localAabbMax = halfExtents + clampValue; + btVector3 aabbSize = m_localAabbMax - m_localAabbMin; + } @@ -30,51 +87,238 @@ btHeightfieldTerrainShape::~btHeightfieldTerrainShape() -void btHeightfieldTerrainShape::getAabb(const btTransform& ,btVector3& aabbMin,btVector3& aabbMax) const +void btHeightfieldTerrainShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ +/* + aabbMin.setValue(-1e30f,-1e30f,-1e30f); + aabbMax.setValue(1e30f,1e30f,1e30f); +*/ + + btVector3 halfExtents = (m_localAabbMax-m_localAabbMin)* m_localScaling * btScalar(0.5); + + btMatrix3x3 abs_b = t.getBasis().absolute(); + btPoint3 center = t.getOrigin(); + btVector3 extent = btVector3(abs_b[0].dot(halfExtents), + abs_b[1].dot(halfExtents), + abs_b[2].dot(halfExtents)); + extent += btVector3(getMargin(),getMargin(),getMargin()); + + aabbMin = center - extent; + aabbMax = center + extent; + + +} + +btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const { - aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btScalar val = 0.f; + if (m_useFloatData) + { + val = m_heightfieldDataFloat[(y*m_width)+x]; + } else + { + //assume unsigned short int + unsigned char heightFieldValue = m_heightfieldDataUnsignedChar[(y*m_width)+x]; + val = heightFieldValue* (m_maxHeight/btScalar(65535)); + } + return val; } + +void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const +{ + + btAssert(x>=0); + btAssert(y>=0); + btAssert(xstartX) + startX = quantizedAabbMin[1]; + if (quantizedAabbMax[1]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[2]; + if (quantizedAabbMax[2]processTriangle(triangle,0,0); + if (quantizedAabbMin[0]>startX) + startX = quantizedAabbMin[0]; + if (quantizedAabbMax[0]startJ) + startJ = quantizedAabbMin[1]; + if (quantizedAabbMax[1]processTriangle(triangle,0,1); -*/ + for(int j=startJ; jprocessTriangle(vertices,x,j); + //second triangle + getVertex(x,j,vertices[0]); + getVertex(x+1,j+1,vertices[1]); + getVertex(x,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + } else + { + //first triangle + getVertex(x,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j,vertices[2]); + callback->processTriangle(vertices,x,j); + //second triangle + getVertex(x+1,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + } + } + } + + } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 85fd49638ce..49f3e106733 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -25,16 +25,46 @@ protected: btVector3 m_localAabbMin; btVector3 m_localAabbMax; - //todo: terrain data + ///terrain data + int m_width; + int m_length; + btScalar m_maxHeight; + union + { + unsigned char* m_heightfieldDataUnsignedChar; + btScalar* m_heightfieldDataFloat; + void* m_heightfieldDataUnknown; + }; + + bool m_useFloatData; + bool m_flipQuadEdges; + bool m_useDiamondSubdivision; + int m_upAxis; + btVector3 m_localScaling; + virtual btScalar getHeightFieldValue(int x,int y) const; + void quantizeWithClamp(int* out, const btVector3& point) const; + void getVertex(int x,int y,btVector3& vertex) const; + + inline bool testQuantizedAabbAgainstQuantizedAabb(int* aabbMin1, int* aabbMax1,const int* aabbMin2,const int* aabbMax2) const + { + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; + } + public: - btHeightfieldTerrainShape(); + btHeightfieldTerrainShape(int width,int height,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); + void setUseDiamondSubdivision(bool useDiamondSubdivision=true) { m_useDiamondSubdivision = useDiamondSubdivision;} + virtual int getShapeType() const { return TERRAIN_SHAPE_PROXYTYPE; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index b213f3819a4..015314bc09f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -16,7 +16,7 @@ subject to the following restrictions: #include "btMinkowskiSumShape.h" -btMinkowskiSumShape::btMinkowskiSumShape(btConvexShape* shapeA,btConvexShape* shapeB) +btMinkowskiSumShape::btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB) :m_shapeA(shapeA), m_shapeB(shapeB) { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index 489b7ddde1e..198faaff9f9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -25,12 +25,12 @@ class btMinkowskiSumShape : public btConvexShape btTransform m_transA; btTransform m_transB; - btConvexShape* m_shapeA; - btConvexShape* m_shapeB; + const btConvexShape* m_shapeA; + const btConvexShape* m_shapeB; public: - btMinkowskiSumShape(btConvexShape* shapeA,btConvexShape* shapeB); + btMinkowskiSumShape(const btConvexShape* shapeA,const btConvexShape* shapeB); virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index be044dbc16a..1897b474057 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -46,6 +46,20 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + int getSphereCount() const + { + return m_numSpheres; + } + + const btVector3& getSpherePosition(int index) const + { + return m_localPositions[index]; + } + + btScalar getSphereRadius(int index) const + { + return m_radi[index]; + } virtual int getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index 1ab336b4d54..687738b7fa9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -btSubsimplexConvexCast::btSubsimplexConvexCast (btConvexShape* convexA,btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) +btSubsimplexConvexCast::btSubsimplexConvexCast (const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver) :m_simplexSolver(simplexSolver), m_convexA(convexA),m_convexB(convexB) { diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h index a2a3193b090..05662db5d23 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -28,12 +28,12 @@ class btConvexShape; class btSubsimplexConvexCast : public btConvexCast { btSimplexSolverInterface* m_simplexSolver; - btConvexShape* m_convexA; - btConvexShape* m_convexB; + const btConvexShape* m_convexA; + const btConvexShape* m_convexB; public: - btSubsimplexConvexCast (btConvexShape* shapeA,btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver); + btSubsimplexConvexCast (const btConvexShape* shapeA,const btConvexShape* shapeB,btSimplexSolverInterface* simplexSolver); //virtual ~btSubsimplexConvexCast(); ///SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects. diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp new file mode 100644 index 00000000000..12a33d7851e --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -0,0 +1,286 @@ +/* +Bullet Continuous Collision Detection and Physics Library +btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marcus Hennix +*/ + + +#include "btConeTwistConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btSimdMinMax.h" +#include + +btConeTwistConstraint::btConeTwistConstraint() +{ +} + + +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, + const btTransform& rbAFrame,const btTransform& rbBFrame) + :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), + m_angularOnly(false) +{ + // flip axis for correct angles + m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); + + m_swingSpan1 = btScalar(1e30); + m_swingSpan2 = btScalar(1e30); + m_twistSpan = btScalar(1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + + m_solveTwistLimit = false; + m_solveSwingLimit = false; + +} + +btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) + :btTypedConstraint(rbA),m_rbAFrame(rbAFrame), + m_angularOnly(false) +{ + m_rbBFrame = m_rbAFrame; + + // flip axis for correct angles + m_rbBFrame.getBasis()[1][0] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][1] *= btScalar(-1.); + m_rbBFrame.getBasis()[1][2] *= btScalar(-1.); + + m_rbBFrame.getBasis()[2][0] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][1] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); + + m_swingSpan1 = btScalar(1e30); + m_swingSpan2 = btScalar(1e30); + m_twistSpan = btScalar(1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + + m_solveTwistLimit = false; + m_solveSwingLimit = false; + +} + +void btConeTwistConstraint::buildJacobian() +{ + m_appliedImpulse = btScalar(0.); + + //set bias, sign, clear accumulator + m_swingCorrection = btScalar(0.); + m_twistLimitSign = btScalar(0.); + m_solveTwistLimit = false; + m_solveSwingLimit = false; + m_accTwistLimitImpulse = btScalar(0.); + m_accSwingLimitImpulse = btScalar(0.); + + if (!m_angularOnly) + { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + + for (int i=0;i<3;i++) + { + new (&m_jac[i]) btJacobianEntry( + m_rbA.getCenterOfMassTransform().getBasis().transpose(), + m_rbB.getCenterOfMassTransform().getBasis().transpose(), + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], + m_rbA.getInvInertiaDiagLocal(), + m_rbA.getInvMass(), + m_rbB.getInvInertiaDiagLocal(), + m_rbB.getInvMass()); + } + } + + btVector3 b1Axis1,b1Axis2,b1Axis3; + btVector3 b2Axis1,b2Axis2; + + b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); + b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); + + btScalar swing1=btScalar(0.),swing2 = btScalar(0.); + + // Get Frame into world space + if (m_swingSpan1 >= btScalar(0.05f)) + { + b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); + swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) ); + } + + if (m_swingSpan2 >= btScalar(0.05f)) + { + b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); + swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) ); + } + + btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); + btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); + btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) + { + m_swingCorrection = EllipseAngle-1.0f; + m_solveSwingLimit = true; + + // Calculate necessary axis & factors + m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); + m_swingAxis.normalize(); + + btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + m_swingAxis *= swingAxisSign; + + m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); + + } + + // Twist limits + if (m_twistSpan >= btScalar(0.)) + { + btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); + btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); + btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); + btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); + + btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); + if (twist <= -m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = -(twist + m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + m_twistAxis *= -1.0f; + + m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + + } else + if (twist > m_twistSpan*lockedFreeFactor) + { + m_twistCorrection = (twist - m_twistSpan); + m_solveTwistLimit = true; + + m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; + m_twistAxis.normalize(); + + m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); + + } + } +} + +void btConeTwistConstraint::solveConstraint(btScalar timeStep) +{ + + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + + btScalar tau = btScalar(0.3); + btScalar damping = btScalar(1.); + + //linear part + if (!m_angularOnly) + { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + + for (int i=0;i<3;i++) + { + const btVector3& normal = m_jac[i].m_linearJointAxis; + btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); + + btScalar rel_vel; + rel_vel = normal.dot(vel); + //positional error (zeroth order error) + btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + m_appliedImpulse += impulse; + btVector3 impulse_vector = normal * impulse; + m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); + m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); + } + } + + { + ///solve angular part + const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); + const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); + + // solve swing limit + if (m_solveSwingLimit) + { + btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); + btScalar impulseMag = amplitude * m_kSwing; + + // Clamp the accumulated impulse + btScalar temp = m_accSwingLimitImpulse; + m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f ); + impulseMag = m_accSwingLimitImpulse - temp; + + btVector3 impulse = m_swingAxis * impulseMag; + + m_rbA.applyTorqueImpulse(impulse); + m_rbB.applyTorqueImpulse(-impulse); + + } + + // solve twist limit + if (m_solveTwistLimit) + { + btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); + btScalar impulseMag = amplitude * m_kTwist; + + // Clamp the accumulated impulse + btScalar temp = m_accTwistLimitImpulse; + m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f ); + impulseMag = m_accTwistLimitImpulse - temp; + + btVector3 impulse = m_twistAxis * impulseMag; + + m_rbA.applyTorqueImpulse(impulse); + m_rbB.applyTorqueImpulse(-impulse); + + } + + } + +} + +void btConeTwistConstraint::updateRHS(btScalar timeStep) +{ + (void)timeStep; + +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h new file mode 100644 index 00000000000..874669c80b3 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -0,0 +1,123 @@ +/* +Bullet Continuous Collision Detection and Physics Library +btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marcus Hennix +*/ + + + +#ifndef CONETWISTCONSTRAINT_H +#define CONETWISTCONSTRAINT_H + +#include "../../LinearMath/btVector3.h" +#include "btJacobianEntry.h" +#include "btTypedConstraint.h" + +class btRigidBody; + + +///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) +class btConeTwistConstraint : public btTypedConstraint +{ + btJacobianEntry m_jac[3]; //3 orthogonal linear constraints + + btTransform m_rbAFrame; + btTransform m_rbBFrame; + + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; + + btScalar m_swingSpan1; + btScalar m_swingSpan2; + btScalar m_twistSpan; + + btVector3 m_swingAxis; + btVector3 m_twistAxis; + + btScalar m_kSwing; + btScalar m_kTwist; + + btScalar m_twistLimitSign; + btScalar m_swingCorrection; + btScalar m_twistCorrection; + + btScalar m_accSwingLimitImpulse; + btScalar m_accTwistLimitImpulse; + + bool m_angularOnly; + bool m_solveTwistLimit; + bool m_solveSwingLimit; + + +public: + + btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); + + btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); + + btConeTwistConstraint(); + + virtual void buildJacobian(); + + virtual void solveConstraint(btScalar timeStep); + + void updateRHS(btScalar timeStep); + + const btRigidBody& getRigidBodyA() const + { + return m_rbA; + } + const btRigidBody& getRigidBodyB() const + { + return m_rbB; + } + + void setAngularOnly(bool angularOnly) + { + m_angularOnly = angularOnly; + } + + void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + { + m_swingSpan1 = _swingSpan1; + m_swingSpan2 = _swingSpan2; + m_twistSpan = _twistSpan; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + } + + const btTransform& getAFrame() { return m_rbAFrame; }; + const btTransform& getBFrame() { return m_rbBFrame; }; + + inline int getSolveTwistLimit() + { + return m_solveTwistLimit; + } + + inline int getSolveSwingLimit() + { + return m_solveTwistLimit; + } + + inline btScalar getTwistLimitSign() + { + return m_twistLimitSign; + } + +}; + +#endif //CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 27e30987549..ad7fc3a269e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -17,58 +17,176 @@ subject to the following restrictions: #include "btHingeConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" #include "LinearMath/btTransformUtil.h" +#include "LinearMath/btSimdMinMax.h" #include + btHingeConstraint::btHingeConstraint(): m_enableAngularMotor(false) { } btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, - btVector3& axisInA,btVector3& axisInB) -:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB), -m_axisInA(axisInA), -m_axisInB(-axisInB), -m_angularOnly(false), -m_enableAngularMotor(false) + btVector3& axisInA,btVector3& axisInB) + :btTypedConstraint(rbA,rbB), + m_angularOnly(false), + m_enableAngularMotor(false) { + m_rbAFrame.getOrigin() = pivotInA; + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); + btScalar projection = rbAxisA1.dot(axisInA); + if (projection > SIMD_EPSILON) + rbAxisA1 = rbAxisA1*projection - axisInA; + else + rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + + btVector3 rbAxisA2 = rbAxisA1.cross(axisInA); + + m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), + rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), + rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + + btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btVector3 rbAxisB2 = rbAxisB1.cross(axisInB); + + + m_rbBFrame.getOrigin() = pivotInB; + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() ); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; } btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)), -m_axisInA(axisInA), -//fixed axis in worldspace -m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA), +:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false) +{ + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + // fixed axis in worldspace + btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); + btScalar projection = rbAxisA1.dot(axisInA); + if (projection > SIMD_EPSILON) + rbAxisA1 = rbAxisA1*projection - axisInA; + else + rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); + + btVector3 rbAxisA2 = axisInA.cross(rbAxisA1); + + m_rbAFrame.getOrigin() = pivotInA; + m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), + rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), + rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); + + + btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA; + + btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); + btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); + btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); + + + m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); + m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), + rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), + rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; +} + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, + const btTransform& rbAFrame, const btTransform& rbBFrame) +:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), m_enableAngularMotor(false) { - + // flip axis + m_rbBFrame.getBasis()[2][0] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][1] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; +} + + + +btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) +:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), +m_angularOnly(false), +m_enableAngularMotor(false) +{ + // flip axis + m_rbBFrame.getBasis()[2][0] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][1] *= btScalar(-1.); + m_rbBFrame.getBasis()[2][2] *= btScalar(-1.); + + + //start with free + m_lowerLimit = btScalar(1e30); + m_upperLimit = btScalar(-1e30); + m_biasFactor = 0.3f; + m_relaxationFactor = 1.0f; + m_limitSoftness = 0.9f; + m_solveLimit = false; } void btHingeConstraint::buildJacobian() { m_appliedImpulse = btScalar(0.); - btVector3 normal(0,0,0); - if (!m_angularOnly) { + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); + btVector3 relPos = pivotBInW - pivotAInW; + + btVector3 normal[3]; + if (relPos.length2() > SIMD_EPSILON) + { + normal[0] = relPos.normalized(); + } + else + { + normal[0].setValue(btScalar(1.0),0,0); + } + + btPlaneSpace1(normal[0], normal[1], normal[2]); + for (int i=0;i<3;i++) { - normal[i] = 1; new (&m_jac[i]) btJacobianEntry( m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(), - m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), - m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(), - normal, + pivotAInW - m_rbA.getCenterOfMassPosition(), + pivotBInW - m_rbB.getCenterOfMassPosition(), + normal[i], m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvMass(), m_rbB.getInvInertiaDiagLocal(), m_rbB.getInvMass()); - normal[i] = 0; } } @@ -79,12 +197,12 @@ void btHingeConstraint::buildJacobian() btVector3 jointAxis0local; btVector3 jointAxis1local; - btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local); + btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); - getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; - btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; + btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); new (&m_jacAng[0]) btJacobianEntry(jointAxis0, m_rbA.getCenterOfMassTransform().getBasis().transpose(), @@ -105,44 +223,71 @@ void btHingeConstraint::buildJacobian() m_rbB.getInvInertiaDiagLocal()); + // Compute limit information + btScalar hingeAngle = getHingeAngle(); + + //set bias, sign, clear accumulator + m_correction = btScalar(0.); + m_limitSign = btScalar(0.); + m_solveLimit = false; + m_accLimitImpulse = btScalar(0.); + + if (m_lowerLimit < m_upperLimit) + { + if (hingeAngle <= m_lowerLimit*m_limitSoftness) + { + m_correction = (m_lowerLimit - hingeAngle); + m_limitSign = 1.0f; + m_solveLimit = true; + } + else if (hingeAngle >= m_upperLimit*m_limitSoftness) + { + m_correction = m_upperLimit - hingeAngle; + m_limitSign = -1.0f; + m_solveLimit = true; + } + } + + //Compute K = J*W*J' for hinge axis + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); } void btHingeConstraint::solveConstraint(btScalar timeStep) { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; + btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); + btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - btVector3 normal(0,0,0); btScalar tau = btScalar(0.3); btScalar damping = btScalar(1.); //linear part if (!m_angularOnly) { + btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); + btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); + + btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); + btVector3 vel = vel1 - vel2; + for (int i=0;i<3;i++) { - normal[i] = 1; + const btVector3& normal = m_jac[i].m_linearJointAxis; btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); - btVector3 vel = vel1 - vel2; btScalar rel_vel; rel_vel = normal.dot(vel); //positional error (zeroth order error) btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping; + btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; m_appliedImpulse += impulse; btVector3 impulse_vector = normal * impulse; m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); - - normal[i] = 0; } } @@ -151,8 +296,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) ///solve angular part // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA; - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB; + btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); @@ -174,7 +319,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) getRigidBodyB().computeAngularImpulseDenominator(normal); // scale for mass and relaxation //todo: expose this 0.9 factor to developer - velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9); + velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; } //solve angular positional correction @@ -190,10 +335,28 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); m_rbB.applyTorqueImpulse(velrelOrthog-angularError); + + // solve limit + if (m_solveLimit) + { + btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; + + btScalar impulseMag = amplitude * m_kHinge; + + // Clamp the accumulated impulse + btScalar temp = m_accLimitImpulse; + m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, 0.0f ); + impulseMag = m_accLimitImpulse - temp; + + + btVector3 impulse = axisA * impulseMag * m_limitSign; + m_rbA.applyTorqueImpulse(impulse); + m_rbB.applyTorqueImpulse(-impulse); + } } //apply motor - if (m_enableAngularMotor) + if (m_enableAngularMotor) { //todo: add limits too btVector3 angularLimit(0,0,0); @@ -204,10 +367,7 @@ void btHingeConstraint::solveConstraint(btScalar timeStep) btScalar desiredMotorVel = m_motorTargetVelocity; btScalar motor_relvel = desiredMotorVel - projRelVel; - btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) + - getRigidBodyB().computeAngularImpulseDenominator(axisA); - - btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;; + btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; //todo: should clip against accumulated impulse btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; @@ -227,3 +387,11 @@ void btHingeConstraint::updateRHS(btScalar timeStep) } +btScalar btHingeConstraint::getHingeAngle() +{ + const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); + const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); + const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); + + return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) ); +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 5c1ceafbc5b..46e6480d43c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -13,6 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ +/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ + #ifndef HINGECONSTRAINT_H #define HINGECONSTRAINT_H @@ -22,7 +24,6 @@ subject to the following restrictions: class btRigidBody; - /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space /// axis defines the orientation of the hinge axis class btHingeConstraint : public btTypedConstraint @@ -30,22 +31,40 @@ class btHingeConstraint : public btTypedConstraint btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor - btVector3 m_pivotInA; - btVector3 m_pivotInB; - btVector3 m_axisInA; - btVector3 m_axisInB; + btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis. + btTransform m_rbBFrame; - bool m_angularOnly; + btScalar m_motorTargetVelocity; + btScalar m_maxMotorImpulse; + + btScalar m_limitSoftness; + btScalar m_biasFactor; + btScalar m_relaxationFactor; + + btScalar m_lowerLimit; + btScalar m_upperLimit; + + btScalar m_kHinge; + + btScalar m_limitSign; + btScalar m_correction; - btScalar m_motorTargetVelocity; - btScalar m_maxMotorImpulse; + btScalar m_accLimitImpulse; + + bool m_angularOnly; bool m_enableAngularMotor; + bool m_solveLimit; + public: - btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB); - btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA); + + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame); + + btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame); btHingeConstraint(); @@ -76,6 +95,33 @@ public: m_maxMotorImpulse = maxMotorImpulse; } + void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f) + { + m_lowerLimit = low; + m_upperLimit = high; + + m_limitSoftness = _softness; + m_biasFactor = _biasFactor; + m_relaxationFactor = _relaxationFactor; + + } + + btScalar getHingeAngle(); + + + const btTransform& getAFrame() { return m_rbAFrame; }; + const btTransform& getBFrame() { return m_rbBFrame; }; + + inline int getSolveLimit() + { + return m_solveLimit; + } + + inline btScalar getLimitSign() + { + return m_limitSign; + } + }; #endif //HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 4366284ea73..14b36ad44fd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -755,6 +755,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod if (getSolverMode() & SOLVER_CACHE_FRIENDLY) { + //you need to provide at least some bodies + //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY + btAssert(bodies); + btAssert(numBodies); return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index f80a0826fbf..29719ec9a3e 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -30,15 +30,17 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" //for debug rendering -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" #include "LinearMath/btIDebugDraw.h" @@ -325,6 +327,19 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) } } +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +{ + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + addCollisionObject(body,group,mask); + } +} + void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) { @@ -370,14 +385,21 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) END_PROFILE("updateActivationState"); } -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint) +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) { m_constraints.push_back(constraint); + if (disableCollisionsBetweenLinkedBodies) + { + constraint->getRigidBodyA().addConstraintRef(constraint); + constraint->getRigidBodyB().addConstraintRef(constraint); + } } void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) { m_constraints.remove(constraint); + constraint->getRigidBodyA().removeConstraintRef(constraint); + constraint->getRigidBodyB().removeConstraintRef(constraint); } void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) @@ -726,10 +748,42 @@ public: } }; - +void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) +{ + btVector3 start = transform.getOrigin(); + + const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); + const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); + const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); + + // XY + getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); + getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); + + // XZ + getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); + + // YZ + getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); +} void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) { + // Draw a small simplex at the center of the object + { + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); + } if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) { @@ -750,14 +804,52 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { const btSphereShape* sphereShape = static_cast(shape); btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color); - //drawSphere + + debugDrawSphere(radius, worldTransform, color); break; } case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast(shape); + + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); + debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + // Draw the ends + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0); + debugDrawSphere(radius, childTransform, color); + } + + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0); + debugDrawSphere(radius, childTransform, color); + } + + // Draw some additional lines + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color); + + break; + } case CONE_SHAPE_PROXYTYPE: { const btConeShape* coneShape = static_cast(shape); @@ -789,12 +881,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, default: { - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + if (shape->isConcave()) { - btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape; - //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - //btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30)); - + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + //todo pass camera, for some culling btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 7ffa2c07ee6..83b90bfeebc 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -80,6 +80,7 @@ protected: void saveKinematicState(btScalar timeStep); + void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); public: @@ -94,7 +95,7 @@ public: virtual void updateAabbs(); - void addConstraint(btTypedConstraint* constraint); + void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); void removeConstraint(btTypedConstraint* constraint); @@ -131,6 +132,8 @@ public: virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void removeRigidBody(btRigidBody* body); void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index d3ea7db8e5c..65b63fad4b5 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -43,7 +43,7 @@ class btDynamicsWorld : public btCollisionWorld virtual void updateAabbs() = 0; - virtual void addConstraint(btTypedConstraint* constraint) { (void)constraint;}; + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;}; virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index dbf73b85bcf..3dae930a49f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -16,8 +16,9 @@ subject to the following restrictions: #include "btRigidBody.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "LinearMath/btMinMax.h" -#include -#include +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btMotionState.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables @@ -305,4 +306,33 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) } +bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) +{ + btRigidBody* otherRb = btRigidBody::upcast(co); + if (!otherRb) + return true; + + for (int i = 0; i < m_constraintRefs.size(); ++i) + { + btTypedConstraint* c = m_constraintRefs[i]; + if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) + return false; + } + + return true; +} +void btRigidBody::addConstraintRef(btTypedConstraint* c) +{ + int index = m_constraintRefs.findLinearSearch(c); + if (index == m_constraintRefs.size()) + m_constraintRefs.push_back(c); + + m_checkCollideWith = true; +} + +void btRigidBody::removeConstraintRef(btTypedConstraint* c) +{ + m_constraintRefs.remove(c); + m_checkCollideWith = m_constraintRefs.size() > 0; +} \ No newline at end of file diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 9da545adb68..0707595d48e 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef RIGIDBODY_H #define RIGIDBODY_H +#include "../../LinearMath/btAlignedObjectArray.h" #include "../../LinearMath/btPoint3.h" #include "../../LinearMath/btTransform.h" #include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -23,7 +24,7 @@ subject to the following restrictions: class btCollisionShape; class btMotionState; - +class btTypedConstraint; extern btScalar gLinearAirDamping; @@ -57,6 +58,9 @@ class btRigidBody : public btCollisionObject //m_optionalMotionState allows to automatic synchronize the world transform for active objects btMotionState* m_optionalMotionState; + //keep track of typed constraints referencing this rigid body + btAlignedObjectArray m_constraintRefs; + public: #ifdef OBSOLETE_MOTIONSTATE_LESS @@ -339,6 +343,11 @@ public: return (getBroadphaseProxy() != 0); } + virtual bool checkCollideWithOverride(btCollisionObject* co); + + void addConstraintRef(btTypedConstraint* c); + void removeConstraintRef(btTypedConstraint* c); + int m_debugBodyId; }; diff --git a/extern/bullet2/src/LinearMath/btMatrix3x3.h b/extern/bullet2/src/LinearMath/btMatrix3x3.h index fda348b42ab..94f53c3c0a5 100644 --- a/extern/bullet2/src/LinearMath/btMatrix3x3.h +++ b/extern/bullet2/src/LinearMath/btMatrix3x3.h @@ -45,6 +45,21 @@ class btMatrix3x3 { zx, zy, zz); } + SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + } + + SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other) + { + m_el[0] = other.m_el[0]; + m_el[1] = other.m_el[1]; + m_el[2] = other.m_el[2]; + return *this; + } + SIMD_FORCE_INLINE btVector3 getColumn(int i) const { return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]); diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index d5a7913b742..50334970ba6 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -212,6 +212,7 @@ public: SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; } + }; @@ -283,6 +284,36 @@ slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) return q1.slerp(q2, t); } +SIMD_FORCE_INLINE btVector3 +quatRotate(btQuaternion& rotation, btVector3& v) +{ + btQuaternion q = rotation * v; + q *= rotation.inverse(); + return btVector3(q.getX(),q.getY(),q.getZ()); +} + +SIMD_FORCE_INLINE btQuaternion +shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +{ + btVector3 c = v0.cross(v1); + btScalar d = v0.dot(v1); + + if (d < -1.0 + SIMD_EPSILON) + return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector + + btScalar s = btSqrt((1.0f + d) * 2.0f); + btScalar rs = 1.0f / s; + + return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f); +} + +SIMD_FORCE_INLINE btQuaternion +shortestArcQuatNormalize(btVector3& v0,btVector3& v1) +{ + v0.normalize(); + v1.normalize(); + return shortestArcQuat(v0,v1); +} #endif diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index 9389fcd8156..a885967c5fa 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -32,7 +32,7 @@ subject to the following restrictions: #define USE_BT_CLOCK 1 #ifdef USE_BT_CLOCK -#ifdef __PPU__ +#ifdef __CELLOS_LV2__ #include #include typedef uint64_t __int64; @@ -49,8 +49,13 @@ typedef uint64_t __int64; #define NOWINRES #define NOMCX #define NOIME +#ifdef _XBOX + #include +#else #include +#endif #include + #else #include #endif @@ -81,7 +86,7 @@ class btClock mStartTick = GetTickCount(); mPrevElapsedTime = 0; #else -#ifdef __PPU__ +#ifdef __CELLOS_LV2__ typedef uint64_t __int64; typedef __int64 ClockSize; @@ -134,7 +139,7 @@ class btClock return msecTicks; #else -#ifdef __PPU__ +#ifdef __CELLOS_LV2__ __int64 freq=sys_time_get_timebase_frequency(); double dFreq=((double) freq) / 1000.0; typedef uint64_t __int64; @@ -149,7 +154,7 @@ class btClock gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000 + (currentTime.tv_usec - mStartTime.tv_usec) / 1000; -#endif //__PPU__ +#endif //__CELLOS_LV2__ #endif } @@ -192,7 +197,7 @@ class btClock return usecTicks; #else -#ifdef __PPU__ +#ifdef __CELLOS_LV2__ __int64 freq=sys_time_get_timebase_frequency(); double dFreq=((double) freq)/ 1000000.0; typedef uint64_t __int64; @@ -207,7 +212,7 @@ class btClock gettimeofday(¤tTime, 0); return (currentTime.tv_sec - mStartTime.tv_sec) * 1000000 + (currentTime.tv_usec - mStartTime.tv_usec); -#endif//__PPU__ +#endif//__CELLOS_LV2__ #endif } @@ -218,12 +223,12 @@ class btClock LONGLONG mPrevElapsedTime; LARGE_INTEGER mStartTime; #else -#ifdef __PPU__ +#ifdef __CELLOS_LV2__ uint64_t mStartTime; #else struct timeval mStartTime; #endif -#endif //__PPU__ +#endif //__CELLOS_LV2__ }; @@ -704,3 +709,4 @@ std::string btProfiler::createStatsString(BlockTimingMethod method) #endif //QUICK_PROF_H + diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 222315c1c74..01ad93e786a 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -35,6 +35,11 @@ subject to the following restrictions: #pragma warning(disable:4786) #define SIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #ifdef _XBOX + #define BT_USE_VMX128 + #else + #define BT_USE_SSE + #endif #endif //__MINGW32__ #include @@ -115,7 +120,6 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #endif - #define SIMD_2_PI btScalar(6.283185307179586232) #define SIMD_PI (SIMD_2_PI * btScalar(0.5)) #define SIMD_HALF_PI (SIMD_2_PI * btScalar(0.25)) @@ -130,6 +134,22 @@ SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); } #define SIMD_INFINITY FLT_MAX #endif +SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x) +{ + btScalar coeff_1 = SIMD_PI / 4.0f; + btScalar coeff_2 = 3.0f * coeff_1; + btScalar abs_y = btFabs(y); + btScalar angle; + if (x >= 0.0f) { + btScalar r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } else { + btScalar r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; +} + SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x) { return btFabs(x) < SIMD_EPSILON; } SIMD_FORCE_INLINE bool btEqual(btScalar a, btScalar eps) { diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index b1f3dfca492..2d55fec83a4 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -42,6 +42,19 @@ public: m_origin(c) {} + SIMD_FORCE_INLINE btTransform (const btTransform& other) + : m_basis(other.m_basis), + m_origin(other.m_origin) + { + } + + SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other) + { + m_basis = other.m_basis; + m_origin = other.m_origin; + return *this; + } + SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) { m_basis = t1.m_basis * t2.m_basis; diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index 89211d230fd..25f016cba8a 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -25,6 +25,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" -- cgit v1.2.3