added some new Bullet files, and upgraded to latest Bullet 2.x
authorErwin Coumans <blender@erwincoumans.com>
Tue, 12 Dec 2006 03:08:15 +0000 (03:08 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Tue, 12 Dec 2006 03:08:15 +0000 (03:08 +0000)
Please make sure to have extern/bullet/src/LinearMath/btAlignedAllocator.cpp in your build, if you add the files by name, instead of wildcard *.cpp

31 files changed:
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h
extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp
extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h
extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
extern/bullet2/src/LinearMath/btAlignedAllocator.cpp [new file with mode: 0644]
extern/bullet2/src/LinearMath/btAlignedAllocator.h [new file with mode: 0644]
extern/bullet2/src/LinearMath/btAlignedObjectArray.h [new file with mode: 0644]
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
extern/bullet2/src/LinearMath/btGeometryUtil.h
extern/bullet2/src/LinearMath/btQuadWord.h
extern/bullet2/src/LinearMath/btScalar.h

index 9df6d7186ec2e1ad3a10c93d56aec34b70551add..fa1561973fb284e3bf1ca3e8ea8cf6c69f9b1790 100644 (file)
@@ -121,8 +121,10 @@ void       btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho
 
 
 
-void   btCollisionWorld::performDiscreteCollisionDetection(btDispatcherInfo& dispatchInfo)
+void   btCollisionWorld::performDiscreteCollisionDetection()
 {
+       btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
        BEGIN_PROFILE("performDiscreteCollisionDetection");
 
 
index 9c5487848295bb2145f0f64b2e311a07dc9ccc9b..bd09d8c4d5d4593bf82e9e1481629f2cd96373d8 100644 (file)
@@ -173,7 +173,7 @@ public:
 
        struct  ClosestRayResultCallback : public RayResultCallback
        {
-               ClosestRayResultCallback(btVector3      rayFromWorld,btVector3  rayToWorld)
+               ClosestRayResultCallback(const btVector3&       rayFromWorld,const btVector3&   rayToWorld)
                :m_rayFromWorld(rayFromWorld),
                m_rayToWorld(rayToWorld),
                m_collisionObject(0)
@@ -237,7 +237,7 @@ public:
 
        void    removeCollisionObject(btCollisionObject* collisionObject);
 
-       virtual void    performDiscreteCollisionDetection(      btDispatcherInfo&       dispatchInfo);
+       virtual void    performDiscreteCollisionDetection();
 
        btDispatcherInfo& getDispatchInfo()
        {
index 332870080043a9c05265f66b49e031d67832f3ed..3f51e9ec0a61472f225c5bd777e0d03ebba23dda 100644 (file)
@@ -268,17 +268,44 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
        int startManifoldIndex = 0;
        int endManifoldIndex = 1;
 
-       for (startManifoldIndex=0;startManifoldIndex<numManifolds;startManifoldIndex = endManifoldIndex)
+       int islandId;
+
+
+               //update the sleeping state for bodies, if all are sleeping
+       for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
        {
-               int islandId = getIslandId(islandmanifold[startManifoldIndex]);
-               for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+               int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+               for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+               {
+               }
+
+               //find the accompanying contact manifold for this islandId
+               int numIslandManifolds = 0;
+               btPersistentManifold** startManifold = 0;
+
+               if (startManifoldIndex<numManifolds)
                {
+                       int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
+                       if (curIslandId == islandId)
+                       {
+                               startManifold = &islandmanifold[startManifoldIndex];
+                       
+                               for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
+                               {
+
+                               }
+                               /// Process the actual simulation, only if not sleeping/deactivated
+                               numIslandManifolds = endManifoldIndex-startManifoldIndex;
+                       }
+
                }
-               /// Process the actual simulation, only if not sleeping/deactivated
-               int numIslandManifolds = endManifoldIndex-startManifoldIndex;
+
+               callback->ProcessIsland(startManifold,numIslandManifolds, islandId);
+
                if (numIslandManifolds)
                {
-                       callback->ProcessIsland(&islandmanifold[startManifoldIndex],numIslandManifolds);
+                       startManifoldIndex = endManifoldIndex;
                }
        }
 }
index 36c51000a6bd3b4b973a6f611f4f0ed3d80e3857..68d9b8038d639719f6734d5c43e1cf1494e0b9b3 100644 (file)
@@ -49,7 +49,7 @@ public:
        {
                virtual ~IslandCallback() {};
 
-               virtual void    ProcessIsland(class btPersistentManifold**      manifolds,int numManifolds) = 0;
+               virtual void    ProcessIsland(class btPersistentManifold**      manifolds,int numManifolds, int islandId) = 0;
        };
 
        void    buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback);
index c810a654834324943def9db70f14e68535d952a9..84188bc8b763e63af9be978956b4c5d70262ec65 100644 (file)
@@ -23,6 +23,7 @@ subject to the following restrictions:
 #include "LinearMath/btMatrix3x3.h"
 #include <vector>
 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 class btOptimizedBvh;
 
@@ -30,8 +31,8 @@ class btOptimizedBvh;
 /// This allows for concave collision objects. This is more general then the Static Concave btTriangleMeshShape.
 class btCompoundShape  : public btCollisionShape
 {
-       std::vector<btTransform>                m_childTransforms;
-       std::vector<btCollisionShape*>  m_childShapes;
+       btAlignedObjectArray<btTransform>               m_childTransforms;
+       btAlignedObjectArray<btCollisionShape*> m_childShapes;
        btVector3                                               m_localAabbMin;
        btVector3                                               m_localAabbMax;
 
index 7a4c7ebf5c02188b54a319595b4536e61443ffd7..5af6e5f03d9b06d10555ca41201b45ec58290eba 100644 (file)
@@ -22,6 +22,7 @@ subject to the following restrictions:
 btConvexHullShape ::btConvexHullShape (const float* points,int numPoints,int stride)
 {
        m_points.resize(numPoints);
+
        unsigned char* pointsBaseAddress = (unsigned char*)points;
 
        for (int i=0;i<numPoints;i++)
index afe7dd8f7a920a9662a1c58c8bf6ca016a89863a..beea0e63201c3a970d115646c7a39321dc539d3d 100644 (file)
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include "btPolyhedralConvexShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types
 
-#include <vector>
+
+#include "LinearMath/btAlignedObjectArray.h"
 
 ///ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)
 ///No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices.
@@ -27,7 +28,7 @@ subject to the following restrictions:
 ///(memory is much slower then the cpu)
 class btConvexHullShape : public btPolyhedralConvexShape
 {
-       std::vector<btPoint3>   m_points;
+       btAlignedObjectArray<btPoint3>  m_points;
 
 public:
        ///this constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory.
index 18b796b39b5c14ccf15356a7831833c1719abadd..37f15e1dcc41ab93de78f547aa6db8f7ee4f067c 100644 (file)
@@ -18,6 +18,11 @@ subject to the following restrictions:
 #include "LinearMath/btAabbUtil2.h"
 
 
+btOptimizedBvh::btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) 
+{ 
+
+}
+
 
 void btOptimizedBvh::build(btStridingMeshInterface* triangles)
 {
index 96172c4e298784001a1acdcf7dc12e9688a6ec63..cb76cb2334013f87197bc94ec75dc1291b6c0394 100644 (file)
@@ -15,9 +15,17 @@ subject to the following restrictions:
 
 #ifndef OPTIMIZED_BVH_H
 #define OPTIMIZED_BVH_H
+
+
 #include "LinearMath/btVector3.h"
+
+
+//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
+
+
 #include <vector>
 
+
 class btStridingMeshInterface;
 
 /// btOptimizedBvhNode contains both internal and leaf node information.
@@ -26,7 +34,7 @@ class btStridingMeshInterface;
 /// and storing aabbmin/max as quantized integers.
 /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
 /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
-struct btOptimizedBvhNode
+ATTRIBUTE_ALIGNED16 (struct btOptimizedBvhNode)
 {
 
        btVector3       m_aabbMin;
@@ -52,12 +60,23 @@ public:
        virtual void processNode(const btOptimizedBvhNode* node) = 0;
 };
 
-typedef std::vector<btOptimizedBvhNode>        NodeArray;
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+//typedef std::vector< unsigned , allocator_type >     container_type;
+const unsigned size = (1 << 20);
+typedef btAlignedAllocator< btOptimizedBvhNode , size >  allocator_type;
+
+//typedef btAlignedObjectArray<btOptimizedBvhNode, allocator_type>     NodeArray;
+
+typedef btAlignedObjectArray<btOptimizedBvhNode>       NodeArray;
 
 
 ///OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
 class btOptimizedBvh
 {
+       NodeArray                       m_leafNodes;
+
        btOptimizedBvhNode*     m_rootNode1;
        
        btOptimizedBvhNode*     m_contiguousNodes;
@@ -65,10 +84,11 @@ class btOptimizedBvh
 
        int                                     m_numNodes;
 
-       NodeArray                       m_leafNodes;
+
 
 public:
-       btOptimizedBvh() :m_rootNode1(0), m_numNodes(0) { }
+       btOptimizedBvh();
+
        virtual ~btOptimizedBvh();
        
        void    build(btStridingMeshInterface* triangles);
index d04341aa809bc28f7d2b340a682ab8a777a1fa14..9a623403846e636f502a2cfeb1539f8c27db5e16 100644 (file)
@@ -17,7 +17,8 @@ subject to the following restrictions:
 #define BT_TRIANGLE_BUFFER_H
 
 #include "btTriangleCallback.h"
-#include <vector>
+//#include <vector>
+#include "LinearMath/btAlignedObjectArray.h"
 
 struct btTriangle
 {
@@ -32,7 +33,7 @@ struct        btTriangle
 class btTriangleBuffer : public btTriangleCallback
 {
 
-       std::vector<btTriangle> m_triangleBuffer;
+       btAlignedObjectArray<btTriangle>        m_triangleBuffer;
        
 public:
 
index 638c8b87fb113672ca7869dd48782e4daace3da8..3ec827c03d9662f47dab38af218779aaaded9a47 100644 (file)
@@ -17,7 +17,7 @@ subject to the following restrictions:
 #define BT_TRIANGLE_INDEX_VERTEX_ARRAY_H
 
 #include "btStridingMeshInterface.h"
-#include <vector>
+#include <LinearMath/btAlignedObjectArray.h>
 
 ///IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements
 ///instead of the number of indices, we pass the number of triangles
@@ -38,7 +38,7 @@ struct        btIndexedMesh
 ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray.
 class btTriangleIndexVertexArray : public btStridingMeshInterface
 {
-       std::vector<btIndexedMesh>      m_indexedMeshes;
+       btAlignedObjectArray<btIndexedMesh>     m_indexedMeshes;
 
                
 public:
index 171dcf33b15927e11efa483262b81a8b8d75344a..1be03d70436a0ab3aa3055d74d831a26d02dcafa 100644 (file)
@@ -18,9 +18,8 @@ subject to the following restrictions:
 #define TRIANGLE_MESH_H
 
 #include "BulletCollision/CollisionShapes/btStridingMeshInterface.h"
-#include <vector>
 #include <LinearMath/btVector3.h>
-
+#include "LinearMath/btAlignedObjectArray.h"
 struct btMyTriangle
 {
        btVector3       m_vert0;
@@ -31,8 +30,7 @@ struct btMyTriangle
 ///TriangleMesh provides storage for a concave triangle mesh. It can be used as data for the btTriangleMeshShape.
 class btTriangleMesh : public btStridingMeshInterface
 {
-       std::vector<btMyTriangle>       m_triangles;
-       
+       btAlignedObjectArray<btMyTriangle>      m_triangles;
 
        public:
                btTriangleMesh ();
index e073797f989792dfeec733cd6fe45394db289184..ce90f798c04a9c801191cdefdb9feb2d71009a6d 100644 (file)
@@ -18,7 +18,7 @@ subject to the following restrictions:
 
 class btPersistentManifold;
 class btRigidBody;
-
+class btTypedConstraint;
 struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
@@ -31,7 +31,7 @@ public:
 
        virtual ~btConstraintSolver() {}
        
-       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
+       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0;
 
 };
 
index b5783f824d42afd7b35b1a5c631ef600207a40b6..b2132a8d4f3f78ec6006159239e45e60abb233dd 100644 (file)
@@ -289,7 +289,7 @@ void        btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
                                btScalar loLimit =  m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
                                btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
 
-                               float projAngle  = -2.*xyz[i];
+                               float projAngle  = -2.f*xyz[i];
                                
                                if (projAngle < loLimit)
                                {
index 7b2a9ba624447b6e9432ca0dd973d5868ae43df1..f72278e2cbffd03fb5e823c27fd5c0ba52c1b666 100644 (file)
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 
 
-btHingeConstraint::btHingeConstraint()
+btHingeConstraint::btHingeConstraint():
+m_enableAngularMotor(false)
 {
 }
 
@@ -28,7 +29,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
 :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
 m_axisInA(axisInA),
 m_axisInB(-axisInB),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
 
 }
@@ -39,7 +41,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,
 m_axisInA(axisInA),
 //fixed axis in worldspace
 m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
-m_angularOnly(false)
+m_angularOnly(false),
+m_enableAngularMotor(false)
 {
        
 }
@@ -73,11 +76,16 @@ void        btHingeConstraint::buildJacobian()
        //these two jointAxis require equal angular velocities for both bodies
 
        //this is unused for now, it's a todo
-       btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-       btVector3 jointAxis0;
-       btVector3 jointAxis1;
-       btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
+       btVector3 jointAxis0local;
+       btVector3 jointAxis1local;
        
+       btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
+
+       getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+       btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
+       btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
+       btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+               
        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
@@ -90,107 +98,18 @@ void       btHingeConstraint::buildJacobian()
                m_rbA.getInvInertiaDiagLocal(),
                m_rbB.getInvInertiaDiagLocal());
 
+       new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
+               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+               m_rbA.getInvInertiaDiagLocal(),
+               m_rbB.getInvInertiaDiagLocal());
+
+
 
 }
 
 void   btHingeConstraint::solveConstraint(btScalar     timeStep)
 {
-//#define NEW_IMPLEMENTATION
-
-#ifdef NEW_IMPLEMENTATION
-       btScalar tau = 0.3f;
-       btScalar damping = 1.f;
-
-       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
-
-       // Dirk: Don't we need to update this after each applied impulse
-       btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-
-       if (!m_angularOnly)
-       {
-               btVector3 normal(0,0,0);
-
-               for (int i=0;i<3;i++)
-               {               
-                       normal[i] = 1;
-                       btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
-
-                       btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-                       btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-                       
-                       btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-                       btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-                       btVector3 vel = vel1 - vel2;
-
-                       // Dirk: Get new angular velocity since it changed after applying an impulse
-                       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-                       angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-       
-                       //velocity error (first order error)
-                       btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
-               
-                       //positional error (zeroth order error)
-                       btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
-                       
-                       btScalar impulse = tau*depth/timeStep * jacDiagABInv -  damping * rel_vel * jacDiagABInv;
-
-                       btVector3 impulse_vector = normal * impulse;
-                       m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
-                       m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-                       
-                       normal[i] = 0;
-               }
-       }
-
-       ///solve angular part
-
-       // get axes in world space
-       btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-       btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
-
-       // constraint axes in world space
-       btVector3 jointAxis0;
-       btVector3 jointAxis1;
-       btPlaneSpace1(axisA,jointAxis0,jointAxis1);
-
-
-       // Dirk: Get new angular velocity since it changed after applying an impulse
-       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-       
-       btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
-       btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);
-       float tau1 = tau;//0.f;
-
-       btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
-       btVector3 angular_impulse0 = jointAxis0 * impulse0;
-
-       m_rbA.applyTorqueImpulse( angular_impulse0);
-       m_rbB.applyTorqueImpulse(-angular_impulse0);
-
-
-
-       // Dirk: Get new angular velocity since it changed after applying an impulse    
-       angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-    angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-
-       btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
-       btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-                                                                                                                                       m_rbB.getLinearVelocity(),angvelB);;
-
-       btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
-       btVector3 angular_impulse1 = jointAxis1 * impulse1;
-
-       m_rbA.applyTorqueImpulse( angular_impulse1);
-       m_rbB.applyTorqueImpulse(-angular_impulse1);
-
-#else
-
 
        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -237,37 +156,68 @@ void      btHingeConstraint::solveConstraint(btScalar     timeStep)
 
                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
-               btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
-               btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
-               btVector3 velrel = angA-angB;
-
-               //solve angular velocity correction
-               float relaxation = 1.f;
-               float len = velrel.length();
-               if (len > 0.00001f)
+
+               btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
+               btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
+
+               btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
+               btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
+               btVector3 velrelOrthog = angAorthog-angBorthog;
                {
-                       btVector3 normal = velrel.normalized();
-                       float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
-                               getRigidBodyB().computeAngularImpulseDenominator(normal);
-                       // scale for mass and relaxation
-                       velrel *= (1.f/denom) * 0.9;
+                       //solve orthogonal angular velocity correction
+                       float relaxation = 1.f;
+                       float len = velrelOrthog.length();
+                       if (len > 0.00001f)
+                       {
+                               btVector3 normal = velrelOrthog.normalized();
+                               float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+                                       getRigidBodyB().computeAngularImpulseDenominator(normal);
+                               // scale for mass and relaxation
+                               //todo:  expose this 0.9 factor to developer
+                               velrelOrthog *= (1.f/denom) * 0.9f;
+                       }
+
+                       //solve angular positional correction
+                       btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
+                       float len2 = angularError.length();
+                       if (len2>0.00001f)
+                       {
+                               btVector3 normal2 = angularError.normalized();
+                               float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+                                               getRigidBodyB().computeAngularImpulseDenominator(normal2);
+                               angularError *= (1.f/denom2) * relaxation;
+                       }
+
+                       m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+                       m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
                }
 
-               //solve angular positional correction
-               btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
-               float len2 = angularError.length();
-               if (len2>0.00001f)
+               //apply motor
+               if (m_enableAngularMotor)
                {
-                       btVector3 normal2 = angularError.normalized();
-                       float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
-                                       getRigidBodyB().computeAngularImpulseDenominator(normal2);
-                       angularError *= (1.f/denom2) * relaxation;
-               }
+                       //todo: add limits too
+                       btVector3 angularLimit(0,0,0);
+
+                       btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+                       btScalar projRelVel = velrel.dot(axisA);
+
+                       btScalar desiredMotorVel = m_motorTargetVelocity;
+                       btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-               m_rbA.applyTorqueImpulse(-velrel+angularError);
-               m_rbB.applyTorqueImpulse(velrel-angularError);
+                       float denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+                                       getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+                       btScalar unclippedMotorImpulse = (1.f/denom3) * motor_relvel;;
+                       //todo: should clip against accumulated impulse
+                       btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
+                       clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
+                       btVector3 motorImp = clippedMotorImpulse * axisA;
+
+                       m_rbA.applyTorqueImpulse(motorImp+angularLimit);
+                       m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
+                       
+               }
        }
-#endif
 
 }
 
index 3bade2b091d666af63dca0f2f2023e9fe0568fd3..553ec135c8ae5265656ae8b5e01ceb6ede49b404 100644 (file)
@@ -29,14 +29,18 @@ class btRigidBody;
 class btHingeConstraint : public btTypedConstraint
 {
        btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
-       btJacobianEntry m_jacAng[2]; //2 orthogonal angular constraints
+       btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
        btVector3       m_pivotInA;
        btVector3       m_pivotInB;
        btVector3       m_axisInA;
        btVector3       m_axisInB;
 
-       bool    m_angularOnly;
+       bool            m_angularOnly;
+
+       float           m_motorTargetVelocity;
+       float           m_maxMotorImpulse;
+       bool            m_enableAngularMotor;
        
 public:
 
@@ -66,7 +70,12 @@ public:
                m_angularOnly = angularOnly;
        }
 
-
+       void    enableAngularMotor(bool enableMotor,float targetVelocity,float maxMotorImpulse)
+       {
+               m_enableAngularMotor  = enableMotor;
+               m_motorTargetVelocity = targetVelocity;
+               m_maxMotorImpulse = maxMotorImpulse;
+       }
 
 };
 
index ce85fc398befa7cdc348c32e222c68417f8a1f2f..94eece73e7c4afbceae393bc76ec24c259fd648e 100644 (file)
@@ -23,6 +23,8 @@ subject to the following restrictions:
 #include "LinearMath/btIDebugDraw.h"
 #include "btJacobianEntry.h"
 #include "LinearMath/btMinMax.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
 
 #ifdef USE_PROFILE
 #include "LinearMath/btQuickprof.h"
@@ -123,7 +125,7 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
 }
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
        
        btContactSolverInfo info = infoGlobal;
@@ -151,6 +153,15 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
                        }
                }
        }
