diff --git a/Demos/Benchmarks/BenchmarkDemo.cpp b/Demos/Benchmarks/BenchmarkDemo.cpp index c986c3e34..5f8da2eba 100644 --- a/Demos/Benchmarks/BenchmarkDemo.cpp +++ b/Demos/Benchmarks/BenchmarkDemo.cpp @@ -14,6 +14,7 @@ subject to the following restrictions: */ + // Collision Radius #define COLLISION_RADIUS 0.0f @@ -281,7 +282,6 @@ void BenchmarkDemo::displayCallback(void) - void BenchmarkDemo::initPhysics() { @@ -316,7 +316,7 @@ void BenchmarkDemo::initPhysics() ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) #ifdef USE_PARALLEL_SOLVER_BENCHMARK - btSequentialImpulseConstraintSolver* sol = new btParallelConstraintSolver; + btConstraintSolver* sol = new btParallelConstraintSolver; #else btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; #endif //USE_PARALLEL_DISPATCHER_BENCHMARK @@ -327,9 +327,11 @@ void BenchmarkDemo::initPhysics() btDiscreteDynamicsWorld* dynamicsWorld; m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration); + ///the following 3 lines increase the performance dramatically, with a little bit of loss of quality + m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; //don't recalculate friction values each frame + dynamicsWorld->getSolverInfo().m_numIterations = 4; //few solver iterations + m_defaultContactProcessingThreshold = 0.f;//used when creating bodies: body->setContactProcessingThreshold(...); - dynamicsWorld->getSolverInfo().m_numIterations = 4; - m_dynamicsWorld->setGravity(btVector3(0,-10,0)); diff --git a/Demos/Benchmarks/main.cpp b/Demos/Benchmarks/main.cpp index ccf2dc6e0..bdaf5d219 100644 --- a/Demos/Benchmarks/main.cpp +++ b/Demos/Benchmarks/main.cpp @@ -22,7 +22,7 @@ subject to the following restrictions: #include "GlutStuff.h" #include "GLDebugDrawer.h" GLDebugDrawer gDebugDrawer; -#define benchmarkDemo benchmarkDemo1 +#define benchmarkDemo benchmarkDemo2 #endif //USE_GRAPHICAL_BENCHMARK @@ -50,6 +50,7 @@ int main(int argc,char** argv) #ifdef USE_GRAPHICAL_BENCHMARK benchmarkDemo.initPhysics(); benchmarkDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); + benchmarkDemo.setDebugMode(benchmarkDemo.getDebugMode() | btIDebugDraw::DBG_NoDeactivation); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bulletphysics.com",&benchmarkDemo); #else //USE_GRAPHICAL_BENCHMARK int d; diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 6f4dd01c5..dee6e8594 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -82,7 +82,8 @@ m_singleStep(false), m_idle(false), m_enableshadows(false), -m_sundirection(btVector3(1,-2,1)*1000) +m_sundirection(btVector3(1,-2,1)*1000), +m_defaultContactProcessingThreshold(BT_LARGE_FLOAT) { #ifndef BT_NO_PROFILE m_profileIterator = CProfileManager::Get_Iterator(); @@ -935,6 +936,7 @@ btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia); btRigidBody* body = new btRigidBody(cInfo); + body->setContactProcessingThreshold(m_defaultContactProcessingThreshold); #else btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); diff --git a/Demos/OpenGL/DemoApplication.h b/Demos/OpenGL/DemoApplication.h index 3115de88a..d77391efa 100644 --- a/Demos/OpenGL/DemoApplication.h +++ b/Demos/OpenGL/DemoApplication.h @@ -94,6 +94,7 @@ protected: GL_ShapeDrawer* m_shapeDrawer; bool m_enableshadows; btVector3 m_sundirection; + btScalar m_defaultContactProcessingThreshold; public: diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index b209bcb9a..041bbe05a 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -240,7 +240,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx }*/ int count = m_overlappingPairArray.size(); int oldCapacity = m_overlappingPairArray.capacity(); - void* mem = &m_overlappingPairArray.expand(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); //this is where we add an actual pair, so also call the 'ghost' if (m_ghostPairCallback) @@ -467,7 +467,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; - void* mem = &m_overlappingPairArray.expand(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); gOverlappingPairs++; diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 147beb4ae..bf77c4954 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -304,7 +304,7 @@ void btConeTwistConstraint::buildJacobian() -void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) @@ -321,9 +321,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 vel1; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; for (int i=0;i<3;i++) @@ -340,8 +340,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); + bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); + bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); } } @@ -352,8 +352,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver // compute current and predicted transforms btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btVector3 omegaA; bodyA.getAngularVelocity(omegaA); - btVector3 omegaB; bodyB.getAngularVelocity(omegaB); + btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); + btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); btTransform trAPred; trAPred.setIdentity(); btVector3 zerovec(0,0,0); btTransformUtil::integrateTransform( @@ -427,15 +427,15 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } else if (m_damping > SIMD_EPSILON) // no motor: do a little damping { - btVector3 angVelA; bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; bodyB.getAngularVelocity(angVelB); + btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); + btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); btVector3 relVel = angVelB - angVelA; if (relVel.length2() > SIMD_EPSILON) { @@ -447,8 +447,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btScalar impulseMag = impulse.length(); btVector3 impulseAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); } } @@ -456,9 +456,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver { ///solve angular part btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); + bodyA.internalGetAngularVelocity(angVelA); btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); + bodyB.internalGetAngularVelocity(angVelB); // solve swing limit if (m_solveSwingLimit) @@ -487,8 +487,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver impulseMag = impulse.length(); btVector3 noTwistSwingAxis = impulse / impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); } @@ -508,8 +508,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver btVector3 impulse = m_twistAxis * impulseMag; - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); + bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); + bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); } } } diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 226bf030c..f310d474e 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -140,7 +140,7 @@ public: void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 409bf5ee8..d97096d9f 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -56,10 +56,6 @@ void btContactConstraint::buildJacobian() } -void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - -} diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index ae4aea72b..63c1a417b 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -54,8 +54,6 @@ public: ///obsolete methods virtual void buildJacobian(); - ///obsolete methods - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); }; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index d228b5247..a970d7062 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -148,7 +148,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) btScalar btRotationalLimitMotor::solveAngularLimits( btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, - btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) + btRigidBody * body0, btRigidBody * body1 ) { if (needApplyTorques()==false) return 0.0f; @@ -167,9 +167,9 @@ btScalar btRotationalLimitMotor::solveAngularLimits( // current velocity difference btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); + body0->internalGetAngularVelocity(angVelA); btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); + body1->internalGetAngularVelocity(angVelB); btVector3 vel_diff; vel_diff = angVelA-angVelB; @@ -220,8 +220,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( //body0->applyTorqueImpulse(motorImp); //body1->applyTorqueImpulse(-motorImp); - bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); + body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); + body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); return clippedMotorImpulse; @@ -271,8 +271,8 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, - btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos) @@ -285,9 +285,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 vel1; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); + body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); btVector3 vel2; - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); + body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); @@ -345,8 +345,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); - bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); + body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); + body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); @@ -677,66 +677,6 @@ int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_o -void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - if (m_useSolveConstraintObsolete) - { - - - m_timeStep = timeStep; - - //calculateTransforms(); - - int i; - - // linear - - btVector3 pointInA = m_calculatedTransformA.getOrigin(); - btVector3 pointInB = m_calculatedTransformB.getOrigin(); - - btScalar jacDiagABInv; - btVector3 linear_axis; - for (i=0;i<3;i++) - { - if (m_linearLimits.isLimited(i)) - { - jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal(); - - if (m_useLinearReferenceFrameA) - linear_axis = m_calculatedTransformA.getBasis().getColumn(i); - else - linear_axis = m_calculatedTransformB.getBasis().getColumn(i); - - m_linearLimits.solveLinearAxis( - m_timeStep, - jacDiagABInv, - m_rbA,bodyA,pointInA, - m_rbB,bodyB,pointInB, - i,linear_axis, m_AnchorPos); - - } - } - - // angular - btVector3 angular_axis; - btScalar angularJacDiagABInv; - for (i=0;i<3;i++) - { - if (m_angularLimits[i].needApplyTorques()) - { - - // get axis - angular_axis = getAxis(i); - - angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal(); - - m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB); - } - } - } -} - - void btGeneric6DofConstraint::updateRHS(btScalar timeStep) { diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 50f5966a0..dae33ba36 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -123,7 +123,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); }; @@ -214,8 +214,8 @@ public: btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, - btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos); @@ -413,8 +413,6 @@ public: void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - void updateRHS(btScalar timeStep); //! Get the rotation axis in global coordinates diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index efae3312d..2bfa55a29 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -261,164 +261,6 @@ void btHingeConstraint::buildJacobian() } } -void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - if (m_useSolveConstraintObsolete) - { - - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); - - btScalar tau = btScalar(0.3); - - //linear part - if (!m_angularOnly) - { - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - - btVector3 vel1,vel2; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - for (int i=0;i<3;i++) - { - const btVector3& normal = m_jac[i].m_linearJointAxis; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btScalar rel_vel; - rel_vel = normal.dot(vel); - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; - m_appliedImpulse += impulse; - btVector3 impulse_vector = normal * impulse; - btVector3 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); - } - } - - - { - ///solve angular part - - // get axes in world space - btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); - btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); - - btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); - - btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; - btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - { - - - //solve orthogonal angular velocity correction - //btScalar relaxation = btScalar(1.); - btScalar len = velrelOrthog.length(); - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + - getRigidBodyB().computeAngularImpulseDenominator(normal); - // scale for mass and relaxation - //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); - - } - - //solve angular positional correction - btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + - getRigidBodyB().computeAngularImpulseDenominator(normal2); - //angularError *= (btScalar(1.)/denom2) * relaxation; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); - - } - - - - - - // solve limit - if (m_solveLimit) - { - btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; - - btScalar impulseMag = amplitude * m_kHinge; - - // Clamp the accumulated impulse - btScalar temp = m_accLimitImpulse; - m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); - impulseMag = m_accLimitImpulse - temp; - - - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); - - } - } - - //apply motor - if (m_enableAngularMotor) - { - //todo: add limits too - btVector3 angularLimit(0,0,0); - - btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; - btScalar projRelVel = velrel.dot(axisA); - - btScalar desiredMotorVel = m_motorTargetVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; - - btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; - - // accumulated impulse clipping: - btScalar fMaxImpulse = m_maxMotorImpulse; - btScalar newAccImpulse = m_accMotorImpulse + unclippedMotorImpulse; - btScalar clippedMotorImpulse = unclippedMotorImpulse; - if (newAccImpulse > fMaxImpulse) - { - newAccImpulse = fMaxImpulse; - clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; - } - else if (newAccImpulse < -fMaxImpulse) - { - newAccImpulse = -fMaxImpulse; - clippedMotorImpulse = newAccImpulse - m_accMotorImpulse; - } - m_accMotorImpulse += clippedMotorImpulse; - - bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); - bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); - - } - } - } - -} - #endif //__SPU__ diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index bdbad3452..a65f3638e 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -112,7 +112,6 @@ public: void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); void updateRHS(btScalar timeStep); diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 8cf83653d..50ad71ab5 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -163,81 +163,6 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const } -void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - - if (m_useSolveConstraintObsolete) - { - btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; - btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; - - - btVector3 normal(0,0,0); - - - // btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); - // btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); - - for (int i=0;i<3;i++) - { - normal[i] = 1; - btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); - - btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); - btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); - //this jacobian entry could be re-used for all iterations - - btVector3 vel1,vel2; - bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); - bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); - btVector3 vel = vel1 - vel2; - - btScalar rel_vel; - rel_vel = normal.dot(vel); - - /* - //velocity error (first order error) - btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, - m_rbB.getLinearVelocity(),angvelB); - */ - - //positional error (zeroth order error) - btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal - - btScalar deltaImpulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv; - - btScalar impulseClamp = m_setting.m_impulseClamp; - - const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse; - if (sum < -impulseClamp) - { - deltaImpulse = -impulseClamp-m_appliedImpulse; - m_appliedImpulse = -impulseClamp; - } - else if (sum > impulseClamp) - { - deltaImpulse = impulseClamp-m_appliedImpulse; - m_appliedImpulse = impulseClamp; - } - else - { - m_appliedImpulse = sum; - } - - - btVector3 impulse_vector = normal * deltaImpulse; - - btVector3 ftorqueAxis1 = rel_pos1.cross(normal); - btVector3 ftorqueAxis2 = rel_pos2.cross(normal); - bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse); - bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse); - - - normal[i] = 0; - } - } - -} void btPoint2PointConstraint::updateRHS(btScalar timeStep) { diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index a828bbec2..b589ee682 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -87,8 +87,6 @@ public: void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - void updateRHS(btScalar timeStep); void setPivotA(const btVector3& pivotA) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e8b048a61..28c4fa8dc 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -57,15 +57,15 @@ static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) #endif//USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -78,24 +78,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowGeneric(body1,body2,c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; @@ -116,19 +116,19 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -138,24 +138,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowLowerLimit(body1,body2,c); #endif } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -169,22 +169,22 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, + btRigidBody& body1, + btRigidBody& body2, const btSolverConstraint& c) { if (c.m_rhsPenetration) { gNumSplitImpulseRecoveries++; btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_pushVelocity) + c.m_relpos1CrossNormal.dot(body1.m_turnVelocity); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_pushVelocity) + c.m_relpos2CrossNormal.dot(body2.m_turnVelocity); + const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -198,12 +198,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri { c.m_appliedPushImpulse = sum; } - body1.internalApplyPushImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); + body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } } - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -215,8 +215,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_pushVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_turnVelocity.mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_turnVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_pushVelocity.mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -226,13 +226,13 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - body1.m_pushVelocity.mVec128 = _mm_add_ps(body1.m_pushVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - body1.m_turnVelocity.mVec128 = _mm_add_ps(body1.m_turnVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.m_pushVelocity.mVec128 = _mm_sub_ps(body2.m_pushVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - body2.m_turnVelocity.mVec128 = _mm_add_ps(body2.m_turnVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); + body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); + body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); #endif @@ -277,28 +277,29 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } - +#if 0 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); - solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); + solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); if (rb) { - solverBody->m_invMass = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); + solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); } else { - solverBody->m_invMass.setValue(0,0,0); + solverBody->internalGetInvMass().setValue(0,0,0); solverBody->m_originalBody = 0; solverBody->m_angularFactor.setValue(1,1,1); } } +#endif @@ -329,19 +330,19 @@ void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); - memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + //memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); solverConstraint.m_contactNormal = normalAxis; - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); + solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_friction = cp.m_combinedFriction; @@ -418,6 +419,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { +#if 0 int solverBodyIdA = -1; if (body.getCompanionId() >= 0) @@ -439,6 +441,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& } } return solverBodyIdA; +#endif + return 0; } #include @@ -451,17 +455,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA=-1; - int solverBodyIdB=-1; - if (manifold->getNumContacts()) - { - solverBodyIdA = getOrInitSolverBody(*colObj0); - solverBodyIdB = getOrInitSolverBody(*colObj1); - } + btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); + btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); + ///avoid collision response between two static objects - if (!solverBodyIdA && !solverBodyIdB) + if (!solverBodyA && !solverBodyB) return; btVector3 rel_pos1; @@ -490,12 +490,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m int frictionIndex = m_tmpSolverContactConstraintPool.size(); { - btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); + btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); + solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); solverConstraint.m_originalContactPoint = &cp; @@ -564,9 +564,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); + rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; @@ -625,12 +625,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m cp.m_lateralFrictionDir2.normalize();//?? applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } else { @@ -640,21 +640,21 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); cp.m_lateralFrictionInitialized = true; } } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); } if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) @@ -665,9 +665,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); } else { frictionConstraint1.m_appliedImpulse = 0.f; @@ -681,9 +681,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m { frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); } else { frictionConstraint2.m_appliedImpulse = 0.f; @@ -730,10 +730,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol constraint->buildJacobian(); } } - - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody,0); - //btRigidBody* rb0=0,*rb1=0; //if (1) @@ -773,12 +769,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = getOrInitSolverBody(rbA); - int solverBodyIdB = getOrInitSolverBody(rbB); - - btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; - + int j; for ( j=0;jm_deltaLinearVelocity.setValue(0.f,0.f,0.f); - bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); - bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); + rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); @@ -949,16 +940,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( for (j=0;jgetRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); } ///solve all contact constraints using SIMD, if available @@ -966,7 +953,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( for (j=0;jgetRigidBodyA()); - int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); - btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; - btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; - - constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); } - ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); for (j=0;jinternalWritebackVelocity(infoGlobal.m_timeStep); } } else { - for ( i=0;iinternalWritebackVelocity(); } } - m_tmpSolverBodyPool.resize(0); m_tmpSolverContactConstraintPool.resize(0); m_tmpSolverNonContactConstraintPool.resize(0); m_tmpSolverContactFrictionConstraintPool.resize(0); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index a084f9ef2..dbb46aa40 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -29,7 +29,6 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - btAlignedObjectArray m_tmpSolverBodyPool; btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; @@ -37,38 +36,46 @@ protected: btAlignedObjectArray m_orderFrictionConstraintPool; btAlignedObjectArray m_tmpConstraintSizesPool; - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; - void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); +// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); void resolveSplitPenetrationSIMD( - btSolverBody& body1, - btSolverBody& body2, + btRigidBody& body1, + btRigidBody& body2, const btSolverConstraint& contactConstraint); void resolveSplitPenetrationImpulseCacheFriendly( - btSolverBody& body1, - btSolverBody& body2, + btRigidBody& body1, + btRigidBody& body2, const btSolverConstraint& contactConstraint); //internal method int getOrInitSolverBody(btCollisionObject& body); - void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); +protected: + static btRigidBody& getFixedBody() + { + static btRigidBody s_fixed(0, 0,0); + s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); + return s_fixed; + } + public: diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp index e5935b959..b69f46da1 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -107,79 +107,9 @@ btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& fram -void btSliderConstraint::buildJacobian() -{ - if (!m_useSolveConstraintObsolete) - { - return; - } - if(m_useLinearReferenceFrameA) - { - buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB); - } - else - { - buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA); - } -} -void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) -{ -#ifndef __SPU__ - //calculate transforms - m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA; - m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB; - m_realPivotAInW = m_calculatedTransformA.getOrigin(); - m_realPivotBInW = m_calculatedTransformB.getOrigin(); - m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X - m_delta = m_realPivotBInW - m_realPivotAInW; - m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis; - m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition(); - m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition(); - btVector3 normalWorld; - int i; - //linear part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacLin[i]) btJacobianEntry( - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - m_relPosA, - m_relPosB, - normalWorld, - rbA.getInvInertiaDiagLocal(), - rbA.getInvMass(), - rbB.getInvInertiaDiagLocal(), - rbB.getInvMass() - ); - m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal(); - m_depth[i] = m_delta.dot(normalWorld); - } - testLinLimits(); - // angular part - for(i = 0; i < 3; i++) - { - normalWorld = m_calculatedTransformA.getBasis().getColumn(i); - new (&m_jacAng[i]) btJacobianEntry( - normalWorld, - rbA.getCenterOfMassTransform().getBasis().transpose(), - rbB.getCenterOfMassTransform().getBasis().transpose(), - rbA.getInvInertiaDiagLocal(), - rbB.getInvInertiaDiagLocal() - ); - } - testAngLimits(); - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA)); - // clear accumulator for motors - m_accumulatedLinMotorImpulse = btScalar(0.0); - m_accumulatedAngMotorImpulse = btScalar(0.0); -#endif //__SPU__ -} - void btSliderConstraint::getInfo1(btConstraintInfo1* info) { if (m_useSolveConstraintObsolete) @@ -222,210 +152,6 @@ void btSliderConstraint::getInfo2(btConstraintInfo2* info) -void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) -{ - if (m_useSolveConstraintObsolete) - { - m_timeStep = timeStep; - if(m_useLinearReferenceFrameA) - { - solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB); - } - else - { - solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA); - } - } -} - - - -void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB) -{ -#ifndef __SPU__ - int i; - // linear - btVector3 velA; - bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA); - btVector3 velB; - bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB); - btVector3 vel = velA - velB; - for(i = 0; i < 3; i++) - { - const btVector3& normal = m_jacLin[i].m_linearJointAxis; - btScalar rel_vel = normal.dot(vel); - // calculate positional error - btScalar depth = m_depth[i]; - // get parameters - btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin); - btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin); - btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin); - // calcutate and apply impulse - btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; - btVector3 impulse_vector = normal * normalImpulse; - - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - if(m_poweredLinMotor && (!i)) - { // apply linear motor - if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) - { - btScalar desiredMotorVel = m_targetLinMotorVelocity; - btScalar motor_relvel = desiredMotorVel + rel_vel; - normalImpulse = -motor_relvel * m_jacLinDiagABInv[i]; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse); - if(new_acc > m_maxLinMotorForce) - { - new_acc = m_maxLinMotorForce; - } - btScalar del = new_acc - m_accumulatedLinMotorImpulse; - if(normalImpulse < btScalar(0.0)) - { - normalImpulse = -del; - } - else - { - normalImpulse = del; - } - m_accumulatedLinMotorImpulse = new_acc; - // apply clamped impulse - impulse_vector = normal * normalImpulse; - //rbA.applyImpulse( impulse_vector, m_relPosA); - //rbB.applyImpulse(-impulse_vector, m_relPosB); - - { - btVector3 ftorqueAxis1 = m_relPosA.cross(normal); - btVector3 ftorqueAxis2 = m_relPosB.cross(normal); - bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - } - - - - } - } - } - // angular - // get axes in world space - btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); - btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); - - btVector3 angVelA; - bodyA.getAngularVelocity(angVelA); - btVector3 angVelB; - bodyB.getAngularVelocity(angVelB); - - btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); - btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); - - btVector3 angAorthog = angVelA - angVelAroundAxisA; - btVector3 angBorthog = angVelB - angVelAroundAxisB; - btVector3 velrelOrthog = angAorthog-angBorthog; - //solve orthogonal angular velocity correction - btScalar len = velrelOrthog.length(); - btScalar orthorImpulseMag = 0.f; - - if (len > btScalar(0.00001)) - { - btVector3 normal = velrelOrthog.normalized(); - btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); - //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; - } - //solve angular positional correction - btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); - btVector3 angularAxis = angularError; - btScalar angularImpulseMag = 0; - - btScalar len2 = angularError.length(); - if (len2>btScalar(0.00001)) - { - btVector3 normal2 = angularError.normalized(); - btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); - angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; - angularError *= angularImpulseMag; - } - // apply impulse - //rbA.applyTorqueImpulse(-velrelOrthog+angularError); - //rbB.applyTorqueImpulse(velrelOrthog-angularError); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag); - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag); - - - btScalar impulseMag; - //solve angular limits - if(m_solveAngLim) - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessLimAng; - } - else - { - impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep; - impulseMag *= m_kAngle * m_softnessDirAng; - } - btVector3 impulse = axisA * impulseMag; - //rbA.applyTorqueImpulse(impulse); - //rbB.applyTorqueImpulse(-impulse); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag); - - - - //apply angular motor - if(m_poweredAngMotor) - { - if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce) - { - btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB; - btScalar projRelVel = velrel.dot(axisA); - - btScalar desiredMotorVel = m_targetAngMotorVelocity; - btScalar motor_relvel = desiredMotorVel - projRelVel; - - btScalar angImpulse = m_kAngle * motor_relvel; - // clamp accumulated impulse - btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse); - if(new_acc > m_maxAngMotorForce) - { - new_acc = m_maxAngMotorForce; - } - btScalar del = new_acc - m_accumulatedAngMotorImpulse; - if(angImpulse < btScalar(0.0)) - { - angImpulse = -del; - } - else - { - angImpulse = del; - } - m_accumulatedAngMotorImpulse = new_acc; - // apply clamped impulse - btVector3 motorImp = angImpulse * axisA; - //rbA.applyTorqueImpulse(motorImp); - //rbB.applyTorqueImpulse(-motorImp); - - bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse); - bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse); - } - } -#endif //__SPU__ -} - diff --git a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index 5c8a9d1b0..7d2a50227 100755 --- a/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -160,7 +160,7 @@ public: btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); // overrides - virtual void buildJacobian(); + virtual void getInfo1 (btConstraintInfo1* info); void getInfo1NonVirtual(btConstraintInfo1* info); @@ -169,8 +169,6 @@ public: void getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); - // access const btRigidBody& getRigidBodyA() const { return m_rbA; } @@ -246,9 +244,6 @@ public: btScalar getLinDepth() { return m_depth[0]; } bool getSolveAngLimit() { return m_solveAngLim; } btScalar getAngDepth() { return m_angDepth; } - // internal - void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); - void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB); // shared code used by ODE solver void calculateTransforms(const btTransform& transA,const btTransform& transB); void testLinLimits(); diff --git a/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/src/BulletDynamics/ConstraintSolver/btSolverBody.h index b3a8eee19..8de515812 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -105,14 +105,13 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2) #endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED16 (struct) btSolverBody +ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete { BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 m_deltaLinearVelocity; btVector3 m_deltaAngularVelocity; btVector3 m_angularFactor; btVector3 m_invMass; - btScalar m_friction; btRigidBody* m_originalBody; btVector3 m_pushVelocity; btVector3 m_turnVelocity; diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index eb1aae1ff..929cf6d3e 100644 --- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -26,7 +26,7 @@ class btRigidBody; ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint +ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -58,12 +58,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint }; union { - int m_solverBodyIdA; + btRigidBody* m_solverBodyA; btScalar m_unusedPadding2; }; union { - int m_solverBodyIdB; + btRigidBody* m_solverBodyB; btScalar m_unusedPadding3; }; diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 5893d2ea2..b24dc4a40 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -20,7 +20,7 @@ class btRigidBody; #include "LinearMath/btScalar.h" #include "btSolverConstraint.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -struct btSolverBody; + class btSerializer; enum btTypedConstraintType @@ -122,7 +122,7 @@ public: }; ///internal method used by the constraint solver, don't use them directly - virtual void buildJacobian() = 0; + virtual void buildJacobian() {}; ///internal method used by the constraint solver, don't use them directly virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) @@ -151,7 +151,7 @@ public: } ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0; + virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) {}; const btRigidBody& getRigidBodyA() const diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 5bcdad9db..d9eb33b94 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -88,6 +88,15 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& m_rigidbodyFlags = 0; + + m_deltaLinearVelocity.setZero(); + m_deltaAngularVelocity.setZero(); + m_invMass = m_inverseMass*m_linearFactor; + m_pushVelocity.setZero(); + m_turnVelocity.setZero(); + + + } @@ -234,6 +243,7 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); + m_invMass = m_linearFactor*m_inverseMass; } @@ -303,6 +313,28 @@ bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) return true; } +void btRigidBody::internalWritebackVelocity(btScalar timeStep) +{ + (void) timeStep; + if (m_inverseMass) + { + setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); + setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); + + //correct the position/orientation based on push/turn recovery + btTransform newTransform; + btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); + setWorldTransform(newTransform); + //m_originalBody->setCompanionId(-1); + } + m_deltaLinearVelocity.setZero(); + m_deltaAngularVelocity .setZero(); + m_pushVelocity.setZero(); + m_turnVelocity.setZero(); +} + + + void btRigidBody::addConstraintRef(btTypedConstraint* c) { int index = m_constraintRefs.findLinearSearch(c); @@ -355,3 +387,4 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali return btRigidBodyDataName; } + diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 0b2eaf263..236e04e08 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -59,7 +59,6 @@ class btRigidBody : public btCollisionObject btVector3 m_linearVelocity; btVector3 m_angularVelocity; btScalar m_inverseMass; - btVector3 m_angularFactor; btVector3 m_linearFactor; btVector3 m_gravity; @@ -91,8 +90,20 @@ class btRigidBody : public btCollisionObject int m_debugBodyId; + +protected: + + ATTRIBUTE_ALIGNED64(btVector3 m_deltaLinearVelocity); + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; + + public: + ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) ///You can use the motion state to synchronize the world transform between physics and graphics objects. @@ -128,7 +139,6 @@ public: btScalar m_additionalAngularDampingThresholdSqr; btScalar m_additionalAngularDampingFactor; - btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): m_mass(mass), m_motionState(motionState), @@ -244,6 +254,7 @@ public: void setLinearFactor(const btVector3& linearFactor) { m_linearFactor = linearFactor; + m_invMass = m_linearFactor*m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; } const btMatrix3x3& getInvInertiaTensorWorld() const { @@ -318,19 +329,6 @@ public: } } - //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) - { - if (m_inverseMass != btScalar(0.)) - { - m_linearVelocity += linearComponent*m_linearFactor*impulseMagnitude; - if (m_angularFactor) - { - m_angularVelocity += angularComponent*m_angularFactor*impulseMagnitude; - } - } - } - void clearForces() { m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); @@ -521,6 +519,88 @@ public: return m_rigidbodyFlags; } + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + btVector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + btVector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const btVector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const btVector3& internalGetInvMass() const + { + return m_invMass; + } + + btVector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + btVector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = getLinearVelocity()+m_deltaLinearVelocity + (getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + } + + SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const + { + angVel = getAngularVelocity()+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + { + if (m_inverseMass) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + if (m_inverseMass) + { + m_pushVelocity += linearComponent*impulseMagnitude; + m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + void internalWritebackVelocity() + { + if (m_inverseMass) + { + setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity); + setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity); + m_deltaLinearVelocity.setZero(); + m_deltaAngularVelocity .setZero(); + //m_originalBody->setCompanionId(-1); + } + } + + + void internalWritebackVelocity(btScalar timeStep); + + + /////////////////////////////////////////////// + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) diff --git a/src/LinearMath/btAlignedObjectArray.h b/src/LinearMath/btAlignedObjectArray.h index 160618186..257317b21 100644 --- a/src/LinearMath/btAlignedObjectArray.h +++ b/src/LinearMath/btAlignedObjectArray.h @@ -205,6 +205,18 @@ class btAlignedObjectArray m_size = newsize; } + SIMD_FORCE_INLINE T& expandNonInitializing( ) + { + int sz = size(); + if( sz == capacity() ) + { + reserve( allocSize(size()) ); + } + m_size++; + + return m_data[sz]; + } + SIMD_FORCE_INLINE T& expand( const T& fillValue=T()) { diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 1bc1fe2c5..8b3a7457d 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -42,6 +42,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #else //#define BT_HAS_ALIGNED_ALLOCATOR @@ -52,6 +53,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE __forceinline #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a + #define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #ifdef _XBOX #define BT_USE_VMX128 @@ -87,6 +89,7 @@ inline int btGetVersion() #if defined (__CELLOS_LV2__) #define SIMD_FORCE_INLINE inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include @@ -108,6 +111,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE __inline #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include @@ -135,6 +139,7 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE inline ///@todo: check out alignment methods for other platforms/compilers #define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + #define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) #define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #ifndef assert #include @@ -156,8 +161,10 @@ inline int btGetVersion() #define SIMD_FORCE_INLINE inline ///@todo: check out alignment methods for other platforms/compilers ///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) + ///#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64))) ///#define ATTRIBUTE_ALIGNED128(a) a __attribute__ ((aligned (128))) #define ATTRIBUTE_ALIGNED16(a) a + #define ATTRIBUTE_ALIGNED64(a) a #define ATTRIBUTE_ALIGNED128(a) a #ifndef assert #include