svn merge -r 16334:16347 https://svn.blender.org/svnroot/bf-blender/trunk/blender...
authorDaniel Genrich <daniel.genrich@gmx.net>
Wed, 3 Sep 2008 10:55:46 +0000 (10:55 +0000)
committerDaniel Genrich <daniel.genrich@gmx.net>
Wed, 3 Sep 2008 10:55:46 +0000 (10:55 +0000)
17 files changed:
1  2 
extern/bullet2/src/Bullet-C-Api.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h
extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
extern/bullet2/src/LinearMath/btConvexHull.h
extern/bullet2/src/LinearMath/btPoolAllocator.h
extern/bullet2/src/SConscript

index 078dcae63bb720de1a6d9f9221668cc88cb9580a,a196f6417bc78a5307b34116488c6be5fa3667e8..8074aed3038449586c4c7f3022ddf3c48c00f9de
@@@ -27,11 -38,134 +38,138 @@@ typedef plReal   plQuaternion[4]
  extern "C" { 
  #endif
  
- 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]);
+ /*    Particular physics SDK */
+       PL_DECLARE_HANDLE(plPhysicsSdkHandle);
+ /*    Dynamics world, belonging to some physics SDK */
+       PL_DECLARE_HANDLE(plDynamicsWorldHandle);
+ /* Rigid Body that can be part of a Dynamics World */ 
+       PL_DECLARE_HANDLE(plRigidBodyHandle);
+ /*    Collision Shape/Geometry, property of a Rigid Body */
+       PL_DECLARE_HANDLE(plCollisionShapeHandle);
+ /* Constraint for Rigid Bodies */
+       PL_DECLARE_HANDLE(plConstraintHandle);
+ /* Triangle Mesh interface */
+       PL_DECLARE_HANDLE(plMeshInterfaceHandle);
+ /* Broadphase Scene/Proxy Handles */
+       PL_DECLARE_HANDLE(plCollisionBroadphaseHandle);
+       PL_DECLARE_HANDLE(plBroadphaseProxyHandle);
+       PL_DECLARE_HANDLE(plCollisionWorldHandle);
+ /*
+       Create and Delete a Physics SDK 
+ */
+       extern  plPhysicsSdkHandle      plNewBulletSdk(); //this could be also another sdk, like ODE, PhysX etc.
+       extern  void            plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk);
+ /* Collision World, not strictly necessary, you can also just create a Dynamics World with Rigid Bodies which internally manages the Collision World with Collision Objects */
+       typedef void(*btBroadphaseCallback)(void* clientData, void* object1,void* object2);
+       extern plCollisionBroadphaseHandle      plCreateSapBroadphase(btBroadphaseCallback beginCallback,btBroadphaseCallback endCallback);
+       extern void     plDestroyBroadphase(plCollisionBroadphaseHandle bp);
+       extern  plBroadphaseProxyHandle plCreateProxy(plCollisionBroadphaseHandle bp, void* clientData, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+       extern void plDestroyProxy(plCollisionBroadphaseHandle bp, plBroadphaseProxyHandle proxyHandle);
+       extern void plSetBoundingBox(plBroadphaseProxyHandle proxyHandle, plReal minX,plReal minY,plReal minZ, plReal maxX,plReal maxY, plReal maxZ);
+ /* todo: add pair cache support with queries like add/remove/find pair */
+       
+       extern plCollisionWorldHandle plCreateCollisionWorld(plPhysicsSdkHandle physicsSdk);
+ /* todo: add/remove objects */
+       
+ /* Dynamics World */
+       extern  plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdk);
+       extern  void           plDeleteDynamicsWorld(plDynamicsWorldHandle world);
+       extern  void    plStepSimulation(plDynamicsWorldHandle, plReal  timeStep);
+       extern  void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+       extern  void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object);
+ /* Rigid Body  */
+       extern  plRigidBodyHandle plCreateRigidBody(    void* user_data,  float mass, plCollisionShapeHandle cshape );
+       extern  void plDeleteRigidBody(plRigidBodyHandle body);
+ /* Collision Shape definition */
+       extern  plCollisionShapeHandle plNewSphereShape(plReal radius);
+       extern  plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z);
+       extern  plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height); 
+       extern  plCollisionShapeHandle plNewConeShape(plReal radius, plReal height);
+       extern  plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height);
+       extern  plCollisionShapeHandle plNewCompoundShape();
+       extern  void    plAddChildShape(plCollisionShapeHandle compoundShape,plCollisionShapeHandle childShape, plVector3 childPos,plQuaternion childOrn);
+       extern  void plDeleteShape(plCollisionShapeHandle shape);
+       /* Convex Meshes */
+       extern  plCollisionShapeHandle plNewConvexHullShape();
+       extern  void            plAddVertex(plCollisionShapeHandle convexHull, plReal x,plReal y,plReal z);
+ /* Concave static triangle meshes */
+       extern  plMeshInterfaceHandle              plNewMeshInterface();
+       extern  void            plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+       extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+       extern  void plSetScaling(plCollisionShapeHandle shape, plVector3 scaling);
+ /* SOLID has Response Callback/Table/Management */
+ /* PhysX has Triggers, User Callbacks and filtering */
+ /* ODE has the typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2); */
+ /*    typedef void plUpdatedPositionCallback(void* userData, plRigidBodyHandle        rbHandle, plVector3 pos); */
+ /*    typedef void plUpdatedOrientationCallback(void* userData, plRigidBodyHandle     rbHandle, plQuaternion orientation); */
+       /* get world transform */
+       extern void     plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix);
+       extern void     plGetPosition(plRigidBodyHandle object,plVector3 position);
+       extern void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation);
+       /* set world transform (position/orientation) */
+       extern  void plSetPosition(plRigidBodyHandle object, const plVector3 position);
+       extern  void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation);
+       extern  void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient);
+       typedef struct plRayCastResult {
+               plRigidBodyHandle               m_body;  
+               plCollisionShapeHandle  m_shape;                
+               plVector3                               m_positionWorld;                
+               plVector3                               m_normalWorld;
+       } plRayCastResult;
+       extern  int plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plRayCastResult res);
+       /* Sweep API */
+       /* 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
  
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..7d2f19998ac7e51fdee06f979ace5c957af05193
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++      
++}
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..8dd0282c59ebef39c4746beabef7d5f05e304a38
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..46c3783c6a01feb9e69c772efcf98f23a2a2a27f
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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()
++{
++
++}
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..50733d1418fa517865e254b4037196049480798c
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..e4bc2628bd4fb9df90b134b30c91d198b4f5ce23
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..ab90c92655909ab2eccb9b09bc87e8abadb6cb1f
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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;
++      }
++
++}
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..e548ea6fc22c5de8d5728b814102d54a1cfbf933
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..0c936971b79f9752f48447049d1e03d979b9a8c8
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..f683bf7d7480fde20f65e0d510cabdc1b6998561
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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()
++
++//----------------------------------------------------------------------------------
++//----------------------------------------------------------------------------------
++
++
++
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..a2affda382d5c68ecad1e82099c0d5a110700d32
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
++
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..175d15dcfcfd8d7f8dbfbaad25266fe034615854
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..0d3583d33d935f5c3e7caf8a48b2cf0d4cca3255
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
index 248c582dcd8fa697070465acc5c77240d92c5b99,b28a46e299e6112f54430f076f458e66216a675f..c2fd71d67fe78bbd8f8230096f38455b8042c055
@@@ -24,97 -24,287 +24,377 @@@ subject to the following restrictions
  #include "btBulletDynamicsCommon.h"
  #include "LinearMath/btAlignedAllocator.h"
  
- #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
- #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
- #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.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/btGjkPairDetector.h"
- #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 +#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
- extern "C"
 +#include "LinearMath/btStackAlloc.h"
 +
+ /*
+       Create and Delete a Physics SDK 
+ */
+ struct        btPhysicsSdk
+ {
+ //    btDispatcher*                           m_dispatcher;
+ //    btOverlappingPairCache*         m_pairCache;
+ //    btConstraintSolver*                     m_constraintSolver
+       btVector3       m_worldAabbMin;
+       btVector3       m_worldAabbMax;
+       //todo: version, hardware/optimization settings etc?
+       btPhysicsSdk()
+               :m_worldAabbMin(-1000,-1000,-1000),
+               m_worldAabbMax(1000,1000,1000)
+       {
+       }
+       
+ };
+ plPhysicsSdkHandle    plNewBulletSdk()
+ {
+       void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
+       return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
+ }
+ void          plDeletePhysicsSdk(plPhysicsSdkHandle   physicsSdk)
+ {
+       btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
+       btAlignedFree(phys);    
+ }
+ /* Dynamics World */
+ plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
+ {
+       btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
+       void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
+       btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
+       mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
+       btDispatcher*                           dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
+       mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
+       btBroadphaseInterface*          pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
+       mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+       btConstraintSolver*                     constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
+       mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
+       return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
+ }
+ void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
+ {
+       //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
+       btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+       btAlignedFree(dynamicsWorld);
+ }
+ void  plStepSimulation(plDynamicsWorldHandle world,   plReal  timeStep)
+ {
+       btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+       assert(dynamicsWorld);
+       dynamicsWorld->stepSimulation(timeStep);
+ }
+ void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+ {
+       btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+       assert(dynamicsWorld);
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       assert(body);
+       dynamicsWorld->addRigidBody(body);
+ }
+ void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+ {
+       btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+       assert(dynamicsWorld);
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       assert(body);
+       dynamicsWorld->removeRigidBody(body);
+ }
+ /* Rigid Body  */
+ plRigidBodyHandle plCreateRigidBody(  void* user_data,  float mass, plCollisionShapeHandle cshape )
+ {
+       btTransform trans;
+       trans.setIdentity();
+       btVector3 localInertia(0,0,0);
+       btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+       assert(shape);
+       if (mass)
+       {
+               shape->calculateLocalInertia(mass,localInertia);
+       }
+       void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
+       btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
+       btRigidBody* body = new (mem)btRigidBody(rbci);
+       body->setWorldTransform(trans);
+       body->setUserPointer(user_data);
+       return (plRigidBodyHandle) body;
+ }
+ void plDeleteRigidBody(plRigidBodyHandle cbody)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
+       assert(body);
+       btAlignedFree( body);
+ }
+ /* Collision Shape definition */
+ plCollisionShapeHandle plNewSphereShape(plReal radius)
+ {
+       void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
+       return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
+       
+ }
+       
+ plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
+ {
+       void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
+       return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
+ }
+ plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
+ {
+       //capsule is convex hull of 2 spheres, so use btMultiSphereShape
+       btVector3 inertiaHalfExtents(radius,height,radius);
+       const int numSpheres = 2;
+       btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
+       btScalar radi[numSpheres] = {radius,radius};
+       void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
+       return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+ }
+ plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
+ {
+       void* mem = btAlignedAlloc(sizeof(btConeShape),16);
+       return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
+ }
+ plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
+ {
+       void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
+       return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
+ }
+ /* Convex Meshes */
+ plCollisionShapeHandle plNewConvexHullShape()
+ {
+       void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
+       return (plCollisionShapeHandle) new (mem)btConvexHullShape();
+ }
+ /* Concave static triangle meshes */
+ plMeshInterfaceHandle            plNewMeshInterface()
+ {
+       return 0;
+ }
+ plCollisionShapeHandle plNewCompoundShape()
+ {
+       void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
+       return (plCollisionShapeHandle) new (mem)btCompoundShape();
+ }
+ void  plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
+ {
+       btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
+       btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
+       btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
+       btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
+       btTransform     localTrans;
+       localTrans.setIdentity();
+       localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
+       localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
+       compoundShape->addChildShape(localTrans,childShape);
+ }
+ void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
+ {
+       btQuaternion orn;
+       orn.setEuler(yaw,pitch,roll);
+       orient[0] = orn.getX();
+       orient[1] = orn.getY();
+       orient[2] = orn.getZ();
+       orient[3] = orn.getW();
+ }
+ //    extern  void            plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+ //    extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+ void          plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
+ {
+       btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
+       (void)colShape;
+       btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
+       btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
+       convexHullShape->addPoint(btPoint3(x,y,z));
+ }
+ void plDeleteShape(plCollisionShapeHandle cshape)
+ {
+       btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+       assert(shape);
+       btAlignedFree(shape);
+ }
+ void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
+ {
+       btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+       assert(shape);
+       btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
+       shape->setLocalScaling(scaling);        
+ }
+ void plSetPosition(plRigidBodyHandle object, const plVector3 position)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       btAssert(body);
+       btVector3 pos(position[0],position[1],position[2]);
+       btTransform worldTrans = body->getWorldTransform();
+       worldTrans.setOrigin(pos);
+       body->setWorldTransform(worldTrans);
+ }
+ void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       btAssert(body);
+       btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
+       btTransform worldTrans = body->getWorldTransform();
+       worldTrans.setRotation(orn);
+       body->setWorldTransform(worldTrans);
+ }
+ void  plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       btAssert(body);
+       body->getWorldTransform().getOpenGLMatrix(matrix);
+ }
+ void  plGetPosition(plRigidBodyHandle object,plVector3 position)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       btAssert(body);
+       const btVector3& pos = body->getWorldTransform().getOrigin();
+       position[0] = pos.getX();
+       position[1] = pos.getY();
+       position[2] = pos.getZ();
+ }
+ void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
+ {
+       btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+       btAssert(body);
+       const btQuaternion& orn = body->getWorldTransform().getRotation();
+       orientation[0] = orn.getX();
+       orientation[1] = orn.getY();
+       orientation[2] = orn.getZ();
+       orientation[3] = orn.getW();
+ }
+ //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;   
 +}
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..8bb80de0225e829d680c2d5749bc1b2d78fb2a90
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
++
index 0000000000000000000000000000000000000000,0000000000000000000000000000000000000000..e9620ac5faa385fb05fd76590573621f0a69c2c7
new file mode 100644 (file)
--- /dev/null
--- /dev/null
@@@ -1,0 -1,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
index 19702782b0dd5bd55a85d16bd419558d9c572b71,19702782b0dd5bd55a85d16bd419558d9c572b71..59b823982f53632812f0f53632f1159bfe8ae2b8
@@@ -22,74 -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'