+
+       {
+               int j;
+               for (j=0;j<numConstraints;j++)
+               {
+                       btTypedConstraint* constraint = constraints[j];
+                       constraint->buildJacobian();
+               }
+       }
        
        //should traverse the contacts random order...
        int iteration;
@@ -171,6 +182,12 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
                                }
                        }
 
+                       for (j=0;j<numConstraints;j++)
+                       {
+                               btTypedConstraint* constraint = constraints[j];
+                               constraint->solveConstraint(info.m_timeStep);
+                       }
+
                        for (j=0;j<totalPoints;j++)
                        {
                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
@@ -197,7 +214,7 @@ float btSequentialImpulseConstraintSolver3::solveGroup(btPersistentManifold** ma
 
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
        
        btContactSolverInfo info = infoGlobal;
@@ -222,6 +239,14 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
                        }
                }
        }
+       {
+               int j;
+               for (j=0;j<numConstraints;j++)
+               {
+                       btTypedConstraint* constraint = constraints[j];
+                       constraint->buildJacobian();
+               }
+       }
        
        //should traverse the contacts random order...
        int iteration;
@@ -230,6 +255,12 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
        {
                int j;
 
+               for (j=0;j<numConstraints;j++)
+               {
+                       btTypedConstraint* constraint = constraints[j];
+                       constraint->solveConstraint(info.m_timeStep);
+               }
+
                for (j=0;j<numManifolds;j++)
                {
                        btPersistentManifold* manifold = manifoldPtr[j];
index ec56af14dacf4820915c886505d4defb103a644e..0989a86e2cd582459f3c80cbd67b37010bec42d6 100644 (file)
@@ -68,7 +68,7 @@ public:
 
        virtual ~btSequentialImpulseConstraintSolver() {}
        
-       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
        void    setSolverMode(int mode)
        {
@@ -88,7 +88,7 @@ public:
 
        btSequentialImpulseConstraintSolver3();
 
-       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+       virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
 
 };
index f59598f2a5383159e7c061a6bc1e3d4f227292c4..fc37cf2e3c3744b38507be0f9ae0797ffc887525 100644 (file)
@@ -261,7 +261,7 @@ void        btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
        dispatchInfo.m_debugDraw = getDebugDrawer();
 
        ///perform collision detection
-       performDiscreteCollisionDetection(dispatchInfo);
+       performDiscreteCollisionDetection();
 
        calculateSimulationIslands();
 
@@ -270,12 +270,9 @@ void       btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
        
 
 
-       ///solve non-contact constraints
-       solveNoncontactConstraints(getSolverInfo());
+       ///solve contact and other joint constraints
+       solveConstraints(getSolverInfo());
        
-       ///solve contact constraints
-       solveContactConstraints(getSolverInfo());
-
        ///CallbackTriggers();
 
        ///integrate transforms
@@ -401,80 +398,117 @@ void     btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
        }
 }
 
