synchronized to latest version of Bullet. added btConeTwistConstraint (useful for...
authorErwin Coumans <blender@erwincoumans.com>
Fri, 6 Jul 2007 03:12:31 +0000 (03:12 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Fri, 6 Jul 2007 03:12:31 +0000 (03:12 +0000)
33 files changed:
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h [new file with mode: 0644]
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
extern/bullet2/src/LinearMath/btMatrix3x3.h
extern/bullet2/src/LinearMath/btQuaternion.h
extern/bullet2/src/LinearMath/btQuickprof.h
extern/bullet2/src/LinearMath/btScalar.h
extern/bullet2/src/LinearMath/btTransform.h
extern/bullet2/src/btBulletDynamicsCommon.h

index 4187a8f29700997361b4e8f2d277e209082db688..60f0a41a9d76ae9f9ffb203e9dc79b3b80814018 100644 (file)
@@ -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)
 {
 }
index e344221217126aafcc7fb5baf986bed48cd9b42d..a81fe3264df7a66c653d3cbf93cf98bff47112d2 100644 (file)
@@ -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
 
+
index 1f265f3dd6b56324136fa26b462d5417586098ee..fb155e7047c75df91653a58a9953f4daab7dfcda 100644 (file)
@@ -41,6 +41,8 @@ struct btSimpleBroadphaseProxy : public btBroadphaseProxy
 class btSimpleBroadphase : public btOverlappingPairCache
 {
 
+protected:
+
        btSimpleBroadphaseProxy*        m_proxies;
        int*                            m_freeProxies;
        int                             m_firstFreeProxy;
index 198276d76df0b16f373aaf5f3a92873c5fd49bc4..b535fac6563e34c483029ed0400d4bd6c3a5c598 100644 (file)
@@ -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 ;
 
index 7b6f066275deaa91c6a57850e893dab69456ad71..d4c0a4e8cb335a289073c298af7f8383d1131d64 100644 (file)
@@ -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) 
 { 
index 93990235afe26757650cd9b4e578fc9d856c95b2..9fb6a67c4a30130805ce22f615e4e0d35097bccf 100644 (file)
@@ -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;
+       }
+
+
 }
 ;
 
index 49f35c2cc90813f5cdb0fcc8f5c1f4f80f4e0a0d..b49036a5b50322b12b27e726b9024ebb0241d3b7 100644 (file)
@@ -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;i<m_collisionObjects.size();i++)
        {
                btCollisionObject*      collisionObject= m_collisionObjects[i];
-
-               //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);
-                       
+               //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);
+                       }       
                }
        }
 
index 0b0286c50f4fa684e0348c4b8af67ceb56950213..b6d80233ab7acbd927774aed8dc188e5e90f5e92 100644 (file)
@@ -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);
 
index 2f96c2e13c5630765153214f9706ac2a239d8967..92f4c8b28a6d935f0e9c46b980d5d35c97c41596 100644 (file)
@@ -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);
index 2d384bd218082618d8e714f9ea3db504da0cfbc3..86dc1f809476147af89af70e3d8474396b5c13bb 100644 (file)
@@ -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];
        }
index 5a359c0cff15127bbded44e90441e33458b3d5cc..94be43c88e2f9dca52ec6ad3b3c1a93527cf64b5 100644 (file)
@@ -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(x<m_width);
+       btAssert(y<m_length);
+
+
+       btScalar        height = getHeightFieldValue(x,y);
+
+       switch (m_upAxis)
+       {
+       case 0:
+               {
+               vertex.setValue(
+                       height,
+                       (-m_width/2 ) + x,
+                       (-m_length/2 ) + y
+                       );
+                       break;
+               }
+       case 1:
+               {
+                       vertex.setValue(
+                       (-m_width/2 ) + x,
+                       height,
+                       (-m_length/2 ) + y
+                       );
+                       break;
+               };
+       case 2:
+               {
+                       vertex.setValue(
+                       (-m_width/2 ) + x,
+                       (-m_length/2 ) + y,
+                       height
+                       );
+                       break;
+               }
+       default:
+               {
+                       //need to get valid m_upAxis
+                       btAssert(0);
+               }
+       }
+
+       vertex*=m_localScaling;
+       
+}
+
+
+void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point) const
+{
+       
+
+       btVector3 clampedPoint(point);
+       clampedPoint.setMax(m_localAabbMin);
+       clampedPoint.setMin(m_localAabbMax);
+
+       btVector3 v = (clampedPoint );// * m_quantization;
+
+       out[0] = (int)(v.getX());
+       out[1] = (int)(v.getY());
+       out[2] = (int)(v.getZ());
+       //correct for
+
+}
+
+
 void   btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
        (void)callback;
        (void)aabbMax;
        (void)aabbMin;
 
