diff options
31 files changed, 633 insertions, 254 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 9df6d7186ec..fa1561973fb 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -121,8 +121,10 @@ void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,sho -void btCollisionWorld::performDiscreteCollisionDetection(btDispatcherInfo& dispatchInfo) +void btCollisionWorld::performDiscreteCollisionDetection() { + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + BEGIN_PROFILE("performDiscreteCollisionDetection"); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index 9c548784829..bd09d8c4d5d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -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() { diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 33287008004..3f51e9ec0a6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -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; } } } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 36c51000a6b..68d9b8038d6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -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); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index c810a654834..84188bc8b76 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -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; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 7a4c7ebf5c0..5af6e5f03d9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -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++) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index afe7dd8f7a9..beea0e63201 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -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. diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 18b796b39b5..37f15e1dcc4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 96172c4e298..cb76cb23340 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -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); diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h index d04341aa809..9a623403846 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleBuffer.h @@ -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: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 638c8b87fb1..3ec827c03d9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -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: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index 171dcf33b15..1be03d70436 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -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 (); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index e073797f989..ce90f798c04 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -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; }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index b5783f824d4..b2132a8d4f3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 7b2a9ba6244..f72278e2cbf 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -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 } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 3bade2b091d..553ec135c8a 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -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; + } }; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ce85fc398be..94eece73e7c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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]; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index ec56af14dac..0989a86e2cd 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -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); }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index f59598f2a53..fc37cf2e3c3 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -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"); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 72619091c6f..8575f8506f1 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -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); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 453b8ec4289..705c023d3a0 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -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) { diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index ce5c76c58e2..fe0124c041a 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -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() diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 248ecfbce4e..c85fead5334 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -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 ); diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index b928248cc2d..fa961e468d7 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -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 index 00000000000..e32bc9f6880 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -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 index 00000000000..159399ce7d4 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.h @@ -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 index 00000000000..79469c03c29 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -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 diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp index 5176d317417..5036894b2b3 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.cpp +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.cpp @@ -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: diff --git a/extern/bullet2/src/LinearMath/btGeometryUtil.h b/extern/bullet2/src/LinearMath/btGeometryUtil.h index 525291e6051..018ffa72296 100644 --- a/extern/bullet2/src/LinearMath/btGeometryUtil.h +++ b/extern/bullet2/src/LinearMath/btGeometryUtil.h @@ -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); }; diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index 28bb2051dea..4331b668210 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -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; diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 46b8145a766..dd76fb2de1a 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -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); } |