+inline int     btGetConstraintIslandId(const btTypedConstraint* lhs)
+{
+       int islandId;
+       
+       const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
+       const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
+       islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
+       return islandId;
+
+}
 
-void   btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo)
+static bool btSortConstraintOnIslandPredicate(const btTypedConstraint* lhs, const btTypedConstraint* rhs)
+{
+       int rIslandId0,lIslandId0;
+       rIslandId0 = btGetConstraintIslandId(rhs);
+       lIslandId0 = btGetConstraintIslandId(lhs);
+       return lIslandId0 < rIslandId0;
+}
+
+void   btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
        
-       BEGIN_PROFILE("solveContactConstraints");
+       BEGIN_PROFILE("solveConstraints");
 
        struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
        {
 
-               btContactSolverInfo& m_solverInfo;
-               btConstraintSolver*     m_solver;
-               btIDebugDraw*   m_debugDrawer;
+               btContactSolverInfo&    m_solverInfo;
+               btConstraintSolver*             m_solver;
+               btTypedConstraint**             m_sortedConstraints;
+               int                                             m_numConstraints;
+               btIDebugDraw*                   m_debugDrawer;
+
+
 
                InplaceSolverIslandCallback(
                        btContactSolverInfo& solverInfo,
                        btConstraintSolver*     solver,
+                       btTypedConstraint** sortedConstraints,
+                       int     numConstraints,
                        btIDebugDraw*   debugDrawer)
                        :m_solverInfo(solverInfo),
                        m_solver(solver),
+                       m_sortedConstraints(sortedConstraints),
+                       m_numConstraints(numConstraints),
                        m_debugDrawer(debugDrawer)
                {
 
                }
 
-               virtual void    ProcessIsland(btPersistentManifold**    manifolds,int numManifolds)
+               virtual void    ProcessIsland(btPersistentManifold**    manifolds,int numManifolds, int islandId)
                {
-                       m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
+                       //also add all non-contact constraints/joints for this island
+                       btTypedConstraint** startConstraint = 0;
+                       int numCurConstraints = 0;
+                       int i;
+                       
+                       //find the first constraint for this island
+                       for (i=0;i<m_numConstraints;i++)
+                       {
+                               if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+                               {
+                                       startConstraint = &m_sortedConstraints[i];
+                                       break;
+                               }
+                       }
+                       //count the number of constraints in this island
+                       for (;i<m_numConstraints;i++)
+                       {
+                               if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+                               {
+                                       numCurConstraints++;
+                               }
+                       }
+
+                       m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer);
                }
 
        };
 
        
