diff options
Diffstat (limited to 'extern/bullet2')
17 files changed, 3464 insertions, 68 deletions
diff --git a/extern/bullet2/src/Bullet-C-Api.h b/extern/bullet2/src/Bullet-C-Api.h index a196f6417bc..8074aed3038 100644 --- a/extern/bullet2/src/Bullet-C-Api.h +++ b/extern/bullet2/src/Bullet-C-Api.h @@ -162,10 +162,14 @@ extern "C" { /* extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); */ /* Continuous Collision Detection API */ + + // needed for source/blender/blenkernel/intern/collision.c + double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]); #ifdef __cplusplus } #endif + #endif //BULLET_C_API_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp new file mode 100644 index 00000000000..7d2f19998ac --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp @@ -0,0 +1,278 @@ +/* +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. +*/ +#include "btOdeContactJoint.h" +#include "btOdeSolverBody.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" + + +//this constant needs to be set up so different solvers give 'similar' results +#define FRICTION_CONSTANT 120.f + + +btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1) +:m_manifold(manifold), +m_index(index), +m_swapBodies(swap), +m_body0(body0), +m_body1(body1) +{ +} + +int m_numRows = 3; + + +void btOdeContactJoint::GetInfo1(Info1 *info) +{ + info->m = m_numRows; + //friction adds another 2... + + info->nub = 0; +} + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + +#define M_SQRT12 btScalar(0.7071067811865475244008443621048490) + +#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ + + + +void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q); +void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q) +{ + if (btFabs(n[2]) > M_SQRT12) { + // choose p in y-z plane + btScalar a = n[1]*n[1] + n[2]*n[2]; + btScalar k = dRecipSqrt (a); + p[0] = 0; + p[1] = -n[2]*k; + p[2] = n[1]*k; + // set q = n x p + q[0] = a*k; + q[1] = -n[0]*p[2]; + q[2] = n[0]*p[1]; + } + else { + // choose p in x-y plane + btScalar a = n[0]*n[0] + n[1]*n[1]; + btScalar k = dRecipSqrt (a); + p[0] = -n[1]*k; + p[1] = n[0]*k; + p[2] = 0; + // set q = n x p + q[0] = -n[2]*p[1]; + q[1] = n[2]*p[0]; + q[2] = a*k; + } +} + + + +void btOdeContactJoint::GetInfo2(Info2 *info) +{ + + int s = info->rowskip; + int s2 = 2*s; + + float swapFactor = m_swapBodies ? -1.f : 1.f; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + + + btManifoldPoint& point = m_manifold->getContactPoint(m_index); + + normal[0] = swapFactor*point.m_normalWorldOnB.x(); + normal[1] = swapFactor*point.m_normalWorldOnB.y(); + normal[2] = swapFactor*point.m_normalWorldOnB.z(); + normal[3] = 0; // @@@ hmmm + + assert(m_body0); + // if (GetBody0()) + btVector3 relativePositionA; + { + relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition; + dVector3 c1; + c1[0] = relativePositionA.x(); + c1[1] = relativePositionA.y(); + c1[2] = relativePositionA.z(); + + // set jacobian for normal + info->J1l[0] = normal[0]; + info->J1l[1] = normal[1]; + info->J1l[2] = normal[2]; + dCROSS (info->J1a,=,c1,normal); + + } + + btVector3 relativePositionB(0,0,0); + if (m_body1) + { + // if (GetBody1()) + + { + dVector3 c2; + btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0); + relativePositionB = point.getPositionWorldOnB() - posBody1; + + // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - + // j->node[1].body->pos[i]; + c2[0] = relativePositionB.x(); + c2[1] = relativePositionB.y(); + c2[2] = relativePositionB.z(); + + info->J2l[0] = -normal[0]; + info->J2l[1] = -normal[1]; + info->J2l[2] = -normal[2]; + dCROSS (info->J2a,= -,c2,normal); + } + } + + btScalar k = info->fps * info->erp; + + float depth = -point.getDistance(); +// if (depth < 0.f) +// depth = 0.f; + + info->c[0] = k * depth; + //float maxvel = .2f; + +// if (info->c[0] > maxvel) +// info->c[0] = maxvel; + + + //can override it, not necessary +// info->cfm[0] = 0.f; +// info->cfm[1] = 0.f; +// info->cfm[2] = 0.f; + + + + // set LCP limits for normal + info->lo[0] = 0; + info->hi[0] = 1e30f;//dInfinity; + info->lo[1] = 0; + info->hi[1] = 0.f; + info->lo[2] = 0.f; + info->hi[2] = 0.f; + +#define DO_THE_FRICTION_2 +#ifdef DO_THE_FRICTION_2 + // now do jacobian for tangential forces + dVector3 t1,t2; // two vectors tangential to normal + + dVector3 c1; + c1[0] = relativePositionA.x(); + c1[1] = relativePositionA.y(); + c1[2] = relativePositionA.z(); + + dVector3 c2; + c2[0] = relativePositionB.x(); + c2[1] = relativePositionB.y(); + c2[2] = relativePositionB.z(); + + //combined friction is available in the contact point + float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction; + + // first friction direction + if (m_numRows >= 2) + { + + + + dPlaneSpace1 (normal,t1,t2); + + info->J1l[s+0] = t1[0]; + info->J1l[s+1] = t1[1]; + info->J1l[s+2] = t1[2]; + dCROSS (info->J1a+s,=,c1,t1); +// if (1) { //j->node[1].body) { + info->J2l[s+0] = -t1[0]; + info->J2l[s+1] = -t1[1]; + info->J2l[s+2] = -t1[2]; + dCROSS (info->J2a+s,= -,c2,t1); +// } + // set right hand side +// if (0) {//j->contact.surface.mode & dContactMotion1) { + //info->c[1] = j->contact.surface.motion1; +// } + // set LCP bounds and friction index. this depends on the approximation + // mode + //1e30f + + + info->lo[1] = -friction;//-j->contact.surface.mu; + info->hi[1] = friction;//j->contact.surface.mu; +// if (1)//j->contact.surface.mode & dContactApprox1_1) + info->findex[1] = 0; + + // set slip (constraint force mixing) +// if (0)//j->contact.surface.mode & dContactSlip1) +// { +// // info->cfm[1] = j->contact.surface.slip1; +// } else +// { +// //info->cfm[1] = 0.f; +// } + } + + // second friction direction + if (m_numRows >= 3) { + info->J1l[s2+0] = t2[0]; + info->J1l[s2+1] = t2[1]; + info->J1l[s2+2] = t2[2]; + dCROSS (info->J1a+s2,=,c1,t2); +// if (1) { //j->node[1].body) { + info->J2l[s2+0] = -t2[0]; + info->J2l[s2+1] = -t2[1]; + info->J2l[s2+2] = -t2[2]; + dCROSS (info->J2a+s2,= -,c2,t2); +// } + + // set right hand side +// if (0){//j->contact.surface.mode & dContactMotion2) { + //info->c[2] = j->contact.surface.motion2; +// } + // set LCP bounds and friction index. this depends on the approximation + // mode +// if (0){//j->contact.surface.mode & dContactMu2) { + //info->lo[2] = -j->contact.surface.mu2; + //info->hi[2] = j->contact.surface.mu2; +// } +// else { + info->lo[2] = -friction; + info->hi[2] = friction; +// } +// if (0)//j->contact.surface.mode & dContactApprox1_2) + +// { +// info->findex[2] = 0; +// } + // set slip (constraint force mixing) +// if (0) //j->contact.surface.mode & dContactSlip2) + +// { + //info->cfm[2] = j->contact.surface.slip2; + +// } + } + +#endif //DO_THE_FRICTION_2 + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h new file mode 100644 index 00000000000..8dd0282c59e --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h @@ -0,0 +1,50 @@ +/* +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 CONTACT_JOINT_H +#define CONTACT_JOINT_H + +#include "btOdeJoint.h" +struct btOdeSolverBody; +class btPersistentManifold; + +class btOdeContactJoint : public btOdeJoint +{ + btPersistentManifold* m_manifold; + int m_index; + bool m_swapBodies; + btOdeSolverBody* m_body0; + btOdeSolverBody* m_body1; + + +public: + + btOdeContactJoint() {}; + + btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); + + //btOdeJoint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); + + + + +}; + +#endif //CONTACT_JOINT_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp new file mode 100644 index 00000000000..46c3783c6a0 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp @@ -0,0 +1,25 @@ +/* +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. +*/ + +#include "btOdeJoint.h" + +btOdeJoint::btOdeJoint() +{ + +} +btOdeJoint::~btOdeJoint() +{ + +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h new file mode 100644 index 00000000000..50733d1418f --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h @@ -0,0 +1,94 @@ +/* +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 btOdeJoint_H +#define btOdeJoint_H + +struct btOdeSolverBody; +class btOdeJoint; + +#include "LinearMath/btScalar.h" + +struct BU_ContactJointNode { + btOdeJoint *joint; // pointer to enclosing btOdeJoint object + btOdeSolverBody* body; // *other* body this joint is connected to +}; +typedef btScalar dVector3[4]; + + +class btOdeJoint { + +public: + // naming convention: the "first" body this is connected to is node[0].body, + // and the "second" body is node[1].body. if this joint is only connected + // to one body then the second body is 0. + + // info returned by getInfo1 function. the constraint dimension is m (<=6). + // i.e. that is the total number of rows in the jacobian. `nub' is the + // number of unbounded variables (which have lo,hi = -/+ infinity). + + btOdeJoint(); + virtual ~btOdeJoint(); + + + struct Info1 { + int m,nub; + }; + + // info returned by getInfo2 function + + struct Info2 { + // 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 *J1l,*J1a,*J2l,*J2a; + + // 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 *c,*cfm; + + // lo and hi limits for variables (set to -/+ infinity on entry). + btScalar *lo,*hi; + + // 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 function table: size of the joint structure, function pointers. + // we do it this way instead of using C++ virtual functions because + // sometimes we need to allocate joints ourself within a memory pool. + + virtual void GetInfo1 (Info1 *info)=0; + virtual void GetInfo2 (Info2 *info)=0; + + int flags; // dJOINT_xxx flags + BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0 + btScalar lambda[6]; // lambda generated by last step +}; + + +#endif //btOdeJoint_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h new file mode 100644 index 00000000000..e4bc2628bd4 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h @@ -0,0 +1,212 @@ +/* + * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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. +*/ + +#define ODE_MACROS +#ifdef ODE_MACROS + +#include "LinearMath/btScalar.h" + +typedef btScalar dVector4[4]; +typedef btScalar dMatrix3[4*3]; +#define dInfinity FLT_MAX + + + +#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ + + + +#define dMULTIPLY0_331NEW(A,op,B,C) \ +{\ + btScalar tmp[3];\ + tmp[0] = C.getX();\ + tmp[1] = C.getY();\ + tmp[2] = C.getZ();\ + dMULTIPLYOP0_331(A,op,B,tmp);\ +} + +#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) +#define dMULTIPLYOP0_331(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B+4),(C)); \ + (A)[2] op dDOT1((B+8),(C)); + +#define dAASSERT btAssert +#define dIASSERT btAssert + +#define REAL float +#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) +inline btScalar dDOT1 (const btScalar *a, const btScalar *b) +{ return dDOTpq(a,b,1,1); } +#define dDOT14(a,b) dDOTpq(a,b,1,4) + +#define dCROSS(a,op,b,c) \ + (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ + (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ + (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); + +/* + * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. + * A is stored by rows, and has `skip' elements per row. the matrix is + * assumed to be already zero, so this does not write zero elements! + * if (plus,minus) is (+,-) then a positive version will be written. + * if (plus,minus) is (-,+) then a negative version will be written. + */ + +#define dCROSSMAT(A,a,skip,plus,minus) \ +{ \ + (A)[1] = minus (a)[2]; \ + (A)[2] = plus (a)[1]; \ + (A)[(skip)+0] = plus (a)[2]; \ + (A)[(skip)+2] = minus (a)[0]; \ + (A)[2*(skip)+0] = minus (a)[1]; \ + (A)[2*(skip)+1] = plus (a)[0]; \ +} + + +#define dMULTIPLYOP2_333(A,op,B,C) \ + (A)[0] op dDOT1((B),(C)); \ + (A)[1] op dDOT1((B),(C+4)); \ + (A)[2] op dDOT1((B),(C+8)); \ + (A)[4] op dDOT1((B+4),(C)); \ + (A)[5] op dDOT1((B+4),(C+4)); \ + (A)[6] op dDOT1((B+4),(C+8)); \ + (A)[8] op dDOT1((B+8),(C)); \ + (A)[9] op dDOT1((B+8),(C+4)); \ + (A)[10] op dDOT1((B+8),(C+8)); + +#define dMULTIPLYOP0_333(A,op,B,C) \ + (A)[0] op dDOT14((B),(C)); \ + (A)[1] op dDOT14((B),(C+1)); \ + (A)[2] op dDOT14((B),(C+2)); \ + (A)[4] op dDOT14((B+4),(C)); \ + (A)[5] op dDOT14((B+4),(C+1)); \ + (A)[6] op dDOT14((B+4),(C+2)); \ + (A)[8] op dDOT14((B+8),(C)); \ + (A)[9] op dDOT14((B+8),(C+1)); \ + (A)[10] op dDOT14((B+8),(C+2)); + +#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) +#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) +#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) + + +//////////////////////////////////////////////////////////////////// +#define EFFICIENT_ALIGNMENT 16 +#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) +/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste + * up to 15 bytes per allocation, depending on what alloca() returns. + */ + +#define dALLOCA16(n) \ + ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) + +//#define ALLOCA dALLOCA16 + +typedef const btScalar *dRealPtr; +typedef btScalar *dRealMutablePtr; +//#define dRealArray(name,n) btScalar name[n]; +//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar)); + +/////////////////////////////////////////////////////////////////////////////// + + //Remotion: 10.10.2007 +#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) ); + +//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar)); +#define dRealAllocaArray(name,size) btScalar *name = NULL; \ + unsigned int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \ + if (memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \ + else{ btAssert(memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())); name = (btScalar*) alloca(memNeeded_##name); } + + + + + +/////////////////////////////////////////////////////////////////////////////// +#if 0 +inline void dSetZero1 (btScalar *a, int n) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = 0; + n--; + } +} + +inline void dSetValue1 (btScalar *a, int n, btScalar value) +{ + dAASSERT (a && n >= 0); + while (n > 0) { + *(a++) = value; + n--; + } +} +#else + +/// This macros are for MSVC and XCode compilers. Remotion. + + +#include <string.h> //for memset + +//Remotion: 10.10.2007 +//------------------------------------------------------------------------------ +#define IS_ALIGNED_16(x) ((size_t(x)&15)==0) +//------------------------------------------------------------------------------ +inline void dSetZero1 (btScalar *dest, int size) +{ + dAASSERT (dest && size >= 0); + memset(dest, 0, size * sizeof(btScalar)); +} +//------------------------------------------------------------------------------ +inline void dSetValue1 (btScalar *dest, int size, btScalar val) +{ + dAASSERT (dest && size >= 0); + int n_mod4 = size & 3; + int n4 = size - n_mod4; +/*#ifdef __USE_SSE__ +//it is not supported on double precision, todo... + if(IS_ALIGNED_16(dest)){ + __m128 xmm0 = _mm_set_ps1(val); + for (int i=0; i<n4; i+=4) + { + _mm_store_ps(&dest[i],xmm0); + } + }else +#endif + */ + + { + for (int i=0; i<n4; i+=4) // Unrolled Loop + { + dest[i ] = val; + dest[i+1] = val; + dest[i+2] = val; + dest[i+3] = val; + } + } + for (int i=n4; i<size; i++){ + dest[i] = val; + } +} +#endif +///////////////////////////////////////////////////////////////////// + + +#endif //USE_SOR_SOLVER + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp new file mode 100644 index 00000000000..ab90c926559 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp @@ -0,0 +1,393 @@ +/* +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. +*/ + + +#include "btOdeQuickstepConstraintSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "btOdeJoint.h" +#include "btOdeContactJoint.h" +#include "btOdeTypedJoint.h" +#include "btOdeSolverBody.h" +#include <new> +#include "LinearMath/btQuickprof.h" + +#include "LinearMath/btIDebugDraw.h" + +#define USE_SOR_SOLVER + +#include "btSorLcp.h" + +#include <math.h> +#include <float.h>//FLT_MAX +#ifdef WIN32 +#include <memory.h> +#endif +#include <string.h> +#include <stdio.h> + +#if defined (WIN32) +#include <malloc.h> +#else +#if defined (__FreeBSD__) +#include <stdlib.h> +#else +#include <alloca.h> +#endif +#endif + +class btOdeJoint; + +//see below +//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island + + +btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver(): + m_cfm(0.f),//1e-5f), + m_erp(0.4f) +{ +} + + +//iterative lcp and penalty method +btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** /*bodies*/,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/) +{ + + m_CurBody = 0; + m_CurJoint = 0; + m_CurTypedJoint = 0; + int j; + + int max_contacts = 0; /// should be 4 //Remotion + for ( j=0;j<numManifolds;j++){ + btPersistentManifold* manifold = manifoldPtr[j]; + if (manifold->getNumContacts() > max_contacts) max_contacts = manifold->getNumContacts(); + } + //if(max_contacts > 4) + // printf(" max_contacts > 4"); + + int numBodies = 0; + m_odeBodies.clear(); + m_odeBodies.reserve(numBulletBodies + 1); //??? + // btOdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES]; + + int numJoints = 0; + m_joints.clear(); + m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //??? + // btOdeJoint* joints [ODE_MAX_SOLVER_JOINTS*2]; + + m_SolverBodyArray.resize(numBulletBodies + 1); + m_JointArray.resize(numManifolds * max_contacts + 4); + m_TypedJointArray.resize(numConstraints + 1); + + + //capture contacts + int body0=-1,body1=-1; + for (j=0;j<numManifolds;j++) + { + btPersistentManifold* manifold = manifoldPtr[j]; + if (manifold->getNumContacts() > 0) + { + body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies); + body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies); + ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); + } + } + + //capture constraints + for (j=0;j<numConstraints;j++) + { + btTypedConstraint * typedconstraint = constraints[j]; + body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),m_odeBodies,numBodies); + body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies); + ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); + } + //if(numBodies > numBulletBodies) + // printf(" numBodies > numBulletBodies"); + //if(numJoints > numManifolds * 4 + numConstraints) + // printf(" numJoints > numManifolds * 4 + numConstraints"); + + + m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do + + //write back resulting velocities + for (int i=0;i<numBodies;i++) + { + if (m_odeBodies[i]->m_invMass) + { + m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity); + m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity); + } + } + + + /// Remotion, just free all this here + m_odeBodies.clear(); + m_joints.clear(); + + m_SolverBodyArray.clear(); + m_JointArray.clear(); + m_TypedJointArray.clear(); + + return 0.f; + +} + +///////////////////////////////////////////////////////////////////////////////// + + +typedef btScalar dQuaternion[4]; +#define _R(i,j) R[(i)*4+(j)] + +void dRfromQ1 (dMatrix3 R, const dQuaternion q); +void dRfromQ1 (dMatrix3 R, const dQuaternion q) +{ + // q = (s,vx,vy,vz) + btScalar qq1 = 2.f*q[1]*q[1]; + btScalar qq2 = 2.f*q[2]*q[2]; + btScalar qq3 = 2.f*q[3]*q[3]; + _R(0,0) = 1.f - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(0,3) = 0.f; + + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1.f - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(1,3) = 0.f; + + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1.f - qq1 - qq2; + _R(2,3) = 0.f; + +} + + + +//int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btOdeSolverBody** bodies,int& numBodies) +int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies) +{ + assert(orgBody); + if (!orgBody || (orgBody->getInvMass() == 0.f) ) + { + return -1; + } + + if (orgBody->getCompanionId()>=0) + { + return orgBody->getCompanionId(); + } + //first try to find + int i,j; + + //if not found, create a new body + // btOdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; + btOdeSolverBody* body = &m_SolverBodyArray[numBodies]; + bodies.push_back(body); // Remotion 10.10.07: + + orgBody->setCompanionId(numBodies); + + numBodies++; + + body->m_originalBody = orgBody; + + body->m_facc.setValue(0,0,0,0); + body->m_tacc.setValue(0,0,0,0); + + body->m_linearVelocity = orgBody->getLinearVelocity(); + body->m_angularVelocity = orgBody->getAngularVelocity(); + body->m_invMass = orgBody->getInvMass(); + body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition(); + body->m_friction = orgBody->getFriction(); + + //are the indices the same ? + for (i=0;i<4;i++) + { + for ( j=0;j<3;j++) + { + body->m_invI[i+4*j] = 0.f; + body->m_I[i+4*j] = 0.f; + } + } + body->m_invI[0+4*0] = orgBody->getInvInertiaDiagLocal().x(); + body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y(); + body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z(); + + body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x(); + body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y(); + body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z(); + + + + + dQuaternion q; + + q[1] = orgBody->getOrientation().x(); + q[2] = orgBody->getOrientation().y(); + q[3] = orgBody->getOrientation().z(); + q[0] = orgBody->getOrientation().w(); + + dRfromQ1(body->m_R,q); + + return numBodies-1; +} + + + + + + + + + +void btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* manifold, + btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, + const btAlignedObjectArray< btOdeSolverBody*> &bodies, + int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) +{ + + + manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), + ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + + int i,numContacts = manifold->getNumContacts(); + + bool swapBodies = (bodyId0 < 0); + + + btOdeSolverBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); + + } + else + { + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); + } + + assert(bodyId0 >= 0); + + btVector3 color(0,1,0); + for (i=0;i<numContacts;i++) + { + + //assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS); + +// if (manifold->getContactPoint(i).getDistance() < 0.0f) + { + + btOdeContactJoint* cont = new (&m_JointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1); + //btOdeContactJoint* cont = new (&gJointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1); + + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + // joints[numJoints++] = cont; + joints.push_back(cont); // Remotion 10.10.07: + numJoints++; + + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + } + + //create a new contact constraint +} + +void btOdeQuickstepConstraintSolver::ConvertTypedConstraint( + btTypedConstraint * constraint, + btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, + const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* /*debugDrawer*/) +{ + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + bool swapBodies = (bodyId0 < 0); + + + btOdeSolverBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); + + } + else + { + body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); + body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); + } + + assert(bodyId0 >= 0); + + + //assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS); + + + btOdeTypedJoint * cont = NULL; + + // Determine constraint type + int joint_type = constraint->getConstraintType(); + switch(joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + case D6_CONSTRAINT_TYPE: + cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1); + //cont = new (&gTypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1); + break; + + }; + + if(cont) + { + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + // joints[numJoints++] = cont; + joints.push_back(cont); // Remotion 10.10.07: + numJoints++; + + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h new file mode 100644 index 00000000000..e548ea6fc22 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h @@ -0,0 +1,109 @@ +/* + * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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 ODE_CONSTRAINT_SOLVER_H +#define ODE_CONSTRAINT_SOLVER_H + +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" + +#include "LinearMath/btAlignedObjectArray.h" +#include "btOdeContactJoint.h" +#include "btOdeTypedJoint.h" +#include "btOdeSolverBody.h" +#include "btSorLcp.h" + +class btRigidBody; +struct btOdeSolverBody; +class btOdeJoint; + +/// btOdeQuickstepConstraintSolver is one of the available solvers for Bullet dynamics framework +/// It uses an adapted version quickstep solver from the Open Dynamics Engine project +class btOdeQuickstepConstraintSolver : public btConstraintSolver +{ +private: + int m_CurBody; + int m_CurJoint; + int m_CurTypedJoint; + + float m_cfm; + float m_erp; + + btSorLcpSolver m_SorLcpSolver; + + btAlignedObjectArray<btOdeSolverBody*> m_odeBodies; + btAlignedObjectArray<btOdeJoint*> m_joints; + + btAlignedObjectArray<btOdeSolverBody> m_SolverBodyArray; + btAlignedObjectArray<btOdeContactJoint> m_JointArray; + btAlignedObjectArray<btOdeTypedJoint> m_TypedJointArray; + + +private: + int ConvertBody(btRigidBody* body,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies); + void ConvertConstraint(btPersistentManifold* manifold, + btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, + const btAlignedObjectArray< btOdeSolverBody*> &bodies, + int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); + + void ConvertTypedConstraint( + btTypedConstraint * constraint, + btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints, + const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); + + +public: + + btOdeQuickstepConstraintSolver(); + + virtual ~btOdeQuickstepConstraintSolver() {} + + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); + + ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal + ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' + void setConstraintForceMixing(float cfm) { + m_cfm = cfm; + } + + ///setErrorReductionParamter sets the maximum amount of error reduction + ///which limits energy addition during penetration depth recovery + void setErrorReductionParamter(float erp) + { + m_erp = erp; + } + + ///clear internal cached data and reset random seed + void reset() + { + m_SorLcpSolver.dRand2_seed = 0; + } + + void setRandSeed(unsigned long seed) + { + m_SorLcpSolver.dRand2_seed = seed; + } + unsigned long getRandSeed() const + { + return m_SorLcpSolver.dRand2_seed; + } +}; + + + + +#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h new file mode 100644 index 00000000000..0c936971b79 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h @@ -0,0 +1,48 @@ +/* +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 ODE_SOLVER_BODY_H +#define ODE_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +typedef btScalar dMatrix3[4*3]; + +///ODE's quickstep needs just a subset of the rigidbody data in its own layout, so make a temp copy +struct btOdeSolverBody +{ + btRigidBody* m_originalBody; + + btVector3 m_centerOfMassPosition; + /// for ode solver-binding + dMatrix3 m_R;//temp + dMatrix3 m_I; + dMatrix3 m_invI; + + int m_odeTag; + float m_invMass; + float m_friction; + + btVector3 m_tacc;//temp + btVector3 m_facc; + + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + +}; + + +#endif //#ifndef ODE_SOLVER_BODY_H + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp new file mode 100644 index 00000000000..f683bf7d748 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp @@ -0,0 +1,880 @@ +/* +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. +*/ +#include "btOdeTypedJoint.h" +#include "btOdeSolverBody.h" +#include "btOdeMacros.h" +#include <stdio.h> + +void btOdeTypedJoint::GetInfo1(Info1 *info) +{ + int joint_type = m_constraint->getConstraintType(); + switch (joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + p2pjoint.GetInfo1(info); + } + break; + case D6_CONSTRAINT_TYPE: + { + OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + d6joint.GetInfo1(info); + } + break; + case SLIDER_CONSTRAINT_TYPE: + { + OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + sliderjoint.GetInfo1(info); + } + break; + }; +} + +void btOdeTypedJoint::GetInfo2(Info2 *info) +{ + int joint_type = m_constraint->getConstraintType(); + switch (joint_type) + { + case POINT2POINT_CONSTRAINT_TYPE: + { + OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + p2pjoint.GetInfo2(info); + } + break; + case D6_CONSTRAINT_TYPE: + { + OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + d6joint.GetInfo2(info); + } + break; + case SLIDER_CONSTRAINT_TYPE: + { + OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); + sliderjoint.GetInfo2(info); + } + break; + }; +} + + +OdeP2PJoint::OdeP2PJoint( + btTypedConstraint * constraint, + int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): + btOdeTypedJoint(constraint,index,swap,body0,body1) +{ +} + + +void OdeP2PJoint::GetInfo1(Info1 *info) +{ + info->m = 3; + info->nub = 3; +} + + +void OdeP2PJoint::GetInfo2(Info2 *info) +{ + + btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint(); + + //retrieve matrices + btTransform body0_trans; + if (m_body0) + { + body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); + } +// btScalar body0_mat[12]; +// body0_mat[0] = body0_trans.getBasis()[0][0]; +// body0_mat[1] = body0_trans.getBasis()[0][1]; +// body0_mat[2] = body0_trans.getBasis()[0][2]; +// body0_mat[4] = body0_trans.getBasis()[1][0]; +// body0_mat[5] = body0_trans.getBasis()[1][1]; +// body0_mat[6] = body0_trans.getBasis()[1][2]; +// body0_mat[8] = body0_trans.getBasis()[2][0]; +// body0_mat[9] = body0_trans.getBasis()[2][1]; +// body0_mat[10] = body0_trans.getBasis()[2][2]; + + btTransform body1_trans; + + if (m_body1) + { + body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); + } +// btScalar body1_mat[12]; +// body1_mat[0] = body1_trans.getBasis()[0][0]; +// body1_mat[1] = body1_trans.getBasis()[0][1]; +// body1_mat[2] = body1_trans.getBasis()[0][2]; +// body1_mat[4] = body1_trans.getBasis()[1][0]; +// body1_mat[5] = body1_trans.getBasis()[1][1]; +// body1_mat[6] = body1_trans.getBasis()[1][2]; +// body1_mat[8] = body1_trans.getBasis()[2][0]; +// body1_mat[9] = body1_trans.getBasis()[2][1]; +// body1_mat[10] = body1_trans.getBasis()[2][2]; + + + + + // anchor points in global coordinates with respect to body PORs. + + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + + btVector3 a1,a2; + + a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->J1a,a1,s,-,+); + if (m_body1) + { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB(); + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + + // set right hand side + btScalar k = info->fps * info->erp; + if (m_body1) + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - + a1[j] - body0_trans.getOrigin()[j]); + } + } + else + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - + body0_trans.getOrigin()[j]); + } + } +} + + +///////////////////limit motor support + +/*! \pre testLimitValue must be called on limot*/ +int bt_get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational) +{ + + + int srow = row * info->rowskip; + + // if the joint is powered, or has joint limits, add in the extra row + int powered = limot->m_enableMotor; + int limit = limot->m_currentLimit; + + if (powered || limit) + { + btScalar *J1 = rotational ? info->J1a : info->J1l; + btScalar *J2 = rotational ? info->J2a : info->J2l; + + J1[srow+0] = ax1[0]; + J1[srow+1] = ax1[1]; + J1[srow+2] = ax1[2]; + if (body1) + { + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + } + + // linear limot torque decoupling step: + // + // if this is a linear limot (e.g. from a slider), 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 powered or limited slider-jointed free + // bodies from gaining angular momentum. + // the solution used here is to apply the constraint forces at the point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + + btVector3 ltd; // Linear Torque Decoupling vector (a torque) + if (!rotational && body1) + { + btVector3 c; + c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0] + -body0->getCenterOfMassPosition()[0]); + c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1] + -body0->getCenterOfMassPosition()[1]); + c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2] + -body0->getCenterOfMassPosition()[2]); + + ltd = c.cross(ax1); + + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[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; + + if (powered) + { + info->cfm[row] = 0.0f;//limot->m_normalCFM; + if (! limit) + { + info->c[row] = limot->m_targetVelocity; + info->lo[row] = -limot->m_maxMotorForce; + info->hi[row] = limot->m_maxMotorForce; + } + } + + if (limit) + { + btScalar k = info->fps * limot->m_ERP; + info->c[row] = -k * limot->m_currentLimitError; + info->cfm[row] = 0.0f;//limot->m_stopCFM; + + if (limot->m_loLimit == limot->m_hiLimit) + { + // limited low and high simultaneously + info->lo[row] = -dInfinity; + info->hi[row] = dInfinity; + } + else + { + if (limit == 1) + { + // low limit + info->lo[row] = 0; + info->hi[row] = SIMD_INFINITY; + } + else + { + // high limit + info->lo[row] = -SIMD_INFINITY; + info->hi[row] = 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) + { + // low limit + if (vel < 0) + { + btScalar newc = -limot->m_bounce* vel; + if (newc > info->c[row]) info->c[row] = newc; + } + } + else + { + // high limit - all those computations are reversed + if (vel > 0) + { + btScalar newc = -limot->m_bounce * vel; + if (newc < info->c[row]) info->c[row] = newc; + } + } + } + } + } + return 1; + } + else return 0; +} + + +///////////////////OdeD6Joint + + + + + +OdeD6Joint::OdeD6Joint( + btTypedConstraint * constraint, + int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): + btOdeTypedJoint(constraint,index,swap,body0,body1) +{ +} + + +void OdeD6Joint::GetInfo1(Info1 *info) +{ + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + //prepare constraint + d6constraint->calculateTransforms(); + info->m = 3; + info->nub = 3; + + //test angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(d6constraint->testAngularLimitMotor(i)) + { + info->m++; + } + } + + +} + + +int OdeD6Joint::setLinearLimits(Info2 *info) +{ + + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + + //retrieve matrices + btTransform body0_trans; + if (m_body0) + { + body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); + } + + btTransform body1_trans; + + if (m_body1) + { + body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); + } + + // anchor points in global coordinates with respect to body PORs. + + int s = info->rowskip; + + // set jacobian + info->J1l[0] = 1; + info->J1l[s+1] = 1; + info->J1l[2*s+2] = 1; + + + btVector3 a1,a2; + + a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); + //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); + dCROSSMAT (info->J1a,a1,s,-,+); + if (m_body1) + { + info->J2l[0] = -1; + info->J2l[s+1] = -1; + info->J2l[2*s+2] = -1; + a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); + + //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); + dCROSSMAT (info->J2a,a2,s,+,-); + } + + + // set right hand side + btScalar k = info->fps * info->erp; + if (m_body1) + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - + a1[j] - body0_trans.getOrigin()[j]); + } + } + else + { + for (int j=0; j<3; j++) + { + info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] - + body0_trans.getOrigin()[j]); + } + } + + return 3; + +} + +int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset) +{ + btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); + int row = row_offset; + //solve angular limits + for (int i=0;i<3 ;i++ ) + { + //if(i==2) continue; + if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) + { + btVector3 axis = d6constraint->getAxis(i); + row += bt_get_limit_motor_info2( + d6constraint->getRotationalLimitMotor(i), + m_body0->m_originalBody, + m_body1 ? m_body1->m_originalBody : NULL, + info,row,axis,1); + } + } + + return row; +} + +void OdeD6Joint::GetInfo2(Info2 *info) +{ + int row = setLinearLimits(info); + setAngularLimits(info, row); +} + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- +/* +OdeSliderJoint +Ported from ODE by Roman Ponomarev (rponom@gmail.com) +April 24, 2008 +*/ + +OdeSliderJoint::OdeSliderJoint( + btTypedConstraint * constraint, + int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1): + btOdeTypedJoint(constraint,index,swap,body0,body1) +{ +} // OdeSliderJoint::OdeSliderJoint() + +//---------------------------------------------------------------------------------- + +void OdeSliderJoint::GetInfo1(Info1* info) +{ + info->nub = 4; + info->m = 4; // Fixed 2 linear + 2 angular + btSliderConstraint * slider = this->getSliderConstraint(); + //prepare constraint + slider->calculateTransforms(); + slider->testLinLimits(); + if(slider->getSolveLinLimit() || slider->getPoweredLinMotor()) + { + info->m++; // limit 3rd linear as well + } + slider->testAngLimits(); + if(slider->getSolveAngLimit() || slider->getPoweredAngMotor()) + { + info->m++; // limit 3rd angular as well + } +} // OdeSliderJoint::GetInfo1() + +//---------------------------------------------------------------------------------- + +void OdeSliderJoint::GetInfo2(Info2 *info) +{ + int i, s = info->rowskip; + btSliderConstraint * slider = this->getSliderConstraint(); + const btTransform& trA = slider->getCalculatedTransformA(); + const btTransform& trB = slider->getCalculatedTransformB(); + // 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->J1a[0] = p[0]; + info->J1a[1] = p[1]; + info->J1a[2] = p[2]; + info->J1a[s+0] = q[0]; + info->J1a[s+1] = q[1]; + info->J1a[s+2] = q[2]; + if(m_body1) + { + info->J2a[0] = -p[0]; + info->J2a[1] = -p[1]; + info->J2a[2] = -p[2]; + info->J2a[s+0] = -q[0]; + info->J2a[s+1] = -q[1]; + info->J2a[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 * slider->getSoftnessOrthoAng(); + btVector3 ax2 = trB.getBasis().getColumn(0); + btVector3 u; + if(m_body1) + { + u = ax1.cross(ax2); + } + else + { + u = ax2.cross(ax1); + } + info->c[0] = k * u.dot(p); + info->c[1] = 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 substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2. + btTransform bodyA_trans = m_body0->m_originalBody->getCenterOfMassTransform(); + btTransform bodyB_trans; + if(m_body1) + { + bodyB_trans = m_body1->m_originalBody->getCenterOfMassTransform(); + } + int s2 = 2 * s, s3 = 3 * s; + btVector3 c; + if(m_body1) + { + c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin(); + btVector3 tmp = btScalar(0.5) * c.cross(p); + + for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i]; + + tmp = btScalar(0.5) * c.cross(q); + + for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i]; + for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; + + for (i=0; i<3; i++) info->J2l[s2+i] = -p[i]; + for (i=0; i<3; i++) info->J2l[s3+i] = -q[i]; + } + for (i=0; i<3; i++) info->J1l[s2+i] = p[i]; + for (i=0; i<3; i++) info->J1l[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 + if(m_body1) + { + ofs = trB.getOrigin() - trA.getOrigin(); + } + else + { + ofs = trA.getOrigin() - trB.getOrigin(); + } + k = info->fps * info->erp * slider->getSoftnessOrthoLin(); + info->c[2] = k * p.dot(ofs); + info->c[3] = 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(slider->getSolveLinLimit()) + { + limit_err = slider->getLinDepth(); + if(m_body1) + { + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + else + { + limit = (limit_err > btScalar(0.0)) ? 2 : 1; + } + } + int powered = 0; + if(slider->getPoweredLinMotor()) + { + powered = 1; + } + // if the slider has joint limits, add in the extra row + if (limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->J1l[srow+0] = ax1[0]; + info->J1l[srow+1] = ax1[1]; + info->J1l[srow+2] = ax1[2]; + if(m_body1) + { + info->J2l[srow+0] = -ax1[0]; + info->J2l[srow+1] = -ax1[1]; + info->J2l[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 point + // halfway between the body centers. there is no penalty (other than an + // extra tiny bit of computation) in doing this adjustment. note that we + // only need to do this if the constraint connects two bodies. + if (m_body1) + { + dVector3 ltd; // Linear Torque Decoupling vector (a torque) + c = btScalar(0.5) * c; + dCROSS (ltd,=,c,ax1); + info->J1a[srow+0] = ltd[0]; + info->J1a[srow+1] = ltd[1]; + info->J1a[srow+2] = ltd[2]; + info->J2a[srow+0] = ltd[0]; + info->J2a[srow+1] = ltd[1]; + info->J2a[srow+2] = ltd[2]; + } + // right-hand part + btScalar lostop = slider->getLowerLinLimit(); + btScalar histop = slider->getUpperLinLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + if(powered) + { + info->cfm[nrow] = btScalar(0.0); + if(!limit) + { + info->c[nrow] = slider->getTargetLinMotorVelocity(); + info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps; + info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps; + } + } + if(limit) + { + k = info->fps * info->erp; + if(m_body1) + { + info->c[nrow] = k * limit_err; + } + else + { + info->c[nrow] = - k * limit_err; + } + info->cfm[nrow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { + // limited low and high simultaneously + info->lo[nrow] = -SIMD_INFINITY; + info->hi[nrow] = SIMD_INFINITY; + } + else + { + if(limit == 1) + { + // low limit + info->lo[nrow] = 0; + info->hi[nrow] = SIMD_INFINITY; + } + else + { + // high limit + info->lo[nrow] = -SIMD_INFINITY; + info->hi[nrow] = 0; + } + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) + btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimLin()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_body0->m_originalBody->getLinearVelocity().dot(ax1); + if(m_body1) + { + vel -= m_body1->m_originalBody->getLinearVelocity().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->c[nrow]) info->c[nrow] = newc; + } + } + else + { + // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->c[nrow]) info->c[nrow] = newc; + } + } + } + info->c[nrow] *= slider->getSoftnessLimLin(); + } // if(limit) + } // if linear limit + // check angular limits + limit_err = btScalar(0.0); + limit = 0; + if(slider->getSolveAngLimit()) + { + limit_err = slider->getAngDepth(); + if(m_body1) + { + limit = (limit_err > btScalar(0.0)) ? 1 : 2; + } + else + { + limit = (limit_err > btScalar(0.0)) ? 2 : 1; + } + } + // if the slider has joint limits, add in the extra row + powered = 0; + if(slider->getPoweredAngMotor()) + { + powered = 1; + } + if(limit || powered) + { + nrow++; + srow = nrow * info->rowskip; + info->J1a[srow+0] = ax1[0]; + info->J1a[srow+1] = ax1[1]; + info->J1a[srow+2] = ax1[2]; + if(m_body1) + { + info->J2a[srow+0] = -ax1[0]; + info->J2a[srow+1] = -ax1[1]; + info->J2a[srow+2] = -ax1[2]; + } + btScalar lostop = slider->getLowerAngLimit(); + btScalar histop = slider->getUpperAngLimit(); + if(limit && (lostop == histop)) + { // the joint motor is ineffective + powered = 0; + } + if(powered) + { + info->cfm[nrow] = btScalar(0.0); + if(!limit) + { + info->c[nrow] = slider->getTargetAngMotorVelocity(); + info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps; + info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps; + } + } + if(limit) + { + k = info->fps * info->erp; + if (m_body1) + { + info->c[nrow] = k * limit_err; + } + else + { + info->c[nrow] = -k * limit_err; + } + info->cfm[nrow] = btScalar(0.0); // stop_cfm; + if(lostop == histop) + { + // limited low and high simultaneously + info->lo[nrow] = -SIMD_INFINITY; + info->hi[nrow] = SIMD_INFINITY; + } + else + { + if (limit == 1) + { + // low limit + info->lo[nrow] = 0; + info->hi[nrow] = SIMD_INFINITY; + } + else + { + // high limit + info->lo[nrow] = -SIMD_INFINITY; + info->hi[nrow] = 0; + } + } + // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) + btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimAng()); + if(bounce > btScalar(0.0)) + { + btScalar vel = m_body0->m_originalBody->getAngularVelocity().dot(ax1); + if(m_body1) + { + vel -= m_body1->m_originalBody->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->c[nrow]) info->c[nrow] = newc; + } + } + else + { + // high limit - all those computations are reversed + if(vel > 0) + { + btScalar newc = -bounce * vel; + if(newc < info->c[nrow]) info->c[nrow] = newc; + } + } + } + info->c[nrow] *= slider->getSoftnessLimAng(); + } // if(limit) + } // if angular limit or powered +} // OdeSliderJoint::GetInfo2() + +//---------------------------------------------------------------------------------- +//---------------------------------------------------------------------------------- + + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h new file mode 100644 index 00000000000..a2affda382d --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h @@ -0,0 +1,142 @@ +/* +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. +*/ +/* +2007-09-09 +Added support for typed joints by Francisco Le?n +email: projectileman@yahoo.com +http://gimpact.sf.net +*/ + +#ifndef TYPED_JOINT_H +#define TYPED_JOINT_H + +#include "btOdeJoint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" + +struct btOdeSolverBody; + +class btOdeTypedJoint : public btOdeJoint +{ +public: + btTypedConstraint * m_constraint; + int m_index; + bool m_swapBodies; + btOdeSolverBody* m_body0; + btOdeSolverBody* m_body1; + + btOdeTypedJoint(){} + btOdeTypedJoint( + btTypedConstraint * constraint, + int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1): + m_constraint(constraint), + m_index(index), + m_swapBodies(swap), + m_body0(body0), + m_body1(body1) + { + } + + virtual void GetInfo1(Info1 *info); + virtual void GetInfo2(Info2 *info); +}; + + + +class OdeP2PJoint : public btOdeTypedJoint +{ +protected: + inline btPoint2PointConstraint * getP2PConstraint() + { + return static_cast<btPoint2PointConstraint * >(m_constraint); + } +public: + + OdeP2PJoint() {}; + + OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); + + //btOdeJoint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); +}; + + +class OdeD6Joint : public btOdeTypedJoint +{ +protected: + inline btGeneric6DofConstraint * getD6Constraint() + { + return static_cast<btGeneric6DofConstraint * >(m_constraint); + } + + int setLinearLimits(Info2 *info); + int setAngularLimits(Info2 *info, int row_offset); + +public: + + OdeD6Joint() {}; + + OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1); + + //btOdeJoint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); +}; + +//! retrieves the constraint info from a btRotationalLimitMotor object +/*! \pre testLimitValue must be called on limot*/ +int bt_get_limit_motor_info2( + btRotationalLimitMotor * limot, + btRigidBody * body0, btRigidBody * body1, + btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational); + +/* +OdeSliderJoint +Ported from ODE by Roman Ponomarev (rponom@gmail.com) +April 24, 2008 +*/ +class OdeSliderJoint : public btOdeTypedJoint +{ +protected: + inline btSliderConstraint * getSliderConstraint() + { + return static_cast<btSliderConstraint * >(m_constraint); + } +public: + + OdeSliderJoint() {}; + + OdeSliderJoint(btTypedConstraint* constraint,int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1); + + //BU_Joint interface for solver + + virtual void GetInfo1(Info1 *info); + + virtual void GetInfo2(Info2 *info); +}; + + + + +#endif //CONTACT_JOINT_H + + + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp new file mode 100644 index 00000000000..175d15dcfcf --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp @@ -0,0 +1,683 @@ +/* + * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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. +*/ + +#include "btSorLcp.h" +#include "btOdeSolverBody.h" + +#ifdef USE_SOR_SOLVER + +// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver. +#include "LinearMath/btScalar.h" + +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include <math.h> +#include <float.h>//FLT_MAX +#ifdef WIN32 +#include <memory.h> +#endif +#include <string.h> +#include <stdio.h> + +#if defined (WIN32) +#include <malloc.h> +#else +#if defined (__FreeBSD__) +#include <stdlib.h> +#else +#include <alloca.h> +#endif +#endif + +#include "btOdeJoint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +//////////////////////////////////////////////////////////////////// +//math stuff +#include "btOdeMacros.h" + +//*************************************************************************** +// configuration + +// for the SOR and CG methods: +// uncomment the following line to use warm starting. this definitely +// help for motor-driven joints. unfortunately it appears to hurt +// with high-friction contacts using the SOR method. use with care + +//#define WARM_STARTING 1 + +// for the SOR method: +// uncomment the following line to randomly reorder constraint rows +// during the solution. depending on the situation, this can help a lot +// or hardly at all, but it doesn't seem to hurt. + +#define RANDOMLY_REORDER_CONSTRAINTS 1 + +//*************************************************************************** +// various common computations involving the matrix J +// compute iMJ = inv(M)*J' +inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, + //OdeSolverBody* const *body, + const btAlignedObjectArray<btOdeSolverBody*> &body, + dRealPtr invI) +{ + int i,j; + dRealMutablePtr iMJ_ptr = iMJ; + dRealMutablePtr J_ptr = J; + for (i=0; i<m; i++) { + int b1 = jb[i*2]; + int b2 = jb[i*2+1]; + btScalar k = body[b1]->m_invMass; + for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; + dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); + if (b2 >= 0) { + k = body[b2]->m_invMass; + for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; + dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); + } + J_ptr += 12; + iMJ_ptr += 12; + } +} + +#if 0 +static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) +{ + int i,j; + + + + dRealMutablePtr out_ptr1 = out + onlyBody1*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + + if (onlyBody2 >= 0) + { + out_ptr1 = out + onlyBody2*6; + + for (j=0; j<6; j++) + out_ptr1[j] = 0; + } + + dRealPtr iMJ_ptr = iMJ; + for (i=0; i<m; i++) { + + int b1 = jb[i*2]; + + dRealMutablePtr out_ptr = out + b1*6; + if ((b1 == onlyBody1) || (b1 == onlyBody2)) + { + for (j=0; j<6; j++) + out_ptr[j] += iMJ_ptr[j] * in[i] ; + } + + iMJ_ptr += 6; + + int b2 = jb[i*2+1]; + if ((b2 == onlyBody1) || (b2 == onlyBody2)) + { + if (b2 >= 0) + { + out_ptr = out + b2*6; + for (j=0; j<6; j++) + out_ptr[j] += iMJ_ptr[j] * in[i]; + } + } + + iMJ_ptr += 6; + + } +} +#endif + + +// compute out = inv(M)*J'*in. + +#if 0 +static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dSetZero1 (out,6*nb); + dRealPtr iMJ_ptr = iMJ; + for (i=0; i<m; i++) { + int b1 = jb[i*2]; + int b2 = jb[i*2+1]; + dRealMutablePtr out_ptr = out + b1*6; + for (j=0; j<6; j++) + out_ptr[j] += iMJ_ptr[j] * in[i]; + iMJ_ptr += 6; + if (b2 >= 0) { + out_ptr = out + b2*6; + for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; + } + iMJ_ptr += 6; + } +} +#endif + + +// compute out = J*in. +inline void multiply_J (int m, dRealMutablePtr J, int *jb, + dRealMutablePtr in, dRealMutablePtr out) +{ + int i,j; + dRealPtr J_ptr = J; + for (i=0; i<m; i++) { + int b1 = jb[i*2]; + int b2 = jb[i*2+1]; + btScalar sum = 0; + dRealMutablePtr in_ptr = in + b1*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + J_ptr += 6; + if (b2 >= 0) { + in_ptr = in + b2*6; + for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; + } + J_ptr += 6; + out[i] = sum; + } +} + +//*************************************************************************** +// SOR-LCP method + +// nb is the number of bodies in the body array. +// J is an m*12 matrix of constraint rows +// jb is an array of first and second body numbers for each constraint row +// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) +// +// this returns lambda and fc (the constraint force). +// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda +// +// b, lo and hi are modified on exit + +//------------------------------------------------------------------------------ +ATTRIBUTE_ALIGNED16(struct) IndexError { + btScalar error; // error to sort on + int findex; + int index; // row index +}; + +//------------------------------------------------------------------------------ +void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, + const btAlignedObjectArray<btOdeSolverBody*> &body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + int numiter,float overRelax, + btStackAlloc* stackAlloc + ) +{ + //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 + AutoBlockSa asaBlock(stackAlloc); + + const int num_iterations = numiter; + const float sor_w = overRelax; // SOR over-relaxation parameter + + int i,j; + +#ifdef WARM_STARTING + // for warm starting, this seems to be necessary to prevent + // jerkiness in motor-driven joints. i have no idea why this works. + for (i=0; i<m; i++) lambda[i] *= 0.9; +#else + dSetZero1 (lambda,m); +#endif + + // the lambda computed at the previous iteration. + // this is used to measure error for when we are reordering the indexes. + dRealAllocaArray (last_lambda,m); + + // a copy of the 'hi' vector in case findex[] is being used + dRealAllocaArray (hicopy,m); + memcpy (hicopy,hi,m*sizeof(float)); + + // precompute iMJ = inv(M)*J' + dRealAllocaArray (iMJ,m*12); + compute_invM_JT (m,J,iMJ,jb,body,invI); + + // compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc + // as we change lambda. +#ifdef WARM_STARTING + multiply_invM_JT (m,nb,iMJ,jb,lambda,fc); +#else + dSetZero1 (invMforce,nb*6); +#endif + + // precompute 1 / diagonals of A + dRealAllocaArray (Ad,m); + dRealPtr iMJ_ptr = iMJ; + dRealMutablePtr J_ptr = J; + for (i=0; i<m; i++) { + float sum = 0; + for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j]; + if (jb[i*2+1] >= 0) { + for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; + } + iMJ_ptr += 12; + J_ptr += 12; + Ad[i] = sor_w / sum;//(sum + cfm[i]); + } + + // scale J and b by Ad + J_ptr = J; + for (i=0; i<m; i++) { + for (j=0; j<12; j++) { + J_ptr[0] *= Ad[i]; + J_ptr++; + } + rhs[i] *= Ad[i]; + } + + // scale Ad by CFM + for (i=0; i<m; i++) + Ad[i] *= cfm[i]; + + // order to solve constraint rows in + //IndexError *order = (IndexError*) alloca (m*sizeof(IndexError)); + IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError)); + + +#ifndef REORDER_CONSTRAINTS + // make sure constraints with findex < 0 come first. + j=0; + for (i=0; i<m; i++) + if (findex[i] < 0) + order[j++].index = i; + for (i=0; i<m; i++) + if (findex[i] >= 0) + order[j++].index = i; + dIASSERT (j==m); +#endif + + for (int iteration=0; iteration < num_iterations; iteration++) { + +#ifdef REORDER_CONSTRAINTS + // constraints with findex < 0 always come first. + if (iteration < 2) { + // for the first two iterations, solve the constraints in + // the given order + for (i=0; i<m; i++) { + order[i].error = i; + order[i].findex = findex[i]; + order[i].index = i; + } + } + else { + // sort the constraints so that the ones converging slowest + // get solved last. use the absolute (not relative) error. + for (i=0; i<m; i++) { + float v1 = dFabs (lambda[i]); + float v2 = dFabs (last_lambda[i]); + float max = (v1 > v2) ? v1 : v2; + if (max > 0) { + //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; + order[i].error = dFabs(lambda[i]-last_lambda[i]); + } + else { + order[i].error = dInfinity; + } + order[i].findex = findex[i]; + order[i].index = i; + } + } + qsort (order,m,sizeof(IndexError),&compare_index_error); +#endif +#ifdef RANDOMLY_REORDER_CONSTRAINTS + if ((iteration & 7) == 0) { + for (i=1; i<m; ++i) { + IndexError tmp = order[i]; + int swapi = dRandInt2(i+1); + order[i] = order[swapi]; + order[swapi] = tmp; + } + } +#endif + + //@@@ potential optimization: swap lambda and last_lambda pointers rather + // than copying the data. we must make sure lambda is properly + // returned to the caller + memcpy (last_lambda,lambda,m*sizeof(float)); + + for (int i=0; i<m; i++) { + // @@@ potential optimization: we could pre-sort J and iMJ, thereby + // linearizing access to those arrays. hmmm, this does not seem + // like a win, but we should think carefully about our memory + // access pattern. + + int index = order[i].index; + J_ptr = J + index*12; + iMJ_ptr = iMJ + index*12; + + // set the limits for this constraint. note that 'hicopy' is used. + // this is the place where the QuickStep method differs from the + // direct LCP solving method, since that method only performs this + // limit adjustment once per time step, whereas this method performs + // once per iteration per constraint row. + // the constraints are ordered so that all lambda[] values needed have + // already been computed. + if (findex[index] >= 0) { + hi[index] = btFabs (hicopy[index] * lambda[findex[index]]); + lo[index] = -hi[index]; + } + + int b1 = jb[index*2]; + int b2 = jb[index*2+1]; + float delta = rhs[index] - lambda[index]*Ad[index]; + dRealMutablePtr fc_ptr = invMforce + 6*b1; + + // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case + delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + + fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + + fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + + fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + + fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; + } + + // compute lambda and clamp it to [lo,hi]. + // @@@ potential optimization: does SSE have clamping instructions + // to save test+jump penalties here? + float new_lambda = lambda[index] + delta; + if (new_lambda < lo[index]) { + delta = lo[index]-lambda[index]; + lambda[index] = lo[index]; + } + else if (new_lambda > hi[index]) { + delta = hi[index]-lambda[index]; + lambda[index] = hi[index]; + } + else { + lambda[index] = new_lambda; + } + + //@@@ a trick that may or may not help + //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); + //delta *= ramp; + + // update invMforce. + // @@@ potential optimization: SIMD for this and the b2 >= 0 case + fc_ptr = invMforce + 6*b1; + fc_ptr[0] += delta * iMJ_ptr[0]; + fc_ptr[1] += delta * iMJ_ptr[1]; + fc_ptr[2] += delta * iMJ_ptr[2]; + fc_ptr[3] += delta * iMJ_ptr[3]; + fc_ptr[4] += delta * iMJ_ptr[4]; + fc_ptr[5] += delta * iMJ_ptr[5]; + // @@@ potential optimization: handle 1-body constraints in a separate + // loop to avoid the cost of test & jump? + if (b2 >= 0) { + fc_ptr = invMforce + 6*b2; + fc_ptr[0] += delta * iMJ_ptr[6]; + fc_ptr[1] += delta * iMJ_ptr[7]; + fc_ptr[2] += delta * iMJ_ptr[8]; + fc_ptr[3] += delta * iMJ_ptr[9]; + fc_ptr[4] += delta * iMJ_ptr[10]; + fc_ptr[5] += delta * iMJ_ptr[11]; + } + } + } + //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 +} + +//------------------------------------------------------------------------------ +void btSorLcpSolver::SolveInternal1 ( + float global_cfm, + float global_erp, + const btAlignedObjectArray<btOdeSolverBody*> &body, int nb, + btAlignedObjectArray<btOdeJoint*> &joint, + int nj, const btContactSolverInfo& solverInfo, + btStackAlloc* stackAlloc) +{ + //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 + AutoBlockSa asaBlock(stackAlloc); + + int numIter = solverInfo.m_numIterations; + float sOr = solverInfo.m_sor; + + int i,j; + + btScalar stepsize1 = dRecip(solverInfo.m_timeStep); + + // number all bodies in the body list - set their tag values + for (i=0; i<nb; i++) + body[i]->m_odeTag = i; + + // make a local copy of the joint array, because we might want to modify it. + // (the "btOdeJoint *const*" declaration says we're allowed to modify the joints + // but not the joint array, because the caller might need it unchanged). + //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints + //btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*)); + //memcpy (joint,_joint,nj * sizeof(btOdeJoint*)); + + // for all bodies, compute the inertia tensor and its inverse in the global + // frame, and compute the rotational force and add it to the torque + // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. + dRealAllocaArray (I,3*4*nb); + dRealAllocaArray (invI,3*4*nb); +/* for (i=0; i<nb; i++) { + dMatrix3 tmp; + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force + dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); + } +*/ + for (i=0; i<nb; i++) { + dMatrix3 tmp; + // compute inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R); + dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); + + // compute inverse inertia tensor in global frame + dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); + dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); + // compute rotational force +// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity); +// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp); + } + + + + + // get joint information (m = total constraint dimension, nub = number of unbounded variables). + // joints with m=0 are inactive and are removed from the joints array + // entirely, so that the code that follows does not consider them. + //@@@ do we really need to save all the info1's + btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1)); + + for (i=0, j=0; j<nj; j++) { // i=dest, j=src + joint[j]->GetInfo1 (info+i); + dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); + if (info[i].m > 0) { + joint[i] = joint[j]; + i++; + } + } + nj = i; + + // create the row offset array + int m = 0; + int *ofs = (int*) ALLOCA (nj*sizeof(int)); + for (i=0; i<nj; i++) { + ofs[i] = m; + m += info[i].m; + } + + // if there are constraints, compute the constraint force + dRealAllocaArray (J,m*12); + int *jb = (int*) ALLOCA (m*2*sizeof(int)); + if (m > 0) { + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. + dRealAllocaArray (c,m); + dRealAllocaArray (cfm,m); + dRealAllocaArray (lo,m); + dRealAllocaArray (hi,m); + + int *findex = (int*) ALLOCA (m*sizeof(int)); + + dSetZero1 (c,m); + dSetValue1 (cfm,m,global_cfm); + dSetValue1 (lo,m,-dInfinity); + dSetValue1 (hi,m, dInfinity); + for (i=0; i<m; i++) findex[i] = -1; + + // get jacobian data from constraints. an m*12 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \ . + // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }-- jacobian for joint 0, body 1 and body 2 (3 rows) + // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 / + // l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // + dSetZero1 (J,m*12); + btOdeJoint::Info2 Jinfo; + Jinfo.rowskip = 12; + Jinfo.fps = stepsize1; + Jinfo.erp = global_erp; + for (i=0; i<nj; i++) { + Jinfo.J1l = J + ofs[i]*12; + Jinfo.J1a = Jinfo.J1l + 3; + Jinfo.J2l = Jinfo.J1l + 6; + Jinfo.J2a = Jinfo.J1l + 9; + Jinfo.c = c + ofs[i]; + Jinfo.cfm = cfm + ofs[i]; + Jinfo.lo = lo + ofs[i]; + Jinfo.hi = hi + ofs[i]; + Jinfo.findex = findex + ofs[i]; + joint[i]->GetInfo2 (&Jinfo); + + if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) + Jinfo.c[0] = solverInfo.m_maxErrorReduction; + + // adjust returned findex values for global index numbering + for (j=0; j<info[i].m; j++) { + if (findex[ofs[i] + j] >= 0) + findex[ofs[i] + j] += ofs[i]; + } + } + + // create an array of body numbers for each joint row + int *jb_ptr = jb; + for (i=0; i<nj; i++) { + int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; + int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; + for (j=0; j<info[i].m; j++) { + jb_ptr[0] = b1; + jb_ptr[1] = b2; + jb_ptr += 2; + } + } + dIASSERT (jb_ptr == jb+2*m); + + // compute the right hand side `rhs' + dRealAllocaArray (tmp1,nb*6); + // put v/h + invM*fe into tmp1 + for (i=0; i<nb; i++) { + btScalar body_invMass = body[i]->m_invMass; + for (j=0; j<3; j++) + tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1; + dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); + for (j=0; j<3; j++) + tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1; + } + + // put J*tmp1 into rhs + dRealAllocaArray (rhs,m); + multiply_J (m,J,jb,tmp1,rhs); + + // complete rhs + for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i]; + + // scale CFM + for (i=0; i<m; i++) + cfm[i] *= stepsize1; + + // load lambda from the value saved on the previous iteration + dRealAllocaArray (lambda,m); +#ifdef WARM_STARTING + dSetZero1 (lambda,m); //@@@ shouldn't be necessary + for (i=0; i<nj; i++) { + memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar)); + } +#endif + + // solve the LCP problem and get lambda and invM*constraint_force + dRealAllocaArray (cforce,nb*6); + + /// SOR_LCP + SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc); + +#ifdef WARM_STARTING + // save lambda for the next iteration + //@@@ note that this doesn't work for contact joints yet, as they are + // recreated every iteration + for (i=0; i<nj; i++) { + memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar)); + } +#endif + + // note that the SOR method overwrites rhs and J at this point, so + // they should not be used again. + // add stepsize * cforce to the body velocity + for (i=0; i<nb; i++) { + for (j=0; j<3; j++) + body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j]; + for (j=0; j<3; j++) + body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; + + } + } + + // compute the velocity update: + // add stepsize * invM * fe to the body velocity + for (i=0; i<nb; i++) { + btScalar body_invMass = body[i]->m_invMass; + btVector3 linvel = body[i]->m_linearVelocity; + btVector3 angvel = body[i]->m_angularVelocity; + + for (j=0; j<3; j++) + { + linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; + } + for (j=0; j<3; j++) + { + body[i]->m_tacc[j] *= solverInfo.m_timeStep; + } + dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); + body[i]->m_angularVelocity = angvel; + } + //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 +} + + +#endif //USE_SOR_SOLVER diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h new file mode 100644 index 00000000000..0d3583d33d9 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h @@ -0,0 +1,112 @@ +/* + * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith + * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. + * All rights reserved. Email: russ@q12.org Web: www.q12.org + Bullet Continuous Collision Detection and Physics Library + Bullet is 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. +*/ + +#define USE_SOR_SOLVER +#ifdef USE_SOR_SOLVER + +#ifndef SOR_LCP_H +#define SOR_LCP_H +struct btOdeSolverBody; +class btOdeJoint; +#include "LinearMath/btScalar.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btStackAlloc.h" + +struct btContactSolverInfo; + + +//============================================================================= +class btSorLcpSolver //Remotion: 11.10.2007 +{ +public: + btSorLcpSolver() + { + dRand2_seed = 0; + } + + void SolveInternal1 (float global_cfm, + float global_erp, + const btAlignedObjectArray<btOdeSolverBody*> &body, int nb, + btAlignedObjectArray<btOdeJoint*> &joint, + int nj, const btContactSolverInfo& solverInfo, + btStackAlloc* stackAlloc + ); + +public: //data + unsigned long dRand2_seed; + +protected: //typedef + typedef const btScalar *dRealPtr; + typedef btScalar *dRealMutablePtr; + +protected: //members + //------------------------------------------------------------------------------ + SIMD_FORCE_INLINE unsigned long dRand2() + { + dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff; + return dRand2_seed; + } + //------------------------------------------------------------------------------ + SIMD_FORCE_INLINE int dRandInt2 (int n) + { + float a = float(n) / 4294967296.0f; + return (int) (float(dRand2()) * a); + } + //------------------------------------------------------------------------------ + void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, + const btAlignedObjectArray<btOdeSolverBody*> &body, + dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, + dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, + int numiter,float overRelax, + btStackAlloc* stackAlloc + ); +}; + + +//============================================================================= +class AutoBlockSa //Remotion: 10.10.2007 +{ + btStackAlloc* stackAlloc; + btBlock* saBlock; +public: + AutoBlockSa(btStackAlloc* stackAlloc_) + { + stackAlloc = stackAlloc_; + saBlock = stackAlloc->beginBlock(); + } + ~AutoBlockSa() + { + stackAlloc->endBlock(saBlock); + } + //operator btBlock* () { return saBlock; } +}; +// //Usage +//void function(btStackAlloc* stackAlloc) +//{ +// AutoBlockSa(stackAlloc); +// ... +// if(...) return; +// return; +//} +//------------------------------------------------------------------------------ + + +#endif //SOR_LCP_H + +#endif //USE_SOR_SOLVER + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index b28a46e299e..c2fd71d67fe 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -24,6 +24,27 @@ subject to the following restrictions: #include "btBulletDynamicsCommon.h" #include "LinearMath/btAlignedAllocator.h" + + +#include "LinearMath/btVector3.h" +#include "LinearMath/btScalar.h" +#include "LinearMath/btMatrix3x3.h" +#include "LinearMath/btTransform.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" + +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" +#include "LinearMath/btStackAlloc.h" + /* Create and Delete a Physics SDK */ @@ -308,3 +329,72 @@ void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation) //plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); // extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); + +double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]) +{ + btVector3 vp(p1[0], p1[1], p1[2]); + btTriangleShape trishapeA(vp, + btVector3(p2[0], p2[1], p2[2]), + btVector3(p3[0], p3[1], p3[2])); + trishapeA.setMargin(0.000001f); + btVector3 vq(q1[0], q1[1], q1[2]); + btTriangleShape trishapeB(vq, + btVector3(q2[0], q2[1], q2[2]), + btVector3(q3[0], q3[1], q3[2])); + trishapeB.setMargin(0.000001f); + + // btVoronoiSimplexSolver sGjkSimplexSolver; + // btGjkEpaPenetrationDepthSolver penSolverPtr; + + static btSimplexSolverInterface sGjkSimplexSolver; + sGjkSimplexSolver.reset(); + + static btGjkEpaPenetrationDepthSolver Solver0; + static btMinkowskiPenetrationDepthSolver Solver1; + + btConvexPenetrationDepthSolver* Solver = NULL; + + Solver = &Solver1; + + btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver); + + convexConvex.m_catchDegeneracies = 1; + + // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0); + + btPointCollector gjkOutput; + btGjkPairDetector::ClosestPointInput input; + + btStackAlloc gStackAlloc(1024*1024*2); + + input.m_stackAlloc = &gStackAlloc; + + btTransform tr; + tr.setIdentity(); + + input.m_transformA = tr; + input.m_transformB = tr; + + convexConvex.getClosestPoints(input, gjkOutput, 0); + + + if (gjkOutput.m_hasResult) + { + + pb[0] = pa[0] = gjkOutput.m_pointInWorld[0]; + pb[1] = pa[1] = gjkOutput.m_pointInWorld[1]; + pb[2] = pa[2] = gjkOutput.m_pointInWorld[2]; + + pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance; + pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance; + pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance; + + normal[0] = gjkOutput.m_normalOnBInWorld[0]; + normal[1] = gjkOutput.m_normalOnBInWorld[1]; + normal[2] = gjkOutput.m_normalOnBInWorld[2]; + + return gjkOutput.m_distance; + } + return -1.0f; +} + diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h new file mode 100644 index 00000000000..8bb80de0225 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -0,0 +1,242 @@ + +/* +Stan Melax Convex Hull Computation +Copyright (c) 2008 Stan Melax http://www.melax.com/ + +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. +*/ + +///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. + +#ifndef CD_HULL_H +#define CD_HULL_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +typedef btAlignedObjectArray<unsigned int> TUIntArray; + +class HullResult +{ +public: + HullResult(void) + { + mPolygons = true; + mNumOutputVertices = 0; + mNumFaces = 0; + mNumIndices = 0; + } + bool mPolygons; // true if indices represents polygons, false indices are triangles + unsigned int mNumOutputVertices; // number of vertices in the output hull + btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices + unsigned int mNumFaces; // the number of faces produced + unsigned int mNumIndices; // the total number of indices + btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices. + +// If triangles, then indices are array indexes into the vertex list. +// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. +}; + +enum HullFlag +{ + QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. + QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. + QF_DEFAULT = QF_TRIANGLES +}; + + +class HullDesc +{ +public: + HullDesc(void) + { + mFlags = QF_DEFAULT; + mVcount = 0; + mVertices = 0; + mVertexStride = sizeof(btVector3); + mNormalEpsilon = 0.001f; + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; + }; + + HullDesc(HullFlag flag, + unsigned int vcount, + const btVector3 *vertices, + unsigned int stride = sizeof(btVector3)) + { + mFlags = flag; + mVcount = vcount; + mVertices = vertices; + mVertexStride = stride; + mNormalEpsilon = btScalar(0.001); + mMaxVertices = 4096; + } + + bool HasHullFlag(HullFlag flag) const + { + if ( mFlags & flag ) return true; + return false; + } + + void SetHullFlag(HullFlag flag) + { + mFlags|=flag; + } + + void ClearHullFlag(HullFlag flag) + { + mFlags&=~flag; + } + + unsigned int mFlags; // flags to use when generating the convex hull. + unsigned int mVcount; // number of vertices in the input point cloud + const btVector3 *mVertices; // the array of vertices. + unsigned int mVertexStride; // the stride of each vertex, in bytes. + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + unsigned int mMaxFaces; +}; + +enum HullError +{ + QE_OK, // success! + QE_FAIL // failed. +}; + +class btPlane +{ + public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} + btPlane():normal(),dist(0){} + +}; + + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + ConvexH() + { + int i; + i=0; + } + ~ConvexH() + { + int i; + i=0; + } + btAlignedObjectArray<btVector3> vertices; + btAlignedObjectArray<HalfEdge> edges; + btAlignedObjectArray<btPlane> facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + btVector3* mVertices; + TUIntArray m_Indices; +}; + + + +///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method. +///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape. +class HullLibrary +{ + + btAlignedObjectArray<class Tri*> m_tris; + +public: + + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst + HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + +private: + + bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); + + class Tri* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(Tri*); + void b2bfix(Tri* s,Tri*t); + + void removeb2b(Tri* s,Tri*t); + + void checkit(Tri *t); + + Tri* extrudable(btScalar epsilon); + + int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + + int calchullgen(btVector3 *verts,int verts_count, int vlimit); + + int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow); + + class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + + void extrude(class Tri* t0,int v); + + ConvexH* test_cube(); + + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. + //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. + //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. + //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. + void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); + + bool CleanupVertices(unsigned int svcount, + const btVector3* svertices, + unsigned int stride, + unsigned int &vcount, // output number of vertices + btVector3* vertices, // location to store the results. + btScalar normalepsilon, + btVector3& scale); +}; + + +#endif + diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h new file mode 100644 index 00000000000..e9620ac5faa --- /dev/null +++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h @@ -0,0 +1,97 @@ +/* +Copyright (c) 2003-2006 Gino van den Bergen / 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_POOL_ALLOCATOR_H +#define _BT_POOL_ALLOCATOR_H + +#include "btScalar.h" +#include "btAlignedAllocator.h" + +///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately. +class btPoolAllocator +{ + int m_elemSize; + int m_maxElements; + int m_freeCount; + void* m_firstFree; + unsigned char* m_pool; + +public: + + btPoolAllocator(int elemSize, int maxElements) + :m_elemSize(elemSize), + m_maxElements(maxElements) + { + m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16); + + unsigned char* p = m_pool; + m_firstFree = p; + m_freeCount = m_maxElements; + int count = m_maxElements; + while (--count) { + *(void**)p = (p + m_elemSize); + p += m_elemSize; + } + *(void**)p = 0; + } + + ~btPoolAllocator() + { + btAlignedFree( m_pool); + } + + int getFreeCount() const + { + return m_freeCount; + } + + void* allocate(int size) + { + // release mode fix + (void)size; + btAssert(!size || size<=m_elemSize); + btAssert(m_freeCount>0); + void* result = m_firstFree; + m_firstFree = *(void**)m_firstFree; + --m_freeCount; + return result; + } + + bool validPtr(void* ptr) + { + if (ptr) { + if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize)) + { + return true; + } + } + return false; + } + + void freeMemory(void* ptr) + { + if (ptr) { + btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize); + + *(void**)ptr = m_firstFree; + m_firstFree = ptr; + ++m_freeCount; + } + } + + +}; + +#endif //_BT_POOL_ALLOCATOR_H diff --git a/extern/bullet2/src/SConscript b/extern/bullet2/src/SConscript index 19702782b0d..59b823982f5 100644 --- a/extern/bullet2/src/SConscript +++ b/extern/bullet2/src/SConscript @@ -22,74 +22,11 @@ elif sys.platform=='darwin': cflags += ['-O2','-pipe', '-fPIC', '-funsigned-char', '-ffast-math'] linearmath_src = env.Glob("LinearMath/*.cpp") -bulletdyn_src = ["BulletDynamics/ConstraintSolver/btContactConstraint.cpp", - "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp", - "BulletDynamics/ConstraintSolver/btHingeConstraint.cpp", - "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp", - "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp", - "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp", - "BulletDynamics/ConstraintSolver/btTypedConstraint.cpp", - "BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp", - "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp", - "BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp", - "BulletDynamics/Dynamics/btRigidBody.cpp", - "BulletDynamics/Vehicle/btRaycastVehicle.cpp", - "BulletDynamics/Dynamics/Bullet-C-API.cpp", - "BulletDynamics/Vehicle/btWheelInfo.cpp"] -collision_src = ["BulletCollision/BroadphaseCollision/btAxisSweep3.cpp", - "BulletCollision/BroadphaseCollision/btBroadphaseProxy.cpp", - "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp", - "BulletCollision/BroadphaseCollision/btDispatcher.cpp", - "BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp", - "BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp", - "BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp", - "BulletCollision/CollisionDispatch/btCollisionObject.cpp", - "BulletCollision/CollisionDispatch/btCollisionWorld.cpp", - "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp", - "BulletCollision/CollisionDispatch/btManifoldResult.cpp", - "BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp", - "BulletCollision/CollisionDispatch/btUnionFind.cpp", - "BulletCollision/CollisionShapes/btBoxShape.cpp", - "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp", - "BulletCollision/CollisionShapes/btCollisionShape.cpp", - "BulletCollision/CollisionShapes/btCompoundShape.cpp", - "BulletCollision/CollisionShapes/btConcaveShape.cpp", - "BulletCollision/CollisionShapes/btConeShape.cpp", - "BulletCollision/CollisionShapes/btConvexHullShape.cpp", - "BulletCollision/CollisionShapes/btConvexShape.cpp", - "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp", - "BulletCollision/CollisionShapes/btCylinderShape.cpp", - "BulletCollision/CollisionShapes/btEmptyShape.cpp", - "BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp", - "BulletCollision/CollisionShapes/btMultiSphereShape.cpp", - "BulletCollision/CollisionShapes/btOptimizedBvh.cpp", - "BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp", - "BulletCollision/CollisionShapes/btTetrahedronShape.cpp", - "BulletCollision/CollisionShapes/btSphereShape.cpp", - "BulletCollision/CollisionShapes/btStaticPlaneShape.cpp", - "BulletCollision/CollisionShapes/btStridingMeshInterface.cpp", - "BulletCollision/CollisionShapes/btTriangleCallback.cpp", - "BulletCollision/CollisionShapes/btTriangleBuffer.cpp", - "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.cpp", - "BulletCollision/CollisionShapes/btTriangleMesh.cpp", - "BulletCollision/CollisionShapes/btTriangleMeshShape.cpp", - "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp", - "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp", - "BulletCollision/NarrowPhaseCollision/btGjkEpa.cpp", - "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp", - "BulletCollision/NarrowPhaseCollision/btConvexCast.cpp", - "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.cpp", - "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp", - "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp", - "BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp", - "BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp", - "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp", - "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp"] + +bulletdyn_src = env.Glob("BulletDynamics/Vehicle/*.cpp") + env.Glob("BulletDynamics/ConstraintSolver/*.cpp") + env.Glob("BulletDynamics/Dynamics/*.cpp") + +collision_src = env.Glob("BulletCollision/BroadphaseCollision/*.cpp") + env.Glob("BulletCollision/CollisionDispatch/*.cpp") +collision_src += env.Glob("BulletCollision/CollisionShapes/*.cpp") + env.Glob("BulletCollision/NarrowPhaseCollision/*.cpp") incs = '. BulletCollision BulletDynamics LinearMath' |