-       /*
-       btVector3 halfExtents = (aabbMax - aabbMin) * btScalar(0.5);
-       btVector3 center = (aabbMax + aabbMin) * btScalar(0.5);
+       //quantize the aabbMin and aabbMax, and adjust the start/end ranges
+
+       int     quantizedAabbMin[3];
+       int     quantizedAabbMax[3];
+
+       btVector3       localAabbMin = aabbMin*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+       btVector3       localAabbMax = aabbMax*btVector3(1.f/m_localScaling[0],1.f/m_localScaling[1],1.f/m_localScaling[2]);
+       
+       quantizeWithClamp(quantizedAabbMin, localAabbMin);
+       quantizeWithClamp(quantizedAabbMax, localAabbMax);
+       
        
-       //TODO
-       //this is where the triangles are generated, given AABB and plane equation (normal/constant)
 
-       btVector3 tangentDir0,tangentDir1;
+       int startX=0;
+       int endX=m_width-1;
+       int startJ=0;
+       int endJ=m_length-1;
 
-       //tangentDir0/tangentDir1 can be precalculated
-       btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1);
+       switch (m_upAxis)
+       {
+       case 0:
+               {
+                       quantizedAabbMin[1]+=m_width/2-1;
+                       quantizedAabbMax[1]+=m_width/2+1;
+                       quantizedAabbMin[2]+=m_length/2-1;
+                       quantizedAabbMax[2]+=m_length/2+1;
 
-       btVector3 supVertex0,supVertex1;
+                       if (quantizedAabbMin[1]>startX)
+                               startX = quantizedAabbMin[1];
+                       if (quantizedAabbMax[1]<endX)
+                               endX = quantizedAabbMax[1];
+                       if (quantizedAabbMin[2]>startJ)
+                               startJ = quantizedAabbMin[2];
+                       if (quantizedAabbMax[2]<endJ)
+                               endJ = quantizedAabbMax[2];
+                       break;
+               }
+       case 1:
+               {
+                       quantizedAabbMin[0]+=m_width/2-1;
+                       quantizedAabbMax[0]+=m_width/2+1;
+                       quantizedAabbMin[2]+=m_length/2-1;
+                       quantizedAabbMax[2]+=m_length/2+1;
 
-       btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal;
-       
-       btVector3 triangle[3];
-       btScalar radius = halfExtents.length();
-       triangle[0] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
-       triangle[1] = projectedCenter + tangentDir0*radius - tangentDir1*radius;
-       triangle[2] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
+                       if (quantizedAabbMin[0]>startX)
+                               startX = quantizedAabbMin[0];
+                       if (quantizedAabbMax[0]<endX)
+                               endX = quantizedAabbMax[0];
+                       if (quantizedAabbMin[2]>startJ)
+                               startJ = quantizedAabbMin[2];
+                       if (quantizedAabbMax[2]<endJ)
+                               endJ = quantizedAabbMax[2];
+                       break;
+               };
+       case 2:
+               {
+                       quantizedAabbMin[0]+=m_width/2-1;
+                       quantizedAabbMax[0]+=m_width/2+1;
+                       quantizedAabbMin[1]+=m_length/2-1;
+                       quantizedAabbMax[1]+=m_length/2+1;
 
-       callback->processTriangle(triangle,0,0);
+                       if (quantizedAabbMin[0]>startX)
+                               startX = quantizedAabbMin[0];
+                       if (quantizedAabbMax[0]<endX)
+                               endX = quantizedAabbMax[0];
+                       if (quantizedAabbMin[1]>startJ)
+                               startJ = quantizedAabbMin[1];
+                       if (quantizedAabbMax[1]<endJ)
+                               endJ = quantizedAabbMax[1];
+                       break;
+               }
+       default:
+               {
+                       //need to get valid m_upAxis
+                       btAssert(0);
+               }
+       }
 
-       triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
-       triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
-       triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;
+       
+  
 
-       callback->processTriangle(triangle,0,1);
-*/
+       for(int j=startJ; j<endJ; j++)
+       {
+               for(int x=startX; x<endX; x++)
+               {
+                       btVector3 vertices[3];
+                       if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j+x) & 1)))
+                       {
+        //first triangle
+        getVertex(x,j,vertices[0]);
+        getVertex(x+1,j,vertices[1]);
+        getVertex(x+1,j+1,vertices[2]);
+        callback->processTriangle(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);
+                       }
+               }
+       }
+
+       
 
 }
 