-       InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver,     m_debugDrawer);
 
        
-       /// solve all the contact points and contact friction
-       m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
-
-       END_PROFILE("solveContactConstraints");
 
-}
-
-
-void   btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo)
-{
-       BEGIN_PROFILE("solveNoncontactConstraints");
-
-       int i;
-       int numConstraints = int(m_constraints.size());
-
-       ///constraint preparation: building jacobians
-       for (i=0;i< numConstraints ; i++ )
+       //sorted version of all btTypedConstraint, based on islandId
+       std::vector<btTypedConstraint*> sortedConstraints;
+       sortedConstraints.resize( m_constraints.size());
+       int i; 
+       for (i=0;i<getNumConstraints();i++)
        {
-               btTypedConstraint* constraint = m_constraints[i];
-               constraint->buildJacobian();
+               sortedConstraints[i] = m_constraints[i];
        }
+       
+       std::sort(sortedConstraints.begin(),sortedConstraints.end(),btSortConstraintOnIslandPredicate);
+       
+       btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
+       
+       InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver, constraintsPtr,sortedConstraints.size(),    m_debugDrawer);
 
-       //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
-       for (int g=0;g<solverInfo.m_numIterations;g++)
-       {
-               //
-               // constraint solving
-               //
-               for (i=0;i< numConstraints ; i++ )
-               {
-                       btTypedConstraint* constraint = m_constraints[i];
-                       constraint->solveConstraint( solverInfo.m_timeStep );
-               }
-       }
+       
+       
+       /// solve all the constraints for this island
+       m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
 
