[Orxonox-commit 212] r2882 - in trunk/src/bullet: BulletCollision/BroadphaseCollision BulletCollision/CollisionDispatch BulletCollision/CollisionShapes BulletCollision/Gimpact BulletCollision/NarrowPhaseCollision BulletDynamics BulletDynamics/Character BulletDynamics/ConstraintSolver BulletDynamics/Dynamics BulletDynamics/Vehicle LinearMath
rgrieder at orxonox.net
rgrieder at orxonox.net
Tue Mar 31 20:05:52 CEST 2009
Author: rgrieder
Date: 2009-03-31 20:05:51 +0200 (Tue, 31 Mar 2009)
New Revision: 2882
Added:
trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h
Modified:
trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp
trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h
trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h
trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h
trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp
trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h
trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp
trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h
trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp
trunk/src/bullet/BulletCollision/Gimpact/gim_math.h
trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
trunk/src/bullet/BulletDynamics/CMakeLists.txt
trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h
trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp
trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h
trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h
trunk/src/bullet/LinearMath/btAlignedAllocator.cpp
trunk/src/bullet/LinearMath/btAlignedAllocator.h
trunk/src/bullet/LinearMath/btAlignedObjectArray.h
trunk/src/bullet/LinearMath/btConvexHull.cpp
trunk/src/bullet/LinearMath/btIDebugDraw.h
trunk/src/bullet/LinearMath/btMatrix3x3.h
trunk/src/bullet/LinearMath/btQuadWord.h
trunk/src/bullet/LinearMath/btQuaternion.h
trunk/src/bullet/LinearMath/btScalar.h
trunk/src/bullet/LinearMath/btTransform.h
trunk/src/bullet/LinearMath/btVector3.h
Log:
Update from Bullet 2.73 to 2.74.
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,7 +19,6 @@
// 3. This notice may not be removed or altered from any source distribution.
#include "btAxisSweep3.h"
-#include <assert.h>
btAxisSweep3::btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache, bool disableRaycastAccelerator)
:btAxisSweep3Internal<unsigned short int>(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache,disableRaycastAccelerator)
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -42,6 +42,7 @@
public:
+ BT_DECLARE_ALIGNED_ALLOCATOR();
class Edge
{
@@ -138,7 +139,7 @@
void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher);
SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;}
- void resetPool();
+ virtual void resetPool(btDispatcher* dispatcher);
void processAllOverlappingPairs(btOverlapCallback* callback);
@@ -220,7 +221,7 @@
}
if (checkCardinality)
- assert(numEdges == m_numHandles*2+1);
+ btAssert(numEdges == m_numHandles*2+1);
}
#endif //DEBUG_BROADPHASE
@@ -346,7 +347,7 @@
m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs
}
- //assert(bounds.HasVolume());
+ //btAssert(bounds.HasVolume());
// init bounds
m_worldAabbMin = worldAabbMin;
@@ -452,7 +453,7 @@
template <typename BP_FP_INT_TYPE>
BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
{
- assert(m_firstFreeHandle);
+ btAssert(m_firstFreeHandle);
BP_FP_INT_TYPE handle = m_firstFreeHandle;
m_firstFreeHandle = getHandle(handle)->GetNextFree();
@@ -464,7 +465,7 @@
template <typename BP_FP_INT_TYPE>
void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
{
- assert(handle > 0 && handle < m_maxHandles);
+ btAssert(handle > 0 && handle < m_maxHandles);
getHandle(handle)->SetNextFree(m_firstFreeHandle);
m_firstFreeHandle = handle;
@@ -587,7 +588,7 @@
}
template <typename BP_FP_INT_TYPE>
-void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool()
+void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* dispatcher)
{
if (m_numHandles == 0)
{
@@ -641,6 +642,7 @@
if (!isDuplicate)
{
+ ///important to use an AABB test that is consistent with the broadphase
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
if (hasOverlap)
@@ -686,10 +688,6 @@
//printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size());
}
-
-
-
-
}
@@ -730,8 +728,8 @@
template <typename BP_FP_INT_TYPE>
void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher)
{
-// assert(bounds.IsFinite());
- //assert(bounds.HasVolume());
+// btAssert(bounds.IsFinite());
+ //btAssert(bounds.HasVolume());
Handle* pHandle = getHandle(handle);
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -64,6 +64,9 @@
///will add some transform later
virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const =0;
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher) {};
+
virtual void printStats() = 0;
};
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -69,7 +69,8 @@
COMPOUND_SHAPE_PROXYTYPE,
SOFTBODY_SHAPE_PROXYTYPE,
-
+ HFFLUID_SHAPE_PROXYTYPE,
+ HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE,
INVALID_SHAPE_PROXYTYPE,
MAX_BROADPHASE_COLLISION_TYPES
@@ -187,7 +188,7 @@
{
//keep them sorted, so the std::set operations work
- if (&proxy0 < &proxy1)
+ if (proxy0.m_uniqueId < proxy1.m_uniqueId)
{
m_pProxy0 = &proxy0;
m_pProxy1 = &proxy1;
@@ -228,8 +229,13 @@
bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b )
{
- return a.m_pProxy0 > b.m_pProxy0 ||
- (a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 > b.m_pProxy1) ||
+ const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1;
+ const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1;
+ const int uidA1 = a.m_pProxy1 ? a.m_pProxy1->m_uniqueId : -1;
+ const int uidB1 = b.m_pProxy1 ? b.m_pProxy1->m_uniqueId : -1;
+
+ return uidA0 > uidB0 ||
+ (a.m_pProxy0 == b.m_pProxy0 && uidA1 > uidB1) ||
(a.m_pProxy0 == b.m_pProxy0 && a.m_pProxy1 == b.m_pProxy1 && a.m_algorithm > b.m_algorithm);
}
};
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -393,12 +393,13 @@
return(n);
}
-//
+#if 0
static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count)
{
while(n&&(count--)) n=n->parent;
return(n);
}
+#endif
//
// Api
@@ -423,9 +424,14 @@
//
void btDbvt::clear()
{
- if(m_root) recursedeletenode(this,m_root);
+ if(m_root)
+ recursedeletenode(this,m_root);
btAlignedFree(m_free);
m_free=0;
+ m_lkhd = -1;
+ m_stkStack.clear();
+ m_opath = 0;
+
}
//
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -57,7 +57,7 @@
// Specific methods implementation
//SSE gives errors on a MSVC 7.1
-#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
+#ifdef BT_USE_SSE
#define DBVT_SELECT_IMPL DBVT_IMPL_SSE
#define DBVT_MERGE_IMPL DBVT_IMPL_SSE
#define DBVT_INT0_IMPL DBVT_IMPL_SSE
@@ -82,7 +82,7 @@
#define DBVT_VIRTUAL_DTOR(a)
#define DBVT_PREFIX template <typename T>
#define DBVT_IPOLICY T& policy
-#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)0;
+#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker;
#else
#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
#define DBVT_VIRTUAL virtual
@@ -146,10 +146,8 @@
DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
+
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
- const btDbvtAabbMm& b,
- const btTransform& xform);
- DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
const btVector3& b);
DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a,
@@ -304,7 +302,7 @@
void collideTTpersistentStack( const btDbvtNode* root0,
const btDbvtNode* root1,
DBVT_IPOLICY);
-
+#if 0
DBVT_PREFIX
void collideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
@@ -316,6 +314,8 @@
const btDbvtNode* root1,
const btTransform& xform1,
DBVT_IPOLICY);
+#endif
+
DBVT_PREFIX
void collideTV( const btDbvtNode* root,
const btDbvtVolume& volume,
@@ -530,22 +530,8 @@
#endif
}
-//
-DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
- const btDbvtAabbMm& b,
- const btTransform& xform)
-{
- const btVector3 d0=xform*b.Center()-a.Center();
- const btVector3 d1=d0*xform.getBasis();
- btScalar s0[2]={0,0};
- btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]};
- a.AddSpan(d0,s0[0],s0[1]);
- b.AddSpan(d1,s1[0],s1[1]);
- if(s0[0]>(s1[1])) return(false);
- if(s0[1]<(s1[0])) return(false);
- return(true);
-}
+
//
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
const btVector3& b)
@@ -848,7 +834,7 @@
}
}
-
+#if 0
//
DBVT_PREFIX
inline void btDbvt::collideTT( const btDbvtNode* root0,
@@ -904,7 +890,6 @@
} while(depth);
}
}
-
//
DBVT_PREFIX
inline void btDbvt::collideTT( const btDbvtNode* root0,
@@ -916,6 +901,7 @@
const btTransform xform=xform0.inverse()*xform1;
collideTT(root0,root1,xform,policy);
}
+#endif
//
DBVT_PREFIX
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -100,7 +100,8 @@
btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data;
#if DBVT_BP_SORTPAIRS
- if(pa>pb) btSwap(pa,pb);
+ if(pa->m_uniqueId>pb->m_uniqueId)
+ btSwap(pa,pb);
#endif
pbp->m_paircache->addOverlappingPair(pa,pb);
++pbp->m_newpairs;
@@ -132,9 +133,7 @@
m_updates_call = 0;
m_updates_done = 0;
m_updates_ratio = 0;
- m_paircache = paircache?
-paircache :
- new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
+ m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_gid = 0;
m_pid = 0;
m_cid = 0;
@@ -210,23 +209,22 @@
aabbMax = proxy->m_aabbMax;
}
-void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+struct BroadphaseRayTester : btDbvt::ICollide
{
-
- struct BroadphaseRayTester : btDbvt::ICollide
+ btBroadphaseRayCallback& m_rayCallback;
+ BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
+ :m_rayCallback(orgCallback)
{
- btBroadphaseRayCallback& m_rayCallback;
- BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
- :m_rayCallback(orgCallback)
- {
- }
- void Process(const btDbvtNode* leaf)
- {
- btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
- m_rayCallback.process(proxy);
- }
- };
+ }
+ void Process(const btDbvtNode* leaf)
+ {
+ btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
+ m_rayCallback.process(proxy);
+ }
+};
+void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
+{
BroadphaseRayTester callback(rayCallback);
m_sets[0].rayTestInternal( m_sets[0].m_root,
@@ -341,11 +339,103 @@
m_clock.reset();
}
#endif
+
+ performDeferredRemoval(dispatcher);
+
}
+void btDbvtBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
+{
+
+ if (m_paircache->hasDeferredRemoval())
+ {
+
+ btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
+
+ //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+
+ int invalidPair = 0;
+
+
+ int i;
+
+ btBroadphasePair previousPair;
+ previousPair.m_pProxy0 = 0;
+ previousPair.m_pProxy1 = 0;
+ previousPair.m_algorithm = 0;
+
+
+ for (i=0;i<overlappingPairArray.size();i++)
+ {
+
+ btBroadphasePair& pair = overlappingPairArray[i];
+
+ bool isDuplicate = (pair == previousPair);
+
+ previousPair = pair;
+
+ bool needsRemoval = false;
+
+ if (!isDuplicate)
+ {
+ //important to perform AABB check that is consistent with the broadphase
+ btDbvtProxy* pa=(btDbvtProxy*)pair.m_pProxy0;
+ btDbvtProxy* pb=(btDbvtProxy*)pair.m_pProxy1;
+ bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
+
+ if (hasOverlap)
+ {
+ needsRemoval = false;
+ } else
+ {
+ needsRemoval = true;
+ }
+ } else
+ {
+ //remove duplicate
+ needsRemoval = true;
+ //should have no algorithm
+ btAssert(!pair.m_algorithm);
+ }
+
+ if (needsRemoval)
+ {
+ m_paircache->cleanOverlappingPair(pair,dispatcher);
+
+ pair.m_pProxy0 = 0;
+ pair.m_pProxy1 = 0;
+ invalidPair++;
+ }
+
+ }
+
+ //perform a sort, to sort 'invalid' pairs to the end
+ overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
+ overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+ }
+}
+
//
void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{
+ /*printf("---------------------------------------------------------\n");
+ printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+ printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+ printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+ {
+ int i;
+ for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+ {
+ printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+ getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+ }
+ printf("\n");
+ }
+*/
+
+
+
SPC(m_profiling.m_total);
/* optimize */
m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100);
@@ -401,17 +491,18 @@
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
if(pairs.size()>0)
{
- const int ci=pairs.size();
- int ni=btMin(ci,btMax<int>(m_newpairs,(ci*m_cupdates)/100));
+
+ int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
for(int i=0;i<ni;++i)
{
- btBroadphasePair& p=pairs[(m_cid+i)%ci];
+ btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1;
if(!Intersect(pa->leaf->volume,pb->leaf->volume))
{
#if DBVT_BP_SORTPAIRS
- if(pa>pb) btSwap(pa,pb);
+ if(pa->m_uniqueId>pb->m_uniqueId)
+ btSwap(pa,pb);
#endif
m_paircache->removeOverlappingPair(pa,pb,dispatcher);
--ni;--i;
@@ -468,6 +559,39 @@
aabbMax=bounds.Maxs();
}
+void btDbvtBroadphase::resetPool(btDispatcher* dispatcher)
+{
+
+ int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
+ if (!totalObjects)
+ {
+ //reset internal dynamic tree data structures
+ m_sets[0].clear();
+ m_sets[1].clear();
+
+ m_deferedcollide = false;
+ m_needcleanup = true;
+ m_prediction = 1/(btScalar)2;
+ m_stageCurrent = 0;
+ m_fixedleft = 0;
+ m_fupdates = 1;
+ m_dupdates = 0;
+ m_cupdates = 10;
+ m_newpairs = 1;
+ m_updates_call = 0;
+ m_updates_done = 0;
+ m_updates_ratio = 0;
+
+ m_gid = 0;
+ m_pid = 0;
+ m_cid = 0;
+ for(int i=0;i<=STAGECOUNT;++i)
+ {
+ m_stageRoots[i]=0;
+ }
+ }
+}
+
//
void btDbvtBroadphase::printStats()
{}
@@ -600,3 +724,4 @@
#if DBVT_BP_PROFILE
#undef SPC
#endif
+
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -24,7 +24,7 @@
//
#define DBVT_BP_PROFILE 0
-#define DBVT_BP_SORTPAIRS 1
+//#define DBVT_BP_SORTPAIRS 1
#define DBVT_BP_PREVENTFALSEUPDATE 0
#define DBVT_BP_ACCURATESLEEPING 0
#define DBVT_BP_ENABLE_BENCHMARK 0
@@ -114,6 +114,13 @@
void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const;
void printStats();
static void benchmark(btBroadphaseInterface*);
+
+
+ void performDeferredRemoval(btDispatcher* dispatcher);
+
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
};
#endif
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -482,3 +482,8 @@
*/
}
+
+void btMultiSapBroadphase::resetPool(btDispatcher* dispatcher)
+{
+ // not yet
+}
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -143,6 +143,9 @@
void quicksort (btBroadphasePairArray& a, int lo, int hi);
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
};
#endif //BT_MULTI_SAP_BROADPHASE
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,6 +19,7 @@
#include "btDispatcher.h"
#include "btCollisionAlgorithm.h"
+#include "LinearMath/btAabbUtil2.h"
#include <stdio.h>
@@ -135,7 +136,8 @@
btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
gFindPairs++;
- if(proxy0>proxy1) btSwap(proxy0,proxy1);
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
@@ -211,7 +213,8 @@
btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
{
- if(proxy0>proxy1) btSwap(proxy0,proxy1);
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
@@ -270,7 +273,8 @@
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
{
gRemovePairs++;
- if(proxy0>proxy1) btSwap(proxy0,proxy1);
+ if(proxy0->m_uniqueId>proxy1->m_uniqueId)
+ btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
@@ -392,8 +396,37 @@
}
}
+void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+ ///need to keep hashmap in sync with pair address, so rebuild all
+ btBroadphasePairArray tmpPairs;
+ int i;
+ for (i=0;i<m_overlappingPairArray.size();i++)
+ {
+ tmpPairs.push_back(m_overlappingPairArray[i]);
+ }
+ for (i=0;i<tmpPairs.size();i++)
+ {
+ removeOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1,dispatcher);
+ }
+
+ for (i = 0; i < m_next.size(); i++)
+ {
+ m_next[i] = BT_NULL_PAIR;
+ }
+ tmpPairs.quickSort(btBroadphasePairSortPredicate());
+
+ for (i=0;i<tmpPairs.size();i++)
+ {
+ addOverlappingPair(tmpPairs[i].m_pProxy0,tmpPairs[i].m_pProxy1);
+ }
+
+
+}
+
+
void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1, btDispatcher* dispatcher )
{
if (!hasDeferredRemoval())
@@ -429,7 +462,7 @@
btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
{
//don't add overlap with own
- assert(proxy0 != proxy1);
+ btAssert(proxy0 != proxy1);
if (!needsBroadphaseCollision(proxy0,proxy1))
return 0;
@@ -460,7 +493,7 @@
if (findIndex < m_overlappingPairArray.size())
{
- //assert(it != m_overlappingPairSet.end());
+ //btAssert(it != m_overlappingPairSet.end());
btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
return pair;
}
@@ -490,8 +523,9 @@
if (callback->processOverlap(*pair))
{
cleanOverlappingPair(*pair,dispatcher);
-
- m_overlappingPairArray.swap(i,m_overlappingPairArray.capacity()-1);
+ pair->m_pProxy0 = 0;
+ pair->m_pProxy1 = 0;
+ m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
m_overlappingPairArray.pop_back();
gOverlappingPairs--;
} else
@@ -591,3 +625,9 @@
processAllOverlappingPairs(&removeCallback,dispatcher);
}
+
+void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
+{
+ //should already be sorted
+}
+
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -84,6 +84,9 @@
virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0;
+
+
};
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
@@ -259,8 +262,11 @@
m_ghostPairCallback = ghostPairCallback;
}
-public:
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher);
+
+protected:
+
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
btOverlappingPairCallback* m_ghostPairCallback;
@@ -369,6 +375,8 @@
m_ghostPairCallback = ghostPairCallback;
}
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher);
+
};
@@ -447,6 +455,9 @@
{
}
+ virtual void sortOverlappingPairs(btDispatcher* dispatcher)
+ {
+ }
};
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -469,7 +469,6 @@
rayAabbMax += aabbMax;
#ifdef RAYAABB2
- btVector3 rayFrom = raySource;
btVector3 rayDir = (rayTarget-raySource);
rayDir.normalize ();
lambda_max = rayDir.dot(rayTarget-raySource);
@@ -558,7 +557,6 @@
btScalar lambda_max = 1.0;
#ifdef RAYAABB2
- btVector3 rayFrom = raySource;
btVector3 rayDirection = (rayTarget-raySource);
rayDirection.normalize ();
lambda_max = rayDirection.dot(rayTarget-raySource);
@@ -817,14 +815,15 @@
//PCK: include
#include <new>
+#if 0
//PCK: consts
static const unsigned BVH_ALIGNMENT = 16;
static const unsigned BVH_ALIGNMENT_MASK = BVH_ALIGNMENT-1;
static const unsigned BVH_ALIGNMENT_BLOCKS = 2;
+#endif
-
unsigned int btQuantizedBvh::getAlignmentSerializationPadding()
{
// I changed this to 0 since the extra padding is not needed or used.
@@ -1146,3 +1145,4 @@
+
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -324,5 +324,7 @@
return aabbOverlap(p0,p1);
}
-
-
+void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
+{
+ //not yet
+}
Modified: trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h
===================================================================
--- trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -108,7 +108,10 @@
return proxy0;
}
+ ///reset broadphase internal structures, to ensure determinism/reproducability
+ virtual void resetPool(btDispatcher* dispatcher);
+
void validate();
protected:
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -52,7 +52,7 @@
for (int j=0;j<MAX_BROADPHASE_COLLISION_TYPES;j++)
{
m_doubleDispatch[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j);
- assert(m_doubleDispatch[i][j]);
+ btAssert(m_doubleDispatch[i][j]);
}
}
@@ -79,8 +79,12 @@
btCollisionObject* body0 = (btCollisionObject*)b0;
btCollisionObject* body1 = (btCollisionObject*)b1;
- btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
-
+ //test for Bullet 2.74: use a relative contact breaking threshold without clamping against 'gContactBreakingThreshold'
+ //btScalar contactBreakingThreshold = btMin(gContactBreakingThreshold,btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold()));
+ btScalar contactBreakingThreshold = btMin(body0->getCollisionShape()->getContactBreakingThreshold(),body1->getCollisionShape()->getContactBreakingThreshold());
+
+ btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold());
+
void* mem = 0;
if (m_persistentManifoldPoolAllocator->getFreeCount())
@@ -91,7 +95,7 @@
mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
}
- btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold);
+ btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold);
manifold->m_index1a = m_manifoldsPtr.size();
m_manifoldsPtr.push_back(manifold);
@@ -146,7 +150,6 @@
-
bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
{
//here you can do filtering
@@ -160,8 +163,8 @@
bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionObject* body1)
{
- assert(body0);
- assert(body1);
+ btAssert(body0);
+ btAssert(body1);
bool needsCollision = true;
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -17,7 +17,10 @@
#include "btCollisionObject.h"
btCollisionObject::btCollisionObject()
- : m_broadphaseHandle(0),
+ : m_anisotropicFriction(1.f,1.f,1.f),
+ m_hasAnisotropicFriction(false),
+ m_contactProcessingThreshold(1e30f),
+ m_broadphaseHandle(0),
m_collisionShape(0),
m_rootCollisionShape(0),
m_collisionFlags(btCollisionObject::CF_STATIC_OBJECT),
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -52,6 +52,11 @@
//without destroying the continuous interpolated motion (which uses this interpolation velocities)
btVector3 m_interpolationLinearVelocity;
btVector3 m_interpolationAngularVelocity;
+
+ btVector3 m_anisotropicFriction;
+ bool m_hasAnisotropicFriction;
+ btScalar m_contactProcessingThreshold;
+
btBroadphaseProxy* m_broadphaseHandle;
btCollisionShape* m_collisionShape;
@@ -114,10 +119,11 @@
{
CO_COLLISION_OBJECT =1,
CO_RIGID_BODY,
- CO_SOFT_BODY,
///CO_GHOST_OBJECT keeps track of all objects overlapping its AABB and that pass its collision filter
///It is useful for collision sensors, explosion objects, character controller etc.
- CO_GHOST_OBJECT
+ CO_GHOST_OBJECT,
+ CO_SOFT_BODY,
+ CO_HF_FLUID
};
SIMD_FORCE_INLINE bool mergesSimulationIslands() const
@@ -126,7 +132,31 @@
return ((m_collisionFlags & (CF_STATIC_OBJECT | CF_KINEMATIC_OBJECT | CF_NO_CONTACT_RESPONSE) )==0);
}
+ const btVector3& getAnisotropicFriction() const
+ {
+ return m_anisotropicFriction;
+ }
+ void setAnisotropicFriction(const btVector3& anisotropicFriction)
+ {
+ m_anisotropicFriction = anisotropicFriction;
+ m_hasAnisotropicFriction = (anisotropicFriction[0]!=1.f) || (anisotropicFriction[1]!=1.f) || (anisotropicFriction[2]!=1.f);
+ }
+ bool hasAnisotropicFriction() const
+ {
+ return m_hasAnisotropicFriction;
+ }
+ ///the constraint solver can discard solving contacts, if the distance is above this threshold. 0 by default.
+ ///Note that using contacts with positive distance can improve stability. It increases, however, the chance of colliding with degerate contacts, such as 'interior' triangle edges
+ void setContactProcessingThreshold( btScalar contactProcessingThreshold)
+ {
+ m_contactProcessingThreshold = contactProcessingThreshold;
+ }
+ btScalar getContactProcessingThreshold() const
+ {
+ return m_contactProcessingThreshold;
+ }
+
SIMD_FORCE_INLINE bool isStaticObject() const {
return (m_collisionFlags & CF_STATIC_OBJECT) != 0;
}
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -69,6 +69,7 @@
//
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
getBroadphase()->destroyProxy(bp,m_dispatcher1);
+ collisionObject->setBroadphaseHandle(0);
}
}
@@ -118,6 +119,39 @@
+void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
+{
+ btVector3 minAabb,maxAabb;
+ colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+ //need to increase the aabb for contact thresholds
+ btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
+ minAabb -= contactThreshold;
+ maxAabb += contactThreshold;
+
+ btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
+
+ //moving objects should be moderately sized, probably something wrong if not
+ if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
+ {
+ bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
+ } else
+ {
+ //something went wrong, investigate
+ //this assert is unwanted in 3D modelers (danger of loosing work)
+ colObj->setActivationState(DISABLE_SIMULATION);
+
+ static bool reportMe = true;
+ if (reportMe && m_debugDrawer)
+ {
+ reportMe = false;
+ m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
+ m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs at continuousphysics.com\n");
+ m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+ m_debugDrawer->reportErrorWarning("Thanks.\n");
+ }
+ }
+}
+
void btCollisionWorld::updateAabbs()
{
BT_PROFILE("updateAabbs");
@@ -130,38 +164,9 @@
//only update aabb of active objects
if (colObj->isActive())
{
- btVector3 minAabb,maxAabb;
- colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
- //need to increase the aabb for contact thresholds
- btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
- minAabb -= contactThreshold;
- maxAabb += contactThreshold;
-
- btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
-
- //moving objects should be moderately sized, probably something wrong if not
- if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
- {
- bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
- } else
- {
- //something went wrong, investigate
- //this assert is unwanted in 3D modelers (danger of loosing work)
- colObj->setActivationState(DISABLE_SIMULATION);
-
- static bool reportMe = true;
- if (reportMe && m_debugDrawer)
- {
- reportMe = false;
- m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
- m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs at continuousphysics.com\n");
- m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
- m_debugDrawer->reportErrorWarning("Thanks.\n");
- }
- }
+ updateSingleAabb(colObj);
}
}
-
}
@@ -293,7 +298,8 @@
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh):
- btTriangleRaycastCallback(from,to),
+ //@BP Mod
+ btTriangleRaycastCallback(from,to, resultCallback->m_flags),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
@@ -342,7 +348,8 @@
BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to,
btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh):
- btTriangleRaycastCallback(from,to),
+ //@BP Mod
+ btTriangleRaycastCallback(from,to, resultCallback->m_flags),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
@@ -663,7 +670,7 @@
{
//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-
+#if 0
#ifdef RECALCULATE_AABB
btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
@@ -672,6 +679,7 @@
const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
#endif
+#endif
//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
//culling already done by broadphase
//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -138,9 +138,10 @@
return m_dispatcher1;
}
+ void updateSingleAabb(btCollisionObject* colObj);
+
virtual void updateAabbs();
-
virtual void setDebugDrawer(btIDebugDraw* debugDrawer)
{
@@ -192,6 +193,8 @@
btCollisionObject* m_collisionObject;
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
+ //@BP Mod - Custom flags, currently used to enable backface culling on tri-meshes, see btRaycastCallback
+ unsigned int m_flags;
virtual ~RayResultCallback()
{
@@ -205,7 +208,9 @@
:m_closestHitFraction(btScalar(1.)),
m_collisionObject(0),
m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter),
- m_collisionFilterMask(btBroadphaseProxy::AllFilter)
+ m_collisionFilterMask(btBroadphaseProxy::AllFilter),
+ //@BP Mod
+ m_flags(0)
{
}
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,6 +19,7 @@
#include "BulletCollision/BroadphaseCollision/btDbvt.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btAabbUtil2.h"
+#include "btManifoldResult.h"
btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
:btActivatingCollisionAlgorithm(ci,body0,body1),
@@ -28,10 +29,22 @@
m_ownsManifold = false;
btCollisionObject* colObj = m_isSwapped? body1 : body0;
+ btAssert (colObj->getCollisionShape()->isCompound());
+
+ btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+ m_compoundShapeRevision = compoundShape->getUpdateRevision();
+
+ preallocateChildAlgorithms(body0,body1);
+}
+
+void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1)
+{
+ btCollisionObject* colObj = m_isSwapped? body1 : body0;
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
- assert (colObj->getCollisionShape()->isCompound());
+ btAssert (colObj->getCollisionShape()->isCompound());
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+
int numChildren = compoundShape->getNumChildShapes();
int i;
@@ -46,14 +59,13 @@
btCollisionShape* tmpShape = colObj->getCollisionShape();
btCollisionShape* childShape = compoundShape->getChildShape(i);
colObj->internalSetTemporaryCollisionShape( childShape );
- m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj,m_sharedManifold);
+ m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(colObj,otherObj,m_sharedManifold);
colObj->internalSetTemporaryCollisionShape( tmpShape );
}
}
}
-
-btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+void btCompoundCollisionAlgorithm::removeChildAlgorithms()
{
int numChildren = m_childCollisionAlgorithms.size();
int i;
@@ -67,9 +79,14 @@
}
}
+btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm()
+{
+ removeChildAlgorithms();
+}
+
struct btCompoundLeafCallback : btDbvt::ICollide
{
@@ -167,13 +184,50 @@
btCollisionObject* colObj = m_isSwapped? body1 : body0;
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
- assert (colObj->getCollisionShape()->isCompound());
+
+
+ btAssert (colObj->getCollisionShape()->isCompound());
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
+ ///btCompoundShape might have changed:
+ ////make sure the internal child collision algorithm caches are still valid
+ if (compoundShape->getUpdateRevision() != m_compoundShapeRevision)
+ {
+ ///clear and update all
+ removeChildAlgorithms();
+
+ preallocateChildAlgorithms(body0,body1);
+ }
+
+
btDbvt* tree = compoundShape->getDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
btCompoundLeafCallback callback(colObj,otherObj,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold);
+ ///we need to refresh all contact manifolds
+ ///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
+ ///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
+ {
+ int i;
+ btManifoldArray manifoldArray;
+ for (i=0;i<m_childCollisionAlgorithms.size();i++)
+ {
+ if (m_childCollisionAlgorithms[i])
+ {
+ m_childCollisionAlgorithms[i]->getAllContactManifolds(manifoldArray);
+ for (int m=0;m<manifoldArray.size();m++)
+ {
+ if (manifoldArray[m]->getNumContacts())
+ {
+ resultOut->setPersistentManifold(manifoldArray[m]);
+ resultOut->refreshContactPoints();
+ resultOut->setPersistentManifold(0);//??necessary?
+ }
+ }
+ manifoldArray.clear();
+ }
+ }
+ }
if (tree)
{
@@ -242,7 +296,7 @@
btCollisionObject* colObj = m_isSwapped? body1 : body0;
btCollisionObject* otherObj = m_isSwapped? body0 : body1;
- assert (colObj->getCollisionShape()->isCompound());
+ btAssert (colObj->getCollisionShape()->isCompound());
btCompoundShape* compoundShape = static_cast<btCompoundShape*>(colObj->getCollisionShape());
@@ -285,3 +339,4 @@
}
+
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -26,6 +26,7 @@
#include "btCollisionCreateFunc.h"
#include "LinearMath/btAlignedObjectArray.h"
class btDispatcher;
+class btCollisionObject;
/// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes
class btCompoundCollisionAlgorithm : public btActivatingCollisionAlgorithm
@@ -35,7 +36,13 @@
class btPersistentManifold* m_sharedManifold;
bool m_ownsManifold;
+
+ int m_compoundShapeRevision;//to keep track of changes, so that childAlgorithm array can be updated
+ void removeChildAlgorithms();
+
+ void preallocateChildAlgorithms(btCollisionObject* body0,btCollisionObject* body1);
+
public:
btCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped);
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -51,6 +51,8 @@
btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
{
+ m_numPerturbationIterations = 0;
+ m_minimumPointsPerturbationThreshold = 3;
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
@@ -59,17 +61,19 @@
{
}
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
: btActivatingCollisionAlgorithm(ci,body0,body1),
m_simplexSolver(simplexSolver),
m_pdSolver(pdSolver),
m_ownManifold (false),
m_manifoldPtr(mf),
-m_lowLevelOfDetail(false)
+m_lowLevelOfDetail(false),
#ifdef USE_SEPDISTANCE_UTIL2
,m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
- (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc())
+ (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
#endif
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{
(void)body0;
(void)body1;
@@ -93,9 +97,64 @@
}
+struct btPerturbedContactResult : public btManifoldResult
+{
+ btManifoldResult* m_originalManifoldResult;
+ btTransform m_transformA;
+ btTransform m_transformB;
+ btTransform m_unPerturbedTransform;
+ bool m_perturbA;
+ btIDebugDraw* m_debugDrawer;
+ btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
+ :m_originalManifoldResult(originalResult),
+ m_transformA(transformA),
+ m_transformB(transformB),
+ m_perturbA(perturbA),
+ m_unPerturbedTransform(unPerturbedTransform),
+ m_debugDrawer(debugDrawer)
+ {
+ }
+ virtual ~ btPerturbedContactResult()
+ {
+ }
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
+ {
+ btVector3 endPt,startPt;
+ btScalar newDepth;
+ btVector3 newNormal;
+
+ if (m_perturbA)
+ {
+ btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
+ endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
+ newDepth = (endPt - pointInWorld).dot(normalOnBInWorld);
+ startPt = endPt+normalOnBInWorld*newDepth;
+ } else
+ {
+ endPt = pointInWorld + normalOnBInWorld*orgDepth;
+ startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
+ newDepth = (endPt - startPt).dot(normalOnBInWorld);
+
+ }
+
+//#define DEBUG_CONTACTS 1
+#ifdef DEBUG_CONTACTS
+ m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
+ m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
+ m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
+#endif //DEBUG_CONTACTS
+
+
+ m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
+ }
+
+};
+
+extern btScalar gContactBreakingThreshold;
+
//
// Convex-Convex collision algorithm
//
@@ -110,6 +169,8 @@
}
resultOut->setPersistentManifold(m_manifoldPtr);
+ //comment-out next line to test multi-contact generation
+ //resultOut->getPersistentManifold()->clearManifold();
btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
@@ -146,9 +207,78 @@
input.m_transformB = body1->getWorldTransform();
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
-
btScalar sepDist = gjkPairDetector.getCachedSeparatingDistance()+dispatchInfo.m_convexConservativeDistanceThreshold;
+ //now perturbe directions to get multiple contact points
+ btVector3 v0,v1;
+ btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
+ btPlaneSpace1(sepNormalWorldSpace,v0,v1);
+ //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+ //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
+ if (resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
+ {
+
+ int i;
+
+ bool perturbeA = true;
+ const btScalar angleLimit = 0.125f * SIMD_PI;
+ btScalar perturbeAngle;
+ btScalar radiusA = min0->getAngularMotionDisc();
+ btScalar radiusB = min1->getAngularMotionDisc();
+ if (radiusA < radiusB)
+ {
+ perturbeAngle = gContactBreakingThreshold /radiusA;
+ perturbeA = true;
+ } else
+ {
+ perturbeAngle = gContactBreakingThreshold / radiusB;
+ perturbeA = false;
+ }
+ if ( perturbeAngle > angleLimit )
+ perturbeAngle = angleLimit;
+
+ btTransform unPerturbedTransform;
+ if (perturbeA)
+ {
+ unPerturbedTransform = input.m_transformA;
+ } else
+ {
+ unPerturbedTransform = input.m_transformB;
+ }
+
+ for ( i=0;i<m_numPerturbationIterations;i++)
+ {
+ btQuaternion perturbeRot(v0,perturbeAngle);
+ btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+ btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
+
+
+ if (perturbeA)
+ {
+ input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
+ input.m_transformB = body1->getWorldTransform();
+#ifdef DEBUG_CONTACTS
+ dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
+#endif //DEBUG_CONTACTS
+ } else
+ {
+ input.m_transformA = body0->getWorldTransform();
+ input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
+#ifdef DEBUG_CONTACTS
+ dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
+#endif
+ }
+
+ btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
+ gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
+
+
+ }
+ }
+
+
+
#ifdef USE_SEPDISTANCE_UTIL2
if (dispatchInfo.m_useConvexConservativeDistanceUtil)
{
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -33,7 +33,9 @@
///for certain pairs that have a small size ratio
///#define USE_SEPDISTANCE_UTIL2 1
-///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+///The convexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations between two convex objects.
+///Multiple contact points are calculated by perturbing the orientation of the smallest object orthogonal to the separating normal.
+///This idea was described by Gino van den Bergen in this forum topic http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=4&t=288&p=888#p888
class btConvexConvexAlgorithm : public btActivatingCollisionAlgorithm
{
#ifdef USE_SEPDISTANCE_UTIL2
@@ -47,13 +49,18 @@
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+
///cache separating vector to speedup collision detection
public:
- btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
+ btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold);
+
virtual ~btConvexConvexAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
@@ -78,9 +85,12 @@
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
+
btConvexPenetrationDepthSolver* m_pdSolver;
btSimplexSolverInterface* m_simplexSolver;
-
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
virtual ~CreateFunc();
@@ -88,7 +98,7 @@
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
- return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
+ return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
}
};
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -4,8 +4,8 @@
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,
+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.
@@ -22,15 +22,17 @@
//#include <stdio.h>
-btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped)
+btConvexPlaneCollisionAlgorithm::btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold)
: btCollisionAlgorithm(ci),
m_ownManifold(false),
m_manifoldPtr(mf),
-m_isSwapped(isSwapped)
+m_isSwapped(isSwapped),
+m_numPerturbationIterations(numPerturbationIterations),
+m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{
btCollisionObject* convexObj = m_isSwapped? col1 : col0;
btCollisionObject* planeObj = m_isSwapped? col0 : col1;
-
+
if (!m_manifoldPtr && m_dispatcher->needsCollision(convexObj,planeObj))
{
m_manifoldPtr = m_dispatcher->getNewManifold(convexObj,planeObj);
@@ -48,30 +50,28 @@
}
}
-
-
-void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexPlaneCollisionAlgorithm::collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
- (void)dispatchInfo;
- (void)resultOut;
- if (!m_manifoldPtr)
- return;
-
- btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+ btCollisionObject* convexObj = m_isSwapped? body1 : body0;
btCollisionObject* planeObj = m_isSwapped? body0: body1;
btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
- bool hasCollision = false;
+ bool hasCollision = false;
const btVector3& planeNormal = planeShape->getPlaneNormal();
const btScalar& planeConstant = planeShape->getPlaneConstant();
+
+ btTransform convexWorldTransform = convexObj->getWorldTransform();
+ btTransform convexInPlaneTrans;
+ convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexWorldTransform;
+ //now perturbe the convex-world transform
+ convexWorldTransform.getBasis()*=btMatrix3x3(perturbeRot);
btTransform planeInConvex;
- planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform();
- btTransform convexInPlaneTrans;
- convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform();
+ planeInConvex= convexWorldTransform.inverse() * planeObj->getWorldTransform();
+
+ btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
- btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);
btVector3 vtxInPlane = convexInPlaneTrans(vtx);
btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);
@@ -87,6 +87,53 @@
btVector3 pOnB = vtxInPlaneWorld;
resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance);
}
+}
+
+
+void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+{
+ (void)dispatchInfo;
+ if (!m_manifoldPtr)
+ return;
+
+ btCollisionObject* convexObj = m_isSwapped? body1 : body0;
+ btCollisionObject* planeObj = m_isSwapped? body0: body1;
+
+ btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape();
+ btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape();
+
+ bool hasCollision = false;
+ const btVector3& planeNormal = planeShape->getPlaneNormal();
+ const btScalar& planeConstant = planeShape->getPlaneConstant();
+
+ //first perform a collision query with the non-perturbated collision objects
+ {
+ btQuaternion rotq(0,0,0,1);
+ collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut);
+ }
+
+ if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold)
+ {
+ btVector3 v0,v1;
+ btPlaneSpace1(planeNormal,v0,v1);
+ //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
+
+ const btScalar angleLimit = 0.125f * SIMD_PI;
+ btScalar perturbeAngle;
+ btScalar radius = convexShape->getAngularMotionDisc();
+ perturbeAngle = gContactBreakingThreshold / radius;
+ if ( perturbeAngle > angleLimit )
+ perturbeAngle = angleLimit;
+
+ btQuaternion perturbeRot(v0,perturbeAngle);
+ for (int i=0;i<m_numPerturbationIterations;i++)
+ {
+ btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
+ btQuaternion rotq(planeNormal,iterationAngle);
+ collideSingleContact(rotq.inverse()*perturbeRot*rotq,body0,body1,dispatchInfo,resultOut);
+ }
+ }
+
if (m_ownManifold)
{
if (m_manifoldPtr->getNumContacts())
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -4,8 +4,8 @@
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,
+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.
@@ -28,18 +28,22 @@
/// Other features are frame-coherency (persistent data) and collision response.
class btConvexPlaneCollisionAlgorithm : public btCollisionAlgorithm
{
- bool m_ownManifold;
+ bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
- bool m_isSwapped;
-
+ bool m_isSwapped;
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
public:
- btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped);
+ btConvexPlaneCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped, int numPerturbationIterations,int minimumPointsPerturbationThreshold);
virtual ~btConvexPlaneCollisionAlgorithm();
virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+ void collideSingleContact (const btQuaternion& perturbeRot, btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
+
virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut);
virtual void getAllContactManifolds(btManifoldArray& manifoldArray)
@@ -52,15 +56,24 @@
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
+ int m_numPerturbationIterations;
+ int m_minimumPointsPerturbationThreshold;
+
+ CreateFunc()
+ : m_numPerturbationIterations(3),
+ m_minimumPointsPerturbationThreshold(3)
+ {
+ }
+
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexPlaneCollisionAlgorithm));
if (!m_swapped)
{
- return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false);
+ return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,false,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
} else
{
- return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true);
+ return new(mem) btConvexPlaneCollisionAlgorithm(0,ci,body0,body1,true,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold);
}
}
};
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -288,3 +288,10 @@
//failed to find an algorithm
return m_emptyCreateFunc;
}
+
+void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+{
+ btConvexConvexAlgorithm::CreateFunc* convexConvex = (btConvexConvexAlgorithm::CreateFunc*) m_convexConvexCreateFunc;
+ convexConvex->m_numPerturbationIterations = numPerturbationIterations;
+ convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold;
+}
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -111,6 +111,14 @@
virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1);
+ ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm.
+ ///By default, this feature is disabled for best performance.
+ ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature.
+ ///@param minimumPointsPerturbationThreshold is the minimum number of points in the contact cache, above which the feature is disabled
+ ///3 is a good value for both params, if you want to enable the feature. This is because the default contact cache contains a maximum of 4 points, and one collision query at the unperturbed orientation is performed first.
+ ///See Bullet/Demos/CollisionDemo for an example how this feature gathers multiple points.
+ ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection.
+ void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3);
};
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btGhostObject.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -63,6 +63,7 @@
btPairCachingGhostObject::~btPairCachingGhostObject()
{
+ m_hashPairCache->~btHashedOverlappingPairCache();
btAlignedFree( m_hashPairCache );
}
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -55,7 +55,7 @@
void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
- assert(m_manifoldPtr);
+ btAssert(m_manifoldPtr);
//order in manifold needs to match
if (depth > m_manifoldPtr->getContactBreakingThreshold())
@@ -92,7 +92,7 @@
newPt.m_partId1 = m_partId1;
newPt.m_index0 = m_index0;
newPt.m_index1 = m_index1;
-
+ //printf("depth=%f\n",depth);
///@todo, check this for any side effects
if (insertIndex >= 0)
{
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btManifoldResult.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -45,6 +45,8 @@
int m_partId1;
int m_index0;
int m_index1;
+
+
public:
btManifoldResult()
@@ -77,6 +79,7 @@
m_index1=index1;
}
+
virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth);
SIMD_FORCE_INLINE void refreshContactPoints()
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -24,7 +24,8 @@
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
-btSimulationIslandManager::btSimulationIslandManager()
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
{
}
@@ -43,10 +44,10 @@
{
{
- btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
-
+
for (int i=0;i<colWorld->getPairCache()->getNumOverlappingPairs();i++)
{
+ btBroadphasePair* pairPtr = colWorld->getPairCache()->getOverlappingPairArrayPtr();
const btBroadphasePair& collisionPair = pairPtr[i];
btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
@@ -185,7 +186,7 @@
// printf("error in island management\n");
}
- assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if (colObj0->getActivationState()== ACTIVE_TAG)
@@ -212,7 +213,7 @@
// printf("error in island management\n");
}
- assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
@@ -233,7 +234,7 @@
// printf("error in island management\n");
}
- assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
@@ -251,11 +252,11 @@
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
-#define SPLIT_ISLANDS 1
-#ifdef SPLIT_ISLANDS
+//#define SPLIT_ISLANDS 1
+//#ifdef SPLIT_ISLANDS
-#endif //SPLIT_ISLANDS
+//#endif //SPLIT_ISLANDS
for (i=0;i<maxNumManifolds ;i++)
@@ -279,11 +280,12 @@
{
colObj0->activate();
}
-#ifdef SPLIT_ISLANDS
- // //filtering for response
- if (dispatcher->needsResponse(colObj0,colObj1))
- m_islandmanifold.push_back(manifold);
-#endif //SPLIT_ISLANDS
+ if(m_splitIslands)
+ {
+ //filtering for response
+ if (dispatcher->needsResponse(colObj0,colObj1))
+ m_islandmanifold.push_back(manifold);
+ }
}
}
}
@@ -303,84 +305,86 @@
BT_PROFILE("processIslands");
-#ifndef SPLIT_ISLANDS
- btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
-
- callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
-#else
- // Sort manifolds, based on islands
- // Sort the vector using predicate and std::sort
- //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+ if(!m_splitIslands)
+ {
+ btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+ int maxNumManifolds = dispatcher->getNumManifolds();
+ callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+ }
+ else
+ {
+ // Sort manifolds, based on islands
+ // Sort the vector using predicate and std::sort
+ //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
- int numManifolds = int (m_islandmanifold.size());
+ int numManifolds = int (m_islandmanifold.size());
- //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
- m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+ //we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+ m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
- //now process all active islands (sets of manifolds for now)
+ //now process all active islands (sets of manifolds for now)
- int startManifoldIndex = 0;
- int endManifoldIndex = 1;
+ int startManifoldIndex = 0;
+ int endManifoldIndex = 1;
- //int islandId;
+ //int islandId;
-
+
-// printf("Start Islands\n");
+ // printf("Start Islands\n");
- //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
- for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
- {
- int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+ //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+ for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+ {
+ int islandId = getUnionFind().getElement(startIslandIndex).m_id;
- bool islandSleeping = false;
-
- for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
- {
- int i = getUnionFind().getElement(endIslandIndex).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- m_islandBodies.push_back(colObj0);
- if (!colObj0->isActive())
- islandSleeping = true;
- }
-
+ bool islandSleeping = false;
+
+ for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+ {
+ int i = getUnionFind().getElement(endIslandIndex).m_sz;
+ btCollisionObject* colObj0 = collisionObjects[i];
+ m_islandBodies.push_back(colObj0);
+ if (!colObj0->isActive())
+ islandSleeping = true;
+ }
+
- //find the accompanying contact manifold for this islandId
- int numIslandManifolds = 0;
- btPersistentManifold** startManifold = 0;
+ //find the accompanying contact manifold for this islandId
+ int numIslandManifolds = 0;
+ btPersistentManifold** startManifold = 0;
- if (startManifoldIndex<numManifolds)
- {
- int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
- if (curIslandId == islandId)
+ if (startManifoldIndex<numManifolds)
{
- startManifold = &m_islandmanifold[startManifoldIndex];
-
- for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+ int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+ if (curIslandId == islandId)
{
+ startManifold = &m_islandmanifold[startManifoldIndex];
+
+ for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+ {
+ }
+ /// Process the actual simulation, only if not sleeping/deactivated
+ numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
- /// Process the actual simulation, only if not sleeping/deactivated
- numIslandManifolds = endManifoldIndex-startManifoldIndex;
+
}
- }
+ if (!islandSleeping)
+ {
+ callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+ // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+ }
+
+ if (numIslandManifolds)
+ {
+ startManifoldIndex = endManifoldIndex;
+ }
- if (!islandSleeping)
- {
- callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
-// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+ m_islandBodies.resize(0);
}
-
- if (numIslandManifolds)
- {
- startManifoldIndex = endManifoldIndex;
- }
+ } // else if(!splitIslands)
- m_islandBodies.resize(0);
- }
-#endif //SPLIT_ISLANDS
-
-
}
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -35,6 +35,7 @@
btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
btAlignedObjectArray<btCollisionObject* > m_islandBodies;
+ bool m_splitIslands;
public:
btSimulationIslandManager();
@@ -65,6 +66,15 @@
void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
+ bool getSplitIslands()
+ {
+ return m_splitIslands;
+ }
+ void setSplitIslands(bool doSplitIslands)
+ {
+ m_splitIslands = doSplitIslands;
+ }
+
};
#endif //SIMULATION_ISLAND_MANAGER_H
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -78,7 +78,7 @@
}
///point on A (worldspace)
- btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
+ ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
///point on B (worldspace)
btVector3 pos1 = col1->getWorldTransform().getOrigin() + radius1* normalOnSurfaceB;
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -14,11 +14,9 @@
*/
#include "btUnionFind.h"
-#include <assert.h>
-
btUnionFind::~btUnionFind()
{
Free();
Modified: trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionDispatch/btUnionFind.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -98,8 +98,8 @@
int find(int x)
{
- //assert(x < m_N);
- //assert(x >= 0);
+ //btAssert(x < m_N);
+ //btAssert(x >= 0);
while (x != m_elements[x].m_id)
{
@@ -110,8 +110,8 @@
m_elements[x].m_id = m_elements[m_elements[x].m_id].m_id;
#endif //
x = m_elements[x].m_id;
- //assert(x < m_N);
- //assert(x >= 0);
+ //btAssert(x < m_N);
+ //btAssert(x >= 0);
}
return x;
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btBoxShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -161,31 +161,25 @@
switch (i)
{
case 0:
- plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
- plane[3] = -halfExtents.x();
+ plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 1:
- plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
- plane[3] = -halfExtents.x();
+ plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 2:
- plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
- plane[3] = -halfExtents.y();
+ plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
break;
case 3:
- plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
- plane[3] = -halfExtents.y();
+ plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
break;
case 4:
- plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
- plane[3] = -halfExtents.z();
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
break;
case 5:
- plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
- plane[3] = -halfExtents.z();
+ plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
break;
default:
- assert(0);
+ btAssert(0);
}
}
@@ -312,7 +306,7 @@
penetrationVector.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
break;
default:
- assert(0);
+ btAssert(0);
}
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btCollisionShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -16,6 +16,9 @@
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+btScalar gContactThresholdFactor=btScalar(0.02);
+
+
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
@@ -44,8 +47,7 @@
btScalar btCollisionShape::getContactBreakingThreshold() const
{
- ///@todo make this 0.1 configurable
- return getAngularMotionDisc() * btScalar(0.1);
+ return getAngularMotionDisc() * gContactThresholdFactor;
}
btScalar btCollisionShape::getAngularMotionDisc() const
{
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -22,7 +22,8 @@
m_localAabbMax(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)),
m_collisionMargin(btScalar(0.)),
m_localScaling(btScalar(1.),btScalar(1.),btScalar(1.)),
-m_dynamicAabbTree(0)
+m_dynamicAabbTree(0),
+m_updateRevision(1)
{
m_shapeType = COMPOUND_SHAPE_PROXYTYPE;
@@ -46,6 +47,7 @@
void btCompoundShape::addChildShape(const btTransform& localTransform,btCollisionShape* shape)
{
+ m_updateRevision++;
//m_childTransforms.push_back(localTransform);
//m_childShapes.push_back(shape);
btCompoundShapeChild child;
@@ -54,8 +56,7 @@
child.m_childShapeType = shape->getShapeType();
child.m_childMargin = shape->getMargin();
- m_children.push_back(child);
-
+
//extend the local aabbMin/aabbMax
btVector3 localAabbMin,localAabbMax;
shape->getAabb(localTransform,localAabbMin,localAabbMax);
@@ -74,10 +75,12 @@
if (m_dynamicAabbTree)
{
const btDbvtVolume bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
- int index = m_children.size()-1;
+ int index = m_children.size();
child.m_node = m_dynamicAabbTree->insert(bounds,(void*)index);
}
+ m_children.push_back(child);
+
}
void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
@@ -99,6 +102,7 @@
void btCompoundShape::removeChildShapeByIndex(int childShapeIndex)
{
+ m_updateRevision++;
btAssert(childShapeIndex >=0 && childShapeIndex < m_children.size());
if (m_dynamicAabbTree)
{
@@ -113,6 +117,7 @@
void btCompoundShape::removeChildShape(btCollisionShape* shape)
{
+ m_updateRevision++;
// Find the children containing the shape specified, and remove those children.
//note: there might be multiple children using the same shape!
for(int i = m_children.size()-1; i >= 0 ; i--)
@@ -139,6 +144,7 @@
{
// Recalculate the local aabb
// Brute force, it iterates over all the shapes left.
+
m_localAabbMin = btVector3(btScalar(1e30),btScalar(1e30),btScalar(1e30));
m_localAabbMax = btVector3(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
@@ -161,8 +167,16 @@
void btCompoundShape::getAabb(const btTransform& trans,btVector3& aabbMin,btVector3& aabbMax) const
{
btVector3 localHalfExtents = btScalar(0.5)*(m_localAabbMax-m_localAabbMin);
+ btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+
+ //avoid an illegal AABB when there are no children
+ if (!m_children.size())
+ {
+ localHalfExtents.setValue(0,0,0);
+ localCenter.setValue(0,0,0);
+ }
localHalfExtents += btVector3(getMargin(),getMargin(),getMargin());
- btVector3 localCenter = btScalar(0.5)*(m_localAabbMax+m_localAabbMin);
+
btMatrix3x3 abs_b = trans.getBasis().absolute();
@@ -173,7 +187,7 @@
abs_b[2].dot(localHalfExtents));
aabbMin = center-extent;
aabbMax = center+extent;
-
+
}
void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btCompoundShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -59,6 +59,9 @@
btDbvt* m_dynamicAabbTree;
+ ///increment m_updateRevision when adding/removing/replacing child shapes, so that some caches can be updated
+ int m_updateRevision;
+
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -152,6 +155,10 @@
///of the collision object by the principal transform.
void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const;
+ int getUpdateRevision() const
+ {
+ return m_updateRevision;
+ }
private:
btScalar m_collisionMargin;
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btConeShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -60,7 +60,7 @@
m_coneIndices[2] = 1;
break;
default:
- assert(0);
+ btAssert(0);
};
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btConvexHullShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -181,7 +181,7 @@
//not yet
bool btConvexHullShape::isInside(const btVector3& ,btScalar ) const
{
- assert(0);
+ btAssert(0);
return false;
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -150,7 +150,7 @@
//not yet
bool btConvexPointCloudShape::isInside(const btVector3& ,btScalar ) const
{
- assert(0);
+ btAssert(0);
return false;
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -154,7 +154,6 @@
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
- btVector3 halfExtents = capsuleShape->getImplicitShapeDimensions();
btScalar halfHeight = capsuleShape->getHalfHeight();
int capsuleUpAxis = capsuleShape->getUpAxis();
@@ -301,8 +300,8 @@
case SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape* sphereShape = (btSphereShape*)this;
- float radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
- float margin = radius + sphereShape->getMarginNonVirtual();
+ btScalar radius = sphereShape->getImplicitShapeDimensions().getX();// * convexShape->getLocalScaling().getX();
+ btScalar margin = radius + sphereShape->getMarginNonVirtual();
const btVector3& center = t.getOrigin();
btVector3 extent(margin,margin,margin);
aabbMin = center - extent;
@@ -314,7 +313,7 @@
case BOX_SHAPE_PROXYTYPE:
{
btBoxShape* convexShape = (btBoxShape*)this;
- float margin=convexShape->getMarginNonVirtual();
+ btScalar margin=convexShape->getMarginNonVirtual();
btVector3 halfExtents = convexShape->getImplicitShapeDimensions();
halfExtents += btVector3(margin,margin,margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
@@ -377,4 +376,4 @@
// should never reach here
btAssert (0);
-}
\ No newline at end of file
+}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -144,7 +144,12 @@
aabbMax = center + extent;
}
-btScalar btHeightfieldTerrainShape::getHeightFieldValue(int x,int y) const
+
+/// This returns the "raw" (user's initial) height, not the actual height.
+/// The actual height needs to be adjusted to be relative to the center
+/// of the heightfield's AABB.
+btScalar
+btHeightfieldTerrainShape::getRawHeightFieldValue(int x,int y) const
{
btScalar val = 0.f;
switch (m_heightDataType)
@@ -181,24 +186,22 @@
-
+/// this returns the vertex in bullet-local coordinates
void btHeightfieldTerrainShape::getVertex(int x,int y,btVector3& vertex) const
{
-
btAssert(x>=0);
btAssert(y>=0);
btAssert(x<m_heightStickWidth);
btAssert(y<m_heightStickLength);
+ btScalar height = getRawHeightFieldValue(x,y);
- btScalar height = getHeightFieldValue(x,y);
-
switch (m_upAxis)
{
case 0:
{
vertex.setValue(
- height,
+ height - m_localOrigin.getX(),
(-m_width/btScalar(2.0)) + x,
(-m_length/btScalar(2.0) ) + y
);
@@ -208,7 +211,7 @@
{
vertex.setValue(
(-m_width/btScalar(2.0)) + x,
- height,
+ height - m_localOrigin.getY(),
(-m_length/btScalar(2.0)) + y
);
break;
@@ -218,7 +221,7 @@
vertex.setValue(
(-m_width/btScalar(2.0)) + x,
(-m_length/btScalar(2.0)) + y,
- height
+ height - m_localOrigin.getZ()
);
break;
}
@@ -230,7 +233,6 @@
}
vertex*=m_localScaling;
-
}
@@ -238,7 +240,7 @@
static inline int
getQuantized
(
-float x
+btScalar x
)
{
if (x < 0.0) {
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -30,6 +30,17 @@
center (as determined by width and length and height, with each
axis multiplied by the localScaling).
+ \b NOTE: be careful with coordinates. If you have a heightfield with a local
+ min height of -100m, and a max height of +500m, you may be tempted to place it
+ at the origin (0,0) and expect the heights in world coordinates to be
+ -100 to +500 meters.
+ Actually, the heights will be -300 to +300m, because bullet will re-center
+ the heightfield based on its AABB (which is determined by the min/max
+ heights). So keep in mind that once you create a btHeightfieldTerrainShape
+ object, the heights will be adjusted relative to the center of the AABB. This
+ is different to the behavior of many rendering engines, but is useful for
+ physics engines.
+
Most (but not all) rendering and heightfield libraries assume upAxis = 1
(that is, the y-axis is "up"). This class allows any of the 3 coordinates
to be "up". Make sure your choice of axis is consistent with your rendering
@@ -88,7 +99,7 @@
btVector3 m_localScaling;
- virtual btScalar getHeightFieldValue(int x,int y) const;
+ virtual btScalar getRawHeightFieldValue(int x,int y) const;
void quantizeWithClamp(int* out, const btVector3& point,int isMax) const;
void getVertex(int x,int y,btVector3& vertex) const;
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -28,8 +28,8 @@
btVector3 btMinkowskiSumShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
- btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(-vec*m_transA.getBasis()));
- btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
+ btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
+ btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(-vec*m_transB.getBasis()));
return supVertexA - supVertexB;
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -31,10 +31,12 @@
BT_DECLARE_ALIGNED_ALLOCATOR();
- btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {}
+ btMultimaterialTriangleMeshShape(): btBvhTriangleMeshShape() {m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;}
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, buildBvh)
{
+ m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
@@ -67,6 +69,8 @@
btMultimaterialTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression,const btVector3& bvhAabbMin,const btVector3& bvhAabbMax, bool buildBvh = true):
btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, buildBvh)
{
+ m_shapeType = MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
+
btVector3 m_triangle[3];
const unsigned char *vertexbase;
int numverts;
@@ -107,11 +111,6 @@
m_materialLookup = NULL;
*/
}
- virtual int getShapeType() const
- {
- return MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE;
- }
-
//debugging
virtual const char* getName()const {return "MULTIMATERIALTRIANGLEMESH";}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btSphereShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -47,6 +47,12 @@
btScalar getRadius() const { return m_implicitShapeDimensions.getX() * m_localScaling.getX();}
+ void setUnscaledRadius(btScalar radius)
+ {
+ m_implicitShapeDimensions.setX(radius);
+ btConvexInternalShape::setMargin(radius);
+ }
+
//debugging
virtual const char* getName()const {return "SPHERE";}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -82,7 +82,8 @@
return (m_hasAabb == 1);
}
-void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax )
+
+void btTriangleIndexVertexArray::setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const
{
m_aabbMin = aabbMin;
m_aabbMax = aabbMax;
@@ -95,3 +96,4 @@
*aabbMax = m_aabbMax;
}
+
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -52,9 +52,9 @@
protected:
IndexedMeshArray m_indexedMeshes;
int m_pad[2];
- int m_hasAabb; // using int instead of bool to maintain alignment
- btVector3 m_aabbMin;
- btVector3 m_aabbMax;
+ mutable int m_hasAabb; // using int instead of bool to maintain alignment
+ mutable btVector3 m_aabbMin;
+ mutable btVector3 m_aabbMax;
public:
@@ -106,7 +106,7 @@
virtual void preallocateIndices(int numindices){(void) numindices;}
virtual bool hasPremadeAabb() const;
- virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax );
+ virtual void setPremadeAabb(const btVector3& aabbMin, const btVector3& aabbMax ) const;
virtual void getPremadeAabb(btVector3* aabbMin, btVector3* aabbMax ) const;
}
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexMaterialArray.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -1,3 +1,4 @@
+
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMesh.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -35,13 +35,13 @@
if (m_use32bitIndices)
{
m_indexedMeshes[0].m_numTriangles = m_32bitIndices.size()/3;
- m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_32bitIndices[0];
+ m_indexedMeshes[0].m_triangleIndexBase = 0;
m_indexedMeshes[0].m_indexType = PHY_INTEGER;
m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(int);
} else
{
m_indexedMeshes[0].m_numTriangles = m_16bitIndices.size()/3;
- m_indexedMeshes[0].m_triangleIndexBase = (unsigned char*) &m_16bitIndices[0];
+ m_indexedMeshes[0].m_triangleIndexBase = 0;
m_indexedMeshes[0].m_indexType = PHY_SHORT;
m_indexedMeshes[0].m_triangleIndexStride = 3*sizeof(short int);
}
@@ -49,12 +49,12 @@
if (m_use4componentVertices)
{
m_indexedMeshes[0].m_numVertices = m_4componentVertices.size();
- m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
+ m_indexedMeshes[0].m_vertexBase = 0;
m_indexedMeshes[0].m_vertexStride = sizeof(btVector3);
} else
{
m_indexedMeshes[0].m_numVertices = m_3componentVertices.size()/3;
- m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
+ m_indexedMeshes[0].m_vertexBase = 0;
m_indexedMeshes[0].m_vertexStride = 3*sizeof(btScalar);
}
@@ -111,9 +111,9 @@
}
}
}
- m_3componentVertices.push_back(vertex.getX());
- m_3componentVertices.push_back(vertex.getY());
- m_3componentVertices.push_back(vertex.getZ());
+ m_3componentVertices.push_back((float)vertex.getX());
+ m_3componentVertices.push_back((float)vertex.getY());
+ m_3componentVertices.push_back((float)vertex.getZ());
m_indexedMeshes[0].m_numVertices++;
m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
return (m_3componentVertices.size()/3)-1;
Modified: trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/CollisionShapes/btTriangleMeshShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -40,7 +40,7 @@
virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
- assert(0);
+ btAssert(0);
return localGetSupportingVertex(vec);
}
Modified: trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -150,8 +150,7 @@
splitIndex = startIndex+ (numIndices>>1);
}
- bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
- btAssert(!unbal);
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
return splitIndex;
Modified: trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -171,8 +171,7 @@
splitIndex = startIndex+ (numIndices>>1);
}
- bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
- btAssert(!unbal);
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
return splitIndex;
Modified: trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/btGImpactShape.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -881,6 +881,8 @@
*/
class btGImpactMeshShape : public btGImpactShapeInterface
{
+ btStridingMeshInterface* m_meshInterface;
+
protected:
btAlignedObjectArray<btGImpactMeshShapePart*> m_mesh_parts;
void buildMeshParts(btStridingMeshInterface * meshInterface)
@@ -907,6 +909,7 @@
public:
btGImpactMeshShape(btStridingMeshInterface * meshInterface)
{
+ m_meshInterface = meshInterface;
buildMeshParts(meshInterface);
}
@@ -922,7 +925,16 @@
}
+ btStridingMeshInterface* getMeshInterface()
+ {
+ return m_meshInterface;
+ }
+ const btStridingMeshInterface* getMeshInterface() const
+ {
+ return m_meshInterface;
+ }
+
int getMeshPartCount() const
{
return m_mesh_parts.size();
@@ -1033,12 +1045,12 @@
}
//! call when reading child shapes
- virtual void lockChildShapes()
+ virtual void lockChildShapes() const
{
btAssert(0);
}
- virtual void unlockChildShapes()
+ virtual void unlockChildShapes() const
{
btAssert(0);
}
Modified: trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/gim_box_set.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -110,8 +110,7 @@
splitIndex = startIndex+ (numIndices>>1);
}
- bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
- btAssert(!unbal);
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
return splitIndex;
}
@@ -180,3 +179,4 @@
_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
}
+
Modified: trunk/src/bullet/BulletCollision/Gimpact/gim_math.h
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/gim_math.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/gim_math.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -107,7 +107,7 @@
///returns a clamped number
#define GIM_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
-#define GIM_GREATER(x, y) fabsf(x) > (y)
+#define GIM_GREATER(x, y) btFabs(x) > (y)
///Swap numbers
#define GIM_SWAP_NUMBERS(a,b){ \
Modified: trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h
===================================================================
--- trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/Gimpact/gim_tri_collision.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -275,7 +275,7 @@
}
else
{
- float sumuv;
+ btScalar sumuv;
sumuv = u+v;
if(sumuv<-G_EPSILON)
{
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -135,6 +135,9 @@
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
+ //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+ if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON)
+ return false;
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
@@ -196,11 +199,10 @@
}
}
-
- //don't report time of impact for motion away from the contact normal (or causes minor penetration)
+
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON)
return false;
-
+
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -150,7 +150,8 @@
// potential exit, they don't overlap
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared))
{
- checkPenetration = false;
+ checkSimplex=true;
+ //checkPenetration = false;
break;
}
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -112,6 +112,12 @@
m_distance1 = dist;
}
+ ///this returns the most recent applied impulse, to satisfy contact constraints by the constraint solver
+ btScalar getAppliedImpulse() const
+ {
+ return m_appliedImpulse;
+ }
+
};
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -183,6 +183,9 @@
}
+ if (insertIndex<0)
+ insertIndex=0;
+
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
m_pointCache[insertIndex] = newPoint;
return insertIndex;
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -55,6 +55,7 @@
int m_cachedPoints;
btScalar m_contactBreakingThreshold;
+ btScalar m_contactProcessingThreshold;
/// sort cached points so most isolated points come first
@@ -70,9 +71,10 @@
btPersistentManifold();
- btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold)
+ btPersistentManifold(void* body0,void* body1,int , btScalar contactBreakingThreshold,btScalar contactProcessingThreshold)
: m_body0(body0),m_body1(body1),m_cachedPoints(0),
- m_contactBreakingThreshold(contactBreakingThreshold)
+ m_contactBreakingThreshold(contactBreakingThreshold),
+ m_contactProcessingThreshold(contactProcessingThreshold)
{
}
@@ -111,6 +113,11 @@
///@todo: get this margin from the current physics / collision environment
btScalar getContactBreakingThreshold() const;
+
+ btScalar getContactProcessingThreshold() const
+ {
+ return m_contactProcessingThreshold;
+ }
int getCacheEntry(const btManifoldPoint& newPoint) const;
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -23,10 +23,12 @@
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "btRaycastCallback.h"
-btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to)
+btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
:
m_from(from),
m_to(to),
+ //@BP Mod
+ m_flags(flags),
m_hitFraction(btScalar(1.))
{
@@ -55,6 +57,12 @@
{
return ; // same sign
}
+ //@BP Mod - Backface filtering
+ if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a > btScalar(0.0)))
+ {
+ // Backface, skip check
+ return;
+ }
const btScalar proj_length=dist_a-dist_b;
const btScalar distance = (dist_a)/(proj_length);
@@ -89,14 +97,18 @@
if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance)
{
+ //@BP Mod
+ // Triangle normal isn't normalized
+ triangleNormal.normalize();
- if ( dist_a > 0 )
+ //@BP Mod - Allow for unflipped normal when raycasting against backfaces
+ if (((m_flags & kF_KeepUnflippedNormal) != 0) || (dist_a <= btScalar(0.0)))
{
- m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
+ m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
}
else
{
- m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
+ m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
}
}
}
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -29,9 +29,20 @@
btVector3 m_from;
btVector3 m_to;
+ //@BP Mod - allow backface filtering and unflipped normals
+ enum EFlags
+ {
+ kF_None = 0,
+ kF_FilterBackfaces = 1 << 0,
+ kF_KeepUnflippedNormal = 1 << 1, // Prevents returned face normal getting flipped when a ray hits a back-facing triangle
+
+ kF_Terminator = 0xFFFFFFFF
+ };
+ unsigned int m_flags;
+
btScalar m_hitFraction;
- btTriangleRaycastCallback(const btVector3& from,const btVector3& to);
+ btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags=0);
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex);
Modified: trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp
===================================================================
--- trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -25,8 +25,6 @@
#include "btVoronoiSimplexSolver.h"
-#include <assert.h>
-//#include <stdio.h>
#define VERTA 0
#define VERTB 1
@@ -37,7 +35,7 @@
void btVoronoiSimplexSolver::removeVertex(int index)
{
- assert(m_numVertices>0);
+ btAssert(m_numVertices>0);
m_numVertices--;
m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
Modified: trunk/src/bullet/BulletDynamics/CMakeLists.txt
===================================================================
--- trunk/src/bullet/BulletDynamics/CMakeLists.txt 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/CMakeLists.txt 2009-03-31 18:05:51 UTC (rev 2882)
@@ -35,6 +35,7 @@
ConstraintSolver/btSolverConstraint.h
ConstraintSolver/btTypedConstraint.h
+ Dynamics/btActionInterface.h
Dynamics/btContinuousDynamicsWorld.h
Dynamics/btDiscreteDynamicsWorld.h
Dynamics/btDynamicsWorld.h
Modified: trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Character/btCharacterControllerInterface.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -17,12 +17,13 @@
#define CHARACTER_CONTROLLER_INTERFACE_H
#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
class btCollisionShape;
class btRigidBody;
class btCollisionWorld;
-class btCharacterControllerInterface
+class btCharacterControllerInterface : public btActionInterface
{
public:
btCharacterControllerInterface () {};
Modified: trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -22,6 +22,8 @@
#include "LinearMath/btDefaultMotionState.h"
#include "btKinematicCharacterController.h"
+static btVector3 upAxisDirection[3] = { btVector3(1.0f, 0.0f, 0.0f), btVector3(0.0f, 1.0f, 0.0f), btVector3(0.0f, 0.0f, 1.0f) };
+
///@todo Interact with dynamic objects,
///Ride kinematicly animated platforms properly
///More realistic (or maybe just a config option) falling
@@ -93,29 +95,28 @@
return direction - parallelComponent(direction, normal);
}
-btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight)
+btKinematicCharacterController::btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis)
{
+ m_upAxis = upAxis;
m_addedMargin = 0.02f;
m_walkDirection.setValue(0,0,0);
m_useGhostObjectSweepTest = true;
m_ghostObject = ghostObject;
m_stepHeight = stepHeight;
m_turnAngle = btScalar(0.0);
- m_convexShape=convexShape;
-
+ m_convexShape=convexShape;
}
btKinematicCharacterController::~btKinematicCharacterController ()
{
}
-
btPairCachingGhostObject* btKinematicCharacterController::getGhostObject()
{
return m_ghostObject;
}
-bool btKinematicCharacterController::recoverFromPenetration (btCollisionWorld* collisionWorld)
+bool btKinematicCharacterController::recoverFromPenetration ( btCollisionWorld* collisionWorld)
{
bool penetration = false;
@@ -172,13 +173,13 @@
{
// phase 1: up
btTransform start, end;
- m_targetPosition = m_currentPosition + btVector3 (btScalar(0.0), m_stepHeight, btScalar(0.0));
+ m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * m_stepHeight;
start.setIdentity ();
end.setIdentity ();
/* FIXME: Handle penetration properly */
- start.setOrigin (m_currentPosition + btVector3(btScalar(0.0), btScalar(0.1), btScalar(0.0)));
+ start.setOrigin (m_currentPosition + upAxisDirection[m_upAxis] * btScalar(0.1f));
end.setOrigin (m_targetPosition);
btKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
@@ -343,8 +344,8 @@
btTransform start, end;
// phase 3: down
- btVector3 step_drop = btVector3(btScalar(0.0), m_currentStepOffset, btScalar(0.0));
- btVector3 gravity_drop = btVector3(btScalar(0.0), m_stepHeight, btScalar(0.0));
+ btVector3 step_drop = upAxisDirection[m_upAxis] * m_currentStepOffset;
+ btVector3 gravity_drop = upAxisDirection[m_upAxis] * m_stepHeight;
m_targetPosition -= (step_drop + gravity_drop);
start.setIdentity ();
@@ -389,7 +390,7 @@
}
-void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
+void btKinematicCharacterController::preStep ( btCollisionWorld* collisionWorld)
{
int numPenetrationLoops = 0;
@@ -412,7 +413,7 @@
}
-void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
+void btKinematicCharacterController::playerStep ( btCollisionWorld* collisionWorld, btScalar dt)
{
btTransform xform;
xform = m_ghostObject->getWorldTransform ();
@@ -468,3 +469,8 @@
{
return true;
}
+
+
+void btKinematicCharacterController::debugDraw(btIDebugDraw* debugDrawer)
+{
+}
\ No newline at end of file
Modified: trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Character/btKinematicCharacterController.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -62,20 +62,42 @@
btVector3 m_touchingNormal;
bool m_useGhostObjectSweepTest;
+
+ int m_upAxis;
btVector3 computeReflectionDirection (const btVector3& direction, const btVector3& normal);
btVector3 parallelComponent (const btVector3& direction, const btVector3& normal);
btVector3 perpindicularComponent (const btVector3& direction, const btVector3& normal);
- bool recoverFromPenetration (btCollisionWorld* collisionWorld);
+ bool recoverFromPenetration ( btCollisionWorld* collisionWorld);
void stepUp (btCollisionWorld* collisionWorld);
void updateTargetPositionBasedOnCollision (const btVector3& hit_normal, btScalar tangentMag = btScalar(0.0), btScalar normalMag = btScalar(1.0));
void stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove);
void stepDown (btCollisionWorld* collisionWorld, btScalar dt);
public:
- btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight);
+ btKinematicCharacterController (btPairCachingGhostObject* ghostObject,btConvexShape* convexShape,btScalar stepHeight, int upAxis = 1);
~btKinematicCharacterController ();
+
+ ///btActionInterface interface
+ virtual void updateAction( btCollisionWorld* collisionWorld,btScalar deltaTime)
+ {
+ preStep ( collisionWorld);
+ playerStep (collisionWorld, deltaTime);
+ }
+
+ ///btActionInterface interface
+ void debugDraw(btIDebugDraw* debugDrawer);
+
+ void setUpAxis (int axis)
+ {
+ if (axis < 0)
+ axis = 0;
+ if (axis > 2)
+ axis = 2;
+ m_upAxis = axis;
+ }
+
virtual void setWalkDirection(const btVector3& walkDirection)
{
m_walkDirection = walkDirection;
@@ -84,8 +106,8 @@
void reset ();
void warp (const btVector3& origin);
- void preStep ( btCollisionWorld* collisionWorld);
- void playerStep (btCollisionWorld* collisionWorld, btScalar dt);
+ void preStep ( btCollisionWorld* collisionWorld);
+ void playerStep ( btCollisionWorld* collisionWorld, btScalar dt);
void setFallSpeed (btScalar fallSpeed);
void setJumpSpeed (btScalar jumpSpeed);
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -22,8 +22,16 @@
#include "LinearMath/btMinMax.h"
#include <new>
+//-----------------------------------------------------------------------------
+
+#define CONETWIST_USE_OBSOLETE_SOLVER false
+#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
+
+//-----------------------------------------------------------------------------
+
btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
}
@@ -31,69 +39,228 @@
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame,const btTransform& rbBFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
- m_angularOnly(false)
+ m_angularOnly(false),
+ m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
- 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;
-
+ init();
}
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
- m_angularOnly(false)
+ m_angularOnly(false),
+ m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
{
m_rbBFrame = m_rbAFrame;
-
- m_swingSpan1 = btScalar(1e30);
- m_swingSpan2 = btScalar(1e30);
- m_twistSpan = btScalar(1e30);
- m_biasFactor = 0.3f;
- m_relaxationFactor = 1.0f;
+ init();
+}
- m_solveTwistLimit = false;
- m_solveSwingLimit = false;
-
-}
-void btConeTwistConstraint::buildJacobian()
+void btConeTwistConstraint::init()
{
- m_appliedImpulse = btScalar(0.);
-
- //set bias, sign, clear accumulator
- m_swingCorrection = btScalar(0.);
- m_twistLimitSign = btScalar(0.);
+ m_angularOnly = false;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
- m_accTwistLimitImpulse = btScalar(0.);
- m_accSwingLimitImpulse = btScalar(0.);
+ m_bMotorEnabled = false;
+ m_maxMotorImpulse = btScalar(-1);
- if (!m_angularOnly)
+ setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30));
+ m_damping = btScalar(0.01);
+ m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+}
+
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
- btVector3 relPos = pivotBInW - pivotAInW;
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ info->m_numConstraintRows = 3;
+ info->nub = 3;
+ calcAngleInfo2();
+ if(m_solveSwingLimit)
+ {
+ info->m_numConstraintRows++;
+ info->nub--;
+ if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+ {
+ info->m_numConstraintRows++;
+ info->nub--;
+ }
+ }
+ if(m_solveTwistLimit)
+ {
+ info->m_numConstraintRows++;
+ info->nub--;
+ }
+ }
+} // btConeTwistConstraint::getInfo1()
+
+//-----------------------------------------------------------------------------
- btVector3 normal[3];
- if (relPos.length2() > SIMD_EPSILON)
+void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+ //retrieve matrices
+ btTransform body0_trans;
+ body0_trans = m_rbA.getCenterOfMassTransform();
+ btTransform body1_trans;
+ body1_trans = m_rbB.getCenterOfMassTransform();
+ // set jacobian
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[info->rowskip+1] = 1;
+ info->m_J1linearAxis[2*info->rowskip+2] = 1;
+ btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+ btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+ btVector3 a1neg = -a1;
+ a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+ btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+ a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ // set right hand side
+ btScalar k = info->fps * info->erp;
+ int j;
+ for (j=0; j<3; j++)
+ {
+ info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+ info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
+ info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+ }
+ int row = 3;
+ int srow = row * info->rowskip;
+ btVector3 ax1;
+ // angular limits
+ if(m_solveSwingLimit)
+ {
+ btScalar *J1 = info->m_J1angularAxis;
+ btScalar *J2 = info->m_J2angularAxis;
+ if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
- normal[0] = relPos.normalized();
+ btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+ btVector3 p = trA.getBasis().getColumn(1);
+ btVector3 q = trA.getBasis().getColumn(2);
+ int srow1 = srow + info->rowskip;
+ J1[srow+0] = p[0];
+ J1[srow+1] = p[1];
+ J1[srow+2] = p[2];
+ J1[srow1+0] = q[0];
+ J1[srow1+1] = q[1];
+ J1[srow1+2] = q[2];
+ J2[srow+0] = -p[0];
+ J2[srow+1] = -p[1];
+ J2[srow+2] = -p[2];
+ J2[srow1+0] = -q[0];
+ J2[srow1+1] = -q[1];
+ J2[srow1+2] = -q[2];
+ btScalar fact = info->fps * m_relaxationFactor;
+ info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
+ info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+ info->m_upperLimit[srow1] = SIMD_INFINITY;
+ srow = srow1 + info->rowskip;
}
else
{
- normal[0].setValue(btScalar(1.0),0,0);
+ ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
+ J1[srow+0] = ax1[0];
+ J1[srow+1] = ax1[1];
+ J1[srow+2] = ax1[2];
+ J2[srow+0] = -ax1[0];
+ J2[srow+1] = -ax1[1];
+ J2[srow+2] = -ax1[2];
+ btScalar k = info->fps * m_biasFactor;
+
+ info->m_constraintError[srow] = k * m_swingCorrection;
+ info->cfm[srow] = 0.0f;
+ // m_swingCorrection is always positive or 0
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ srow += info->rowskip;
}
+ }
+ if(m_solveTwistLimit)
+ {
+ ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
+ btScalar *J1 = info->m_J1angularAxis;
+ btScalar *J2 = info->m_J2angularAxis;
+ J1[srow+0] = ax1[0];
+ J1[srow+1] = ax1[1];
+ J1[srow+2] = ax1[2];
+ J2[srow+0] = -ax1[0];
+ J2[srow+1] = -ax1[1];
+ J2[srow+2] = -ax1[2];
+ btScalar k = info->fps * m_biasFactor;
+ info->m_constraintError[srow] = k * m_twistCorrection;
+ info->cfm[srow] = 0.0f;
+ if(m_twistSpan > 0.0f)
+ {
- btPlaneSpace1(normal[0], normal[1], normal[2]);
+ if(m_twistCorrection > 0.0f)
+ {
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ srow += info->rowskip;
+ }
+}
+
+//-----------------------------------------------------------------------------
- for (int i=0;i<3;i++)
+void btConeTwistConstraint::buildJacobian()
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ m_appliedImpulse = btScalar(0.);
+ m_accTwistLimitImpulse = btScalar(0.);
+ m_accSwingLimitImpulse = btScalar(0.);
+
+ if (!m_angularOnly)
{
- new (&m_jac[i]) btJacobianEntry(
+ 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(),
@@ -103,9 +270,243 @@
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
+ }
}
+
+ calcAngleInfo2();
}
+}
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+
+ btScalar tau = btScalar(0.3);
+
+ //linear part
+ if (!m_angularOnly)
+ {
+ btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+ btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+ btVector3 vel1;
+ bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+ btVector3 vel2;
+ bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+ 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 ftorqueAxis1 = rel_pos1.cross(normal);
+ btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+ bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+ bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+
+ }
+ }
+
+ // apply motor
+ if (m_bMotorEnabled)
+ {
+ // compute current and predicted transforms
+ btTransform trACur = m_rbA.getCenterOfMassTransform();
+ btTransform trBCur = m_rbB.getCenterOfMassTransform();
+ btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
+ btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+ btTransform trAPred; trAPred.setIdentity();
+ btVector3 zerovec(0,0,0);
+ btTransformUtil::integrateTransform(
+ trACur, zerovec, omegaA, timeStep, trAPred);
+ btTransform trBPred; trBPred.setIdentity();
+ btTransformUtil::integrateTransform(
+ trBCur, zerovec, omegaB, timeStep, trBPred);
+
+ // compute desired transforms in world
+ btTransform trPose(m_qTarget);
+ btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
+ btTransform trADes = trBPred * trABDes;
+ btTransform trBDes = trAPred * trABDes.inverse();
+
+ // compute desired omegas in world
+ btVector3 omegaADes, omegaBDes;
+
+ btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
+ btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
+
+ // compute delta omegas
+ btVector3 dOmegaA = omegaADes - omegaA;
+ btVector3 dOmegaB = omegaBDes - omegaB;
+
+ // compute weighted avg axis of dOmega (weighting based on inertias)
+ btVector3 axisA, axisB;
+ btScalar kAxisAInv = 0, kAxisBInv = 0;
+
+ if (dOmegaA.length2() > SIMD_EPSILON)
+ {
+ axisA = dOmegaA.normalized();
+ kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
+ }
+
+ if (dOmegaB.length2() > SIMD_EPSILON)
+ {
+ axisB = dOmegaB.normalized();
+ kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
+ }
+
+ btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
+
+ static bool bDoTorque = true;
+ if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
+ {
+ avgAxis.normalize();
+ kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
+ kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
+ btScalar kInvCombined = kAxisAInv + kAxisBInv;
+
+ btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
+ (kInvCombined * kInvCombined);
+
+ if (m_maxMotorImpulse >= 0)
+ {
+ btScalar fMaxImpulse = m_maxMotorImpulse;
+ if (m_bNormalizedMotorStrength)
+ fMaxImpulse = fMaxImpulse/kAxisAInv;
+
+ btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
+ btScalar newUnclampedMag = newUnclampedAccImpulse.length();
+ if (newUnclampedMag > fMaxImpulse)
+ {
+ newUnclampedAccImpulse.normalize();
+ newUnclampedAccImpulse *= fMaxImpulse;
+ impulse = newUnclampedAccImpulse - m_accMotorImpulse;
+ }
+ m_accMotorImpulse += impulse;
+ }
+
+ btScalar impulseMag = impulse.length();
+ btVector3 impulseAxis = impulse / impulseMag;
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+
+ }
+ }
+ else // no motor: do a little damping
+ {
+ const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
+ const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
+ btVector3 relVel = angVelB - angVelA;
+ if (relVel.length2() > SIMD_EPSILON)
+ {
+ btVector3 relVelAxis = relVel.normalized();
+ btScalar m_kDamping = btScalar(1.) /
+ (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
+ getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
+ btVector3 impulse = m_damping * m_kDamping * relVel;
+
+ btScalar impulseMag = impulse.length();
+ btVector3 impulseAxis = impulse / impulseMag;
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+ }
+ }
+
+ // joint limits
+ {
+ ///solve angular part
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(angVelB);
+
+ // solve swing limit
+ if (m_solveSwingLimit)
+ {
+ btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
+ btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
+ if (relSwingVel > 0)
+ amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
+ btScalar impulseMag = amplitude * m_kSwing;
+
+ // Clamp the accumulated impulse
+ btScalar temp = m_accSwingLimitImpulse;
+ m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
+ impulseMag = m_accSwingLimitImpulse - temp;
+
+ btVector3 impulse = m_swingAxis * impulseMag;
+
+ // don't let cone response affect twist
+ // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
+ {
+ btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
+ btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
+ impulse = impulseNoTwistCouple;
+ }
+
+ impulseMag = impulse.length();
+ btVector3 noTwistSwingAxis = impulse / impulseMag;
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+ }
+
+
+ // solve twist limit
+ if (m_solveTwistLimit)
+ {
+ btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
+ btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
+ if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
+ amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
+ btScalar impulseMag = amplitude * m_kTwist;
+
+ // Clamp the accumulated impulse
+ btScalar temp = m_accTwistLimitImpulse;
+ m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
+ impulseMag = m_accTwistLimitImpulse - temp;
+
+ btVector3 impulse = m_twistAxis * impulseMag;
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
+ }
+ }
+ }
+
+}
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::updateRHS(btScalar timeStep)
+{
+ (void)timeStep;
+
+}
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo()
+{
+ m_swingCorrection = btScalar(0.);
+ m_twistLimitSign = btScalar(0.);
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
@@ -122,20 +523,17 @@
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
-// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
-
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
-// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
@@ -152,17 +550,11 @@
{
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
@@ -172,118 +564,426 @@
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
+ m_twistAngle = twist;
- btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
+ btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : 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;
+ }
+ else if (twist > m_twistSpan*lockedFreeFactor)
+ {
+ m_twistCorrection = (twist - m_twistSpan);
+ m_solveTwistLimit = true;
+ m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
+ m_twistAxis.normalize();
+ }
+ }
+} // btConeTwistConstraint::calcAngleInfo()
- m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
- getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
- } else
- if (twist > m_twistSpan*lockedFreeFactor)
+static btVector3 vTwist(1,0,0); // twist axis in constraint's space
+
+//-----------------------------------------------------------------------------
+
+void btConeTwistConstraint::calcAngleInfo2()
+{
+ m_swingCorrection = btScalar(0.);
+ m_twistLimitSign = btScalar(0.);
+ m_solveTwistLimit = false;
+ m_solveSwingLimit = false;
+
+ {
+ // compute rotation of A wrt B (in constraint space)
+ btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
+ btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+ btQuaternion qAB = qB.inverse() * qA;
+
+ // split rotation into cone and twist
+ // (all this is done from B's perspective. Maybe I should be averaging axes...)
+ btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+ btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+ btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+ if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
+ {
+ btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
+ computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
+
+ if (swingAngle > swingLimit * m_limitSoftness)
{
- m_twistCorrection = (twist - m_twistSpan);
- m_solveTwistLimit = true;
+ m_solveSwingLimit = true;
- m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
- m_twistAxis.normalize();
+ // compute limit ratio: 0->1, where
+ // 0 == beginning of soft limit
+ // 1 == hard/real limit
+ m_swingLimitRatio = 1.f;
+ if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
+ {
+ m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
+ (swingLimit - swingLimit * m_limitSoftness);
+ }
- m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
- getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+ // swing correction tries to get back to soft limit
+ m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
+ // adjustment of swing axis (based on ellipse normal)
+ adjustSwingAxisToUseEllipseNormal(swingAxis);
+
+ // Calculate necessary axis & factors
+ m_swingAxis = quatRotate(qB, -swingAxis);
+
+ m_twistAxisA.setValue(0,0,0);
+
+ m_kSwing = btScalar(1.) /
+ (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
+ getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
- }
-}
+ }
+ else
+ {
+ // you haven't set any limits;
+ // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
+ // anyway, we have either hinge or fixed joint
+ btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+ btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+ btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+ btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+ btVector3 target;
+ btScalar x = ivB.dot(ivA);
+ btScalar y = ivB.dot(jvA);
+ btScalar z = ivB.dot(kvA);
+ if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
+ { // fixed. We'll need to add one more row to constraint
+ if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+ {
+ m_solveSwingLimit = true;
+ m_swingAxis = -ivB.cross(ivA);
+ }
+ }
+ else
+ {
+ if(m_swingSpan1 < m_fixThresh)
+ { // hinge around Y axis
+ if(!(btFuzzyZero(y)))
+ {
+ m_solveSwingLimit = true;
+ if(m_swingSpan2 >= m_fixThresh)
+ {
+ y = btScalar(0.f);
+ btScalar span2 = btAtan2(z, x);
+ if(span2 > m_swingSpan2)
+ {
+ x = btCos(m_swingSpan2);
+ z = btSin(m_swingSpan2);
+ }
+ else if(span2 < -m_swingSpan2)
+ {
+ x = btCos(m_swingSpan2);
+ z = -btSin(m_swingSpan2);
+ }
+ }
+ }
+ }
+ else
+ { // hinge around Z axis
+ if(!btFuzzyZero(z))
+ {
+ m_solveSwingLimit = true;
+ if(m_swingSpan1 >= m_fixThresh)
+ {
+ z = btScalar(0.f);
+ btScalar span1 = btAtan2(y, x);
+ if(span1 > m_swingSpan1)
+ {
+ x = btCos(m_swingSpan1);
+ y = btSin(m_swingSpan1);
+ }
+ else if(span1 < -m_swingSpan1)
+ {
+ x = btCos(m_swingSpan1);
+ y = -btSin(m_swingSpan1);
+ }
+ }
+ }
+ }
+ target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
+ target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
+ target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
+ target.normalize();
+ m_swingAxis = -ivB.cross(target);
+ m_swingCorrection = m_swingAxis.length();
+ m_swingAxis.normalize();
+ }
+ }
-void btConeTwistConstraint::solveConstraint(btScalar timeStep)
-{
+ if (m_twistSpan >= btScalar(0.f))
+ {
+ btVector3 twistAxis;
+ computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ if (m_twistAngle > m_twistSpan*m_limitSoftness)
+ {
+ m_solveTwistLimit = true;
- btScalar tau = btScalar(0.3);
+ m_twistLimitRatio = 1.f;
+ if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
+ {
+ m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
+ (m_twistSpan - m_twistSpan * m_limitSoftness);
+ }
- //linear part
- if (!m_angularOnly)
- {
- btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
- btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+ // twist correction tries to get back to soft limit
+ m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
- btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
+ m_twistAxis = quatRotate(qB, -twistAxis);
- for (int i=0;i<3;i++)
- {
- const btVector3& normal = m_jac[i].m_linearJointAxis;
- btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+ m_kTwist = btScalar(1.) /
+ (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
+ getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+ }
- 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());
+ if (m_solveSwingLimit)
+ m_twistAxisA = quatRotate(qA, -twistAxis);
}
+ else
+ {
+ m_twistAngle = btScalar(0.f);
+ }
}
-
+} // btConeTwistConstraint::calcAngleInfo2()
+
+
+
+// given a cone rotation in constraint space, (pre: twist must already be removed)
+// this method computes its corresponding swing angle and axis.
+// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
+void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
+ btScalar& swingAngle, // out
+ btVector3& vSwingAxis, // out
+ btScalar& swingLimit) // out
+{
+ swingAngle = qCone.getAngle();
+ if (swingAngle > SIMD_EPSILON)
{
- ///solve angular part
- const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
- const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-
- // solve swing limit
- if (m_solveSwingLimit)
+ vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
+ vSwingAxis.normalize();
+ if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
{
- btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
- btScalar impulseMag = amplitude * m_kSwing;
+ // non-zero twist?! this should never happen.
+ int wtf = 0; wtf = wtf;
+ }
- // Clamp the accumulated impulse
- btScalar temp = m_accSwingLimitImpulse;
- m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
- impulseMag = m_accSwingLimitImpulse - temp;
+ // Compute limit for given swing. tricky:
+ // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
+ // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
- btVector3 impulse = m_swingAxis * impulseMag;
+ // For starters, compute the direction from center to surface of ellipse.
+ // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
+ // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
+ btScalar xEllipse = vSwingAxis.y();
+ btScalar yEllipse = -vSwingAxis.z();
- m_rbA.applyTorqueImpulse(impulse);
- m_rbB.applyTorqueImpulse(-impulse);
+ // Now, we use the slope of the vector (using x/yEllipse) and find the length
+ // of the line that intersects the ellipse:
+ // x^2 y^2
+ // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+ // a^2 b^2
+ // Do the math and it should be clear.
+ swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
+ if (fabs(xEllipse) > SIMD_EPSILON)
+ {
+ btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+ btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+ norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+ btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+ swingLimit = sqrt(swingLimit2);
}
- // solve twist limit
- if (m_solveTwistLimit)
+ // test!
+ /*swingLimit = m_swingSpan2;
+ if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
{
- btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
- btScalar impulseMag = amplitude * m_kTwist;
+ btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
+ btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
+ btScalar phi = asin(sinphi);
+ btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
+ btScalar alpha = 3.14159f - theta - phi;
+ btScalar sinalpha = sin(alpha);
+ swingLimit = m_swingSpan1 * sinphi/sinalpha;
+ }*/
+ }
+ else if (swingAngle < 0)
+ {
+ // this should never happen!
+ int wtf = 0; wtf = wtf;
+ }
+}
- // Clamp the accumulated impulse
- btScalar temp = m_accTwistLimitImpulse;
- m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
- impulseMag = m_accTwistLimitImpulse - temp;
+btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
+{
+ // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
+ btScalar xEllipse = btCos(fAngleInRadians);
+ btScalar yEllipse = btSin(fAngleInRadians);
- btVector3 impulse = m_twistAxis * impulseMag;
+ // Use the slope of the vector (using x/yEllipse) and find the length
+ // of the line that intersects the ellipse:
+ // x^2 y^2
+ // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
+ // a^2 b^2
+ // Do the math and it should be clear.
- m_rbA.applyTorqueImpulse(impulse);
- m_rbB.applyTorqueImpulse(-impulse);
+ float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
+ if (fabs(xEllipse) > SIMD_EPSILON)
+ {
+ btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
+ btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
+ norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
+ btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
+ swingLimit = sqrt(swingLimit2);
+ }
- }
-
+ // convert into point in constraint space:
+ // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
+ btVector3 vSwingAxis(0, xEllipse, -yEllipse);
+ btQuaternion qSwing(vSwingAxis, swingLimit);
+ btVector3 vPointInConstraintSpace(fLength,0,0);
+ return quatRotate(qSwing, vPointInConstraintSpace);
+}
+
+// given a twist rotation in constraint space, (pre: cone must already be removed)
+// this method computes its corresponding angle and axis.
+void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
+ btScalar& twistAngle, // out
+ btVector3& vTwistAxis) // out
+{
+ btQuaternion qMinTwist = qTwist;
+ twistAngle = qTwist.getAngle();
+
+ if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+ {
+ qMinTwist = operator-(qTwist);
+ twistAngle = qMinTwist.getAngle();
}
+ if (twistAngle < 0)
+ {
+ // this should never happen
+ int wtf = 0; wtf = wtf;
+ }
+ vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+ if (twistAngle > SIMD_EPSILON)
+ vTwistAxis.normalize();
}
-void btConeTwistConstraint::updateRHS(btScalar timeStep)
+
+void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
- (void)timeStep;
+ // the swing axis is computed as the "twist-free" cone rotation,
+ // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
+ // so, if we're outside the limits, the closest way back inside the cone isn't
+ // along the vector back to the center. better (and more stable) to use the ellipse normal.
+ // convert swing axis to direction from center to surface of ellipse
+ // (ie. rotate 2D vector by PI/2)
+ btScalar y = -vSwingAxis.z();
+ btScalar z = vSwingAxis.y();
+
+ // do the math...
+ if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
+ {
+ // compute gradient/normal of ellipse surface at current "point"
+ btScalar grad = y/z;
+ grad *= m_swingSpan2 / m_swingSpan1;
+
+ // adjust y/z to represent normal at point (instead of vector to point)
+ if (y > 0)
+ y = fabs(grad * z);
+ else
+ y = -fabs(grad * z);
+
+ // convert ellipse direction back to swing axis
+ vSwingAxis.setZ(-y);
+ vSwingAxis.setY( z);
+ vSwingAxis.normalize();
+ }
}
+
+
+
+void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
+{
+ btTransform trACur = m_rbA.getCenterOfMassTransform();
+ btTransform trBCur = m_rbB.getCenterOfMassTransform();
+ btTransform trABCur = trBCur.inverse() * trACur;
+ btQuaternion qABCur = trABCur.getRotation();
+ btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
+ btQuaternion qConstraintCur = trConstraintCur.getRotation();
+
+ btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
+ setMotorTargetInConstraintSpace(qConstraint);
+}
+
+
+void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
+{
+ m_qTarget = q;
+
+ // clamp motor target to within limits
+ {
+ btScalar softness = 1.f;//m_limitSoftness;
+
+ // split into twist and cone
+ btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
+ btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
+ btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
+
+ // clamp cone
+ if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
+ {
+ btScalar swingAngle, swingLimit; btVector3 swingAxis;
+ computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
+
+ if (fabs(swingAngle) > SIMD_EPSILON)
+ {
+ if (swingAngle > swingLimit*softness)
+ swingAngle = swingLimit*softness;
+ else if (swingAngle < -swingLimit*softness)
+ swingAngle = -swingLimit*softness;
+ qTargetCone = btQuaternion(swingAxis, swingAngle);
+ }
+ }
+
+ // clamp twist
+ if (m_twistSpan >= btScalar(0.05f))
+ {
+ btScalar twistAngle; btVector3 twistAxis;
+ computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
+
+ if (fabs(twistAngle) > SIMD_EPSILON)
+ {
+ // eddy todo: limitSoftness used here???
+ if (twistAngle > m_twistSpan*softness)
+ twistAngle = m_twistSpan*softness;
+ else if (twistAngle < -m_twistSpan*softness)
+ twistAngle = -m_twistSpan*softness;
+ qTargetTwist = btQuaternion(twistAxis, twistAngle);
+ }
+ }
+
+ m_qTarget = qTargetCone * qTargetTwist;
+ }
+}
+
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+
+
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -42,10 +42,14 @@
btScalar m_biasFactor;
btScalar m_relaxationFactor;
+ btScalar m_damping;
+
btScalar m_swingSpan1;
btScalar m_swingSpan2;
btScalar m_twistSpan;
+ btScalar m_fixThresh;
+
btVector3 m_swingAxis;
btVector3 m_twistAxis;
@@ -56,6 +60,8 @@
btScalar m_swingCorrection;
btScalar m_twistCorrection;
+ btScalar m_twistAngle;
+
btScalar m_accSwingLimitImpulse;
btScalar m_accTwistLimitImpulse;
@@ -63,6 +69,19 @@
bool m_solveTwistLimit;
bool m_solveSwingLimit;
+ bool m_useSolveConstraintObsolete;
+
+ // not yet used...
+ btScalar m_swingLimitRatio;
+ btScalar m_twistLimitRatio;
+ btVector3 m_twistAxisA;
+
+ // motor
+ bool m_bMotorEnabled;
+ bool m_bNormalizedMotorStrength;
+ btQuaternion m_qTarget;
+ btScalar m_maxMotorImpulse;
+ btVector3 m_accMotorImpulse;
public:
@@ -74,8 +93,13 @@
virtual void buildJacobian();
- virtual void solveConstraint(btScalar timeStep);
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
void updateRHS(btScalar timeStep);
const btRigidBody& getRigidBodyA() const
@@ -92,8 +116,33 @@
m_angularOnly = angularOnly;
}
- void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 0.8f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+ void setLimit(int limitIndex,btScalar limitValue)
{
+ switch (limitIndex)
+ {
+ case 3:
+ {
+ m_twistSpan = limitValue;
+ break;
+ }
+ case 4:
+ {
+ m_swingSpan2 = limitValue;
+ break;
+ }
+ case 5:
+ {
+ m_swingSpan1 = limitValue;
+ break;
+ }
+ default:
+ {
+ }
+ };
+ }
+
+ void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+ {
m_swingSpan1 = _swingSpan1;
m_swingSpan2 = _swingSpan2;
m_twistSpan = _twistSpan;
@@ -121,6 +170,60 @@
return m_twistLimitSign;
}
+ void calcAngleInfo();
+ void calcAngleInfo2();
+
+ inline btScalar getSwingSpan1()
+ {
+ return m_swingSpan1;
+ }
+ inline btScalar getSwingSpan2()
+ {
+ return m_swingSpan2;
+ }
+ inline btScalar getTwistSpan()
+ {
+ return m_twistSpan;
+ }
+ inline btScalar getTwistAngle()
+ {
+ return m_twistAngle;
+ }
+ bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+
+ void setDamping(btScalar damping) { m_damping = damping; }
+
+ void enableMotor(bool b) { m_bMotorEnabled = b; }
+ void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
+ void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+
+ btScalar getFixThresh() { return m_fixThresh; }
+ void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
+
+ // setMotorTarget:
+ // q: the desired rotation of bodyA wrt bodyB.
+ // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
+ // note: don't forget to enableMotor()
+ void setMotorTarget(const btQuaternion &q);
+
+ // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+ void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+ btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+
+
+
+protected:
+ void init();
+
+ void computeConeLimitInfo(const btQuaternion& qCone, // in
+ btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+
+ void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+ btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+
+ void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
};
#endif //CONETWISTCONSTRAINT_H
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -22,7 +22,7 @@
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
-#define ASSERT2 assert
+#define ASSERT2 btAssert
#define USE_INTERNAL_APPLY_IMPULSE 1
@@ -52,7 +52,7 @@
btVector3 vel = vel1 - vel2;
- btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+ btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
@@ -114,7 +114,7 @@
btScalar Kcor = Kerp *Kfps;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- assert(cpd);
+ btAssert(cpd);
btScalar distance = cpd->m_penetration;
btScalar positionalError = Kcor *-distance;
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
@@ -166,7 +166,7 @@
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- assert(cpd);
+ btAssert(cpd);
btScalar combinedFriction = cpd->m_friction;
@@ -255,7 +255,7 @@
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- assert(cpd);
+ btAssert(cpd);
btScalar combinedFriction = cpd->m_friction;
@@ -337,7 +337,7 @@
btScalar Kcor = Kerp *Kfps;
btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
- assert(cpd);
+ btAssert(cpd);
btScalar distance = cpd->m_penetration;
btScalar positionalError = Kcor *-distance;
btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btContactSolverInfo.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -22,7 +22,12 @@
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4,
SOLVER_USE_FRICTION_WARMSTARTING = 8,
- SOLVER_CACHE_FRIENDLY = 16
+ SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+ SOLVER_CACHE_FRIENDLY = 128,
+ SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
+ SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster.
};
struct btContactSolverInfoData
@@ -39,6 +44,7 @@
btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse
+ btScalar m_globalCfm;//constraint force mixing
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_linearSlop;
@@ -65,12 +71,13 @@
m_numIterations = 10;
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.1);
- m_sor = btScalar(1.3);
+ m_globalCfm = btScalar(0.);
+ m_sor = btScalar(1.);
m_splitImpulse = false;
m_splitImpulsePenetrationThreshold = -0.02f;
m_linearSlop = btScalar(0.0);
m_warmstartingFactor=btScalar(0.85);
- m_solverMode = SOLVER_CACHE_FRIENDLY | SOLVER_RANDMIZE_ORDER | SOLVER_USE_WARMSTARTING;
+ m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD ;//SOLVER_RANDMIZE_ORDER
m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
}
};
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,15 +19,40 @@
http://gimpact.sf.net
*/
-
#include "btGeneric6DofConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btTransformUtil.h"
#include <new>
+#define D6_USE_OBSOLETE_METHOD false
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+:btTypedConstraint(D6_CONSTRAINT_TYPE),
+m_useLinearReferenceFrameA(true),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+}
+
+//-----------------------------------------------------------------------------
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+, m_frameInA(frameInA)
+, m_frameInB(frameInB),
+m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
+{
+
+}
+//-----------------------------------------------------------------------------
+
+
#define GENERIC_D6_DISABLE_WARMSTARTING 1
+//-----------------------------------------------------------------------------
+
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
{
@@ -36,51 +61,48 @@
return mat[i][j];
}
+//-----------------------------------------------------------------------------
+
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
{
-// // rot = cy*cz -cy*sz sy
-// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
-// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
-//
+ // // rot = cy*cz -cy*sz sy
+ // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
+ // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
+ //
- if (btGetMatrixElem(mat,2) < btScalar(1.0))
+ btScalar fi = btGetMatrixElem(mat,2);
+ if (fi < btScalar(1.0f))
+ {
+ if (fi > btScalar(-1.0f))
{
- if (btGetMatrixElem(mat,2) > btScalar(-1.0))
- {
- xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
- xyz[1] = btAsin(btGetMatrixElem(mat,2));
- xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
- return true;
- }
- else
- {
- // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
- xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
- xyz[1] = -SIMD_HALF_PI;
- xyz[2] = btScalar(0.0);
- return false;
- }
+ xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+ xyz[1] = btAsin(btGetMatrixElem(mat,2));
+ xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+ return true;
}
else
{
- // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
- xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
- xyz[1] = SIMD_HALF_PI;
- xyz[2] = 0.0;
-
+ // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
+ xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = -SIMD_HALF_PI;
+ xyz[2] = btScalar(0.0);
+ return false;
}
-
-
+ }
+ else
+ {
+ // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
+ xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+ xyz[1] = SIMD_HALF_PI;
+ xyz[2] = 0.0;
+ }
return false;
}
-
-
//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
-
int btRotationalLimitMotor::testLimitValue(btScalar test_value)
{
if(m_loLimit>m_hiLimit)
@@ -104,212 +126,239 @@
m_currentLimit = 0;//Free from violation
return 0;
-
+
}
+//-----------------------------------------------------------------------------
btScalar btRotationalLimitMotor::solveAngularLimits(
- btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
- btRigidBody * body0, btRigidBody * body1)
+ btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+ btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
{
- if (needApplyTorques()==false) return 0.0f;
+ if (needApplyTorques()==false) return 0.0f;
- btScalar target_velocity = m_targetVelocity;
- btScalar maxMotorForce = m_maxMotorForce;
+ btScalar target_velocity = m_targetVelocity;
+ btScalar maxMotorForce = m_maxMotorForce;
//current error correction
- if (m_currentLimit!=0)
- {
- target_velocity = -m_ERP*m_currentLimitError/(timeStep);
- maxMotorForce = m_maxLimitForce;
- }
+ if (m_currentLimit!=0)
+ {
+ target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+ maxMotorForce = m_maxLimitForce;
+ }
- maxMotorForce *= timeStep;
+ maxMotorForce *= timeStep;
- // current velocity difference
- btVector3 vel_diff = body0->getAngularVelocity();
- if (body1)
- {
- vel_diff -= body1->getAngularVelocity();
- }
+ // current velocity difference
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(angVelB);
+ btVector3 vel_diff;
+ vel_diff = angVelA-angVelB;
- btScalar rel_vel = axis.dot(vel_diff);
+
+ btScalar rel_vel = axis.dot(vel_diff);
+
// correction velocity
- btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
+ btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
- if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
- {
- return 0.0f;//no need for applying force
- }
+ if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
+ {
+ return 0.0f;//no need for applying force
+ }
// correction impulse
- btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+ btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
- btScalar clippedMotorImpulse;
+ btScalar clippedMotorImpulse;
- ///@todo: should clip against accumulated impulse
- if (unclippedMotorImpulse>0.0f)
- {
- clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
- }
- else
- {
- clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
- }
+ ///@todo: should clip against accumulated impulse
+ if (unclippedMotorImpulse>0.0f)
+ {
+ clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+ }
+ else
+ {
+ clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+ }
// sort with accumulated impulses
- btScalar lo = btScalar(-1e30);
- btScalar hi = btScalar(1e30);
+ btScalar lo = btScalar(-1e30);
+ btScalar hi = btScalar(1e30);
- btScalar oldaccumImpulse = m_accumulatedImpulse;
- btScalar sum = oldaccumImpulse + clippedMotorImpulse;
- m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+ btScalar oldaccumImpulse = m_accumulatedImpulse;
+ btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+ m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
- clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+ clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+ btVector3 motorImp = clippedMotorImpulse * axis;
+ //body0->applyTorqueImpulse(motorImp);
+ //body1->applyTorqueImpulse(-motorImp);
- btVector3 motorImp = clippedMotorImpulse * axis;
+ bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
+ bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
- body0->applyTorqueImpulse(motorImp);
- if (body1) body1->applyTorqueImpulse(-motorImp);
+ return clippedMotorImpulse;
- return clippedMotorImpulse;
-
}
//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
+
+
+
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
-btScalar btTranslationalLimitMotor::solveLinearAxis(
- btScalar timeStep,
- btScalar jacDiagABInv,
- btRigidBody& body1,const btVector3 &pointInA,
- btRigidBody& body2,const btVector3 &pointInB,
- int limit_index,
- const btVector3 & axis_normal_on_a,
- const btVector3 & anchorPos)
+
+
+int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
{
+ btScalar loLimit = m_lowerLimit[limitIndex];
+ btScalar hiLimit = m_upperLimit[limitIndex];
+ if(loLimit > hiLimit)
+ {
+ m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimitError[limitIndex] = btScalar(0.f);
+ return 0;
+ }
-///find relative velocity
-// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
-// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
- btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
- btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+ if (test_value < loLimit)
+ {
+ m_currentLimit[limitIndex] = 2;//low limit violation
+ m_currentLimitError[limitIndex] = test_value - loLimit;
+ return 2;
+ }
+ else if (test_value> hiLimit)
+ {
+ m_currentLimit[limitIndex] = 1;//High limit violation
+ m_currentLimitError[limitIndex] = test_value - hiLimit;
+ return 1;
+ };
- btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
+ m_currentLimit[limitIndex] = 0;//Free from violation
+ m_currentLimitError[limitIndex] = btScalar(0.f);
+ return 0;
+} // btTranslationalLimitMotor::testLimitValue()
- btScalar rel_vel = axis_normal_on_a.dot(vel);
+//-----------------------------------------------------------------------------
+btScalar btTranslationalLimitMotor::solveLinearAxis(
+ btScalar timeStep,
+ btScalar jacDiagABInv,
+ btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+ btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
+ int limit_index,
+ const btVector3 & axis_normal_on_a,
+ const btVector3 & anchorPos)
+{
+ ///find relative velocity
+ // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+ // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+ btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+ btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
-/// apply displacement correction
+ btVector3 vel1;
+ bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+ btVector3 vel2;
+ bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+ btVector3 vel = vel1 - vel2;
-//positional error (zeroth order error)
- btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
- btScalar lo = btScalar(-1e30);
- btScalar hi = btScalar(1e30);
+ btScalar rel_vel = axis_normal_on_a.dot(vel);
- btScalar minLimit = m_lowerLimit[limit_index];
- btScalar maxLimit = m_upperLimit[limit_index];
- //handle the limits
- if (minLimit < maxLimit)
- {
- {
- if (depth > maxLimit)
- {
- depth -= maxLimit;
- lo = btScalar(0.);
- }
- else
- {
- if (depth < minLimit)
- {
- depth -= minLimit;
- hi = btScalar(0.);
- }
- else
- {
- return 0.0f;
- }
- }
- }
- }
+ /// apply displacement correction
- btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+ //positional error (zeroth order error)
+ btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+ btScalar lo = btScalar(-1e30);
+ btScalar hi = btScalar(1e30);
+ btScalar minLimit = m_lowerLimit[limit_index];
+ btScalar maxLimit = m_upperLimit[limit_index];
+ //handle the limits
+ if (minLimit < maxLimit)
+ {
+ {
+ if (depth > maxLimit)
+ {
+ depth -= maxLimit;
+ lo = btScalar(0.);
+ }
+ else
+ {
+ if (depth < minLimit)
+ {
+ depth -= minLimit;
+ hi = btScalar(0.);
+ }
+ else
+ {
+ return 0.0f;
+ }
+ }
+ }
+ }
- btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
- btScalar sum = oldNormalImpulse + normalImpulse;
- m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
- normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+ btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
- btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
- body1.applyImpulse( impulse_vector, rel_pos1);
- body2.applyImpulse(-impulse_vector, rel_pos2);
- return normalImpulse;
-}
-//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
-btGeneric6DofConstraint::btGeneric6DofConstraint()
- :btTypedConstraint(D6_CONSTRAINT_TYPE),
- m_useLinearReferenceFrameA(true)
-{
-}
+ btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+ btScalar sum = oldNormalImpulse + normalImpulse;
+ m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+ normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
-btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
- : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
- , m_frameInA(frameInA)
- , m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
-{
+ btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+ //body1.applyImpulse( impulse_vector, rel_pos1);
+ //body2.applyImpulse(-impulse_vector, rel_pos2);
-}
+ btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
+ btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
+ bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+ bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+ return normalImpulse;
+}
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
void btGeneric6DofConstraint::calculateAngleInfo()
{
btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
-
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
-
-
-
// in euler angle mode we do not actually constrain the angular velocity
- // along the axes axis[0] and axis[2] (although we do use axis[1]) :
- //
- // to get constrain w2-w1 along ...not
- // ------ --------------------- ------
- // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
- // d(angle[1])/dt = 0 ax[1]
- // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
- //
- // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
- // to prove the result for angle[0], write the expression for angle[0] from
- // GetInfo1 then take the derivative. to prove this for angle[2] it is
- // easier to take the euler rate expression for d(angle[2])/dt with respect
- // to the components of w and set that to 0.
-
+ // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+ //
+ // to get constrain w2-w1 along ...not
+ // ------ --------------------- ------
+ // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
+ // d(angle[1])/dt = 0 ax[1]
+ // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
+ //
+ // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+ // to prove the result for angle[0], write the expression for angle[0] from
+ // GetInfo1 then take the derivative. to prove this for angle[2] it is
+ // easier to take the euler rate expression for d(angle[2])/dt with respect
+ // to the components of w and set that to 0.
btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
@@ -317,34 +366,29 @@
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+ m_calculatedAxis[0].normalize();
+ m_calculatedAxis[1].normalize();
+ m_calculatedAxis[2].normalize();
-// if(m_debugDrawer)
-// {
-//
-// char buff[300];
-// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
-// m_calculatedAxisAngleDiff[0],
-// m_calculatedAxisAngleDiff[1],
-// m_calculatedAxisAngleDiff[2]);
-// m_debugDrawer->reportErrorWarning(buff);
-// }
-
}
+//-----------------------------------------------------------------------------
+
void btGeneric6DofConstraint::calculateTransforms()
{
- m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
- m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
-
- calculateAngleInfo();
+ m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+ m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+ calculateLinearInfo();
+ calculateAngleInfo();
}
+//-----------------------------------------------------------------------------
void btGeneric6DofConstraint::buildLinearJacobian(
- btJacobianEntry & jacLinear,const btVector3 & normalWorld,
- const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+ btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+ const btVector3 & pivotAInW,const btVector3 & pivotBInW)
{
- new (&jacLinear) btJacobianEntry(
+ new (&jacLinear) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
@@ -354,13 +398,14 @@
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
-
}
+//-----------------------------------------------------------------------------
+
void btGeneric6DofConstraint::buildAngularJacobian(
- btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+ btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
{
- new (&jacAngular) btJacobianEntry(jointAxisW,
+ new (&jacAngular) btJacobianEntry(jointAxisW,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
@@ -368,142 +413,260 @@
}
+//-----------------------------------------------------------------------------
+
bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
{
- btScalar angle = m_calculatedAxisAngleDiff[axis_index];
-
- //test limits
- m_angularLimits[axis_index].testLimitValue(angle);
- return m_angularLimits[axis_index].needApplyTorques();
+ btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+ //test limits
+ m_angularLimits[axis_index].testLimitValue(angle);
+ return m_angularLimits[axis_index].needApplyTorques();
}
+//-----------------------------------------------------------------------------
+
void btGeneric6DofConstraint::buildJacobian()
{
+ if (m_useSolveConstraintObsolete)
+ {
- // Clear accumulated impulses for the next simulation step
- m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
- int i;
- for(i = 0; i < 3; i++)
- {
- m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
- }
- //calculates transform
- calculateTransforms();
+ // Clear accumulated impulses for the next simulation step
+ m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+ int i;
+ for(i = 0; i < 3; i++)
+ {
+ m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+ }
+ //calculates transform
+ calculateTransforms();
-// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
-// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
- calcAnchorPos();
- btVector3 pivotAInW = m_AnchorPos;
- btVector3 pivotBInW = m_AnchorPos;
+ // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+ // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+ calcAnchorPos();
+ btVector3 pivotAInW = m_AnchorPos;
+ btVector3 pivotBInW = m_AnchorPos;
-// not used here
-// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
-// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+ // not used here
+ // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+ // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
- btVector3 normalWorld;
- //linear part
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.isLimited(i))
- {
- if (m_useLinearReferenceFrameA)
- normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
- else
- normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
+ btVector3 normalWorld;
+ //linear part
+ for (i=0;i<3;i++)
+ {
+ if (m_linearLimits.isLimited(i))
+ {
+ if (m_useLinearReferenceFrameA)
+ normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+ else
+ normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
- buildLinearJacobian(
- m_jacLinear[i],normalWorld ,
- pivotAInW,pivotBInW);
+ buildLinearJacobian(
+ m_jacLinear[i],normalWorld ,
+ pivotAInW,pivotBInW);
- }
- }
+ }
+ }
- // angular part
- for (i=0;i<3;i++)
- {
- //calculates error angle
- if (testAngularLimitMotor(i))
- {
- normalWorld = this->getAxis(i);
- // Create angular atom
- buildAngularJacobian(m_jacAng[i],normalWorld);
- }
- }
+ // angular part
+ for (i=0;i<3;i++)
+ {
+ //calculates error angle
+ if (testAngularLimitMotor(i))
+ {
+ normalWorld = this->getAxis(i);
+ // Create angular atom
+ buildAngularJacobian(m_jacAng[i],normalWorld);
+ }
+ }
+ }
+}
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ //prepare constraint
+ calculateTransforms();
+ info->m_numConstraintRows = 0;
+ info->nub = 6;
+ int i;
+ //test linear limits
+ for(i = 0; i < 3; i++)
+ {
+ if(m_linearLimits.needApplyForce(i))
+ {
+ info->m_numConstraintRows++;
+ info->nub--;
+ }
+ }
+ //test angular limits
+ for (i=0;i<3 ;i++ )
+ {
+ if(testAngularLimitMotor(i))
+ {
+ info->m_numConstraintRows++;
+ info->nub--;
+ }
+ }
+ }
}
+//-----------------------------------------------------------------------------
-void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
+void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
{
- m_timeStep = timeStep;
+ btAssert(!m_useSolveConstraintObsolete);
+ int row = setLinearLimits(info);
+ setAngularLimits(info, row);
+}
- //calculateTransforms();
+//-----------------------------------------------------------------------------
- int i;
+int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
+{
+ btGeneric6DofConstraint * d6constraint = this;
+ int row = 0;
+ //solve linear limits
+ btRotationalLimitMotor limot;
+ for (int i=0;i<3 ;i++ )
+ {
+ if(m_linearLimits.needApplyForce(i))
+ { // re-use rotational motor code
+ limot.m_bounce = btScalar(0.f);
+ limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+ limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
+ limot.m_damping = m_linearLimits.m_damping;
+ limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
+ limot.m_ERP = m_linearLimits.m_restitution;
+ limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
+ limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
+ limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
+ limot.m_maxLimitForce = btScalar(0.f);
+ limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
+ limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
+ btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
+ row += get_limit_motor_info2(&limot, &m_rbA, &m_rbB, info, row, axis, 0);
+ }
+ }
+ return row;
+}
- // linear
+//-----------------------------------------------------------------------------
- btVector3 pointInA = m_calculatedTransformA.getOrigin();
- btVector3 pointInB = m_calculatedTransformB.getOrigin();
+int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
+{
+ btGeneric6DofConstraint * d6constraint = this;
+ int row = row_offset;
+ //solve angular limits
+ for (int i=0;i<3 ;i++ )
+ {
+ if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+ {
+ btVector3 axis = d6constraint->getAxis(i);
+ row += get_limit_motor_info2(
+ d6constraint->getRotationalLimitMotor(i),
+ &m_rbA,
+ &m_rbB,
+ info,row,axis,1);
+ }
+ }
- btScalar jacDiagABInv;
- btVector3 linear_axis;
- for (i=0;i<3;i++)
- {
- if (m_linearLimits.isLimited(i))
- {
- jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+ return row;
+}
- if (m_useLinearReferenceFrameA)
- linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
- else
- linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+//-----------------------------------------------------------------------------
- m_linearLimits.solveLinearAxis(
- m_timeStep,
- jacDiagABInv,
- m_rbA,pointInA,
- m_rbB,pointInB,
- i,linear_axis, m_AnchorPos);
+void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
- }
- }
- // angular
- btVector3 angular_axis;
- btScalar angularJacDiagABInv;
- for (i=0;i<3;i++)
- {
- if (m_angularLimits[i].needApplyTorques())
- {
+ m_timeStep = timeStep;
- // get axis
- angular_axis = getAxis(i);
+ //calculateTransforms();
- angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+ int i;
- m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
- }
- }
+ // linear
+
+ btVector3 pointInA = m_calculatedTransformA.getOrigin();
+ btVector3 pointInB = m_calculatedTransformB.getOrigin();
+
+ btScalar jacDiagABInv;
+ btVector3 linear_axis;
+ for (i=0;i<3;i++)
+ {
+ if (m_linearLimits.isLimited(i))
+ {
+ jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+ if (m_useLinearReferenceFrameA)
+ linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+ else
+ linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+
+ m_linearLimits.solveLinearAxis(
+ m_timeStep,
+ jacDiagABInv,
+ m_rbA,bodyA,pointInA,
+ m_rbB,bodyB,pointInB,
+ i,linear_axis, m_AnchorPos);
+
+ }
+ }
+
+ // angular
+ btVector3 angular_axis;
+ btScalar angularJacDiagABInv;
+ for (i=0;i<3;i++)
+ {
+ if (m_angularLimits[i].needApplyTorques())
+ {
+
+ // get axis
+ angular_axis = getAxis(i);
+
+ angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+
+ m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
+ }
+ }
+ }
}
+//-----------------------------------------------------------------------------
+
void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
{
- (void)timeStep;
+ (void)timeStep;
}
+//-----------------------------------------------------------------------------
+
btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
{
- return m_calculatedAxis[axis_index];
+ return m_calculatedAxis[axis_index];
}
+//-----------------------------------------------------------------------------
+
btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
{
- return m_calculatedAxisAngleDiff[axis_index];
+ return m_calculatedAxisAngleDiff[axis_index];
}
+//-----------------------------------------------------------------------------
+
void btGeneric6DofConstraint::calcAnchorPos(void)
{
btScalar imA = m_rbA.getInvMass();
@@ -523,3 +686,144 @@
return;
} // btGeneric6DofConstraint::calcAnchorPos()
+//-----------------------------------------------------------------------------
+
+void btGeneric6DofConstraint::calculateLinearInfo()
+{
+ m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
+ m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
+ for(int i = 0; i < 3; i++)
+ {
+ m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
+ }
+} // btGeneric6DofConstraint::calculateLinearInfo()
+
+//-----------------------------------------------------------------------------
+
+int btGeneric6DofConstraint::get_limit_motor_info2(
+ btRotationalLimitMotor * limot,
+ btRigidBody * body0, btRigidBody * body1,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
+{
+ int srow = row * info->rowskip;
+ int powered = limot->m_enableMotor;
+ int limit = limot->m_currentLimit;
+ if (powered || limit)
+ { // if the joint is powered, or has joint limits, add in the extra row
+ btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
+ btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
+ J1[srow+0] = ax1[0];
+ J1[srow+1] = ax1[1];
+ J1[srow+2] = ax1[2];
+ if(rotational)
+ {
+ J2[srow+0] = -ax1[0];
+ J2[srow+1] = -ax1[1];
+ J2[srow+2] = -ax1[2];
+ }
+ if((!rotational) && limit)
+ {
+ btVector3 ltd; // Linear Torque Decoupling vector
+ btVector3 c = m_calculatedTransformB.getOrigin() - body0->getCenterOfMassPosition();
+ ltd = c.cross(ax1);
+ info->m_J1angularAxis[srow+0] = ltd[0];
+ info->m_J1angularAxis[srow+1] = ltd[1];
+ info->m_J1angularAxis[srow+2] = ltd[2];
+
+ c = m_calculatedTransformB.getOrigin() - body1->getCenterOfMassPosition();
+ ltd = -c.cross(ax1);
+ info->m_J2angularAxis[srow+0] = ltd[0];
+ info->m_J2angularAxis[srow+1] = ltd[1];
+ info->m_J2angularAxis[srow+2] = ltd[2];
+ }
+ // if we're limited low and high simultaneously, the joint motor is
+ // ineffective
+ if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+ info->m_constraintError[srow] = btScalar(0.f);
+ if (powered)
+ {
+ info->cfm[srow] = 0.0f;
+ if(!limit)
+ {
+ info->m_constraintError[srow] += limot->m_targetVelocity;
+ info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
+ info->m_upperLimit[srow] = limot->m_maxMotorForce;
+ }
+ }
+ if(limit)
+ {
+ btScalar k = info->fps * limot->m_ERP;
+ if(!rotational)
+ {
+ info->m_constraintError[srow] += k * limot->m_currentLimitError;
+ }
+ else
+ {
+ info->m_constraintError[srow] += -k * limot->m_currentLimitError;
+ }
+ info->cfm[srow] = 0.0f;
+ if (limot->m_loLimit == limot->m_hiLimit)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ if (limit == 1)
+ {
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ {
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // deal with bounce
+ if (limot->m_bounce > 0)
+ {
+ // calculate joint velocity
+ btScalar vel;
+ if (rotational)
+ {
+ vel = body0->getAngularVelocity().dot(ax1);
+ if (body1)
+ vel -= body1->getAngularVelocity().dot(ax1);
+ }
+ else
+ {
+ vel = body0->getLinearVelocity().dot(ax1);
+ if (body1)
+ vel -= body1->getLinearVelocity().dot(ax1);
+ }
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if (limit == 1)
+ {
+ if (vel < 0)
+ {
+ btScalar newc = -limot->m_bounce* vel;
+ if (newc > info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ else
+ {
+ if (vel > 0)
+ {
+ btScalar newc = -limot->m_bounce * vel;
+ if (newc < info->m_constraintError[srow])
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ }
+ return 1;
+ }
+ else return 0;
+}
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -30,6 +30,8 @@
class btRigidBody;
+
+
//! Rotation Limit structure for generic joints
class btRotationalLimitMotor
{
@@ -92,7 +94,7 @@
//! Is limited
bool isLimited()
{
- if(m_loLimit>=m_hiLimit) return false;
+ if(m_loLimit > m_hiLimit) return false;
return true;
}
@@ -110,9 +112,8 @@
int testLimitValue(btScalar test_value);
//! apply the correction impulses for two bodies
- btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
+ btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
-
};
@@ -129,6 +130,11 @@
btScalar m_damping;//!< Damping for linear limit
btScalar m_restitution;//! Bounce parameter for linear limit
//!@}
+ bool m_enableMotor[3];
+ btVector3 m_targetVelocity;//!< target motor velocity
+ btVector3 m_maxMotorForce;//!< max force on motor
+ btVector3 m_currentLimitError;//! How much is violated this limit
+ int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit
btTranslationalLimitMotor()
{
@@ -139,6 +145,12 @@
m_limitSoftness = 0.7f;
m_damping = btScalar(1.0f);
m_restitution = btScalar(0.5f);
+ for(int i=0; i < 3; i++)
+ {
+ m_enableMotor[i] = false;
+ m_targetVelocity[i] = btScalar(0.f);
+ m_maxMotorForce[i] = btScalar(0.f);
+ }
}
btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
@@ -150,6 +162,12 @@
m_limitSoftness = other.m_limitSoftness ;
m_damping = other.m_damping;
m_restitution = other.m_restitution;
+ for(int i=0; i < 3; i++)
+ {
+ m_enableMotor[i] = other.m_enableMotor[i];
+ m_targetVelocity[i] = other.m_targetVelocity[i];
+ m_maxMotorForce[i] = other.m_maxMotorForce[i];
+ }
}
//! Test limit
@@ -163,13 +181,19 @@
{
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
}
+ inline bool needApplyForce(int limitIndex)
+ {
+ if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
+ return true;
+ }
+ int testLimitValue(int limitIndex, btScalar test_value);
btScalar solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
- btRigidBody& body1,const btVector3 &pointInA,
- btRigidBody& body2,const btVector3 &pointInB,
+ btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
+ btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos);
@@ -247,6 +271,7 @@
btTransform m_calculatedTransformB;
btVector3 m_calculatedAxisAngleDiff;
btVector3 m_calculatedAxis[3];
+ btVector3 m_calculatedLinearDiff;
btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
@@ -262,13 +287,18 @@
}
+ int setAngularLimits(btConstraintInfo2 *info, int row_offset);
+ int setLinearLimits(btConstraintInfo2 *info);
+
void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
const btVector3 & pivotAInW,const btVector3 & pivotBInW);
void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+ // tests linear limits
+ void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
@@ -276,6 +306,10 @@
public:
+
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
+
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofConstraint();
@@ -330,8 +364,12 @@
//! performs Jacobian calculation, and also calculates angle differences and axis
virtual void buildJacobian();
- virtual void solveConstraint(btScalar timeStep);
+ virtual void getInfo1 (btConstraintInfo1* info);
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
void updateRHS(btScalar timeStep);
//! Get the rotation axis in global coordinates
@@ -432,6 +470,11 @@
virtual void calcAnchorPos(void); // overridable
+ int get_limit_motor_info2( btRotationalLimitMotor * limot,
+ btRigidBody * body0, btRigidBody * body1,
+ btConstraintInfo2 *info, int row, btVector3& ax1, int rotational);
+
+
};
#endif //GENERIC_6DOF_CONSTRAINT_H
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,19 +19,33 @@
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h"
#include <new>
+#include "btSolverBody.h"
+//-----------------------------------------------------------------------------
+#define HINGE_USE_OBSOLETE_SOLVER false
+
+//-----------------------------------------------------------------------------
+
+
btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(false)
{
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
+//-----------------------------------------------------------------------------
+
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
- btVector3& axisInA,btVector3& axisInB)
+ btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
- m_enableAngularMotor(false)
+ m_enableAngularMotor(false),
+ m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+ m_useReferenceFrameA(useReferenceFrameA)
{
m_rbAFrame.getOrigin() = pivotInA;
@@ -60,9 +74,9 @@
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
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() );
+ 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);
@@ -71,13 +85,15 @@
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
-
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
+//-----------------------------------------------------------------------------
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -90,7 +106,7 @@
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
- btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+ btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
@@ -109,19 +125,19 @@
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
+//-----------------------------------------------------------------------------
+
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
- const btTransform& rbAFrame, const btTransform& rbBFrame)
+ const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
{
- // flip axis
- m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
- m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
- m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
@@ -129,22 +145,20 @@
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
+//-----------------------------------------------------------------------------
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
m_angularOnly(false),
-m_enableAngularMotor(false)
+m_enableAngularMotor(false),
+m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
+m_useReferenceFrameA(useReferenceFrameA)
{
///not providing rigidbody B means implicitly using worldspace for body B
- // flip axis
- m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
- m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
- m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
//start with free
@@ -154,33 +168,38 @@
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
+ m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
+//-----------------------------------------------------------------------------
+
void btHingeConstraint::buildJacobian()
{
- m_appliedImpulse = btScalar(0.);
-
- if (!m_angularOnly)
+ if (m_useSolveConstraintObsolete)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
- btVector3 relPos = pivotBInW - pivotAInW;
+ m_appliedImpulse = btScalar(0.);
- btVector3 normal[3];
- if (relPos.length2() > SIMD_EPSILON)
+ if (!m_angularOnly)
{
- normal[0] = relPos.normalized();
- }
- else
- {
- normal[0].setValue(btScalar(1.0),0,0);
- }
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ btVector3 relPos = pivotBInW - pivotAInW;
- btPlaneSpace1(normal[0], normal[1], normal[2]);
+ btVector3 normal[3];
+ if (relPos.length2() > SIMD_EPSILON)
+ {
+ normal[0] = relPos.normalized();
+ }
+ else
+ {
+ normal[0].setValue(btScalar(1.0),0,0);
+ }
- for (int i=0;i<3;i++)
- {
- new (&m_jac[i]) btJacobianEntry(
+ 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(),
@@ -190,213 +209,458 @@
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
+ }
}
- }
- //calculate two perpendicular jointAxis, orthogonal to hingeAxis
- //these two jointAxis require equal angular velocities for both bodies
+ //calculate two perpendicular jointAxis, orthogonal to hingeAxis
+ //these two jointAxis require equal angular velocities for both bodies
- //this is unused for now, it's a todo
- btVector3 jointAxis0local;
- btVector3 jointAxis1local;
-
- btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
-
- 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_rbAFrame.getBasis().getColumn(2);
+ //this is unused for now, it's a todo
+ btVector3 jointAxis0local;
+ btVector3 jointAxis1local;
- new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
+ btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
- new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
+ 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_rbAFrame.getBasis().getColumn(2);
+
+ new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
- new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
- m_rbA.getCenterOfMassTransform().getBasis().transpose(),
- m_rbB.getCenterOfMassTransform().getBasis().transpose(),
- m_rbA.getInvInertiaDiagLocal(),
- m_rbB.getInvInertiaDiagLocal());
+ new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
+ new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
+ m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+ m_rbA.getInvInertiaDiagLocal(),
+ m_rbB.getInvInertiaDiagLocal());
- // Compute limit information
- btScalar hingeAngle = getHingeAngle();
+ // clear accumulator
+ m_accLimitImpulse = btScalar(0.);
- //set bias, sign, clear accumulator
- m_correction = btScalar(0.);
- m_limitSign = btScalar(0.);
- m_solveLimit = false;
- m_accLimitImpulse = btScalar(0.);
+ // test angular limit
+ testLimit();
-// if (m_lowerLimit < m_upperLimit)
- if (m_lowerLimit <= m_upperLimit)
+ //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::getInfo1(btConstraintInfo1* info)
+{
+ if (m_useSolveConstraintObsolete)
{
-// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
- if (hingeAngle <= m_lowerLimit)
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ }
+ else
+ {
+ info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
+ info->nub = 1;
+ //prepare constraint
+ testLimit();
+ if(getSolveLimit() || getEnableAngularMotor())
{
- m_correction = (m_lowerLimit - hingeAngle);
- m_limitSign = 1.0f;
- m_solveLimit = true;
- }
-// else if (hingeAngle >= m_upperLimit*m_limitSoftness)
- else if (hingeAngle >= m_upperLimit)
- {
- m_correction = m_upperLimit - hingeAngle;
- m_limitSign = -1.0f;
- m_solveLimit = true;
+ info->m_numConstraintRows++; // limit 3rd anguar as well
+ info->nub--;
}
}
+} // btHingeConstraint::getInfo1 ()
- //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::getInfo2 (btConstraintInfo2* info)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, s = info->rowskip;
+ // transforms in world space
+ btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+ btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
+ // pivot point
+ btVector3 pivotAInW = trA.getOrigin();
+ btVector3 pivotBInW = trB.getOrigin();
+ // linear (all fixed)
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[s + 1] = 1;
+ info->m_J1linearAxis[2 * s + 2] = 1;
+ btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
+ btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
+ btVector3 a1neg = -a1;
+ a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
+ btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
+ a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+ // linear RHS
+ btScalar k = info->fps * info->erp;
+ for(i = 0; i < 3; i++)
+ {
+ info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
+ }
+ // make rotations around X and Y equal
+ // the hinge axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the hinge axis should be equal. thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ // get hinge axis (Z)
+ btVector3 ax1 = trA.getBasis().getColumn(2);
+ // get 2 orthos to hinge axis (X, Y)
+ btVector3 p = trA.getBasis().getColumn(0);
+ btVector3 q = trA.getBasis().getColumn(1);
+ // set the two hinge angular rows
+ int s3 = 3 * info->rowskip;
+ int s4 = 4 * info->rowskip;
+
+ info->m_J1angularAxis[s3 + 0] = p[0];
+ info->m_J1angularAxis[s3 + 1] = p[1];
+ info->m_J1angularAxis[s3 + 2] = p[2];
+ info->m_J1angularAxis[s4 + 0] = q[0];
+ info->m_J1angularAxis[s4 + 1] = q[1];
+ info->m_J1angularAxis[s4 + 2] = q[2];
+
+ info->m_J2angularAxis[s3 + 0] = -p[0];
+ info->m_J2angularAxis[s3 + 1] = -p[1];
+ info->m_J2angularAxis[s3 + 2] = -p[2];
+ info->m_J2angularAxis[s4 + 0] = -q[0];
+ info->m_J2angularAxis[s4 + 1] = -q[1];
+ info->m_J2angularAxis[s4 + 2] = -q[2];
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the hinge back into alignment.
+ // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+ // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ btVector3 ax2 = trB.getBasis().getColumn(2);
+ btVector3 u = ax1.cross(ax2);
+ info->m_constraintError[s3] = k * u.dot(p);
+ info->m_constraintError[s4] = k * u.dot(q);
+ // check angular limits
+ int nrow = 4; // last filled row
+ int srow;
+ btScalar limit_err = btScalar(0.0);
+ int limit = 0;
+ if(getSolveLimit())
+ {
+ limit_err = m_correction * m_referenceSign;
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+ }
+ // if the hinge has joint limits or motor, add in the extra row
+ int powered = 0;
+ if(getEnableAngularMotor())
+ {
+ powered = 1;
+ }
+ if(limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1angularAxis[srow+0] = ax1[0];
+ info->m_J1angularAxis[srow+1] = ax1[1];
+ info->m_J1angularAxis[srow+2] = ax1[2];
+
+ info->m_J2angularAxis[srow+0] = -ax1[0];
+ info->m_J2angularAxis[srow+1] = -ax1[1];
+ info->m_J2angularAxis[srow+2] = -ax1[2];
+
+ btScalar lostop = getLowerLimit();
+ btScalar histop = getUpperLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ info->m_constraintError[srow] = btScalar(0.0f);
+ if(powered)
+ {
+ info->cfm[srow] = btScalar(0.0);
+ btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
+ info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
+ info->m_lowerLimit[srow] = - m_maxMotorImpulse;
+ info->m_upperLimit[srow] = m_maxMotorImpulse;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0);
+ if(lostop == histop)
+ {
+ // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+ btScalar bounce = m_relaxationFactor;
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+ vel -= m_rbB.getAngularVelocity().dot(ax1);
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= m_biasFactor;
+ } // if(limit)
+ } // if angular limit or powered
}
-void btHingeConstraint::solveConstraint(btScalar timeStep)
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ if (m_useSolveConstraintObsolete)
+ {
- btScalar tau = btScalar(0.3);
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
- //linear part
- if (!m_angularOnly)
- {
- btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
- btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+ btScalar tau = btScalar(0.3);
- btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
+ //linear part
+ if (!m_angularOnly)
+ {
+ btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+ btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
- for (int i=0;i<3;i++)
- {
- const btVector3& normal = m_jac[i].m_linearJointAxis;
- btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+ btVector3 vel1,vel2;
+ bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+ bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+ 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 - 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());
+ 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;
+ btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+ btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+ bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+ bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+ }
}
- }
-
- {
- ///solve angular part
+
+ {
+ ///solve angular part
- // get axes in world space
- btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
- btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
+ // get axes in world space
+ 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();
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(angVelB);
- btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
- btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+ btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+ btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
- btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
- btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
- btVector3 velrelOrthog = angAorthog-angBorthog;
- {
- //solve orthogonal angular velocity correction
- btScalar relaxation = btScalar(1.);
- btScalar len = velrelOrthog.length();
- if (len > btScalar(0.00001))
+ btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+ btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+ btVector3 velrelOrthog = angAorthog-angBorthog;
{
- btVector3 normal = velrelOrthog.normalized();
- btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
- getRigidBodyB().computeAngularImpulseDenominator(normal);
- // scale for mass and relaxation
- velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
- }
+
- //solve angular positional correction
- btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
- btScalar len2 = angularError.length();
- if (len2>btScalar(0.00001))
- {
- btVector3 normal2 = angularError.normalized();
- btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
- getRigidBodyB().computeAngularImpulseDenominator(normal2);
- angularError *= (btScalar(1.)/denom2) * relaxation;
- }
+ //solve orthogonal angular velocity correction
+ btScalar relaxation = btScalar(1.);
+ btScalar len = velrelOrthog.length();
+ if (len > btScalar(0.00001))
+ {
+ btVector3 normal = velrelOrthog.normalized();
+ btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+ getRigidBodyB().computeAngularImpulseDenominator(normal);
+ // scale for mass and relaxation
+ //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
- m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
- m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
- // 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;
+ //solve angular positional correction
+ btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
+ btScalar len2 = angularError.length();
+ if (len2>btScalar(0.00001))
+ {
+ btVector3 normal2 = angularError.normalized();
+ btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+ getRigidBodyB().computeAngularImpulseDenominator(normal2);
+ //angularError *= (btScalar(1.)/denom2) * relaxation;
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
- // Clamp the accumulated impulse
- btScalar temp = m_accLimitImpulse;
- m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
- impulseMag = m_accLimitImpulse - temp;
+ }
+
+
- btVector3 impulse = axisA * impulseMag * m_limitSign;
- m_rbA.applyTorqueImpulse(impulse);
- m_rbB.applyTorqueImpulse(-impulse);
+
+ // 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, btScalar(0) );
+ impulseMag = m_accLimitImpulse - temp;
+
+
+
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
+
+ }
}
- }
- //apply motor
- if (m_enableAngularMotor)
- {
- //todo: add limits too
- btVector3 angularLimit(0,0,0);
+ //apply motor
+ if (m_enableAngularMotor)
+ {
+ //todo: add limits too
+ btVector3 angularLimit(0,0,0);
- btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
- btScalar projRelVel = velrel.dot(axisA);
+ btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+ btScalar projRelVel = velrel.dot(axisA);
- btScalar desiredMotorVel = m_motorTargetVelocity;
- btScalar motor_relvel = desiredMotorVel - projRelVel;
+ btScalar desiredMotorVel = m_motorTargetVelocity;
+ btScalar motor_relvel = desiredMotorVel - projRelVel;
- 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;
- btVector3 motorImp = clippedMotorImpulse * axisA;
-
- m_rbA.applyTorqueImpulse(motorImp+angularLimit);
- m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+ 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;
+ btVector3 motorImp = clippedMotorImpulse * axisA;
+ bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
+ bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
+
+ }
}
}
}
+//-----------------------------------------------------------------------------
+
void btHingeConstraint::updateRHS(btScalar timeStep)
{
(void)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) );
+ btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+ return m_referenceSign * angle;
}
+//-----------------------------------------------------------------------------
+
+void btHingeConstraint::testLimit()
+{
+ // Compute limit information
+ m_hingeAngle = getHingeAngle();
+ m_correction = btScalar(0.);
+ m_limitSign = btScalar(0.);
+ m_solveLimit = false;
+ if (m_lowerLimit <= m_upperLimit)
+ {
+ if (m_hingeAngle <= m_lowerLimit)
+ {
+ m_correction = (m_lowerLimit - m_hingeAngle);
+ m_limitSign = 1.0f;
+ m_solveLimit = true;
+ }
+ else if (m_hingeAngle >= m_upperLimit)
+ {
+ m_correction = m_upperLimit - m_hingeAngle;
+ m_limitSign = -1.0f;
+ m_solveLimit = true;
+ }
+ }
+ return;
+} // btHingeConstraint::testLimit()
+
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
+//-----------------------------------------------------------------------------
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -53,28 +53,36 @@
btScalar m_correction;
btScalar m_accLimitImpulse;
+ btScalar m_hingeAngle;
+ btScalar m_referenceSign;
bool m_angularOnly;
bool m_enableAngularMotor;
bool m_solveLimit;
+ bool m_useSolveConstraintObsolete;
+ bool m_useReferenceFrameA;
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, bool useReferenceFrameA = false);
- btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+ btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA = false);
- btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+ btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
- btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+ btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
btHingeConstraint();
virtual void buildJacobian();
- virtual void solveConstraint(btScalar timeStep);
+ virtual void getInfo1 (btConstraintInfo1* info);
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
void updateRHS(btScalar timeStep);
const btRigidBody& getRigidBodyA() const
@@ -86,6 +94,16 @@
return m_rbB;
}
+ btRigidBody& getRigidBodyA()
+ {
+ return m_rbA;
+ }
+
+ btRigidBody& getRigidBodyB()
+ {
+ return m_rbB;
+ }
+
void setAngularOnly(bool angularOnly)
{
m_angularOnly = angularOnly;
@@ -122,7 +140,9 @@
btScalar getHingeAngle();
+ void testLimit();
+
const btTransform& getAFrame() { return m_rbAFrame; };
const btTransform& getBFrame() { return m_rbBFrame; };
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -21,33 +21,38 @@
btPoint2PointConstraint::btPoint2PointConstraint()
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
+m_useSolveConstraintObsolete(false)
{
}
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_useSolveConstraintObsolete(false)
{
}
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_useSolveConstraintObsolete(false)
{
}
void btPoint2PointConstraint::buildJacobian()
{
- m_appliedImpulse = btScalar(0.);
+ ///we need it for both methods
+ {
+ m_appliedImpulse = btScalar(0.);
- btVector3 normal(0,0,0);
+ btVector3 normal(0,0,0);
- for (int i=0;i<3;i++)
- {
- normal[i] = 1;
- new (&m_jac[i]) btJacobianEntry(
+ 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(),
@@ -58,64 +63,162 @@
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
+ }
}
}
-void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
+
+void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
{
- btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
- btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+ if (m_useSolveConstraintObsolete)
+ {
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
+ } else
+ {
+ info->m_numConstraintRows = 3;
+ info->nub = 3;
+ }
+}
+void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
+{
+ btAssert(!m_useSolveConstraintObsolete);
- btVector3 normal(0,0,0);
+ //retrieve matrices
+ btTransform body0_trans;
+ body0_trans = m_rbA.getCenterOfMassTransform();
+ btTransform body1_trans;
+ body1_trans = m_rbB.getCenterOfMassTransform();
+
+ // anchor points in global coordinates with respect to body PORs.
+
+ // set jacobian
+ info->m_J1linearAxis[0] = 1;
+ info->m_J1linearAxis[info->rowskip+1] = 1;
+ info->m_J1linearAxis[2*info->rowskip+2] = 1;
+
+ btVector3 a1 = body0_trans.getBasis()*getPivotInA();
+ {
+ btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
+ btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
+ btVector3 a1neg = -a1;
+ a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+
+ /*info->m_J2linearAxis[0] = -1;
+ info->m_J2linearAxis[s+1] = -1;
+ info->m_J2linearAxis[2*s+2] = -1;
+ */
+ btVector3 a2 = body1_trans.getBasis()*getPivotInB();
+
+ {
+ btVector3 a2n = -a2;
+ btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
+ btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
+ btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
+ a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
+ }
+
-// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
- for (int i=0;i<3;i++)
- {
- normal[i] = 1;
- btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+ // set right hand side
+ btScalar k = info->fps * info->erp;
+ int j;
- btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
- btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
- //this jacobian entry could be re-used for all iterations
-
- 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);
+ for (j=0; j<3; j++)
+ {
+ info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+ //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
+ }
- /*
- //velocity error (first order error)
- btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
- m_rbB.getLinearVelocity(),angvelB);
- */
+ btScalar impulseClamp = m_setting.m_impulseClamp;//
+ for (j=0; j<3; j++)
+ {
+ if (m_setting.m_impulseClamp > 0)
+ {
+ info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
+ info->m_upperLimit[j*info->rowskip] = impulseClamp;
+ }
+ }
- //positional error (zeroth order error)
- btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+}
+
+
+void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+ btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+
+
+ btVector3 normal(0,0,0);
- btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
- btScalar impulseClamp = m_setting.m_impulseClamp;
- if (impulseClamp > 0)
- {
- if (impulse < -impulseClamp)
- impulse = -impulseClamp;
- if (impulse > impulseClamp)
- impulse = impulseClamp;
- }
+ // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+ // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
- 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());
+ for (int i=0;i<3;i++)
+ {
+ normal[i] = 1;
+ btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
+
+ btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+ btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+ //this jacobian entry could be re-used for all iterations
+
+ btVector3 vel1,vel2;
+ bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+ bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+ btVector3 vel = vel1 - vel2;
+
+ btScalar rel_vel;
+ rel_vel = normal.dot(vel);
+
+ /*
+ //velocity error (first order error)
+ btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+ m_rbB.getLinearVelocity(),angvelB);
+ */
- normal[i] = 0;
+ //positional error (zeroth order error)
+ btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
+
+ btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
+
+ btScalar impulseClamp = m_setting.m_impulseClamp;
+
+ const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
+ if (sum < -impulseClamp)
+ {
+ deltaImpulse = -impulseClamp-m_appliedImpulse;
+ m_appliedImpulse = -impulseClamp;
+ }
+ else if (sum > impulseClamp)
+ {
+ deltaImpulse = impulseClamp-m_appliedImpulse;
+ m_appliedImpulse = impulseClamp;
+ }
+ else
+ {
+ m_appliedImpulse = sum;
+ }
+
+
+ btVector3 impulse_vector = normal * deltaImpulse;
+
+ btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
+ btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
+ bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
+ bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);
+
+
+ normal[i] = 0;
+ }
}
}
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -50,6 +50,9 @@
public:
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
+
btConstraintSetting m_setting;
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
@@ -60,9 +63,13 @@
virtual void buildJacobian();
+ virtual void getInfo1 (btConstraintInfo1* info);
- virtual void solveConstraint(btScalar timeStep);
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
void updateRHS(btScalar timeStep);
void setPivotA(const btVector3& pivotA)
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -15,7 +15,6 @@
//#define COMPUTE_IMPULSE_DENOM 1
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
-//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
#include "btSequentialImpulseConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -32,440 +31,268 @@
#include "LinearMath/btQuickprof.h"
#include "btSolverBody.h"
#include "btSolverConstraint.h"
-
-
#include "LinearMath/btAlignedObjectArray.h"
+#include <string.h> //for memset
+btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_btSeed2(0)
+{
-int totalCpd = 0;
+}
-int gTotalContactPoints = 0;
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
+{
+}
-struct btOrderIndex
+#ifdef USE_SIMD
+#include <emmintrin.h>
+#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
+static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
{
- int m_manifoldIndex;
- int m_pointIndex;
-};
+ __m128 result = _mm_mul_ps( vec0, vec1);
+ return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
+}
+#endif//USE_SIMD
+// Project Gauss Seidel or the equivalent Sequential Impulse
+void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+#ifdef USE_SIMD
+ __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+ __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+ __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+ __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+ __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+ __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+ btSimdScalar resultLowerLess,resultUpperLess;
+ resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+ resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+ __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+ __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
+ __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+ __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+ __m128 impulseMagnitude = deltaImpulse;
+ body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+ body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+ body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+ body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+ resolveSingleConstraintRowGeneric(body1,body2,c);
+#endif
+}
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+{
+ btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+ const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+ const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
-#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
-static btOrderIndex gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+ const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
+ deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+ deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+ const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+ if (sum < c.m_lowerLimit)
+ {
+ deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_lowerLimit;
+ }
+ else if (sum > c.m_upperLimit)
+ {
+ deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_upperLimit;
+ }
+ else
+ {
+ c.m_appliedImpulse = sum;
+ }
+ if (body1.m_invMass)
+ body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+ if (body2.m_invMass)
+ body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
+}
-unsigned long btSequentialImpulseConstraintSolver::btRand2()
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
- m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
- return m_btSeed2;
+#ifdef USE_SIMD
+ __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
+ __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
+ __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
+ __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+ __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
+ __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
+ btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
+ btSimdScalar resultLowerLess,resultUpperLess;
+ resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
+ resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
+ __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
+ deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
+ c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
+ __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
+ __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
+ __m128 impulseMagnitude = deltaImpulse;
+ body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
+ body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
+ body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
+ body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+#else
+ resolveSingleConstraintRowLowerLimit(body1,body2,c);
+#endif
}
-
-
-//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
-int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
+// Project Gauss Seidel or the equivalent Sequential Impulse
+ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
- // seems good; xor-fold and modulus
- const unsigned long un = static_cast<unsigned long>(n);
- unsigned long r = btRand2();
+ btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
+ const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
+ const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
- // note: probably more aggressive than it needs to be -- might be
- // able to get away without one or two of the innermost branches.
- if (un <= 0x00010000UL) {
- r ^= (r >> 16);
- if (un <= 0x00000100UL) {
- r ^= (r >> 8);
- if (un <= 0x00000010UL) {
- r ^= (r >> 4);
- if (un <= 0x00000004UL) {
- r ^= (r >> 2);
- if (un <= 0x00000002UL) {
- r ^= (r >> 1);
- }
- }
- }
- }
- }
-
- return (int) (r % un);
+ deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
+ deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
+ const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
+ if (sum < c.m_lowerLimit)
+ {
+ deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
+ c.m_appliedImpulse = c.m_lowerLimit;
+ }
+ else
+ {
+ c.m_appliedImpulse = sum;
+ }
+ if (body1.m_invMass)
+ body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
+ if (body2.m_invMass)
+ body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
}
-
-bool MyContactDestroyedCallback(void* userPersistentData);
-bool MyContactDestroyedCallback(void* userPersistentData)
+unsigned long btSequentialImpulseConstraintSolver::btRand2()
{
- assert (userPersistentData);
- btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
- btAlignedFree(cpd);
- totalCpd--;
- //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
- return true;
+ m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
+ return m_btSeed2;
}
-btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_btSeed2(0)
+//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
+int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
{
- gContactDestroyedCallback = &MyContactDestroyedCallback;
+ // seems good; xor-fold and modulus
+ const unsigned long un = static_cast<unsigned long>(n);
+ unsigned long r = btRand2();
- //initialize default friction/contact funcs
- int i,j;
- for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
- for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
- {
-
- m_contactDispatch[i][j] = resolveSingleCollision;
- m_frictionDispatch[i][j] = resolveSingleFriction;
+ // note: probably more aggressive than it needs to be -- might be
+ // able to get away without one or two of the innermost branches.
+ if (un <= 0x00010000UL) {
+ r ^= (r >> 16);
+ if (un <= 0x00000100UL) {
+ r ^= (r >> 8);
+ if (un <= 0x00000010UL) {
+ r ^= (r >> 4);
+ if (un <= 0x00000004UL) {
+ r ^= (r >> 2);
+ if (un <= 0x00000002UL) {
+ r ^= (r >> 1);
+ }
+ }
+ }
}
+ }
+
+ return (int) (r % un);
}
-btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
-{
-}
-void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
-void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
{
- btRigidBody* rb = btRigidBody::upcast(collisionObject);
+ btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
+
+ solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
if (rb)
{
- solverBody->m_angularVelocity = rb->getAngularVelocity() ;
- solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
- solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = rb->getInvMass();
- solverBody->m_linearVelocity = rb->getLinearVelocity();
solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor();
} else
{
- solverBody->m_angularVelocity.setValue(0,0,0);
- solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
- solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = 0.f;
- solverBody->m_linearVelocity.setValue(0,0,0);
solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f;
}
- solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
- solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
}
int gNumSplitImpulseRecoveries = 0;
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
-btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
{
btScalar rest = restitution * -rel_vel;
return rest;
}
-void resolveSplitPenetrationImpulseCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo);
-//SIMD_FORCE_INLINE
-void resolveSplitPenetrationImpulseCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo)
+void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
+void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
{
- (void)solverInfo;
-
- if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
- {
-
- gNumSplitImpulseRecoveries++;
- btScalar normalImpulse;
-
- // Optimized version of projected relative velocity, use precomputed cross products with normal
- // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
- // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
- // btVector3 vel = vel1 - vel2;
- // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
- btScalar rel_vel;
- btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
- + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
- btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
- + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
-
- rel_vel = vel1Dotn-vel2Dotn;
-
-
- btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
- // btScalar positionalError = contactConstraint.m_penetration;
-
- btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
- btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
- btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
- normalImpulse = penetrationImpulse+velocityImpulse;
-
- // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
- btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
- btScalar sum = oldNormalImpulse + normalImpulse;
- contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
- normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
-
- body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
-
- body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
-
- }
-
-}
-
-
-//velocity + friction
-//response between two dynamic objects with friction
-
-btScalar resolveSingleCollisionCombinedCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo);
-
-//SIMD_FORCE_INLINE
-btScalar resolveSingleCollisionCombinedCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo)
-{
- (void)solverInfo;
-
- btScalar normalImpulse;
-
+ if (colObj && colObj->hasAnisotropicFriction())
{
-
-
- // Optimized version of projected relative velocity, use precomputed cross products with normal
- // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
- // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
- // btVector3 vel = vel1 - vel2;
- // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
-
- btScalar rel_vel;
- btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
- + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
- btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
- + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-
- rel_vel = vel1Dotn-vel2Dotn;
-
- btScalar positionalError = 0.f;
- if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
- {
- positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
- }
-
- btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
- btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
- btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
- normalImpulse = penetrationImpulse+velocityImpulse;
-
-
- // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
- btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
- btScalar sum = oldNormalImpulse + normalImpulse;
- contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
- normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
- body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
- contactConstraint.m_angularComponentA,normalImpulse);
-
- body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
- contactConstraint.m_angularComponentB,-normalImpulse);
+ // transform to local coordinates
+ btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
+ const btVector3& friction_scaling = colObj->getAnisotropicFriction();
+ //apply anisotropic friction
+ loc_lateral *= friction_scaling;
+ // ... and transform it back to global coordinates
+ frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
}
-
- return normalImpulse;
}
-#ifndef NO_FRICTION_TANGENTIALS
-btScalar resolveSingleFrictionCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo,
- btScalar appliedNormalImpulse);
-
-//SIMD_FORCE_INLINE
-btScalar resolveSingleFrictionCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- const btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo,
- btScalar appliedNormalImpulse)
-{
- (void)solverInfo;
-
-
- const btScalar combinedFriction = contactConstraint.m_friction;
-
- const btScalar limit = appliedNormalImpulse * combinedFriction;
-
- if (appliedNormalImpulse>btScalar(0.))
- //friction
- {
-
- btScalar j1;
- {
-
- btScalar rel_vel;
- const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
- + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
- const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
- + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
- rel_vel = vel1Dotn-vel2Dotn;
-
- // calculate j that moves us to zero relative velocity
- j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
-#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
-#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
- btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
- contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-
- if (limit < contactConstraint.m_appliedImpulse)
- {
- contactConstraint.m_appliedImpulse = limit;
- } else
- {
- if (contactConstraint.m_appliedImpulse < -limit)
- contactConstraint.m_appliedImpulse = -limit;
- }
- j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
-#else
- if (limit < j1)
- {
- j1 = limit;
- } else
- {
- if (j1 < -limit)
- j1 = -limit;
- }
-
-#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
-
- //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
- //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
-
-
-
- }
-
- body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-
- body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-
- }
- return 0.f;
-}
-
-
-#else
-
-//velocity + friction
-//response between two dynamic objects with friction
-btScalar resolveSingleFrictionCacheFriendly(
- btSolverBody& body1,
- btSolverBody& body2,
- btSolverConstraint& contactConstraint,
- const btContactSolverInfo& solverInfo)
-{
-
- btVector3 vel1;
- btVector3 vel2;
- btScalar normalImpulse(0.f);
-
- {
- const btVector3& normal = contactConstraint.m_contactNormal;
- if (contactConstraint.m_penetration < 0.f)
- return 0.f;
-
-
- body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
- body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
- btVector3 vel = vel1 - vel2;
- btScalar rel_vel;
- rel_vel = normal.dot(vel);
-
- btVector3 lat_vel = vel - normal * rel_vel;
- btScalar lat_rel_vel = lat_vel.length2();
-
- btScalar combinedFriction = contactConstraint.m_friction;
- const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
- const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
-
-
- if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
- {
- lat_rel_vel = btSqrt(lat_rel_vel);
-
- lat_vel /= lat_rel_vel;
- btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
- btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
- btScalar friction_impulse = lat_rel_vel /
- (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
- btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
-
- GEN_set_min(friction_impulse, normal_impulse);
- GEN_set_max(friction_impulse, -normal_impulse);
- body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
- body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
- }
- }
-
- return normalImpulse;
-}
-
-#endif //NO_FRICTION_TANGENTIALS
-
-
-
-
-
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
{
+
btRigidBody* body0=btRigidBody::upcast(colObj0);
btRigidBody* body1=btRigidBody::upcast(colObj1);
- btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+ btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
+ memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
- solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
solverConstraint.m_frictionIndex = frictionIndex;
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = 0;
- solverConstraint.m_appliedImpulse = btScalar(0.);
- solverConstraint.m_appliedPushImpulse = 0.f;
- solverConstraint.m_penetration = 0.f;
+ solverConstraint.m_appliedImpulse = 0.f;
+ // solverConstraint.m_appliedPushImpulse = 0.f;
+
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
- solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+ solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
}
{
- btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+ btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
- solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+ solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
}
#ifdef COMPUTE_IMPULSE_DENOM
@@ -482,7 +309,7 @@
}
if (body1)
{
- vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec);
}
@@ -491,352 +318,333 @@
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
- return solverConstraint;
-}
+#ifdef _USE_JACOBIAN
+ solverConstraint.m_jac = btJacobianEntry (
+ rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
+ body0->getInvInertiaDiagLocal(),
+ body0->getInvMass(),
+ body1->getInvInertiaDiagLocal(),
+ body1->getInvMass());
+#endif //_USE_JACOBIAN
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
-{
- BT_PROFILE("solveGroupCacheFriendlySetup");
- (void)stackAlloc;
- (void)debugDrawer;
+ rel_vel = vel1Dotn+vel2Dotn;
+ btScalar positionalError = 0.f;
- if (!(numConstraints + numManifolds))
+ btSimdScalar velocityError = - rel_vel;
+ btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+ solverConstraint.m_rhs = velocityImpulse;
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = 0;
+ solverConstraint.m_upperLimit = 1e10f;
+ }
+
+ return solverConstraint;
+}
+
+int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
+{
+ int solverBodyIdA = -1;
+
+ if (body.getCompanionId() >= 0)
{
-// printf("empty\n");
- return 0.f;
+ //body has already been converted
+ solverBodyIdA = body.getCompanionId();
+ } else
+ {
+ btRigidBody* rb = btRigidBody::upcast(&body);
+ if (rb && rb->getInvMass())
+ {
+ solverBodyIdA = m_tmpSolverBodyPool.size();
+ btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+ initSolverBody(&solverBody,&body);
+ body.setCompanionId(solverBodyIdA);
+ } else
+ {
+ return 0;//assume first one is a fixed solver body
+ }
}
- btPersistentManifold* manifold = 0;
- btCollisionObject* colObj0=0,*colObj1=0;
+ return solverBodyIdA;
+}
+#include <stdio.h>
- //btRigidBody* rb0=0,*rb1=0;
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+ btCollisionObject* colObj0=0,*colObj1=0;
- BEGIN_PROFILE("refreshManifolds");
+ colObj0 = (btCollisionObject*)manifold->getBody0();
+ colObj1 = (btCollisionObject*)manifold->getBody1();
- int i;
-
-
+ int solverBodyIdA=-1;
+ int solverBodyIdB=-1;
- for (i=0;i<numManifolds;i++)
+ if (manifold->getNumContacts())
{
- manifold = manifoldPtr[i];
- rb1 = (btRigidBody*)manifold->getBody1();
- rb0 = (btRigidBody*)manifold->getBody0();
-
- manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
-
+ solverBodyIdA = getOrInitSolverBody(*colObj0);
+ solverBodyIdB = getOrInitSolverBody(*colObj1);
}
- END_PROFILE("refreshManifolds");
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+ ///avoid collision response between two static objects
+ if (!solverBodyIdA && !solverBodyIdB)
+ return;
-
+ btVector3 rel_pos1;
+ btVector3 rel_pos2;
+ btScalar relaxation;
+ for (int j=0;j<manifold->getNumContacts();j++)
+ {
+ btManifoldPoint& cp = manifold->getContactPoint(j);
- //int sizeofSB = sizeof(btSolverBody);
- //int sizeofSC = sizeof(btSolverConstraint);
+ if (cp.getDistance() <= manifold->getContactProcessingThreshold())
+ {
+ const btVector3& pos1 = cp.getPositionWorldOnA();
+ const btVector3& pos2 = cp.getPositionWorldOnB();
- //if (1)
- {
- //if m_stackAlloc, try to pack bodies/constraints to speed up solving
-// btBlock* sablock;
-// sablock = stackAlloc->beginBlock();
+ rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
+ rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
- // int memsize = 16;
-// unsigned char* stackMemory = stackAlloc->allocate(memsize);
-
- //todo: use stack allocator for this temp memory
-// int minReservation = numManifolds*2;
+ relaxation = 1.f;
+ btScalar rel_vel;
+ btVector3 vel;
- //m_tmpSolverBodyPool.reserve(minReservation);
+ int frictionIndex = m_tmpSolverContactConstraintPool.size();
- //don't convert all bodies, only the one we need so solver the constraints
-/*
- {
- for (int i=0;i<numBodies;i++)
{
- btRigidBody* rb = btRigidBody::upcast(bodies[i]);
- if (rb && (rb->getIslandTag() >= 0))
- {
- btAssert(rb->getCompanionId() < 0);
- int solverBodyId = m_tmpSolverBodyPool.size();
- btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
- initSolverBody(&solverBody,rb);
- rb->setCompanionId(solverBodyId);
- }
- }
- }
-*/
-
- //m_tmpSolverConstraintPool.reserve(minReservation);
- //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
+ btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+ btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+ btRigidBody* rb1 = btRigidBody::upcast(colObj1);
- {
- int i;
+ solverConstraint.m_solverBodyIdA = solverBodyIdA;
+ solverConstraint.m_solverBodyIdB = solverBodyIdB;
- for (i=0;i<numManifolds;i++)
- {
- manifold = manifoldPtr[i];
- colObj0 = (btCollisionObject*)manifold->getBody0();
- colObj1 = (btCollisionObject*)manifold->getBody1();
-
- int solverBodyIdA=-1;
- int solverBodyIdB=-1;
+ solverConstraint.m_originalContactPoint = &cp;
- if (manifold->getNumContacts())
+ btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+ btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
{
-
-
-
- if (colObj0->getIslandTag() >= 0)
+#ifdef COMPUTE_IMPULSE_DENOM
+ btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+ btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else
+ btVector3 vec;
+ btScalar denom0 = 0.f;
+ btScalar denom1 = 0.f;
+ if (rb0)
{
- if (colObj0->getCompanionId() >= 0)
- {
- //body has already been converted
- solverBodyIdA = colObj0->getCompanionId();
- } else
- {
- solverBodyIdA = m_tmpSolverBodyPool.size();
- btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
- initSolverBody(&solverBody,colObj0);
- colObj0->setCompanionId(solverBodyIdA);
- }
- } else
- {
- //create a static body
- solverBodyIdA = m_tmpSolverBodyPool.size();
- btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
- initSolverBody(&solverBody,colObj0);
+ vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+ denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
-
- if (colObj1->getIslandTag() >= 0)
+ if (rb1)
{
- if (colObj1->getCompanionId() >= 0)
- {
- solverBodyIdB = colObj1->getCompanionId();
- } else
- {
- solverBodyIdB = m_tmpSolverBodyPool.size();
- btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
- initSolverBody(&solverBody,colObj1);
- colObj1->setCompanionId(solverBodyIdB);
- }
- } else
- {
- //create a static body
- solverBodyIdB = m_tmpSolverBodyPool.size();
- btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
- initSolverBody(&solverBody,colObj1);
+ vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+ denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
+#endif //COMPUTE_IMPULSE_DENOM
+
+ btScalar denom = relaxation/(denom0+denom1);
+ solverConstraint.m_jacDiagABInv = denom;
}
- btVector3 rel_pos1;
- btVector3 rel_pos2;
- btScalar relaxation;
+ solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+ solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+ solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
- for (int j=0;j<manifold->getNumContacts();j++)
+
+ btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+ btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+ vel = vel1 - vel2;
+
+ rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+ btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
+
+
+ solverConstraint.m_friction = cp.m_combinedFriction;
+
+ btScalar restitution = 0.f;
+
+ if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
{
-
- btManifoldPoint& cp = manifold->getContactPoint(j);
-
- if (cp.getDistance() <= btScalar(0.))
+ restitution = 0.f;
+ } else
+ {
+ restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
+ if (restitution <= btScalar(0.))
{
-
- const btVector3& pos1 = cp.getPositionWorldOnA();
- const btVector3& pos2 = cp.getPositionWorldOnB();
+ restitution = 0.f;
+ };
+ }
- rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
- rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-
- relaxation = 1.f;
- btScalar rel_vel;
- btVector3 vel;
+ ///warm starting (or zero if disabled)
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+ {
+ solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+ if (rb0)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+ if (rb1)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+ } else
+ {
+ solverConstraint.m_appliedImpulse = 0.f;
+ }
- int frictionIndex = m_tmpSolverConstraintPool.size();
+ // solverConstraint.m_appliedPushImpulse = 0.f;
- {
- btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
- btRigidBody* rb0 = btRigidBody::upcast(colObj0);
- btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
+ + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
- solverConstraint.m_solverBodyIdA = solverBodyIdA;
- solverConstraint.m_solverBodyIdB = solverBodyIdB;
- solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
+ rel_vel = vel1Dotn+vel2Dotn;
- solverConstraint.m_originalContactPoint = &cp;
+ btScalar positionalError = 0.f;
+ positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+ btScalar velocityError = restitution - rel_vel;// * damping;
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_cfm = 0.f;
+ solverConstraint.m_lowerLimit = 0;
+ solverConstraint.m_upperLimit = 1e10f;
+ }
- btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
- solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
- btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
- solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
- {
-#ifdef COMPUTE_IMPULSE_DENOM
- btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
- btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else
- btVector3 vec;
- btScalar denom0 = 0.f;
- btScalar denom1 = 0.f;
- if (rb0)
- {
- vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
- denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
- }
- if (rb1)
- {
- vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
- denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
- }
-#endif //COMPUTE_IMPULSE_DENOM
-
- btScalar denom = relaxation/(denom0+denom1);
- solverConstraint.m_jacDiagABInv = denom;
- }
- solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
- solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
- solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
+ /////setup the friction constraints
- btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
- btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-
- vel = vel1 - vel2;
-
- rel_vel = cp.m_normalWorldOnB.dot(vel);
-
- solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
- //solverConstraint.m_penetration = cp.getDistance();
- solverConstraint.m_friction = cp.m_combinedFriction;
-
-
- if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+ if (1)
+ {
+ solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+ if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+ {
+ cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+ btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+ if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+ {
+ cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
- solverConstraint.m_restitution = 0.f;
- } else
- {
- solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
- if (solverConstraint.m_restitution <= btScalar(0.))
- {
- solverConstraint.m_restitution = 0.f;
- };
+ cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+ cp.m_lateralFrictionDir2.normalize();//??
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
+ cp.m_lateralFrictionInitialized = true;
+ } else
+ {
+ //re-calculate friction direction every frame, todo: check if this is really needed
+ btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
-
- btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
-
-
-
- if (solverConstraint.m_restitution > penVel)
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
- solverConstraint.m_penetration = btScalar(0.);
+ applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
+ applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
-
-
-
- ///warm starting (or zero if disabled)
+ cp.m_lateralFrictionInitialized = true;
+ }
+
+ } else
+ {
+ addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ }
+
+ if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+ {
+ {
+ btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
- solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+ frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
if (rb1)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
} else
{
- solverConstraint.m_appliedImpulse = 0.f;
+ frictionConstraint1.m_appliedImpulse = 0.f;
}
+ }
- solverConstraint.m_appliedPushImpulse = 0.f;
-
- solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
- if (!cp.m_lateralFrictionInitialized)
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+ if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
- cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
- btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
- if (lat_rel_vel > SIMD_EPSILON)//0.0f)
- {
- cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
- addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
- {
- cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
- cp.m_lateralFrictionDir2.normalize();//??
- addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- cp.m_lateralFrictionInitialized = true;
- }
- } else
- {
- //re-calculate friction direction every frame, todo: check if this is really needed
- btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
- addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
- {
- cp.m_lateralFrictionInitialized = true;
- }
- }
-
+ frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+ if (rb0)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+ if (rb1)
+ m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
} else
{
- addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
- if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
- addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+ frictionConstraint2.m_appliedImpulse = 0.f;
}
-
- if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
- {
- {
- btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
- if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
- {
- frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
- if (rb0)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
- if (rb1)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
- } else
- {
- frictionConstraint1.m_appliedImpulse = 0.f;
- }
- }
- {
- btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
- if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
- {
- frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
- if (rb0)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
- if (rb1)
- m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
- } else
- {
- frictionConstraint2.m_appliedImpulse = 0.f;
- }
- }
- }
}
-
-
+ } else
+ {
+ btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+ frictionConstraint1.m_appliedImpulse = 0.f;
+ if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+ {
+ btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+ frictionConstraint2.m_appliedImpulse = 0.f;
+ }
}
}
}
+
+
}
}
-
- btContactSolverInfo info = infoGlobal;
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+ BT_PROFILE("solveGroupCacheFriendlySetup");
+ (void)stackAlloc;
+ (void)debugDrawer;
+
+
+ if (!(numConstraints + numManifolds))
{
+ // printf("empty\n");
+ return 0.f;
+ }
+
+ if (1)
+ {
int j;
for (j=0;j<numConstraints;j++)
{
@@ -844,12 +652,164 @@
constraint->buildJacobian();
}
}
-
-
- int numConstraintPool = m_tmpSolverConstraintPool.size();
- int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+ btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+ initSolverBody(&fixedBody,0);
+ //btRigidBody* rb0=0,*rb1=0;
+
+ //if (1)
+ {
+ {
+
+ int totalNumRows = 0;
+ int i;
+ //calculate the total number of contraint rows
+ for (i=0;i<numConstraints;i++)
+ {
+
+ btTypedConstraint::btConstraintInfo1 info1;
+ constraints[i]->getInfo1(&info1);
+ totalNumRows += info1.m_numConstraintRows;
+ }
+ m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
+
+ btTypedConstraint::btConstraintInfo1 info1;
+ info1.m_numConstraintRows = 0;
+
+
+ ///setup the btSolverConstraints
+ int currentRow = 0;
+
+ for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
+ {
+ constraints[i]->getInfo1(&info1);
+ if (info1.m_numConstraintRows)
+ {
+ btAssert(currentRow<totalNumRows);
+
+ btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+ btTypedConstraint* constraint = constraints[i];
+
+
+
+ btRigidBody& rbA = constraint->getRigidBodyA();
+ btRigidBody& rbB = constraint->getRigidBodyB();
+
+ int solverBodyIdA = getOrInitSolverBody(rbA);
+ int solverBodyIdB = getOrInitSolverBody(rbB);
+
+ btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
+ btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+
+ int j;
+ for ( j=0;j<info1.m_numConstraintRows;j++)
+ {
+ memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
+ currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
+ currentConstraintRow[j].m_upperLimit = FLT_MAX;
+ currentConstraintRow[j].m_appliedImpulse = 0.f;
+ currentConstraintRow[j].m_appliedPushImpulse = 0.f;
+ currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
+ currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
+ }
+
+ bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+ bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
+ bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
+
+
+
+ btTypedConstraint::btConstraintInfo2 info2;
+ info2.fps = 1.f/infoGlobal.m_timeStep;
+ info2.erp = infoGlobal.m_erp;
+ info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
+ info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
+ info2.m_J2linearAxis = 0;
+ info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
+ info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
+ ///the size of btSolverConstraint needs be a multiple of btScalar
+ btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
+ info2.m_constraintError = ¤tConstraintRow->m_rhs;
+ info2.cfm = ¤tConstraintRow->m_cfm;
+ info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
+ info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
+ constraints[i]->getInfo2(&info2);
+
+ ///finalize the constraint setup
+ for ( j=0;j<info1.m_numConstraintRows;j++)
+ {
+ btSolverConstraint& solverConstraint = currentConstraintRow[j];
+
+ {
+ const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
+ solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
+ }
+ {
+ const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
+ solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
+ }
+
+ {
+ btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
+ btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
+ btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
+ btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
+
+ btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
+ sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+ sum += iMJlB.dot(solverConstraint.m_contactNormal);
+ sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+
+ solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+ }
+
+
+ ///fix rhs
+ ///todo: add force/torque accelerators
+ {
+ btScalar rel_vel;
+ btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
+ btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
+
+ rel_vel = vel1Dotn+vel2Dotn;
+
+ btScalar restitution = 0.f;
+ btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
+ btScalar velocityError = restitution - rel_vel;// * damping;
+ btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+ btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+ solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+ solverConstraint.m_appliedImpulse = 0.f;
+
+ }
+ }
+ }
+ }
+ }
+
+ {
+ int i;
+ btPersistentManifold* manifold = 0;
+ btCollisionObject* colObj0=0,*colObj1=0;
+
+
+ for (i=0;i<numManifolds;i++)
+ {
+ manifold = manifoldPtr[i];
+ convertContact(manifold,infoGlobal);
+ }
+ }
+ }
+
+ btContactSolverInfo info = infoGlobal;
+
+
+
+ int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+ int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderTmpConstraintPool.resize(numConstraintPool);
m_orderFrictionConstraintPool.resize(numFrictionPool);
@@ -865,8 +825,6 @@
}
}
-
-
return 0.f;
}
@@ -874,9 +832,10 @@
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
- int numConstraintPool = m_tmpSolverConstraintPool.size();
- int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
+ int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+ int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
//should traverse the contacts random order...
int iteration;
{
@@ -903,113 +862,134 @@
}
}
- for (j=0;j<numConstraints;j++)
+ if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
- btTypedConstraint* constraint = constraints[j];
- ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
-
- if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
+ ///solve all joint constraints, using SIMD, if available
+ for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
- m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
+ btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+ resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
}
- if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
+
+ for (j=0;j<numConstraints;j++)
{
- m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
+ int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+ int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+ btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+ btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+ constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
- constraint->solveConstraint(infoGlobal.m_timeStep);
-
- if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
+ ///solve all contact constraints using SIMD, if available
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ for (j=0;j<numPoolConstraints;j++)
{
- m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+ resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+
}
- if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
+ ///solve all friction constraints, using SIMD, if available
+ int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+ for (j=0;j<numFrictionPoolConstraints;j++)
{
- m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
- }
+ btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+ btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
- }
+ if (totalImpulse>btScalar(0))
+ {
+ solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+ solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+ resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ }
+ } else
{
- int numPoolConstraints = m_tmpSolverConstraintPool.size();
- for (j=0;j<numPoolConstraints;j++)
+
+ ///solve all joint constraints
+ for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
-
- const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
- resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
- m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+ btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
+ resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
}
- }
- {
- int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
-
- for (j=0;j<numFrictionPoolConstraints;j++)
+ for (j=0;j<numConstraints;j++)
{
- const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
- btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
- m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
+ int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
+ int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
+ btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+ btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
- resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
- m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
- totalImpulse);
+ constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
- }
-
-
- }
-
- if (infoGlobal.m_splitImpulse)
- {
-
- for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
- {
+ ///solve all contact constraints
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+ for (j=0;j<numPoolConstraints;j++)
{
- int numPoolConstraints = m_tmpSolverConstraintPool.size();
- int j;
- for (j=0;j<numPoolConstraints;j++)
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+ resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+ }
+ ///solve all friction constraints
+ int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+ for (j=0;j<numFrictionPoolConstraints;j++)
+ {
+ btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+ btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+ if (totalImpulse>btScalar(0))
{
- const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+ solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+ solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
- resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
- m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+ resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
}
- }
- }
+ }
+ }
return 0.f;
}
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
{
+
+
+
+ BT_PROFILE("solveGroup");
+ //we only implement SOLVER_CACHE_FRIENDLY now
+ //you need to provide at least some bodies
+ btAssert(bodies);
+ btAssert(numBodies);
+
int i;
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
- int numPoolConstraints = m_tmpSolverConstraintPool.size();
+ int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
+
for (j=0;j<numPoolConstraints;j++)
{
-
- const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+
+ const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{
- pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
- pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+ pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+ pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
}
//do a callback here?
-
}
if (infoGlobal.m_splitImpulse)
@@ -1021,425 +1001,28 @@
} else
{
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
- {
- m_tmpSolverBodyPool[i].writebackVelocity();
- }
+ {
+ m_tmpSolverBodyPool[i].writebackVelocity();
+ }
}
-// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
-/*
- printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
- printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
- printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
-
-
- printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
- printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
- printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
-*/
-
m_tmpSolverBodyPool.resize(0);
- m_tmpSolverConstraintPool.resize(0);
- m_tmpSolverFrictionConstraintPool.resize(0);
+ m_tmpSolverContactConstraintPool.resize(0);
+ m_tmpSolverNonContactConstraintPool.resize(0);
+ m_tmpSolverContactFrictionConstraintPool.resize(0);
-
return 0.f;
}
-/// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
-{
- BT_PROFILE("solveGroup");
- if (infoGlobal.m_solverMode & 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);
- }
-
- btContactSolverInfo info = infoGlobal;
- int numiter = infoGlobal.m_numIterations;
- int totalPoints = 0;
- {
- short j;
- for (j=0;j<numManifolds;j++)
- {
- btPersistentManifold* manifold = manifoldPtr[j];
- prepareConstraints(manifold,info,debugDrawer);
- for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
- {
- gOrder[totalPoints].m_manifoldIndex = j;
- gOrder[totalPoints].m_pointIndex = p;
- totalPoints++;
- }
- }
- }
- {
- int j;
- for (j=0;j<numConstraints;j++)
- {
- btTypedConstraint* constraint = constraints[j];
- constraint->buildJacobian();
- }
- }
-
-
- //should traverse the contacts random order...
- int iteration;
-
- {
- for ( iteration = 0;iteration<numiter;iteration++)
- {
- int j;
- if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
- {
- if ((iteration & 7) == 0) {
- for (j=0; j<totalPoints; ++j) {
- btOrderIndex tmp = gOrder[j];
- int swapi = btRandInt2(j+1);
- gOrder[j] = gOrder[swapi];
- gOrder[swapi] = tmp;
- }
- }
- }
-
- for (j=0;j<numConstraints;j++)
- {
- btTypedConstraint* constraint = constraints[j];
- constraint->solveConstraint(info.m_timeStep);
- }
-
- for (j=0;j<totalPoints;j++)
- {
- btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
- solve( (btRigidBody*)manifold->getBody0(),
- (btRigidBody*)manifold->getBody1()
- ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
- }
-
- for (j=0;j<totalPoints;j++)
- {
- btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
- solveFriction((btRigidBody*)manifold->getBody0(),
- (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
- }
-
- }
- }
-
-
-
-
- return btScalar(0.);
-}
-
-
-
-
-
-
-
-void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
-{
-
- (void)debugDrawer;
-
- btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
- btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
-
-
- //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
- {
-#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
- manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-#endif //FORCE_REFESH_CONTACT_MANIFOLDS
- int numpoints = manifoldPtr->getNumContacts();
-
- gTotalContactPoints += numpoints;
-
-
- for (int i=0;i<numpoints ;i++)
- {
- btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
- if (cp.getDistance() <= btScalar(0.))
- {
- const btVector3& pos1 = cp.getPositionWorldOnA();
- const btVector3& pos2 = cp.getPositionWorldOnB();
-
- btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
- btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
-
-
- //this jacobian entry is re-used for all iterations
- btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
- body1->getCenterOfMassTransform().getBasis().transpose(),
- rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
- body1->getInvInertiaDiagLocal(),body1->getInvMass());
-
-
- btScalar jacDiagAB = jac.getDiagonal();
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
- if (cpd)
- {
- //might be invalid
- cpd->m_persistentLifeTime++;
- if (cpd->m_persistentLifeTime != cp.getLifeTime())
- {
- //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
- new (cpd) btConstraintPersistentData;
- cpd->m_persistentLifeTime = cp.getLifeTime();
-
- } else
- {
- //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
-
- }
- } else
- {
-
- //todo: should this be in a pool?
- void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
- cpd = new (mem)btConstraintPersistentData;
- assert(cpd);
-
- totalCpd ++;
- //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
- cp.m_userPersistentData = cpd;
- cpd->m_persistentLifeTime = cp.getLifeTime();
- //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
-
- }
- assert(cpd);
-
- cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
-
- //Dependent on Rigidbody A and B types, fetch the contact/friction response func
- //perhaps do a similar thing for friction/restutution combiner funcs...
-
- cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
- cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
-
- btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
- btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
- btVector3 vel = vel1 - vel2;
- btScalar rel_vel;
- rel_vel = cp.m_normalWorldOnB.dot(vel);
-
- btScalar combinedRestitution = cp.m_combinedRestitution;
-
- cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
- cpd->m_friction = cp.m_combinedFriction;
- if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
- {
- cpd->m_restitution = 0.f;
- } else
- {
- cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
- if (cpd->m_restitution <= btScalar(0.))
- {
- cpd->m_restitution = btScalar(0.0);
- };
- }
-
- //restitution and penetration work in same direction so
- //rel_vel
-
- btScalar penVel = -cpd->m_penetration/info.m_timeStep;
-
- if (cpd->m_restitution > penVel)
- {
- cpd->m_penetration = btScalar(0.);
- }
-
-
- btScalar relaxation = info.m_damping;
- if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
- {
- cpd->m_appliedImpulse *= relaxation;
- } else
- {
- cpd->m_appliedImpulse =btScalar(0.);
- }
-
- //for friction
- cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
-
- //re-calculate friction direction every frame, todo: check if this is really needed
- btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
-
-
-#define NO_FRICTION_WARMSTART 1
-
- #ifdef NO_FRICTION_WARMSTART
- cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
- cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
- #endif //NO_FRICTION_WARMSTART
- btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
- btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
- btScalar denom = relaxation/(denom0+denom1);
- cpd->m_jacDiagABInvTangent0 = denom;
-
-
- denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
- denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
- denom = relaxation/(denom0+denom1);
- cpd->m_jacDiagABInvTangent1 = denom;
-
- btVector3 totalImpulse =
- #ifndef NO_FRICTION_WARMSTART
- cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
- cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
- #endif //NO_FRICTION_WARMSTART
- cp.m_normalWorldOnB*cpd->m_appliedImpulse;
-
-
-
- ///
- {
- btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
- cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
- btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
- cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
- }
- {
- btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
- cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
- }
- {
- btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
- cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
- }
- {
- btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
- cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
- }
- {
- btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
- cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
- }
-
- ///
-
-
-
- //apply previous frames impulse on both bodies
- body0->applyImpulse(totalImpulse, rel_pos1);
- body1->applyImpulse(-totalImpulse, rel_pos2);
- }
-
- }
- }
-}
-
-
-btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
- btScalar maxImpulse = btScalar(0.);
-
- {
-
-
- {
- if (cp.getDistance() <= btScalar(0.))
- {
-
-
-
- {
-
- //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
- btScalar impulse = resolveSingleCollisionCombined(
- *body0,*body1,
- cp,
- info);
-
- if (maxImpulse < impulse)
- maxImpulse = impulse;
-
- }
- }
- }
- }
- return maxImpulse;
-}
-
-
-
-btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
- btScalar maxImpulse = btScalar(0.);
-
- {
-
-
- {
- if (cp.getDistance() <= btScalar(0.))
- {
-
-
-
- {
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
- btScalar impulse = cpd->m_contactSolverFunc(
- *body0,*body1,
- cp,
- info);
-
- if (maxImpulse < impulse)
- maxImpulse = impulse;
-
- }
- }
- }
- }
- return maxImpulse;
-}
-
-btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
-{
-
- (void)debugDrawer;
- (void)iter;
-
-
- {
-
-
- {
-
- if (cp.getDistance() <= btScalar(0.))
- {
-
- btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
- cpd->m_frictionSolverFunc(
- *body0,*body1,
- cp,
- info);
-
-
- }
- }
-
-
- }
- return btScalar(0.);
-}
-
-
void btSequentialImpulseConstraintSolver::reset()
{
m_btSeed2 = 0;
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -23,67 +23,59 @@
#include "btSolverConstraint.h"
-///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
-///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
-///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
-///Applies impulses for combined restitution and penetration recovery and to simulate friction
+
+///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
class btSequentialImpulseConstraintSolver : public btConstraintSolver
{
+protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
- btAlignedObjectArray<btSolverConstraint> m_tmpSolverConstraintPool;
- btAlignedObjectArray<btSolverConstraint> m_tmpSolverFrictionConstraintPool;
+ btConstraintArray m_tmpSolverContactConstraintPool;
+ btConstraintArray m_tmpSolverNonContactConstraintPool;
+ btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
-
-protected:
- btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
- btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
- void prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
-
- ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
- ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
-
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
-public:
+ void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
-
- btSequentialImpulseConstraintSolver();
+ void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
- ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
- ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
- void setContactSolverFunc(ContactSolverFunc func,int type0,int type1)
- {
- m_contactDispatch[type0][type1] = func;
- }
+ void resolveSplitPenetrationImpulseCacheFriendly(
+ btSolverBody& body1,
+ btSolverBody& body2,
+ const btSolverConstraint& contactConstraint,
+ const btContactSolverInfo& solverInfo);
+
+ //internal method
+ int getOrInitSolverBody(btCollisionObject& body);
+
+ void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+ void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
- ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
- ///See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
- void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1)
- {
- m_frictionDispatch[type0][type1] = func;
- }
+ void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+ void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
+
+public:
- virtual ~btSequentialImpulseConstraintSolver();
- virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+ btSequentialImpulseConstraintSolver();
+ virtual ~btSequentialImpulseConstraintSolver();
- virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
- btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+ virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+ btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
-
///clear internal cached data and reset random seed
virtual void reset();
-
- btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
-
-
unsigned long btRand2();
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -68,7 +68,9 @@
btSliderConstraint::btSliderConstraint()
:btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
- m_useLinearReferenceFrameA(true)
+ m_useLinearReferenceFrameA(true),
+ m_useSolveConstraintObsolete(false)
+// m_useSolveConstraintObsolete(true)
{
initParams();
} // btSliderConstraint::btSliderConstraint()
@@ -79,7 +81,9 @@
: btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
- m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+ m_useLinearReferenceFrameA(useLinearReferenceFrameA),
+ m_useSolveConstraintObsolete(false)
+// m_useSolveConstraintObsolete(true)
{
initParams();
} // btSliderConstraint::btSliderConstraint()
@@ -88,6 +92,10 @@
void btSliderConstraint::buildJacobian()
{
+ if (!m_useSolveConstraintObsolete)
+ {
+ return;
+ }
if(m_useLinearReferenceFrameA)
{
buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
@@ -155,27 +163,372 @@
//-----------------------------------------------------------------------------
-void btSliderConstraint::solveConstraint(btScalar timeStep)
+void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{
- m_timeStep = timeStep;
- if(m_useLinearReferenceFrameA)
+ if (m_useSolveConstraintObsolete)
{
- solveConstraintInt(m_rbA, m_rbB);
+ info->m_numConstraintRows = 0;
+ info->nub = 0;
}
else
{
- solveConstraintInt(m_rbB, m_rbA);
+ info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
+ info->nub = 2;
+ //prepare constraint
+ calculateTransforms();
+ testLinLimits();
+ if(getSolveLinLimit() || getPoweredLinMotor())
+ {
+ info->m_numConstraintRows++; // limit 3rd linear as well
+ info->nub--;
+ }
+ testAngLimits();
+ if(getSolveAngLimit() || getPoweredAngMotor())
+ {
+ info->m_numConstraintRows++; // limit 3rd angular as well
+ info->nub--;
+ }
}
+} // btSliderConstraint::getInfo1()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::getInfo2(btConstraintInfo2* info)
+{
+ btAssert(!m_useSolveConstraintObsolete);
+ int i, s = info->rowskip;
+ const btTransform& trA = getCalculatedTransformA();
+ const btTransform& trB = getCalculatedTransformB();
+ btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
+ // make rotations around Y and Z equal
+ // the slider axis should be the only unconstrained
+ // rotational axis, the angular velocity of the two bodies perpendicular to
+ // the slider axis should be equal. thus the constraint equations are
+ // p*w1 - p*w2 = 0
+ // q*w1 - q*w2 = 0
+ // where p and q are unit vectors normal to the slider axis, and w1 and w2
+ // are the angular velocity vectors of the two bodies.
+ // get slider axis (X)
+ btVector3 ax1 = trA.getBasis().getColumn(0);
+ // get 2 orthos to slider axis (Y, Z)
+ btVector3 p = trA.getBasis().getColumn(1);
+ btVector3 q = trA.getBasis().getColumn(2);
+ // set the two slider rows
+ info->m_J1angularAxis[0] = p[0];
+ info->m_J1angularAxis[1] = p[1];
+ info->m_J1angularAxis[2] = p[2];
+ info->m_J1angularAxis[s+0] = q[0];
+ info->m_J1angularAxis[s+1] = q[1];
+ info->m_J1angularAxis[s+2] = q[2];
+
+ info->m_J2angularAxis[0] = -p[0];
+ info->m_J2angularAxis[1] = -p[1];
+ info->m_J2angularAxis[2] = -p[2];
+ info->m_J2angularAxis[s+0] = -q[0];
+ info->m_J2angularAxis[s+1] = -q[1];
+ info->m_J2angularAxis[s+2] = -q[2];
+ // compute the right hand side of the constraint equation. set relative
+ // body velocities along p and q to bring the slider back into alignment.
+ // if ax1,ax2 are the unit length slider axes as computed from body1 and
+ // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+ // if "theta" is the angle between ax1 and ax2, we need an angular velocity
+ // along u to cover angle erp*theta in one step :
+ // |angular_velocity| = angle/time = erp*theta / stepsize
+ // = (erp*fps) * theta
+ // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+ // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+ // ...as ax1 and ax2 are unit length. if theta is smallish,
+ // theta ~= sin(theta), so
+ // angular_velocity = (erp*fps) * (ax1 x ax2)
+ // ax1 x ax2 is in the plane space of ax1, so we project the angular
+ // velocity to p and q to find the right hand side.
+ btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
+ btVector3 ax2 = trB.getBasis().getColumn(0);
+ btVector3 u = ax1.cross(ax2);
+ info->m_constraintError[0] = k * u.dot(p);
+ info->m_constraintError[s] = k * u.dot(q);
+ // pull out pos and R for both bodies. also get the connection
+ // vector c = pos2-pos1.
+ // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
+ // result in three equations, so we project along the planespace vectors
+ // so that sliding along the slider axis is disregarded. for symmetry we
+ // also consider rotation around center of mass of two bodies (factA and factB).
+ btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
+ btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
+ int s2 = 2 * s, s3 = 3 * s;
+ btVector3 c;
+ btScalar miA = m_rbA.getInvMass();
+ btScalar miB = m_rbB.getInvMass();
+ btScalar miS = miA + miB;
+ btScalar factA, factB;
+ if(miS > btScalar(0.f))
+ {
+ factA = miB / miS;
+ }
+ else
+ {
+ factA = btScalar(0.5f);
+ }
+ if(factA > 0.99f) factA = 0.99f;
+ if(factA < 0.01f) factA = 0.01f;
+ factB = btScalar(1.0f) - factA;
+ c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+ btVector3 tmp = c.cross(p);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+ tmp = c.cross(q);
+ for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+ for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+ for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+ for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+ // compute two elements of right hand side. we want to align the offset
+ // point (in body 2's frame) with the center of body 1.
+ btVector3 ofs; // offset point in global coordinates
+ ofs = trB.getOrigin() - trA.getOrigin();
+ k = info->fps * info->erp * getSoftnessOrthoLin();
+ info->m_constraintError[s2] = k * p.dot(ofs);
+ info->m_constraintError[s3] = k * q.dot(ofs);
+ int nrow = 3; // last filled row
+ int srow;
+ // check linear limits linear
+ btScalar limit_err = btScalar(0.0);
+ int limit = 0;
+ if(getSolveLinLimit())
+ {
+ limit_err = getLinDepth() * signFact;
+ limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+ }
+ int powered = 0;
+ if(getPoweredLinMotor())
+ {
+ powered = 1;
+ }
+ // if the slider has joint limits or motor, add in the extra row
+ if (limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1linearAxis[srow+0] = ax1[0];
+ info->m_J1linearAxis[srow+1] = ax1[1];
+ info->m_J1linearAxis[srow+2] = ax1[2];
+ // linear torque decoupling step:
+ //
+ // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
+ // do not create a torque couple. in other words, the points that the
+ // constraint force is applied at must lie along the same ax1 axis.
+ // a torque couple will result in limited slider-jointed free
+ // bodies from gaining angular momentum.
+ // the solution used here is to apply the constraint forces at the center of mass of the two bodies
+ btVector3 ltd; // Linear Torque Decoupling vector (a torque)
+// c = btScalar(0.5) * c;
+ ltd = c.cross(ax1);
+ info->m_J1angularAxis[srow+0] = factA*ltd[0];
+ info->m_J1angularAxis[srow+1] = factA*ltd[1];
+ info->m_J1angularAxis[srow+2] = factA*ltd[2];
+ info->m_J2angularAxis[srow+0] = factB*ltd[0];
+ info->m_J2angularAxis[srow+1] = factB*ltd[1];
+ info->m_J2angularAxis[srow+2] = factB*ltd[2];
+ // right-hand part
+ btScalar lostop = getLowerLinLimit();
+ btScalar histop = getUpperLinLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ info->m_constraintError[srow] = 0.;
+ info->m_lowerLimit[srow] = 0.;
+ info->m_upperLimit[srow] = 0.;
+ if(powered)
+ {
+ info->cfm[nrow] = btScalar(0.0);
+ btScalar tag_vel = getTargetLinMotorVelocity();
+ btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
+// info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
+ info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
+ info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
+ info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ { // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
+ vel -= m_rbB.getLinearVelocity().dot(ax1);
+ vel *= signFact;
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if (newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimLin();
+ } // if(limit)
+ } // if linear limit
+ // check angular limits
+ limit_err = btScalar(0.0);
+ limit = 0;
+ if(getSolveAngLimit())
+ {
+ limit_err = getAngDepth();
+ limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+ }
+ // if the slider has joint limits, add in the extra row
+ powered = 0;
+ if(getPoweredAngMotor())
+ {
+ powered = 1;
+ }
+ if(limit || powered)
+ {
+ nrow++;
+ srow = nrow * info->rowskip;
+ info->m_J1angularAxis[srow+0] = ax1[0];
+ info->m_J1angularAxis[srow+1] = ax1[1];
+ info->m_J1angularAxis[srow+2] = ax1[2];
+
+ info->m_J2angularAxis[srow+0] = -ax1[0];
+ info->m_J2angularAxis[srow+1] = -ax1[1];
+ info->m_J2angularAxis[srow+2] = -ax1[2];
+
+ btScalar lostop = getLowerAngLimit();
+ btScalar histop = getUpperAngLimit();
+ if(limit && (lostop == histop))
+ { // the joint motor is ineffective
+ powered = 0;
+ }
+ if(powered)
+ {
+ info->cfm[srow] = btScalar(0.0);
+ btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
+ info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
+ info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
+ info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
+ }
+ if(limit)
+ {
+ k = info->fps * info->erp;
+ info->m_constraintError[srow] += k * limit_err;
+ info->cfm[srow] = btScalar(0.0); // stop_cfm;
+ if(lostop == histop)
+ {
+ // limited low and high simultaneously
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else if(limit == 1)
+ { // low limit
+ info->m_lowerLimit[srow] = 0;
+ info->m_upperLimit[srow] = SIMD_INFINITY;
+ }
+ else
+ { // high limit
+ info->m_lowerLimit[srow] = -SIMD_INFINITY;
+ info->m_upperLimit[srow] = 0;
+ }
+ // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+ btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
+ if(bounce > btScalar(0.0))
+ {
+ btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
+ vel -= m_rbB.getAngularVelocity().dot(ax1);
+ // only apply bounce if the velocity is incoming, and if the
+ // resulting c[] exceeds what we already have.
+ if(limit == 1)
+ { // low limit
+ if(vel < 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc > info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ else
+ { // high limit - all those computations are reversed
+ if(vel > 0)
+ {
+ btScalar newc = -bounce * vel;
+ if(newc < info->m_constraintError[srow])
+ {
+ info->m_constraintError[srow] = newc;
+ }
+ }
+ }
+ }
+ info->m_constraintError[srow] *= getSoftnessLimAng();
+ } // if(limit)
+ } // if angular limit or powered
+} // btSliderConstraint::getInfo2()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
+{
+ if (m_useSolveConstraintObsolete)
+ {
+ m_timeStep = timeStep;
+ if(m_useLinearReferenceFrameA)
+ {
+ solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
+ }
+ else
+ {
+ solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
+ }
+ }
} // btSliderConstraint::solveConstraint()
//-----------------------------------------------------------------------------
-void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{
int i;
// linear
- btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
- btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+ btVector3 velA;
+ bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
+ btVector3 velB;
+ bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
btVector3 vel = velA - velB;
for(i = 0; i < 3; i++)
{
@@ -190,8 +543,18 @@
// calcutate and apply impulse
btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
btVector3 impulse_vector = normal * normalImpulse;
- rbA.applyImpulse( impulse_vector, m_relPosA);
- rbB.applyImpulse(-impulse_vector, m_relPosB);
+
+ //rbA.applyImpulse( impulse_vector, m_relPosA);
+ //rbB.applyImpulse(-impulse_vector, m_relPosB);
+ {
+ btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+ btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+ bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+ bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+ }
+
+
+
if(m_poweredLinMotor && (!i))
{ // apply linear motor
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
@@ -217,8 +580,18 @@
m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse
impulse_vector = normal * normalImpulse;
- rbA.applyImpulse( impulse_vector, m_relPosA);
- rbB.applyImpulse(-impulse_vector, m_relPosB);
+ //rbA.applyImpulse( impulse_vector, m_relPosA);
+ //rbB.applyImpulse(-impulse_vector, m_relPosB);
+
+ {
+ btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
+ btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
+ bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
+ bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
+ }
+
+
+
}
}
}
@@ -227,8 +600,10 @@
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
- const btVector3& angVelA = rbA.getAngularVelocity();
- const btVector3& angVelB = rbB.getAngularVelocity();
+ btVector3 angVelA;
+ bodyA.getAngularVelocity(angVelA);
+ btVector3 angVelB;
+ bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
@@ -238,24 +613,38 @@
btVector3 velrelOrthog = angAorthog-angBorthog;
//solve orthogonal angular velocity correction
btScalar len = velrelOrthog.length();
+ btScalar orthorImpulseMag = 0.f;
+
if (len > btScalar(0.00001))
{
btVector3 normal = velrelOrthog.normalized();
btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
- velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+ //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+ orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
}
//solve angular positional correction
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+ btVector3 angularAxis = angularError;
+ btScalar angularImpulseMag = 0;
+
btScalar len2 = angularError.length();
if (len2>btScalar(0.00001))
{
btVector3 normal2 = angularError.normalized();
btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
- angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+ angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+ angularError *= angularImpulseMag;
}
// apply impulse
- rbA.applyTorqueImpulse(-velrelOrthog+angularError);
- rbB.applyTorqueImpulse(velrelOrthog-angularError);
+ //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+ //rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
+
+
btScalar impulseMag;
//solve angular limits
if(m_solveAngLim)
@@ -269,8 +658,14 @@
impulseMag *= m_kAngle * m_softnessDirAng;
}
btVector3 impulse = axisA * impulseMag;
- rbA.applyTorqueImpulse(impulse);
- rbB.applyTorqueImpulse(-impulse);
+ //rbA.applyTorqueImpulse(impulse);
+ //rbB.applyTorqueImpulse(-impulse);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
+
+
+
//apply angular motor
if(m_poweredAngMotor)
{
@@ -301,8 +696,11 @@
m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse
btVector3 motorImp = angImpulse * axisA;
- m_rbA.applyTorqueImpulse(motorImp);
- m_rbB.applyTorqueImpulse(-motorImp);
+ //rbA.applyTorqueImpulse(motorImp);
+ //rbB.applyTorqueImpulse(-motorImp);
+
+ bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
+ bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
}
}
} // btSliderConstraint::solveConstraint()
@@ -312,7 +710,7 @@
//-----------------------------------------------------------------------------
void btSliderConstraint::calculateTransforms(void){
- if(m_useLinearReferenceFrameA)
+ if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
{
m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
@@ -325,7 +723,14 @@
m_realPivotAInW = m_calculatedTransformA.getOrigin();
m_realPivotBInW = m_calculatedTransformB.getOrigin();
m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
- m_delta = m_realPivotBInW - m_realPivotAInW;
+ if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
+ {
+ m_delta = m_realPivotBInW - m_realPivotAInW;
+ }
+ else
+ {
+ m_delta = m_realPivotAInW - m_realPivotBInW;
+ }
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
btVector3 normalWorld;
int i;
@@ -367,7 +772,6 @@
} // btSliderConstraint::testLinLimits()
//-----------------------------------------------------------------------------
-
void btSliderConstraint::testAngLimits(void)
{
@@ -379,6 +783,7 @@
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
+ m_angPos = rot;
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
@@ -391,12 +796,9 @@
}
}
} // btSliderConstraint::testAngLimits()
-
//-----------------------------------------------------------------------------
-
-
btVector3 btSliderConstraint::getAncorInA(void)
{
btVector3 ancorInA;
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -46,6 +46,8 @@
class btSliderConstraint : public btTypedConstraint
{
protected:
+ ///for backwards compatibility during the transition to 'getInfo/getInfo2'
+ bool m_useSolveConstraintObsolete;
btTransform m_frameInA;
btTransform m_frameInB;
// use frameA fo define limits, if true
@@ -104,6 +106,7 @@
btVector3 m_relPosB;
btScalar m_linPos;
+ btScalar m_angPos;
btScalar m_angDepth;
btScalar m_kAngle;
@@ -126,7 +129,13 @@
btSliderConstraint();
// overrides
virtual void buildJacobian();
- virtual void solveConstraint(btScalar timeStep);
+ virtual void getInfo1 (btConstraintInfo1* info);
+
+ virtual void getInfo2 (btConstraintInfo2* info);
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
+
+
// access
const btRigidBody& getRigidBodyA() const { return m_rbA; }
const btRigidBody& getRigidBodyB() const { return m_rbB; }
@@ -194,6 +203,7 @@
void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
btScalar getLinearPos() { return m_linPos; }
+
// access for ODE solver
bool getSolveLinLimit() { return m_solveLinLim; }
@@ -202,10 +212,11 @@
btScalar getAngDepth() { return m_angDepth; }
// internal
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
- void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+ void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
// shared code used by ODE solver
void calculateTransforms(void);
void testLinLimits(void);
+ void testLinLimits2(btConstraintInfo2* info);
void testAngLimits(void);
// access for PE Solver
btVector3 getAncorInA(void);
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -23,88 +23,152 @@
#include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h"
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef BT_USE_SSE
+#define USE_SIMD 1
+#endif //
+
+#ifdef USE_SIMD
+
+struct btSimdScalar
+{
+ SIMD_FORCE_INLINE btSimdScalar()
+ {
+
+ }
+
+ SIMD_FORCE_INLINE btSimdScalar(float fl)
+ :m_vec128 (_mm_set1_ps(fl))
+ {
+ }
+
+ SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
+ :m_vec128(v128)
+ {
+ }
+ union
+ {
+ __m128 m_vec128;
+ float m_floats[4];
+ int m_ints[4];
+ btScalar m_unusedPadding;
+ };
+ SIMD_FORCE_INLINE __m128 get128()
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE const __m128 get128() const
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE void set128(__m128 v128)
+ {
+ m_vec128 = v128;
+ }
+
+ SIMD_FORCE_INLINE operator __m128()
+ {
+ return m_vec128;
+ }
+ SIMD_FORCE_INLINE operator const __m128() const
+ {
+ return m_vec128;
+ }
+
+ SIMD_FORCE_INLINE operator float() const
+ {
+ return m_floats[0];
+ }
+
+};
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar
+operator*(const btSimdScalar& v1, const btSimdScalar& v2)
+{
+ return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+}
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar
+operator+(const btSimdScalar& v1, const btSimdScalar& v2)
+{
+ return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+}
+
+
+#else
+#define btSimdScalar btScalar
+#endif
+
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
-
- btMatrix3x3 m_worldInvInertiaTensor;
-
- btVector3 m_angularVelocity;
- btVector3 m_linearVelocity;
- btVector3 m_centerOfMassPosition;
+ btVector3 m_deltaLinearVelocity;
+ btVector3 m_deltaAngularVelocity;
+ btScalar m_angularFactor;
+ btScalar m_invMass;
+ btScalar m_friction;
+ btRigidBody* m_originalBody;
btVector3 m_pushVelocity;
- btVector3 m_turnVelocity;
+ //btVector3 m_turnVelocity;
- float m_angularFactor;
- float m_invMass;
- float m_friction;
- btRigidBody* m_originalBody;
-
- SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+ SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{
- velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+ if (m_originalBody)
+ velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+ else
+ velocity.setValue(0,0,0);
}
- //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
- SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+ SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{
- if (m_invMass)
- {
- m_linearVelocity += linearComponent*impulseMagnitude;
- m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
- }
+ if (m_originalBody)
+ angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+ else
+ angVel.setValue(0,0,0);
}
-
- SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+
+
+ //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+ SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
- if (m_invMass)
+ //if (m_invMass)
{
- m_pushVelocity += linearComponent*impulseMagnitude;
- m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+ m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+ m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
+/*
+
void writebackVelocity()
{
if (m_invMass)
{
- m_originalBody->setLinearVelocity(m_linearVelocity);
- m_originalBody->setAngularVelocity(m_angularVelocity);
+ m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+ m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
-
+ */
- void writebackVelocity(btScalar timeStep)
+ void writebackVelocity(btScalar timeStep=0)
{
if (m_invMass)
{
- m_originalBody->setLinearVelocity(m_linearVelocity);
- m_originalBody->setAngularVelocity(m_angularVelocity);
-
- //correct the position/orientation based on push/turn recovery
- btTransform newTransform;
- btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
- m_originalBody->setWorldTransform(newTransform);
-
+ m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+ m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
-
- void readVelocity()
- {
- if (m_invMass)
- {
- m_linearVelocity = m_originalBody->getLinearVelocity();
- m_angularVelocity = m_originalBody->getAngularVelocity();
- }
- }
-
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,40 +19,64 @@
class btRigidBody;
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
+#include "btJacobianEntry.h"
//#define NO_FRICTION_TANGENTIALS 1
+#include "btSolverBody.h"
+
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
- btVector3 m_relpos1CrossNormal;
- btVector3 m_contactNormal;
+ btVector3 m_relpos1CrossNormal;
+ btVector3 m_contactNormal;
- btVector3 m_relpos2CrossNormal;
- btVector3 m_angularComponentA;
+ btVector3 m_relpos2CrossNormal;
+ //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
- btVector3 m_angularComponentB;
-
- mutable btScalar m_appliedPushImpulse;
+ btVector3 m_angularComponentA;
+ btVector3 m_angularComponentB;
- mutable btScalar m_appliedImpulse;
- int m_solverBodyIdA;
- int m_solverBodyIdB;
+ mutable btSimdScalar m_appliedPushImpulse;
+ mutable btSimdScalar m_appliedImpulse;
+
btScalar m_friction;
- btScalar m_restitution;
btScalar m_jacDiagABInv;
- btScalar m_penetration;
-
+ union
+ {
+ int m_numConsecutiveRowsPerKernel;
+ btScalar m_unusedPadding0;
+ };
+ union
+ {
+ int m_frictionIndex;
+ btScalar m_unusedPadding1;
+ };
+ union
+ {
+ int m_solverBodyIdA;
+ btScalar m_unusedPadding2;
+ };
+ union
+ {
+ int m_solverBodyIdB;
+ btScalar m_unusedPadding3;
+ };
- int m_constraintType;
- int m_frictionIndex;
- void* m_originalContactPoint;
- int m_unusedPadding[1];
+ union
+ {
+ void* m_originalContactPoint;
+ btScalar m_unusedPadding4;
+ };
+ btScalar m_rhs;
+ btScalar m_cfm;
+ btScalar m_lowerLimit;
+ btScalar m_upperLimit;
enum btSolverConstraintType
{
@@ -61,11 +85,9 @@
};
};
+typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
-
-
-
#endif //BT_SOLVER_CONSTRAINT_H
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,13 +19,16 @@
static btRigidBody s_fixed(0, 0,0);
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
:m_userConstraintType(-1),
m_userConstraintId(-1),
m_constraintType (type),
m_rbA(s_fixed),
m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
@@ -35,7 +38,8 @@
m_constraintType (type),
m_rbA(rbA),
m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
@@ -48,9 +52,63 @@
m_constraintType (type),
m_rbA(rbA),
m_rbB(rbB),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
{
s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
}
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+ if(lowLim > uppLim)
+ {
+ return btScalar(1.0f);
+ }
+ else if(lowLim == uppLim)
+ {
+ return btScalar(0.0f);
+ }
+ btScalar lim_fact = btScalar(1.0f);
+ btScalar delta_max = vel / timeFact;
+ if(delta_max < btScalar(0.0f))
+ {
+ if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+ {
+ lim_fact = (lowLim - pos) / delta_max;
+ }
+ else if(pos < lowLim)
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ else
+ {
+ lim_fact = btScalar(1.0f);
+ }
+ }
+ else if(delta_max > btScalar(0.0f))
+ {
+ if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+ {
+ lim_fact = (uppLim - pos) / delta_max;
+ }
+ else if(pos > uppLim)
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ else
+ {
+ lim_fact = btScalar(1.0f);
+ }
+ }
+ else
+ {
+ lim_fact = btScalar(0.0f);
+ }
+ return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+
Modified: trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h
===================================================================
--- trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/ConstraintSolver/btTypedConstraint.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -18,14 +18,18 @@
class btRigidBody;
#include "LinearMath/btScalar.h"
+#include "btSolverConstraint.h"
+struct btSolverBody;
+
+
+
enum btTypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE,
HINGE_CONSTRAINT_TYPE,
CONETWIST_CONSTRAINT_TYPE,
D6_CONSTRAINT_TYPE,
- VEHICLE_CONSTRAINT_TYPE,
SLIDER_CONSTRAINT_TYPE
};
@@ -48,6 +52,7 @@
btRigidBody& m_rbA;
btRigidBody& m_rbB;
btScalar m_appliedImpulse;
+ btScalar m_dbgDrawSize;
public:
@@ -55,13 +60,55 @@
btTypedConstraint(btTypedConstraintType type);
virtual ~btTypedConstraint() {};
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
-
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
+ struct btConstraintInfo1 {
+ int m_numConstraintRows,nub;
+ };
+
+ struct btConstraintInfo2 {
+ // integrator parameters: frames per second (1/stepsize), default error
+ // reduction parameter (0..1).
+ btScalar fps,erp;
+
+ // for the first and second body, pointers to two (linear and angular)
+ // n*3 jacobian sub matrices, stored by rows. these matrices will have
+ // been initialized to 0 on entry. if the second body is zero then the
+ // J2xx pointers may be 0.
+ btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+
+ // elements to jump from one row to the next in J's
+ int rowskip;
+
+ // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+ // "constraint force mixing" vector. c is set to zero on entry, cfm is
+ // set to a constant value (typically very small or zero) value on entry.
+ btScalar *m_constraintError,*cfm;
+
+ // lo and hi limits for variables (set to -/+ infinity on entry).
+ btScalar *m_lowerLimit,*m_upperLimit;
+
+ // findex vector for variables. see the LCP solver interface for a
+ // description of what this does. this is set to -1 on entry.
+ // note that the returned indexes are relative to the first index of
+ // the constraint.
+ int *findex;
+ };
+
+
virtual void buildJacobian() = 0;
- virtual void solveConstraint(btScalar timeStep) = 0;
+ virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
+ {
+ }
+ virtual void getInfo1 (btConstraintInfo1* info)=0;
+ virtual void getInfo2 (btConstraintInfo2* info)=0;
+
+ virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
+
+ btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
+
const btRigidBody& getRigidBodyA() const
{
return m_rbA;
@@ -94,7 +141,7 @@
{
m_userConstraintId = uid;
}
-
+
int getUserConstraintId() const
{
return m_userConstraintId;
@@ -114,7 +161,16 @@
{
return m_constraintType;
}
-
+
+ void setDbgDrawSize(btScalar dbgDrawSize)
+ {
+ m_dbgDrawSize = dbgDrawSize;
+ }
+ btScalar getDbgDrawSize()
+ {
+ return m_dbgDrawSize;
+ }
+
};
#endif //TYPED_CONSTRAINT_H
Modified: trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/Bullet-C-API.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -110,16 +110,16 @@
void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
- assert(dynamicsWorld);
+ btAssert(dynamicsWorld);
dynamicsWorld->stepSimulation(timeStep);
}
void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
- assert(dynamicsWorld);
+ btAssert(dynamicsWorld);
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
- assert(body);
+ btAssert(body);
dynamicsWorld->addRigidBody(body);
}
@@ -127,9 +127,9 @@
void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
{
btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
- assert(dynamicsWorld);
+ btAssert(dynamicsWorld);
btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
- assert(body);
+ btAssert(body);
dynamicsWorld->removeRigidBody(body);
}
@@ -142,7 +142,7 @@
trans.setIdentity();
btVector3 localInertia(0,0,0);
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
- assert(shape);
+ btAssert(shape);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
@@ -158,7 +158,7 @@
void plDeleteRigidBody(plRigidBodyHandle cbody)
{
btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
- assert(body);
+ btAssert(body);
btAlignedFree( body);
}
@@ -262,13 +262,13 @@
void plDeleteShape(plCollisionShapeHandle cshape)
{
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
- assert(shape);
+ btAssert(shape);
btAlignedFree(shape);
}
void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
{
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
- assert(shape);
+ btAssert(shape);
btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
shape->setLocalScaling(scaling);
}
Added: trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h (rev 0)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -0,0 +1,39 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _BT_ACTION_INTERFACE_H
+#define _BT_ACTION_INTERFACE_H
+
+class btIDebugDraw;
+class btCollisionWorld;
+
+#include "LinearMath/btScalar.h"
+
+///Basic interface to allow actions such as vehicles and characters to be updated inside a btDynamicsWorld
+class btActionInterface
+{
+ public:
+
+ virtual ~btActionInterface()
+ {
+ }
+
+ virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0;
+
+ virtual void debugDraw(btIDebugDraw* debugDrawer) = 0;
+
+};
+
+#endif //_BT_ACTION_INTERFACE_H
\ No newline at end of file
Property changes on: trunk/src/bullet/BulletDynamics/Dynamics/btActionInterface.h
___________________________________________________________________
Added: svn:eol-style
+ native
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -90,9 +90,8 @@
integrateTransforms(timeStep * toi);
///update vehicle simulation
- updateVehicles(timeStep);
+ updateActions(timeStep);
-
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -29,6 +29,11 @@
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
//for debug rendering
#include "BulletCollision/CollisionShapes/btBoxShape.h"
@@ -46,15 +51,7 @@
#include "LinearMath/btIDebugDraw.h"
-
-//vehicle
-#include "BulletDynamics/Vehicle/btRaycastVehicle.h"
-#include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
-#include "BulletDynamics/Vehicle/btWheelInfo.h"
-//character
-#include "BulletDynamics/Character/btCharacterControllerInterface.h"
-
-#include "LinearMath/btIDebugDraw.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btMotionState.h"
@@ -113,7 +110,6 @@
btRigidBody* body = btRigidBody::upcast(colObj);
if (body)
{
- btTransform predictedTrans;
if (body->getActivationState() != ISLAND_SLEEPING)
{
if (body->isKinematicObject())
@@ -148,8 +144,26 @@
}
}
}
+ bool drawConstraints = false;
+ if (getDebugDrawer())
+ {
+ int mode = getDebugDrawer()->getDebugMode();
+ if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+ {
+ drawConstraints = true;
+ }
+ }
+ if(drawConstraints)
+ {
+ for(int i = getNumConstraints()-1; i>=0 ;i--)
+ {
+ btTypedConstraint* constraint = getConstraint(i);
+ debugDrawConstraint(constraint);
+ }
+ }
+
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
{
int i;
@@ -190,32 +204,11 @@
}
- for ( i=0;i<this->m_vehicles.size();i++)
+ if (getDebugDrawer() && getDebugDrawer()->getDebugMode())
{
- for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+ for (i=0;i<m_actions.size();i++)
{
- btVector3 wheelColor(0,255,255);
- if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
- {
- wheelColor.setValue(0,0,255);
- } else
- {
- wheelColor.setValue(255,0,255);
- }
-
- btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
-
- btVector3 axle = btVector3(
- m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
- m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
- m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
-
-
- //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
- //debug wheels (cylinders)
- m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
- m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
-
+ m_actions[i]->debugDraw(m_debugDrawer);
}
}
}
@@ -287,7 +280,7 @@
synchronizeSingleMotionState(body);
}
}
-
+/*
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
{
for ( int i=0;i<this->m_vehicles.size();i++)
@@ -299,7 +292,9 @@
}
}
}
+ */
+
}
@@ -339,7 +334,8 @@
//process some debugging flags
if (getDebugDrawer())
{
- gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
+ btIDebugDraw* debugDrawer = getDebugDrawer ();
+ gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
}
if (numSimulationSubSteps)
{
@@ -403,10 +399,8 @@
integrateTransforms(timeStep);
///update vehicle simulation
- updateVehicles(timeStep);
+ updateActions(timeStep);
- updateCharacters(timeStep);
-
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
@@ -470,31 +464,17 @@
}
-void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
+void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)
{
- BT_PROFILE("updateVehicles");
+ BT_PROFILE("updateActions");
- for ( int i=0;i<m_vehicles.size();i++)
+ for ( int i=0;i<m_actions.size();i++)
{
- btRaycastVehicle* vehicle = m_vehicles[i];
- vehicle->updateVehicle( timeStep);
+ m_actions[i]->updateAction( this, timeStep);
}
}
-
-void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
-{
- BT_PROFILE("updateCharacters");
- for ( int i=0;i<m_characters.size();i++)
- {
- btCharacterControllerInterface* character = m_characters[i];
- character->preStep (this);
- character->playerStep (this,timeStep);
- }
-}
-
-
void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
{
BT_PROFILE("updateActivationState");
@@ -549,27 +529,38 @@
constraint->getRigidBodyB().removeConstraintRef(constraint);
}
-void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
+void btDiscreteDynamicsWorld::addAction(btActionInterface* action)
{
- m_vehicles.push_back(vehicle);
+ m_actions.push_back(action);
}
-void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
+void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
{
- m_vehicles.remove(vehicle);
+ m_actions.remove(action);
}
-void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
+
+void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
{
- m_characters.push_back(character);
+ addAction(vehicle);
}
-void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
+void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
{
- m_characters.remove(character);
+ removeAction(vehicle);
}
+void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
+{
+ addAction(character);
+}
+void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
+{
+ removeAction(character);
+}
+
+
SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)
{
int islandId;
@@ -642,8 +633,11 @@
{
if (islandId<0)
{
- ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
- m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+ if (numManifolds + m_numConstraints)
+ {
+ ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+ m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+ }
} else
{
//also add all non-contact constraints/joints for this island
@@ -689,7 +683,7 @@
sortedConstraints[i] = m_constraints[i];
}
-// assert(0);
+// btAssert(0);
@@ -753,14 +747,16 @@
btCollisionObject* m_me;
btScalar m_allowedPenetration;
btOverlappingPairCache* m_pairCache;
+ btDispatcher* m_dispatcher;
public:
- btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) :
+ btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
m_allowedPenetration(0.0f),
m_me(me),
- m_pairCache(pairCache)
+ m_pairCache(pairCache),
+ m_dispatcher(dispatcher)
{
}
@@ -795,20 +791,26 @@
if (!ClosestConvexResultCallback::needsCollision(proxy0))
return false;
- ///don't do CCD when there are already contact points (touching contact/penetration)
- btAlignedObjectArray<btPersistentManifold*> manifoldArray;
- btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
- if (collisionPair)
+ btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
+
+ //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
+ if (m_dispatcher->needsResponse(m_me,otherObj))
{
- if (collisionPair->m_algorithm)
+ ///don't do CCD when there are already contact points (touching contact/penetration)
+ btAlignedObjectArray<btPersistentManifold*> manifoldArray;
+ btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
+ if (collisionPair)
{
- manifoldArray.resize(0);
- collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
- for (int j=0;j<manifoldArray.size();j++)
+ if (collisionPair->m_algorithm)
{
- btPersistentManifold* manifold = manifoldArray[j];
- if (manifold->getNumContacts()>0)
- return false;
+ manifoldArray.resize(0);
+ collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
+ for (int j=0;j<manifoldArray.size();j++)
+ {
+ btPersistentManifold* manifold = manifoldArray[j];
+ if (manifold->getNumContacts()>0)
+ return false;
+ }
}
}
}
@@ -846,7 +848,7 @@
{
gNumClampedCcdMotions++;
- btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
+ btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
@@ -1187,6 +1189,189 @@
}
+void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
+{
+ bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
+ bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
+ btScalar dbgDrawSize = constraint->getDbgDrawSize();
+ if(dbgDrawSize <= btScalar(0.f))
+ {
+ return;
+ }
+
+ switch(constraint->getConstraintType())
+ {
+ case POINT2POINT_CONSTRAINT_TYPE:
+ {
+ btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
+ btTransform tr;
+ tr.setIdentity();
+ btVector3 pivot = p2pC->getPivotInA();
+ pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;
+ tr.setOrigin(pivot);
+ getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ // that ideally should draw the same frame
+ pivot = p2pC->getPivotInB();
+ pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;
+ tr.setOrigin(pivot);
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ }
+ break;
+ case HINGE_CONSTRAINT_TYPE:
+ {
+ btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
+ btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ btScalar minAng = pHinge->getLowerLimit();
+ btScalar maxAng = pHinge->getUpperLimit();
+ if(minAng == maxAng)
+ {
+ break;
+ }
+ bool drawSect = true;
+ if(minAng > maxAng)
+ {
+ minAng = btScalar(0.f);
+ maxAng = SIMD_2_PI;
+ drawSect = false;
+ }
+ if(drawLimits)
+ {
+ btVector3& center = tr.getOrigin();
+ btVector3 normal = tr.getBasis().getColumn(2);
+ btVector3 axis = tr.getBasis().getColumn(0);
+ getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
+ }
+ }
+ break;
+ case CONETWIST_CONSTRAINT_TYPE:
+ {
+ btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
+ btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ if(drawLimits)
+ {
+ //const btScalar length = btScalar(5);
+ const btScalar length = dbgDrawSize;
+ static int nSegments = 8*4;
+ btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
+ btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
+ pPrev = tr * pPrev;
+ for (int i=0; i<nSegments; i++)
+ {
+ fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
+ btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
+ pCur = tr * pCur;
+ getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
+
+ if (i%(nSegments/8) == 0)
+ getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
+
+ pPrev = pCur;
+ }
+ btScalar tws = pCT->getTwistSpan();
+ btScalar twa = pCT->getTwistAngle();
+ bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
+ if(useFrameB)
+ {
+ tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
+ }
+ else
+ {
+ tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
+ }
+ btVector3 pivot = tr.getOrigin();
+ btVector3 normal = tr.getBasis().getColumn(0);
+ btVector3 axis1 = tr.getBasis().getColumn(1);
+ getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
+
+ }
+ }
+ break;
+ case D6_CONSTRAINT_TYPE:
+ {
+ btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
+ btTransform tr = p6DOF->getCalculatedTransformA();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ tr = p6DOF->getCalculatedTransformB();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ if(drawLimits)
+ {
+ tr = p6DOF->getCalculatedTransformA();
+ const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+ btVector3 up = tr.getBasis().getColumn(2);
+ btVector3 axis = tr.getBasis().getColumn(0);
+ btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+ btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+ btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+ btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+ getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
+ axis = tr.getBasis().getColumn(1);
+ btScalar ay = p6DOF->getAngle(1);
+ btScalar az = p6DOF->getAngle(2);
+ btScalar cy = btCos(ay);
+ btScalar sy = btSin(ay);
+ btScalar cz = btCos(az);
+ btScalar sz = btSin(az);
+ btVector3 ref;
+ ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+ ref[1] = -sz*axis[0] + cz*axis[1];
+ ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+ tr = p6DOF->getCalculatedTransformB();
+ btVector3 normal = -tr.getBasis().getColumn(0);
+ btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+ btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+ if(minFi > maxFi)
+ {
+ getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
+ }
+ else if(minFi < maxFi)
+ {
+ getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
+ }
+ tr = p6DOF->getCalculatedTransformA();
+ btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+ btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+ getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
+ }
+ }
+ break;
+ case SLIDER_CONSTRAINT_TYPE:
+ {
+ btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
+ btTransform tr = pSlider->getCalculatedTransformA();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ tr = pSlider->getCalculatedTransformB();
+ if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+ if(drawLimits)
+ {
+ btTransform tr = pSlider->getCalculatedTransformA();
+ btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
+ btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
+ getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
+ btVector3 normal = tr.getBasis().getColumn(0);
+ btVector3 axis = tr.getBasis().getColumn(1);
+ btScalar a_min = pSlider->getLowerAngLimit();
+ btScalar a_max = pSlider->getUpperAngLimit();
+ const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
+ getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
+ }
+ }
+ break;
+ default :
+ break;
+ }
+ return;
+} // btDiscreteDynamicsWorld::debugDrawConstraint()
+
+
+
+
+
void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
{
if (m_ownsConstraintSolver)
@@ -1215,3 +1400,5 @@
{
return m_constraints[index];
}
+
+
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -23,10 +23,8 @@
class btConstraintSolver;
class btSimulationIslandManager;
class btTypedConstraint;
+class btActionInterface;
-
-class btRaycastVehicle;
-class btCharacterControllerInterface;
class btIDebugDraw;
#include "LinearMath/btAlignedObjectArray.h"
@@ -52,12 +50,8 @@
bool m_ownsIslandManager;
bool m_ownsConstraintSolver;
+ btAlignedObjectArray<btActionInterface*> m_actions;
- btAlignedObjectArray<btRaycastVehicle*> m_vehicles;
-
- btAlignedObjectArray<btCharacterControllerInterface*> m_characters;
-
-
int m_profileTimings;
virtual void predictUnconstraintMotion(btScalar timeStep);
@@ -70,10 +64,8 @@
void updateActivationState(btScalar timeStep);
- void updateVehicles(btScalar timeStep);
+ void updateActions(btScalar timeStep);
- void updateCharacters(btScalar timeStep);
-
void startProfiling(btScalar timeStep);
virtual void internalSingleStepSimulation( btScalar timeStep);
@@ -105,15 +97,10 @@
virtual void removeConstraint(btTypedConstraint* constraint);
- virtual void addVehicle(btRaycastVehicle* vehicle);
+ virtual void addAction(btActionInterface*);
- virtual void removeVehicle(btRaycastVehicle* vehicle);
+ virtual void removeAction(btActionInterface*);
- virtual void addCharacter(btCharacterControllerInterface* character);
-
- virtual void removeCharacter(btCharacterControllerInterface* character);
-
-
btSimulationIslandManager* getSimulationIslandManager()
{
return m_islandManager;
@@ -130,6 +117,7 @@
}
virtual void setGravity(const btVector3& gravity);
+
virtual btVector3 getGravity () const;
virtual void addRigidBody(btRigidBody* body);
@@ -140,6 +128,8 @@
void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
+ void debugDrawConstraint(btTypedConstraint* constraint);
+
virtual void debugDrawWorld();
virtual void setConstraintSolver(btConstraintSolver* solver);
@@ -169,6 +159,21 @@
(void) numTasks;
}
+ ///obsolete, use updateActions instead
+ virtual void updateVehicles(btScalar timeStep)
+ {
+ updateActions(timeStep);
+ }
+
+ ///obsolete, use addAction instead
+ virtual void addVehicle(btActionInterface* vehicle);
+ ///obsolete, use removeAction instead
+ virtual void removeVehicle(btActionInterface* vehicle);
+ ///obsolete, use addAction instead
+ virtual void addCharacter(btActionInterface* character);
+ ///obsolete, use removeAction instead
+ virtual void removeCharacter(btActionInterface* character);
+
};
#endif //BT_DISCRETE_DYNAMICS_WORLD_H
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btDynamicsWorld.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -20,11 +20,11 @@
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class btTypedConstraint;
-class btRaycastVehicle;
+class btActionInterface;
class btConstraintSolver;
class btDynamicsWorld;
-class btCharacterControllerInterface;
+
/// Type for the callback for each tick
typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
@@ -72,15 +72,10 @@
virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
- virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
+ virtual void addAction(btActionInterface* action) = 0;
- virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
+ virtual void removeAction(btActionInterface* action) = 0;
- virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;}
-
- virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;}
-
-
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
virtual void setGravity(const btVector3& gravity) = 0;
@@ -129,6 +124,16 @@
}
+ ///obsolete, use addAction instead.
+ virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;}
+ ///obsolete, use removeAction instead
+ virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;}
+ ///obsolete, use addAction instead.
+ virtual void addCharacter(btActionInterface* character) {(void)character;}
+ ///obsolete, use removeAction instead
+ virtual void removeCharacter(btActionInterface* character) {(void)character;}
+
+
};
#endif //BT_DYNAMICS_WORLD_H
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -46,6 +46,7 @@
m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
m_angularFactor = btScalar(1.);
m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+ m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
m_linearDamping = btScalar(0.);
@@ -124,6 +125,7 @@
{
m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
}
+ m_gravity_acceleration = acceleration;
}
Modified: trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Dynamics/btRigidBody.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -48,6 +48,7 @@
btScalar m_angularFactor;
btVector3 m_gravity;
+ btVector3 m_gravity_acceleration;
btVector3 m_invInertiaLocal;
btVector3 m_totalForce;
btVector3 m_totalTorque;
@@ -181,7 +182,7 @@
const btVector3& getGravity() const
{
- return m_gravity;
+ return m_gravity_acceleration;
}
void setDamping(btScalar lin_damping, btScalar ang_damping);
@@ -231,8 +232,18 @@
{
m_totalForce += force;
}
+
+ const btVector3& getTotalForce()
+ {
+ return m_totalForce;
+ };
+
+ const btVector3& getTotalTorque()
+ {
+ return m_totalTorque;
+ };
- const btVector3& getInvInertiaDiagLocal()
+ const btVector3& getInvInertiaDiagLocal() const
{
return m_invInertiaLocal;
};
Modified: trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp
===================================================================
--- trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,15 +19,13 @@
#include "btVehicleRaycaster.h"
#include "btWheelInfo.h"
#include "LinearMath/btMinMax.h"
-
-
+#include "LinearMath/btIDebugDraw.h"
#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
static btRigidBody s_fixedObject( 0,0,0);
btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster )
-: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
-m_vehicleRaycaster(raycaster),
+:m_vehicleRaycaster(raycaster),
m_pitchControl(btScalar(0.))
{
m_chassisBody = chassis;
@@ -87,7 +85,7 @@
const btTransform& btRaycastVehicle::getWheelTransformWS( int wheelIndex ) const
{
- assert(wheelIndex < getNumWheels());
+ btAssert(wheelIndex < getNumWheels());
const btWheelInfo& wheel = m_wheelInfo[wheelIndex];
return wheel.m_worldTransform;
@@ -175,7 +173,7 @@
btVehicleRaycaster::btVehicleRaycasterResult rayResults;
- assert(m_vehicleRaycaster);
+ btAssert(m_vehicleRaycaster);
void* object = m_vehicleRaycaster->castRay(source,target,rayResults);
@@ -359,7 +357,7 @@
void btRaycastVehicle::setSteeringValue(btScalar steering,int wheel)
{
- assert(wheel>=0 && wheel < getNumWheels());
+ btAssert(wheel>=0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_steering = steering;
@@ -375,7 +373,7 @@
void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
{
- assert(wheel>=0 && wheel < getNumWheels());
+ btAssert(wheel>=0 && wheel < getNumWheels());
btWheelInfo& wheelInfo = getWheelInfo(wheel);
wheelInfo.m_engineForce = force;
}
@@ -704,6 +702,36 @@
}
+
+void btRaycastVehicle::debugDraw(btIDebugDraw* debugDrawer)
+{
+
+ for (int v=0;v<this->getNumWheels();v++)
+ {
+ btVector3 wheelColor(0,255,255);
+ if (getWheelInfo(v).m_raycastInfo.m_isInContact)
+ {
+ wheelColor.setValue(0,0,255);
+ } else
+ {
+ wheelColor.setValue(255,0,255);
+ }
+
+ btVector3 wheelPosWS = getWheelInfo(v).m_worldTransform.getOrigin();
+
+ btVector3 axle = btVector3(
+ getWheelInfo(v).m_worldTransform.getBasis()[0][getRightAxis()],
+ getWheelInfo(v).m_worldTransform.getBasis()[1][getRightAxis()],
+ getWheelInfo(v).m_worldTransform.getBasis()[2][getRightAxis()]);
+
+ //debug wheels (cylinders)
+ debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
+ debugDrawer->drawLine(wheelPosWS,getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
+
+ }
+}
+
+
void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3& to, btVehicleRaycasterResult& result)
{
// RayResultCallback& resultCallback;
@@ -727,3 +755,4 @@
}
return 0;
}
+
Modified: trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h
===================================================================
--- trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/BulletDynamics/Vehicle/btRaycastVehicle.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -17,11 +17,12 @@
class btDynamicsWorld;
#include "LinearMath/btAlignedObjectArray.h"
#include "btWheelInfo.h"
+#include "BulletDynamics/Dynamics/btActionInterface.h"
class btVehicleTuning;
///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
-class btRaycastVehicle : public btTypedConstraint
+class btRaycastVehicle : public btActionInterface
{
btAlignedObjectArray<btVector3> m_forwardWS;
@@ -73,13 +74,24 @@
virtual ~btRaycastVehicle() ;
-
+
+ ///btActionInterface interface
+ virtual void updateAction( btCollisionWorld* collisionWorld, btScalar step)
+ {
+ updateVehicle(step);
+ }
+
+
+ ///btActionInterface interface
+ void debugDraw(btIDebugDraw* debugDrawer);
+
const btTransform& getChassisWorldTransform() const;
btScalar rayCast(btWheelInfo& wheel);
virtual void updateVehicle(btScalar step);
-
+
+
void resetSuspension();
btScalar getSteeringValue(int wheel) const;
@@ -175,18 +187,8 @@
m_indexForwardAxis = forwardIndex;
}
- virtual void buildJacobian()
- {
- //not yet
- }
- virtual void solveConstraint(btScalar timeStep)
- {
- (void)timeStep;
- //not yet
- }
-
};
class btDefaultVehicleRaycaster : public btVehicleRaycaster
Modified: trunk/src/bullet/LinearMath/btAlignedAllocator.cpp
===================================================================
--- trunk/src/bullet/LinearMath/btAlignedAllocator.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btAlignedAllocator.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,6 +19,21 @@
int gNumAlignedFree = 0;
int gTotalBytesAlignedAllocs = 0;//detect memory leaks
+static void *btAllocDefault(size_t size)
+{
+ return malloc(size);
+}
+
+static void btFreeDefault(void *ptr)
+{
+ free(ptr);
+}
+
+static btAllocFunc *sAllocFunc = btAllocDefault;
+static btFreeFunc *sFreeFunc = btFreeDefault;
+
+
+
#if defined (BT_HAS_ALIGNED_ALLOCATOR)
#include <malloc.h>
static void *btAlignedAllocDefault(size_t size, int alignment)
@@ -49,7 +64,7 @@
char *real;
unsigned long offset;
- real = (char *)malloc(size + sizeof(void *) + (alignment-1));
+ real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
if (real) {
offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1);
ret = (void *)((real + sizeof(void *)) + offset);
@@ -66,25 +81,14 @@
if (ptr) {
real = *((void **)(ptr)-1);
- free(real);
+ sFreeFunc(real);
}
}
#endif
-static void *btAllocDefault(size_t size)
-{
- return malloc(size);
-}
-static void btFreeDefault(void *ptr)
-{
- free(ptr);
-}
-
static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
-static btAllocFunc *sAllocFunc = btAllocDefault;
-static btFreeFunc *sFreeFunc = btFreeDefault;
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
{
Modified: trunk/src/bullet/LinearMath/btAlignedAllocator.h
===================================================================
--- trunk/src/bullet/LinearMath/btAlignedAllocator.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btAlignedAllocator.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -38,7 +38,7 @@
void* btAlignedAllocInternal (size_t size, int alignment);
void btAlignedFreeInternal (void* ptr);
- #define btAlignedAlloc(a,b) btAlignedAllocInternal(a,b)
+ #define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
#endif
@@ -49,9 +49,12 @@
typedef void *(btAllocFunc)(size_t size);
typedef void (btFreeFunc)(void *memblock);
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
+void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
-void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
+
///The btAlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
template < typename T , unsigned Alignment >
Modified: trunk/src/bullet/LinearMath/btAlignedObjectArray.h
===================================================================
--- trunk/src/bullet/LinearMath/btAlignedObjectArray.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btAlignedObjectArray.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -58,7 +58,7 @@
{
return (size ? size*2 : 1);
}
- SIMD_FORCE_INLINE void copy(int start,int end, T* dest)
+ SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const
{
int i;
for (i=start;i<end;++i)
@@ -120,13 +120,21 @@
clear();
}
- SIMD_FORCE_INLINE int capacity() const
- { // return current length of allocated storage
- return m_capacity;
+ ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
+ btAlignedObjectArray(const btAlignedObjectArray& otherArray)
+ {
+ init();
+
+ int otherSize = otherArray.size();
+ resize (otherSize);
+ otherArray.copy(0, otherSize, m_data);
}
+
+
+ /// return the number of elements in the array
SIMD_FORCE_INLINE int size() const
- { // return length of sequence
+ {
return m_size;
}
@@ -141,6 +149,7 @@
}
+ ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void clear()
{
destroy(0,size());
@@ -156,6 +165,8 @@
m_data[m_size].~T();
}
+ ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+ ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T())
{
int curSize = size();
@@ -219,6 +230,11 @@
}
+ /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+ SIMD_FORCE_INLINE int capacity() const
+ {
+ return m_capacity;
+ }
SIMD_FORCE_INLINE void reserve(int _Count)
{ // determine new minimum length of allocated storage
Modified: trunk/src/bullet/LinearMath/btConvexHull.cpp
===================================================================
--- trunk/src/bullet/LinearMath/btConvexHull.cpp 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btConvexHull.cpp 2009-03-31 18:05:51 UTC (rev 2882)
@@ -262,8 +262,8 @@
int ma=-1;
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
{
- btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
- btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
+ btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
+ btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(ma==m && mb==m)
{
@@ -275,8 +275,8 @@
int mc = ma;
for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
{
- btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
- btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
+ btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
+ btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(mc==m && md==m)
{
Modified: trunk/src/bullet/LinearMath/btIDebugDraw.h
===================================================================
--- trunk/src/bullet/LinearMath/btIDebugDraw.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btIDebugDraw.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -29,6 +29,7 @@
#define IDEBUG_DRAW__H
#include "btVector3.h"
+#include "btTransform.h"
///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
@@ -52,11 +53,26 @@
DBG_EnableSatComparison = 256,
DBG_DisableBulletLCP = 512,
DBG_EnableCCD = 1024,
+ DBG_DrawConstraints = (1 << 11),
+ DBG_DrawConstraintLimits = (1 << 12),
DBG_MAX_DEBUG_DRAW_MODE
};
virtual ~btIDebugDraw() {};
+ virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
+ {
+ drawLine (from, to, fromColor);
+ }
+
+ virtual void drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha)
+ {
+ }
+
+ virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
+ {
+ }
+
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
@@ -109,6 +125,171 @@
edgecoord[i]*=-1.f;
}
}
+ void drawTransform(const btTransform& transform, btScalar orthoLen)
+ {
+ btVector3 start = transform.getOrigin();
+ drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
+ drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
+ drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+ }
+
+ void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
+ const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
+ {
+ const btVector3& vx = axis;
+ btVector3 vy = normal.cross(axis);
+ btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+ int nSteps = (int)((maxAngle - minAngle) / step);
+ if(!nSteps) nSteps = 1;
+ btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
+ if(drawSect)
+ {
+ drawLine(center, prev, color);
+ }
+ for(int i = 1; i <= nSteps; i++)
+ {
+ btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
+ btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
+ drawLine(prev, next, color);
+ prev = next;
+ }
+ if(drawSect)
+ {
+ drawLine(center, prev, color);
+ }
+ }
+ void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
+ btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
+ {
+ btVector3 vA[74];
+ btVector3 vB[74];
+ btVector3 *pvA = vA, *pvB = vB, *pT;
+ btVector3 npole = center + up * radius;
+ btVector3 spole = center - up * radius;
+ btVector3 arcStart;
+ btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
+ const btVector3& kv = up;
+ const btVector3& iv = axis;
+ btVector3 jv = kv.cross(iv);
+ bool drawN = false;
+ bool drawS = false;
+ if(minTh <= -SIMD_HALF_PI)
+ {
+ minTh = -SIMD_HALF_PI + step;
+ drawN = true;
+ }
+ if(maxTh >= SIMD_HALF_PI)
+ {
+ maxTh = SIMD_HALF_PI - step;
+ drawS = true;
+ }
+ if(minTh > maxTh)
+ {
+ minTh = -SIMD_HALF_PI + step;
+ maxTh = SIMD_HALF_PI - step;
+ drawN = drawS = true;
+ }
+ int n_hor = (int)((maxTh - minTh) / step) + 1;
+ if(n_hor < 2) n_hor = 2;
+ btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
+ bool isClosed = false;
+ if(minPs > maxPs)
+ {
+ minPs = -SIMD_PI + step;
+ maxPs = SIMD_PI;
+ isClosed = true;
+ }
+ else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
+ {
+ isClosed = true;
+ }
+ else
+ {
+ isClosed = false;
+ }
+ int n_vert = (int)((maxPs - minPs) / step) + 1;
+ if(n_vert < 2) n_vert = 2;
+ btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
+ for(int i = 0; i < n_hor; i++)
+ {
+ btScalar th = minTh + btScalar(i) * step_h;
+ btScalar sth = radius * btSin(th);
+ btScalar cth = radius * btCos(th);
+ for(int j = 0; j < n_vert; j++)
+ {
+ btScalar psi = minPs + btScalar(j) * step_v;
+ btScalar sps = btSin(psi);
+ btScalar cps = btCos(psi);
+ pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
+ if(i)
+ {
+ drawLine(pvA[j], pvB[j], color);
+ }
+ else if(drawS)
+ {
+ drawLine(spole, pvB[j], color);
+ }
+ if(j)
+ {
+ drawLine(pvB[j-1], pvB[j], color);
+ }
+ else
+ {
+ arcStart = pvB[j];
+ }
+ if((i == (n_hor - 1)) && drawN)
+ {
+ drawLine(npole, pvB[j], color);
+ }
+ if(isClosed)
+ {
+ if(j == (n_vert-1))
+ {
+ drawLine(arcStart, pvB[j], color);
+ }
+ }
+ else
+ {
+ if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
+ {
+ drawLine(center, pvB[j], color);
+ }
+ }
+ }
+ pT = pvA; pvA = pvB; pvB = pT;
+ }
+ }
+
+ void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
+ {
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMin[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMin[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMin[1], bbMax[2]), btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMax[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(btVector3(bbMin[0], bbMax[1], bbMax[2]), btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ }
+ void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btTransform& trans, const btVector3& color)
+ {
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMin[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMin[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMin[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMin[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMin[1], bbMax[2]), trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMax[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), color);
+ drawLine(trans * btVector3(bbMin[0], bbMax[1], bbMax[2]), trans * btVector3(bbMin[0], bbMin[1], bbMax[2]), color);
+ }
};
Modified: trunk/src/bullet/LinearMath/btMatrix3x3.h
===================================================================
--- trunk/src/bullet/LinearMath/btMatrix3x3.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btMatrix3x3.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -192,6 +192,15 @@
btScalar(0.0), btScalar(1.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(1.0));
}
+
+ static const btMatrix3x3& getIdentity()
+ {
+ static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
+ btScalar(0.0), btScalar(1.0), btScalar(0.0),
+ btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ return identityMatrix;
+ }
+
/**@brief Fill the values of the matrix into a 9 element array
* @param m The array to be filled */
void getOpenGLSubMatrix(btScalar *m) const
Modified: trunk/src/bullet/LinearMath/btQuadWord.h
===================================================================
--- trunk/src/bullet/LinearMath/btQuadWord.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btQuadWord.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -24,13 +24,13 @@
#include <altivec.h>
#endif
-/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion.
+/**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
-ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
+ATTRIBUTE_ALIGNED16(class) btQuadWord
#else
-class btQuadWordStorage
+class btQuadWord
#endif
{
protected:
@@ -45,15 +45,11 @@
{
return mVec128;
}
+protected:
#else //__CELLOS_LV2__ __SPU__
btScalar m_floats[4];
#endif //__CELLOS_LV2__ __SPU__
-};
-
-/** @brief The btQuadWord is base-class for vectors, points */
-class btQuadWord : public btQuadWordStorage
-{
public:
@@ -134,11 +130,7 @@
// :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
{
}
- /**@brief Copy constructor */
- SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
- {
- *((btQuadWordStorage*)this) = q;
- }
+
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @param y Value of y
Modified: trunk/src/bullet/LinearMath/btQuaternion.h
===================================================================
--- trunk/src/bullet/LinearMath/btQuaternion.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btQuaternion.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -19,6 +19,7 @@
#include "btVector3.h"
+#include "btQuadWord.h"
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
class btQuaternion : public btQuadWord {
@@ -57,7 +58,7 @@
void setRotation(const btVector3& axis, const btScalar& angle)
{
btScalar d = axis.length();
- assert(d != btScalar(0.0));
+ btAssert(d != btScalar(0.0));
btScalar s = btSin(angle * btScalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
btCos(angle * btScalar(0.5)));
@@ -176,7 +177,7 @@
* @param s The inverse scale factor */
btQuaternion operator/(const btScalar& s) const
{
- assert(s != btScalar(0.0));
+ btAssert(s != btScalar(0.0));
return *this * (btScalar(1.0) / s);
}
@@ -184,7 +185,7 @@
* @param s The scale factor */
btQuaternion& operator/=(const btScalar& s)
{
- assert(s != btScalar(0.0));
+ btAssert(s != btScalar(0.0));
return *this *= btScalar(1.0) / s;
}
@@ -198,7 +199,7 @@
btScalar angle(const btQuaternion& q) const
{
btScalar s = btSqrt(length2() * q.length2());
- assert(s != btScalar(0.0));
+ btAssert(s != btScalar(0.0));
return btAcos(dot(q) / s);
}
/**@brief Return the angle of rotation represented by this quaternion */
@@ -274,6 +275,12 @@
}
}
+ static const btQuaternion& getIdentity()
+ {
+ static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
+ return identityQuat;
+ }
+
SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
Modified: trunk/src/bullet/LinearMath/btScalar.h
===================================================================
--- trunk/src/bullet/LinearMath/btScalar.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btScalar.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -25,7 +25,7 @@
#include <cfloat>
#include <float.h>
-#define BT_BULLET_VERSION 273
+#define BT_BULLET_VERSION 274
inline int btGetVersion()
{
@@ -45,7 +45,7 @@
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#else
- #define BT_HAS_ALIGNED_ALLOCATOR
+ //#define BT_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
@@ -61,8 +61,14 @@
#define BT_HAVE_NATIVE_FSEL
#define btFsel(a,b,c) __fsel((a),(b),(c))
#else
+
+#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
#define BT_USE_SSE
- #endif
+ #include <emmintrin.h>
+#endif
+
+ #endif//_XBOX
+
#endif //__MINGW32__
#include <assert.h>
@@ -124,6 +130,9 @@
//non-windows systems
#define SIMD_FORCE_INLINE inline
+ ///@todo: check out alignment methods for other platforms/compilers
+ ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
+ ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128)))
#define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#ifndef assert
@@ -227,12 +236,8 @@
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); }
SIMD_FORCE_INLINE btScalar btExp(btScalar x) { return expf(x); }
SIMD_FORCE_INLINE btScalar btLog(btScalar x) { return logf(x); }
- #if defined( __MINGW32__ )
- SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return pow(x,y); }
- #else
- SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
- #endif
-
+SIMD_FORCE_INLINE btScalar btPow(btScalar x,btScalar y) { return powf(x,y); }
+
#endif
#define SIMD_2_PI btScalar(6.283185307179586232)
Modified: trunk/src/bullet/LinearMath/btTransform.h
===================================================================
--- trunk/src/bullet/LinearMath/btTransform.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btTransform.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -190,11 +190,10 @@
btTransform operator*(const btTransform& t) const;
/**@brief Return an identity transform */
- static btTransform getIdentity()
+ static const btTransform& getIdentity()
{
- btTransform tr;
- tr.setIdentity();
- return tr;
+ static const btTransform identityTransform(btMatrix3x3::getIdentity());
+ return identityTransform;
}
private:
Modified: trunk/src/bullet/LinearMath/btVector3.h
===================================================================
--- trunk/src/bullet/LinearMath/btVector3.h 2009-03-31 16:54:47 UTC (rev 2881)
+++ trunk/src/bullet/LinearMath/btVector3.h 2009-03-31 18:05:51 UTC (rev 2882)
@@ -17,47 +17,76 @@
#ifndef SIMD__VECTOR3_H
#define SIMD__VECTOR3_H
-#include "btQuadWord.h"
+#include "btScalar.h"
+#include "btScalar.h"
+#include "btMinMax.h"
/**@brief btVector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
* Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
*/
-class btVector3 : public btQuadWord {
+ATTRIBUTE_ALIGNED16(class) btVector3
+{
public:
+
+#if defined (__SPU__) && defined (__CELLOS_LV2__)
+ union {
+ vec_float4 mVec128;
+ btScalar m_floats[4];
+ };
+public:
+ vec_float4 get128() const
+ {
+ return mVec128;
+ }
+public:
+#else //__CELLOS_LV2__ __SPU__
+#ifdef BT_USE_SSE // WIN32
+ union {
+ __m128 mVec128;
+ btScalar m_floats[4];
+ };
+ SIMD_FORCE_INLINE __m128 get128() const
+ {
+ return mVec128;
+ }
+ SIMD_FORCE_INLINE void set128(__m128 v128)
+ {
+ mVec128 = v128;
+ }
+#else
+ btScalar m_floats[4];
+#endif
+#endif //__CELLOS_LV2__ __SPU__
+
+ public:
+
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btVector3() {}
- /**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
- * Note: Vector3 derives from btQuadWordStorage*/
- SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q)
- : btQuadWord(q)
- {
- }
+
/**@brief Constructor from scalars
* @param x X value
* @param y Y value
* @param z Z value
*/
- SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
- :btQuadWord(x,y,z,btScalar(0.))
+ SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
{
+ m_floats[0] = x;
+ m_floats[1] = y;
+ m_floats[2] = z;
+ m_floats[3] = btScalar(0.);
}
-// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
-// : btQuadWord(x,y,z,w)
-// {
-// }
-
/**@brief Add a vector to this one
* @param The vector to add to this one */
SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
{
- m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z();
+ m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
return *this;
}
@@ -66,14 +95,14 @@
* @param The vector to subtract */
SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
{
- m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z();
+ m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
return *this;
}
/**@brief Scale the vector
* @param s Scale factor */
SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
{
- m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s;
+ m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
return *this;
}
@@ -89,7 +118,7 @@
* @param v The other vector in the dot product */
SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
{
- return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z();
+ return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
}
/**@brief Return the length of the vector squared */
@@ -148,30 +177,30 @@
SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
{
return btVector3(
- m_floats[1] * v.z() - m_floats[2] * v.y(),
- m_floats[2] * v.x() - m_floats[0] * v.z(),
- m_floats[0] * v.y() - m_floats[1] * v.x());
+ m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
+ m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
+ m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
}
SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
{
- return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) +
- m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) +
- m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
+ return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
+ m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
+ m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
}
/**@brief Return the axis with the smallest value
* Note return values are 0,1,2 for x, y, or z */
SIMD_FORCE_INLINE int minAxis() const
{
- return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2);
+ return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
}
/**@brief Return the axis with the largest value
* Note return values are 0,1,2 for x, y, or z */
SIMD_FORCE_INLINE int maxAxis() const
{
- return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0);
+ return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
}
SIMD_FORCE_INLINE int furthestAxis() const
@@ -187,9 +216,9 @@
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
{
btScalar s = btScalar(1.0) - rt;
- m_floats[0] = s * v0.x() + rt * v1.x();
- m_floats[1] = s * v0.y() + rt * v1.y();
- m_floats[2] = s * v0.z() + rt * v1.z();
+ m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
+ m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
+ m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
//don't do the unused w component
// m_co[3] = s * v0[3] + rt * v1[3];
}
@@ -199,55 +228,128 @@
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
{
- return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
- m_floats[1] + (v.y() - m_floats[1]) * t,
- m_floats[2] + (v.z() - m_floats[2]) * t);
+ return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
+ m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
+ m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
}
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
{
- m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z();
+ m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
return *this;
}
-
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
+ /**@brief Set the x value */
+ SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
+ /**@brief Set the y value */
+ SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
+ /**@brief Set the z value */
+ SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;};
+ /**@brief Set the w value */
+ SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
+ /**@brief Return the x value */
+ SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
+ /**@brief Return the y value */
+ SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
+ /**@brief Return the z value */
+ SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
+ /**@brief Return the w value */
+ SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
+ //SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
+ //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
+ ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+ SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
+ SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
+
+ SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
+ {
+ return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
+ }
+
+ SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
+ {
+ return !(*this == other);
+ }
+
+ /**@brief Set each element to the max of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMax(const btVector3& other)
+ {
+ btSetMax(m_floats[0], other.m_floats[0]);
+ btSetMax(m_floats[1], other.m_floats[1]);
+ btSetMax(m_floats[2], other.m_floats[2]);
+ btSetMax(m_floats[3], other.w());
+ }
+ /**@brief Set each element to the min of the current values and the values of another btVector3
+ * @param other The other btVector3 to compare with
+ */
+ SIMD_FORCE_INLINE void setMin(const btVector3& other)
+ {
+ btSetMin(m_floats[0], other.m_floats[0]);
+ btSetMin(m_floats[1], other.m_floats[1]);
+ btSetMin(m_floats[2], other.m_floats[2]);
+ btSetMin(m_floats[3], other.w());
+ }
+
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3] = 0.f;
+ }
+
+ void getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
+ {
+ v0->setValue(0. ,-z() ,y());
+ v1->setValue(z() ,0. ,-x());
+ v2->setValue(-y() ,x() ,0.);
+ }
+
};
/**@brief Return the sum of two vectors (Point symantics)*/
SIMD_FORCE_INLINE btVector3
operator+(const btVector3& v1, const btVector3& v2)
{
- return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z());
+ return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
}
/**@brief Return the elementwise product of two vectors */
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v1, const btVector3& v2)
{
- return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z());
+ return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
}
/**@brief Return the difference between two vectors */
SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v1, const btVector3& v2)
{
- return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z());
+ return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
}
/**@brief Return the negative of the vector */
SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v)
{
- return btVector3(-v.x(), -v.y(), -v.z());
+ return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
}
/**@brief Return the vector scaled by s */
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btScalar& s)
{
- return btVector3(v.x() * s, v.y() * s, v.z() * s);
+ return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
}
/**@brief Return the vector scaled by s */
@@ -269,7 +371,7 @@
SIMD_FORCE_INLINE btVector3
operator/(const btVector3& v1, const btVector3& v2)
{
- return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z());
+ return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
}
/**@brief Return the dot product between two vectors */
@@ -325,12 +427,8 @@
return v1.lerp(v2, t);
}
-/**@brief Test if each element of the vector is equivalent */
-SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
-{
- return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
-}
+
SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
{
return (v - *this).length2();
@@ -404,7 +502,7 @@
if (m_floats[2] > maxVal)
{
maxIndex = 2;
- maxVal = m_floats[2];
+ maxVal =m_floats[2];
}
if (m_floats[3] > maxVal)
{
@@ -437,7 +535,7 @@
if (m_floats[2] < minVal)
{
minIndex = 2;
- minVal = m_floats[2];
+ minVal =m_floats[2];
}
if (m_floats[3] < minVal)
{
@@ -455,6 +553,40 @@
return absolute4().maxAxis4();
}
+
+
+
+ /**@brief Set x,y,z and zero w
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ */
+
+
+/* void getValue(btScalar *m) const
+ {
+ m[0] = m_floats[0];
+ m[1] = m_floats[1];
+ m[2] =m_floats[2];
+ }
+*/
+/**@brief Set the values
+ * @param x Value of x
+ * @param y Value of y
+ * @param z Value of z
+ * @param w Value of w
+ */
+ SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
+ {
+ m_floats[0]=x;
+ m_floats[1]=y;
+ m_floats[2]=z;
+ m_floats[3]=w;
+ }
+
+
+
+
};
More information about the Orxonox-commit
mailing list