index 85fd49638cefbf073229a4fd62fe871c845e1d3e..49f3e106733189194238a197c3265355d40d0989 100644 (file)
@@ -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;
index b213f3819a42ca715c636f36f36e190c2019353a..015314bc09f0143f43e9656ac3dccd905ff3e9b4 100644 (file)
@@ -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)
 {
index 489b7ddde1ee3d3806c92a493a26431f72642f57..198faaff9f97cdafed0ba515d47bba1a0d61ed92 100644 (file)
@@ -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;
 
index be044dbc16a04b8652927d2bdc047479a92558c5..1897b474057f2cc9a5dfb269d22ab0eab3415b59 100644 (file)
@@ -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; }
 
index 1ab336b4d5454e653ed8a6e053d7ae92604af2d3..687738b7fa9b2d7e5b3281f99aa3ec332f6222e3 100644 (file)
@@ -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)
 {
index a2a3193b0907304ed97862ba1bd1fc4fc1509f35..05662db5d23187ae5f01cc20e5a45e2ce9227f57 100644 (file)
@@ -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 (file)
index 0000000..12a33d7
--- /dev/null
@@ -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 <new>
+
+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 (file)
index 0000000..874669c
--- /dev/null
@@ -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
index 27e3098754958e2ce7c723bc73e5fe39d609a095..ad7fc3a269e875fa0f938663dfc0ab1a6eb9493b 100644 (file)
@@ -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 <new>
 
+
 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)  );
+}
index 5c1ceafbc5bd92372ea31be2d968e5fd64317aa2..46e6480d43cfbfb2faedf78ed80de239228c4251 100644 (file)
@@ -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
index 4366284ea7373255173a7ab3b00ff68059e913c2..14b36ad44fde40f87411e507a1ae39e48bc926d4 100644 (file)
@@ -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);
        }
 
index f80a0826fbfaa46cad3ba160bda66279fd5da1b4..29719ec9a3ec3becfc9d39d3aaf966f435913df3 100644 (file)
@@ -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<const btSphereShape*>(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<const btMultiSphereShape*>(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<const btCapsuleShape*>(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<const btConeShape*>(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));
index 7ffa2c07ee6734b96f55608fa1fd9fba61c00975..83b90bfeebccde5d54bd4e3a46ff866783e713bf 100644 (file)
@@ -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);
index d3ea7db8e5ce6049251994549c46036cf810896e..65b63fad4b596ff357df99a523634dda7087e03d 100644 (file)
@@ -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;};
 
index dbf73b85bcfb211b7ddccb9d9cc23520272d2b54..3dae930a49fe7a81736d976bf7bf6f7b997ab729 100644 (file)
@@ -16,8 +16,9 @@ subject to the following restrictions:
 #include "btRigidBody.h"
 #include "BulletCollision/CollisionShapes/btConvexShape.h"
 #include "LinearMath/btMinMax.h"
-#include <LinearMath/btTransformUtil.h>
-#include <LinearMath/btMotionState.h>
+#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
index 9da545adb68953a3c486fe5f828c7957a7c0edc6..0707595d48e717a34350b37eb1034dc7d0cf74bf 100644 (file)
@@ -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<btTypedConstraint*> 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;
 };
 
index fda348b42abfb7afbe7a085c44a98aaacbff6ac2..94f53c3c0a5cf9764a6a0781b2d7eccf8e0cdc80 100644 (file)
@@ -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]);
index d5a7913b74218c55e2ae0fae6db38534d3ef5316..50334970ba64b3565128f4e45e3d574ce245b5e7 100644 (file)
@@ -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
 
index 9389fcd8156742ebf2205af53672d9ea997230f6..a885967c5fa3f808956dfb771cb8281a4f35b2b5 100644 (file)
@@ -32,7 +32,7 @@ subject to the following restrictions:
 #define USE_BT_CLOCK 1
 
 #ifdef USE_BT_CLOCK
-#ifdef __PPU__
+#ifdef __CELLOS_LV2__
 #include <sys/sys_time.h>
 #include <stdio.h>
 typedef uint64_t __int64;
@@ -49,8 +49,13 @@ typedef uint64_t __int64;
    #define NOWINRES 
    #define NOMCX 
    #define NOIME 
+#ifdef _XBOX
+       #include <Xtl.h>
+#else
        #include <windows.h>
+#endif
        #include <time.h>
+
 #else
        #include <sys/time.h>
 #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(&currentTime, 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(&currentTime, 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
 
+
index 222315c1c7456244016c13fc932d1b582de2a297..01ad93e786a7c2bfd6d538d59419138c6c87db55 100644 (file)
@@ -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 <assert.h>
@@ -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) {
index b1f3dfca4926a92b26032f5efbeed10e6b1b99de..2d55fec83a4091c019804d66f45495756d4406c9 100644 (file)
@@ -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;
index 89211d230fd6cdabf7014a35e3e281b2a53d2ade..25f016cba8a63efacc7f35f268423115e716fa52 100644 (file)
@@ -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"