-       END_PROFILE("solveNoncontactConstraints");
+       END_PROFILE("solveConstraints");
 
 }
 
+
+
+
 void   btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
        BEGIN_PROFILE("calculateSimulationIslands");
index 72619091c6ff86a3a62a1cc9fb0066170a317efa..8575f8506f136180324a270e58d737dbd8d76645 100644 (file)
@@ -66,10 +66,8 @@ protected:
                
        void    calculateSimulationIslands();
 
-       void    solveNoncontactConstraints(btContactSolverInfo& solverInfo);
-
-       void    solveContactConstraints(btContactSolverInfo& solverInfo);
-
+       void    solveConstraints(btContactSolverInfo& solverInfo);
+       
        void    updateActivationState(float timeStep);
 
        void    updateVehicles(float timeStep);
index 453b8ec4289749f235fc5c0bbf4e79edd36c1e7a..705c023d3a0b0246319ade20f4a1fcd7f8c81263 100644 (file)
@@ -110,16 +110,17 @@ btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisi
 #endif //OBSOLETE_MOTIONSTATE_LESS
 
 
-#define EXPERIMENTAL_JITTER_REMOVAL 1
+//#define EXPERIMENTAL_JITTER_REMOVAL 1
 #ifdef EXPERIMENTAL_JITTER_REMOVAL
 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
 //doesn't work very well yet (value 0 disabled this damping)
 //note there this influences deactivation thresholds!
 float gClippedAngvelThresholdSqr = 0.01f;
 float  gClippedLinearThresholdSqr = 0.01f;
-float  gJitterVelocityDampingFactor = 1.f;
 #endif //EXPERIMENTAL_JITTER_REMOVAL
 
+float  gJitterVelocityDampingFactor = 1.f;
+
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {
 
index ce5c76c58e2e8977dc70cff54bd001a693f64413..fe0124c041ae317455838366928128c315a68845 100644 (file)
@@ -46,13 +46,13 @@ int         btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
        ///apply gravity, predict motion
        predictUnconstraintMotion(timeStep);
 
-       btDispatcherInfo        dispatchInfo;
+       btDispatcherInfo&       dispatchInfo = getDispatchInfo();
        dispatchInfo.m_timeStep = timeStep;
        dispatchInfo.m_stepCount = 0;
        dispatchInfo.m_debugDraw = getDebugDrawer();
 
        ///perform collision detection
-       performDiscreteCollisionDetection(dispatchInfo );
+       performDiscreteCollisionDetection();
 
        ///solve contact constraints
        int numManifolds = m_dispatcher1->getNumManifolds();
@@ -63,7 +63,7 @@ int           btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
                btContactSolverInfo infoGlobal;
                infoGlobal.m_timeStep = timeStep;
                
-               m_constraintSolver->solveGroup(manifoldPtr, numManifolds,infoGlobal,m_debugDrawer);
+               m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer);
        }
 
        ///integrate transforms
@@ -101,7 +101,10 @@ void       btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
 {
        body->setGravity(m_gravity);
 
-       addCollisionObject(body);
+       if (body->getCollisionShape())
+       {
+               addCollisionObject(body);
+       }
 }
 
 void   btSimpleDynamicsWorld::updateAabbs()
index 248ecfbce4efbde2363d1a4fcd63ffe4accb4c8e..c85fead53340f7d8ac33c3a637e145d9d279158a 100644 (file)
@@ -128,11 +128,10 @@ void      btRaycastVehicle::updateWheelTransform( int wheelIndex , bool interpolatedT
 void btRaycastVehicle::resetSuspension()
 {
 
-       std::vector<btWheelInfo>::iterator wheelIt;
-       for (wheelIt = m_wheelInfo.begin();
-       !(wheelIt == m_wheelInfo.end());wheelIt++)
+       int i;
+       for (i=0;i<m_wheelInfo.size();  i++)
        {
-                       btWheelInfo& wheel = *wheelIt;
+                       btWheelInfo& wheel = m_wheelInfo[i];
                        wheel.m_raycastInfo.m_suspensionLength = wheel.getSuspensionRestLength();
                        wheel.m_suspensionRelativeVelocity = 0.0f;
                        
@@ -285,23 +284,21 @@ void btRaycastVehicle::updateVehicle( btScalar step )
        //
        // simulate suspension
        //
-       std::vector<btWheelInfo>::iterator wheelIt;
+       
        int i=0;
-       for (wheelIt = m_wheelInfo.begin();
-       !(wheelIt == m_wheelInfo.end());wheelIt++,i++)
+       for (i=0;i<m_wheelInfo.size();i++)
        {
                btScalar depth; 
-               depth = rayCast( *wheelIt );
+               depth = rayCast( m_wheelInfo[i]);
        }
 
        updateSuspension(step);
 
        
-       for (wheelIt = m_wheelInfo.begin();
-       !(wheelIt == m_wheelInfo.end());wheelIt++)
+       for (i=0;i<m_wheelInfo.size();i++)
        {
                //apply suspension force
-               btWheelInfo& wheel = *wheelIt;
+               btWheelInfo& wheel = m_wheelInfo[i];
                
                float suspensionForce = wheel.m_wheelsSuspensionForce;
                
@@ -322,10 +319,9 @@ void btRaycastVehicle::updateVehicle( btScalar step )
        updateFriction( step);
 
        
-       for (wheelIt = m_wheelInfo.begin();
-       !(wheelIt == m_wheelInfo.end());wheelIt++)
+       for (i=0;i<m_wheelInfo.size();i++)
        {
-               btWheelInfo& wheel = *wheelIt;
+               btWheelInfo& wheel = m_wheelInfo[i];
                btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
                btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
 
index b928248cc2dc7ad820882c77c84b777afb8617cc..fa961e468d77a6818746f0a2259425c4ac74cce0 100644 (file)
@@ -15,7 +15,7 @@
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include "btVehicleRaycaster.h"
 class btDynamicsWorld;
-
+#include "LinearMath/btAlignedObjectArray.h"
 #include "btWheelInfo.h"
 
 class btVehicleTuning;
@@ -95,7 +95,7 @@ public:
                return int (m_wheelInfo.size());
        }
        
-       std::vector<btWheelInfo>        m_wheelInfo;
+       btAlignedObjectArray<btWheelInfo>       m_wheelInfo;
 
 
        const btWheelInfo&      getWheelInfo(int index) const;
diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp
new file mode 100644 (file)
index 0000000..e32bc9f
--- /dev/null
@@ -0,0 +1,47 @@
+/*
+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 "btAlignedAllocator.h"
+
+
+#if defined (WIN32) && !defined(__MINGW32__) && !defined(__CYGWIN__)
+
+#include <malloc.h>
+void*  btAlignedAlloc  (int size, int alignment)
+{
+       return _aligned_malloc(size,alignment);
+}
+
+void   btAlignedFree   (void* ptr)
+{
+       _aligned_free(ptr);
+}
+
+#else
+
+///todo
+///will add some multi-platform version that works without _aligned_malloc/_aligned_free
+
+void*  btAlignedAlloc  (int size, int alignment)
+{
+       return new char[size];
+}
+
+void   btAlignedFree   (void* ptr)
+{
+       delete ptr;
+}
+
+#endif
\ No newline at end of file
diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.h b/extern/bullet2/src/LinearMath/btAlignedAllocator.h
new file mode 100644 (file)
index 0000000..159399c
--- /dev/null
@@ -0,0 +1,77 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_ALIGNED_ALLOCATOR
+#define BT_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "btScalar.h"
+
+void*  btAlignedAlloc  (int size, int alignment);
+
+void   btAlignedFree   (void* ptr);
+
+
+typedef int    size_type;
+
+
+template < typename T , unsigned Alignment >
+class btAlignedAllocator {
+       
+       typedef btAlignedAllocator< T , Alignment > self_type;
+       
+public:
+       //just going down a list:
+       btAlignedAllocator() {}
+       
+       btAlignedAllocator( const self_type & ) {}
+       
+       template < typename Other >
+       btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
+
+       typedef const T*         const_pointer;
+       typedef const T&         const_reference;
+       typedef T*               pointer;
+       typedef T&               reference;
+       typedef T                value_type;
+
+       pointer       address   ( reference        ref ) const                           { return &ref; }
+       const_pointer address   ( const_reference  ref ) const                           { return &ref; }
+       pointer       allocate  ( size_type        n   , const_pointer *      hint = 0 ) {
+               return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
+       }
+       void          construct ( pointer          ptr , const value_type &   value    ) { new (ptr) value_type( value ); }
+       void          deallocate( pointer          ptr ) {
+               btAlignedFree( reinterpret_cast< void * >( ptr ) );
+       }
+       void          destroy   ( pointer          ptr )                                 { ptr->~value_type(); }
+       
+
+       template < typename O > struct rebind {
+               typedef btAlignedAllocator< O , Alignment > other;
+       };
+       template < typename O >
+       self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
+
+       friend bool operator==( const self_type & , const self_type & ) { return true; }
+};
+
+
+
+#endif //BT_ALIGNED_ALLOCATOR
+
diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h
new file mode 100644 (file)
index 0000000..79469c0
--- /dev/null
@@ -0,0 +1,175 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#ifndef BT_OBJECT_ARRAY__
+#define BT_OBJECT_ARRAY__
+
+#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
+#include "btAlignedAllocator.h"
+
+///btAlignedObjectArray uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid STL alignment issues to add SIMD/SSE data
+template <typename T> 
+//template <class T> 
+class btAlignedObjectArray
+{
+       int                                     m_size;
+       int                                     m_capacity;
+       T*                                      m_data;
+
+       btAlignedAllocator<T , 16>      m_allocator;
+
+       protected:
+               SIMD_FORCE_INLINE       int     allocSize(int size)
+               {
+                       return (size ? size*2 : 1);
+               }
+               SIMD_FORCE_INLINE       void    copy(int start,int end, T* dest)
+               {
+                       int i;
+                       for (i=0;i<m_size;++i)
+                               dest[i] = m_data[i];
+               }
+
+               SIMD_FORCE_INLINE       void    init()
+               {
+                       m_data = 0;
+                       m_size = 0;
+                       m_capacity = 0;
+               }
+               SIMD_FORCE_INLINE       void    destroy(int first,int last)
+               {
+                       int i;
+                       for (i=0; i<m_size;i++)
+                       {
+                               m_data[i].~T();
+                       }
+               }
+
+               SIMD_FORCE_INLINE       void* allocate(int size)
+               {
+                       if (size)
+                               return m_allocator.allocate(size);
+                       return 0;
+               }
+
+               SIMD_FORCE_INLINE       void    deallocate()
+               {
+                       if(m_data)      
+                               m_allocator.deallocate(m_data);
+               }
+
+
+       public:
+               
+               btAlignedObjectArray()
+               {
+                       init();
+               }
+
+               ~btAlignedObjectArray()
+               {
+                       clear();
+               }
+
+               SIMD_FORCE_INLINE       int capacity() const
+               {       // return current length of allocated storage
+                       return m_capacity;
+               }
+               
+               SIMD_FORCE_INLINE       int size() const
+               {       // return length of sequence
+                       return m_size;
+               }
+               
+               SIMD_FORCE_INLINE const T& operator[](int n) const
+               {
+                       return m_data[n];
+               }
+
+               SIMD_FORCE_INLINE T& operator[](int n)
+               {
+                       return m_data[n];
+               }
+               
+
+               SIMD_FORCE_INLINE       void    clear()
+               {
+                       destroy(0,size());
+                       
+                       deallocate();
+                       
+                       init();
+               }
+
+               SIMD_FORCE_INLINE       void    pop_back()
+               {
+                       m_size--;
+                       m_data[m_size].~T();
+               }
+
+               SIMD_FORCE_INLINE       void    resize(int newsize)
+               {
+                       if (newsize > size())
+                       {
+                               reserve(newsize);
+                       }
+
+                       m_size = newsize;
+               }
+       
+
+
+               SIMD_FORCE_INLINE       void push_back(const T& _Val)
+               {       
+                       int sz = size();
+                       if( sz == capacity() )
+                       {
+                               reserve( allocSize(size()) );
+                       }
+                       
+                       m_data[size()] = _Val;
+                       //::new ( m_data[m_size] ) T(_Val);
+                       m_size++;
+               }
+
+       
+               
+               SIMD_FORCE_INLINE       void reserve(int _Count)
+               {       // determine new minimum length of allocated storage
+                       if (capacity() < _Count)
+                       {       // not enough room, reallocate
+                               if (capacity() < _Count)
+                               {
+                                       T*      s = (T*)allocate(_Count);
+
+                                       copy(0, size(), s);
+
+                                       destroy(0,size());
+
+                                       deallocate();
+
+                                       m_data = s;
+                                       
+                                       m_capacity = _Count;
+
+                               }
+                       }
+               }
+
+};
+
+#endif //BT_OBJECT_ARRAY__
\ No newline at end of file
index 5176d317417007b1994ef5b7be00ec428ddc1cf4..5036894b2b3496b4d455c2f829f068d51d86ffd3 100644 (file)
@@ -16,7 +16,7 @@ subject to the following restrictions:
 
 #include "btGeometryUtil.h"
 
-bool   btGeometryUtil::isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin)
+bool   btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, float        margin)
 {
        int numbrushes = planeEquations.size();
        for (int i=0;i<numbrushes;i++)
@@ -33,7 +33,7 @@ bool  btGeometryUtil::isPointInsidePlanes(const std::vector<btVector3>& planeEqua
 }
 
 
-bool   btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float      margin)
+bool   btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, float     margin)
 {
        int numvertices = vertices.size();
        for (int i=0;i<numvertices;i++)
@@ -48,7 +48,7 @@ bool  btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const
        return true;
 }
 
-bool notExist(const btVector3& planeEquation,const std::vector<btVector3>& planeEquations)
+bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
 {
        int numbrushes = planeEquations.size();
        for (int i=0;i<numbrushes;i++)
@@ -62,7 +62,7 @@ bool notExist(const btVector3& planeEquation,const std::vector<btVector3>& plane
        return true;
 }
 
-void   btGeometryUtil::getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut )
+void   btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
 {
                const int numvertices = vertices.size();
        // brute force:
@@ -110,7 +110,7 @@ void        btGeometryUtil::getPlaneEquationsFromVertices(std::vector<btVector3>& verti
 
 }
 
-void   btGeometryUtil::getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut )
+void   btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
 {
        const int numbrushes = planeEquations.size();
        // brute force:
index 525291e605146450320bd0c10f3afa3d586d8041..018ffa72296a9e9727dac5b42a3f69c4862dde33 100644 (file)
@@ -15,23 +15,24 @@ subject to the following restrictions:
 
 #ifndef BT_GEOMETRY_UTIL_H
 #define BT_GEOMETRY_UTIL_H
-#include <vector>
+
 #include "btVector3.h"
+#include "btAlignedObjectArray.h"
 
 class btGeometryUtil
 {
        public:
        
        
-               static void     getPlaneEquationsFromVertices(std::vector<btVector3>& vertices, std::vector<btVector3>& planeEquationsOut );
+               static void     getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
 
-               static void     getVerticesFromPlaneEquations(const std::vector<btVector3>& planeEquations , std::vector<btVector3>& verticesOut );
+               static void     getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
        
-               static bool     isInside(const std::vector<btVector3>& vertices, const btVector3& planeNormal, float    margin);
+               static bool     isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, float   margin);
                
-               static bool     isPointInsidePlanes(const std::vector<btVector3>& planeEquations, const btVector3& point, float margin);
+               static bool     isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, float        margin);
 
-               static bool     areVerticesBehindPlane(const btVector3& planeNormal, const std::vector<btVector3>& vertices, float      margin);
+               static bool     areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, float     margin);
 
 };
 
index 28bb2051dea6b216db5e310200c1022b8cc1525f..4331b668210d35a5102e4ecf9ce398ef131f479b 100644 (file)
@@ -21,11 +21,10 @@ subject to the following restrictions:
 
 
 
-
-class  btQuadWord
+ATTRIBUTE_ALIGNED16 (class     btQuadWord)
 {
        protected:
-               ATTRIBUTE_ALIGNED16     (btScalar       m_x);
+               btScalar        m_x;
                btScalar        m_y;
                btScalar        m_z;
                btScalar        m_unusedW;
index 46b8145a76662b1a0bdde017bf40e0fa866d032a..dd76fb2de1aa0e07a1aa870290cad063bf57a5c6 100644 (file)
@@ -27,15 +27,15 @@ subject to the following restrictions:
 
                #if defined(__MINGW32__) || defined(__CYGWIN__)
                        #define SIMD_FORCE_INLINE inline
+                       #define ATTRIBUTE_ALIGNED16(a) a
                #else
                        #pragma warning(disable:4530)
                        #pragma warning(disable:4996)
                        #pragma warning(disable:4786)
                        #define SIMD_FORCE_INLINE __forceinline
+                       #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
                #endif //__MINGW32__
-       
-               //#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
-               #define ATTRIBUTE_ALIGNED16(a) a
+
                #include <assert.h>
                #define btAssert assert
 #else
@@ -54,8 +54,10 @@ subject to the following restrictions:
 
 typedef float    btScalar;
 
-#if defined (__sun) || defined (__sun__) || defined (__sparc) || defined (__APPLE__)
-//use double float precision operation on those platforms for Blender
+///older compilers (gcc 3.x) and Sun needs double versions of srqt etc.
+///exclude Apple Intel (it's assumed to be a Macbook or newer Intel Dual Core processor)
+#if defined (__sun) || defined (__sun__) || defined (__sparc) || (defined (__APPLE__) && ! defined (__i386__))
+//use slow double float precision operation on those platforms
                
 SIMD_FORCE_INLINE btScalar btSqrt(btScalar x) { return sqrt(x); }
 SIMD_FORCE_INLINE btScalar btFabs(btScalar x) { return fabs(x); }