Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Get rid of btSolverBody and use btRigidBody directly. btSolverBody didn't improve performance after all, due to random-access

Browse Source 
Tweak the BenchmarkDemo a bit: 

1) disable deactivation in graphical mode
2) add some settings that increase performance in the BenchmarkDemo2 (1000 stack) from 35ms to 15ms on this quad core (at the cost of a bit of quality)
This commit is contained in:
erwin.coumans
2010-02-11 20:30:56 +00:00
parent bb8d1b11df
commit d4c3633405
26 changed files with 348 additions and 805 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Demos/Benchmarks/BenchmarkDemo.cpp
View File

@@ -14,6 +14,7 @@ subject to the following restrictions:
*/ */
// Collision Radius // Collision Radius
#define COLLISION_RADIUS 0.0f #define COLLISION_RADIUS 0.0f
@@ -281,7 +282,6 @@ void BenchmarkDemo::displayCallback(void)
void BenchmarkDemo::initPhysics() 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) ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
#ifdef USE_PARALLEL_SOLVER_BENCHMARK #ifdef USE_PARALLEL_SOLVER_BENCHMARK
btSequentialImpulseConstraintSolver* sol = new btParallelConstraintSolver; btConstraintSolver* sol = new btParallelConstraintSolver;
#else #else
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
#endif //USE_PARALLEL_DISPATCHER_BENCHMARK #endif //USE_PARALLEL_DISPATCHER_BENCHMARK
@@ -327,9 +327,11 @@ void BenchmarkDemo::initPhysics()
btDiscreteDynamicsWorld* dynamicsWorld; btDiscreteDynamicsWorld* dynamicsWorld;
m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration); 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)); m_dynamicsWorld->setGravity(btVector3(0,-10,0));

3
Demos/Benchmarks/main.cpp
View File

@@ -22,7 +22,7 @@ subject to the following restrictions:
#include "GlutStuff.h" #include "GlutStuff.h"
#include "GLDebugDrawer.h" #include "GLDebugDrawer.h"
GLDebugDrawer gDebugDrawer; GLDebugDrawer gDebugDrawer;
#define benchmarkDemo benchmarkDemo1 #define benchmarkDemo benchmarkDemo2
#endif //USE_GRAPHICAL_BENCHMARK #endif //USE_GRAPHICAL_BENCHMARK
@@ -50,6 +50,7 @@ int main(int argc,char** argv)
#ifdef USE_GRAPHICAL_BENCHMARK #ifdef USE_GRAPHICAL_BENCHMARK
benchmarkDemo.initPhysics(); benchmarkDemo.initPhysics();
benchmarkDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); benchmarkDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
benchmarkDemo.setDebugMode(benchmarkDemo.getDebugMode() | btIDebugDraw::DBG_NoDeactivation);
return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bulletphysics.com",&benchmarkDemo); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bulletphysics.com",&benchmarkDemo);
#else //USE_GRAPHICAL_BENCHMARK #else //USE_GRAPHICAL_BENCHMARK
int d; int d;

4
Demos/OpenGL/DemoApplication.cpp
View File

@@ -82,7 +82,8 @@ m_singleStep(false),
m_idle(false), m_idle(false),
m_enableshadows(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 #ifndef BT_NO_PROFILE
m_profileIterator = CProfileManager::Get_Iterator(); m_profileIterator = CProfileManager::Get_Iterator();
@@ -935,6 +936,7 @@ btRigidBody* DemoApplication::localCreateRigidBody(float mass, const btTransform
btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia); btRigidBody::btRigidBodyConstructionInfo cInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(cInfo); btRigidBody* body = new btRigidBody(cInfo);
body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
#else #else
btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);

1
Demos/OpenGL/DemoApplication.h
View File

@@ -94,6 +94,7 @@ protected:
GL_ShapeDrawer* m_shapeDrawer; GL_ShapeDrawer* m_shapeDrawer;
bool m_enableshadows; bool m_enableshadows;
btVector3 m_sundirection; btVector3 m_sundirection;
btScalar m_defaultContactProcessingThreshold;
public: public:

4
src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp
View File

@@ -240,7 +240,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx
}*/ }*/
int count = m_overlappingPairArray.size(); int count = m_overlappingPairArray.size();
int oldCapacity = m_overlappingPairArray.capacity(); 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' //this is where we add an actual pair, so also call the 'ghost'
if (m_ghostPairCallback) if (m_ghostPairCallback)
@@ -467,7 +467,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP
if (!needsBroadphaseCollision(proxy0,proxy1)) if (!needsBroadphaseCollision(proxy0,proxy1))
return 0; return 0;
void* mem = &m_overlappingPairArray.expand(); void* mem = &m_overlappingPairArray.expandNonInitializing();
btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1);
gOverlappingPairs++; gOverlappingPairs++;

38
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
View File

@@ -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__ #ifndef __SPU__
if (m_useSolveConstraintObsolete) if (m_useSolveConstraintObsolete)
@@ -321,9 +321,9 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1; btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2; btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
@@ -340,8 +340,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btVector3 ftorqueAxis1 = rel_pos1.cross(normal); btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal); btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-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 // compute current and predicted transforms
btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trACur = m_rbA.getCenterOfMassTransform();
btTransform trBCur = m_rbB.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform();
btVector3 omegaA; bodyA.getAngularVelocity(omegaA); btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
btVector3 omegaB; bodyB.getAngularVelocity(omegaB); btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
btTransform trAPred; trAPred.setIdentity(); btTransform trAPred; trAPred.setIdentity();
btVector3 zerovec(0,0,0); btVector3 zerovec(0,0,0);
btTransformUtil::integrateTransform( btTransformUtil::integrateTransform(
@@ -427,15 +427,15 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btScalar impulseMag = impulse.length(); btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag; btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.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 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
{ {
btVector3 angVelA; bodyA.getAngularVelocity(angVelA); btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB; bodyB.getAngularVelocity(angVelB); btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
btVector3 relVel = angVelB - angVelA; btVector3 relVel = angVelB - angVelA;
if (relVel.length2() > SIMD_EPSILON) if (relVel.length2() > SIMD_EPSILON)
{ {
@@ -447,8 +447,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
btScalar impulseMag = impulse.length(); btScalar impulseMag = impulse.length();
btVector3 impulseAxis = impulse / impulseMag; btVector3 impulseAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.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 ///solve angular part
btVector3 angVelA; btVector3 angVelA;
bodyA.getAngularVelocity(angVelA); bodyA.internalGetAngularVelocity(angVelA);
btVector3 angVelB; btVector3 angVelB;
bodyB.getAngularVelocity(angVelB); bodyB.internalGetAngularVelocity(angVelB);
// solve swing limit // solve swing limit
if (m_solveSwingLimit) if (m_solveSwingLimit)
@@ -487,8 +487,8 @@ void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
impulseMag = impulse.length(); impulseMag = impulse.length();
btVector3 noTwistSwingAxis = impulse / impulseMag; btVector3 noTwistSwingAxis = impulse / impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.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; btVector3 impulse = m_twistAxis * impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
} }
} }
} }

2
src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
View File

@@ -140,7 +140,7 @@ public:
void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); 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); void updateRHS(btScalar timeStep);

4
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -56,10 +56,6 @@ void btContactConstraint::buildJacobian()
} }
void btContactConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
}

2
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -54,8 +54,6 @@ public:
///obsolete methods ///obsolete methods
virtual void buildJacobian(); virtual void buildJacobian();
///obsolete methods
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
}; };

82
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
View File

@@ -148,7 +148,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
btScalar btRotationalLimitMotor::solveAngularLimits( btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv, btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB) btRigidBody * body0, btRigidBody * body1 )
{ {
if (needApplyTorques()==false) return 0.0f; if (needApplyTorques()==false) return 0.0f;
@@ -167,9 +167,9 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
// current velocity difference // current velocity difference
btVector3 angVelA; btVector3 angVelA;
bodyA.getAngularVelocity(angVelA); body0->internalGetAngularVelocity(angVelA);
btVector3 angVelB; btVector3 angVelB;
bodyB.getAngularVelocity(angVelB); body1->internalGetAngularVelocity(angVelB);
btVector3 vel_diff; btVector3 vel_diff;
vel_diff = angVelA-angVelB; vel_diff = angVelA-angVelB;
@@ -220,8 +220,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
//body0->applyTorqueImpulse(motorImp); //body0->applyTorqueImpulse(motorImp);
//body1->applyTorqueImpulse(-motorImp); //body1->applyTorqueImpulse(-motorImp);
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
return clippedMotorImpulse; return clippedMotorImpulse;
@@ -271,8 +271,8 @@ int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_valu
btScalar btTranslationalLimitMotor::solveLinearAxis( btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep, btScalar timeStep,
btScalar jacDiagABInv, btScalar jacDiagABInv,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, btRigidBody& body2,const btVector3 &pointInB,
int limit_index, int limit_index,
const btVector3 & axis_normal_on_a, const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos) const btVector3 & anchorPos)
@@ -285,9 +285,9 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
btVector3 vel1; btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2; btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
btScalar rel_vel = axis_normal_on_a.dot(vel); 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 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
btVector3 ftorqueAxis2 = rel_pos2.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); body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-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) void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
{ {

8
src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
View File

@@ -123,7 +123,7 @@ public:
int testLimitValue(btScalar test_value); int testLimitValue(btScalar test_value);
//! apply the correction impulses for two bodies //! 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 solveLinearAxis(
btScalar timeStep, btScalar timeStep,
btScalar jacDiagABInv, btScalar jacDiagABInv,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, btRigidBody& body2,const btVector3 &pointInB,
int limit_index, int limit_index,
const btVector3 & axis_normal_on_a, const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos); 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); 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); void updateRHS(btScalar timeStep);
//! Get the rotation axis in global coordinates //! Get the rotation axis in global coordinates

158
src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
View File

@@ -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__ #endif //__SPU__

1
src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
View File

@@ -112,7 +112,6 @@ public:
void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB); 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); 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); void updateRHS(btScalar timeStep);

75
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
View File

@@ -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) void btPoint2PointConstraint::updateRHS(btScalar timeStep)
{ {

2
src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
View File

@@ -87,8 +87,6 @@ public:
void getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans); 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 updateRHS(btScalar timeStep);
void setPivotA(const btVector3& pivotA) void setPivotA(const btVector3& pivotA)

272
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -57,15 +57,15 @@ static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
#endif//USE_SIMD #endif//USE_SIMD
// Project Gauss Seidel or the equivalent Sequential Impulse // 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 #ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __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 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 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.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.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(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_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); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@@ -78,24 +78,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); 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) ); 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 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().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)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().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.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().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)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else #else
resolveSingleConstraintRowGeneric(body1,body2,c); resolveSingleConstraintRowGeneric(body1,body2,c);
#endif #endif
} }
// Project Gauss Seidel or the equivalent Sequential Impulse // 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; 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 deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
@@ -116,19 +116,19 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,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 #ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __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 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 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.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.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(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_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); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@@ -138,24 +138,24 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 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) ); 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 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().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)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().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.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().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)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else #else
resolveSingleConstraintRowLowerLimit(body1,body2,c); resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif #endif
} }
// Project Gauss Seidel or the equivalent Sequential Impulse // 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; 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 deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
@@ -169,22 +169,22 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse); body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
} }
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1, btRigidBody& body1,
btSolverBody& body2, btRigidBody& body2,
const btSolverConstraint& c) const btSolverConstraint& c)
{ {
if (c.m_rhsPenetration) if (c.m_rhsPenetration)
{ {
gNumSplitImpulseRecoveries++; gNumSplitImpulseRecoveries++;
btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; 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 deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_pushVelocity) + c.m_relpos2CrossNormal.dot(body2.m_turnVelocity); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
@@ -198,12 +198,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
{ {
c.m_appliedPushImpulse = sum; c.m_appliedPushImpulse = sum;
} }
body1.internalApplyPushImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyPushImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,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 #ifdef USE_SIMD
if (!c.m_rhsPenetration) if (!c.m_rhsPenetration)
@@ -215,8 +215,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __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 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 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.m_turnVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_pushVelocity.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(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_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); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
@@ -226,13 +226,13 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 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) ); 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 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.m_pushVelocity.mVec128 = _mm_add_ps(body1.m_pushVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().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)); body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().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.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().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)); body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else #else
resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
#endif #endif
@@ -277,28 +277,29 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
} }
#if 0
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
{ {
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
solverBody->m_pushVelocity.setValue(0.f,0.f,0.f); solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
solverBody->m_turnVelocity.setValue(0.f,0.f,0.f); solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
if (rb) 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_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_angularFactor = rb->getAngularFactor();
} else } else
{ {
solverBody->m_invMass.setValue(0,0,0); solverBody->internalGetInvMass().setValue(0,0,0);
solverBody->m_originalBody = 0; solverBody->m_originalBody = 0;
solverBody->m_angularFactor.setValue(1,1,1); 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* body0=btRigidBody::upcast(colObj0);
btRigidBody* body1=btRigidBody::upcast(colObj1); btRigidBody* body1=btRigidBody::upcast(colObj1);
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand(); btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
memset(&solverConstraint,0xff,sizeof(btSolverConstraint)); //memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_frictionIndex = frictionIndex;
solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_friction = cp.m_combinedFriction;
@@ -418,6 +419,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
{ {
#if 0
int solverBodyIdA = -1; int solverBodyIdA = -1;
if (body.getCompanionId() >= 0) if (body.getCompanionId() >= 0)
@@ -439,6 +441,8 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
} }
} }
return solverBodyIdA; return solverBodyIdA;
#endif
return 0;
} }
#include <stdio.h> #include <stdio.h>
@@ -451,17 +455,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
colObj0 = (btCollisionObject*)manifold->getBody0(); colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1(); colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA=-1;
int solverBodyIdB=-1;
if (manifold->getNumContacts()) btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
{ btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
solverBodyIdA = getOrInitSolverBody(*colObj0);
solverBodyIdB = getOrInitSolverBody(*colObj1);
}
///avoid collision response between two static objects ///avoid collision response between two static objects
if (!solverBodyIdA && !solverBodyIdB) if (!solverBodyA && !solverBodyB)
return; return;
btVector3 rel_pos1; btVector3 rel_pos1;
@@ -490,12 +490,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
int frictionIndex = m_tmpSolverContactConstraintPool.size(); int frictionIndex = m_tmpSolverContactConstraintPool.size();
{ {
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1); btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
solverConstraint.m_originalContactPoint = &cp; solverConstraint.m_originalContactPoint = &cp;
@@ -564,9 +564,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
{ {
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0) 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) 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 } else
{ {
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
@@ -625,12 +625,12 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir2.normalize();//?? cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,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(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,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; cp.m_lateralFrictionInitialized = true;
} else } else
{ {
@@ -640,21 +640,21 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
{ {
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,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(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,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; cp.m_lateralFrictionInitialized = true;
} }
} else } 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)) 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) 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; frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0) 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) 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 } else
{ {
frictionConstraint1.m_appliedImpulse = 0.f; frictionConstraint1.m_appliedImpulse = 0.f;
@@ -681,9 +681,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
{ {
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0) 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) 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 } else
{ {
frictionConstraint2.m_appliedImpulse = 0.f; frictionConstraint2.m_appliedImpulse = 0.f;
@@ -730,10 +730,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
constraint->buildJacobian(); constraint->buildJacobian();
} }
} }
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btRigidBody* rb0=0,*rb1=0; //btRigidBody* rb0=0,*rb1=0;
//if (1) //if (1)
@@ -773,12 +769,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbA = constraint->getRigidBodyA();
btRigidBody& rbB = constraint->getRigidBodyB(); btRigidBody& rbB = constraint->getRigidBodyB();
int solverBodyIdA = getOrInitSolverBody(rbA);
int solverBodyIdB = getOrInitSolverBody(rbB);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
int j; int j;
for ( j=0;j<info1.m_numConstraintRows;j++) for ( j=0;j<info1.m_numConstraintRows;j++)
{ {
@@ -787,14 +778,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
currentConstraintRow[j].m_upperLimit = FLT_MAX; currentConstraintRow[j].m_upperLimit = FLT_MAX;
currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; currentConstraintRow[j].m_solverBodyA = &rbA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; currentConstraintRow[j].m_solverBodyB = &rbB;
} }
bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaAngularVelocity.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;j<m_tmpSolverNonContactConstraintPool.size();j++) for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{ {
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
} }
for (j=0;j<numConstraints;j++) for (j=0;j<numConstraints;j++)
{ {
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
} }
///solve all contact constraints using SIMD, if available ///solve all contact constraints using SIMD, if available
@@ -966,7 +953,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
} }
///solve all friction constraints, using SIMD, if available ///solve all friction constraints, using SIMD, if available
@@ -981,7 +968,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold);
} }
} }
} else } else
@@ -991,25 +978,19 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{ {
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
} }
for (j=0;j<numConstraints;j++) for (j=0;j<numConstraints;j++)
{ {
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
} }
///solve all contact constraints ///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
} }
///solve all friction constraints ///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
@@ -1023,50 +1004,48 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
} }
} }
} }
} }
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
{
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{ {
if (infoGlobal.m_solverMode & SOLVER_SIMD) for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{ {
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{ {
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{ {
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
}
}
else
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
} }
} }
} }
} }
else
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
}
}
}
}
}
} }
return 0.f; return 0.f;
@@ -1123,20 +1102,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
{ {
for ( i=0;i<m_tmpSolverBodyPool.size();i++) for ( i=0;i<numBodies;i++)
{ {
m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep); btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body)
body->internalWritebackVelocity(infoGlobal.m_timeStep);
} }
} else } else
{ {
for ( i=0;i<m_tmpSolverBodyPool.size();i++) for ( i=0;i<numBodies;i++)
{ {
m_tmpSolverBodyPool[i].writebackVelocity(); btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body)
body->internalWritebackVelocity();
} }
} }
m_tmpSolverBodyPool.resize(0);
m_tmpSolverContactConstraintPool.resize(0); m_tmpSolverContactConstraintPool.resize(0);
m_tmpSolverNonContactConstraintPool.resize(0); m_tmpSolverNonContactConstraintPool.resize(0);
m_tmpSolverContactFrictionConstraintPool.resize(0); m_tmpSolverContactFrictionConstraintPool.resize(0);

29
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -29,7 +29,6 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
{ {
protected: protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool;
@@ -37,38 +36,46 @@ protected:
btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool;
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> 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 ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2; unsigned long m_btSeed2;
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); // void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD( void resolveSplitPenetrationSIMD(
btSolverBody& body1, btRigidBody& body1,
btSolverBody& body2, btRigidBody& body2,
const btSolverConstraint& contactConstraint); const btSolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly( void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1, btRigidBody& body1,
btSolverBody& body2, btRigidBody& body2,
const btSolverConstraint& contactConstraint); const btSolverConstraint& contactConstraint);
//internal method //internal method
int getOrInitSolverBody(btCollisionObject& body); 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: public:

274
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -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) void btSliderConstraint::getInfo1(btConstraintInfo1* info)
{ {
if (m_useSolveConstraintObsolete) 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__
}

7
src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
View File

@@ -160,7 +160,7 @@ public:
btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA);
// overrides // overrides
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info); virtual void getInfo1 (btConstraintInfo1* info);
void getInfo1NonVirtual(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); 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 // access
const btRigidBody& getRigidBodyA() const { return m_rbA; } const btRigidBody& getRigidBodyA() const { return m_rbA; }
@@ -246,9 +244,6 @@ public:
btScalar getLinDepth() { return m_depth[0]; } btScalar getLinDepth() { return m_depth[0]; }
bool getSolveAngLimit() { return m_solveAngLim; } bool getSolveAngLimit() { return m_solveAngLim; }
btScalar getAngDepth() { return m_angDepth; } 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 // shared code used by ODE solver
void calculateTransforms(const btTransform& transA,const btTransform& transB); void calculateTransforms(const btTransform& transA,const btTransform& transB);
void testLinLimits(); void testLinLimits();

3
src/BulletDynamics/ConstraintSolver/btSolverBody.h
View File

@@ -105,14 +105,13 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2)
#endif #endif
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. ///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(); BT_DECLARE_ALIGNED_ALLOCATOR();
btVector3 m_deltaLinearVelocity; btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity; btVector3 m_deltaAngularVelocity;
btVector3 m_angularFactor; btVector3 m_angularFactor;
btVector3 m_invMass; btVector3 m_invMass;
btScalar m_friction;
btRigidBody* m_originalBody; btRigidBody* m_originalBody;
btVector3 m_pushVelocity; btVector3 m_pushVelocity;
btVector3 m_turnVelocity; btVector3 m_turnVelocity;

6
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -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. ///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(); BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -58,12 +58,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
}; };
union union
{ {
int m_solverBodyIdA; btRigidBody* m_solverBodyA;
btScalar m_unusedPadding2; btScalar m_unusedPadding2;
}; };
union union
{ {
int m_solverBodyIdB; btRigidBody* m_solverBodyB;
btScalar m_unusedPadding3; btScalar m_unusedPadding3;
}; };

6
src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
View File

@@ -20,7 +20,7 @@ class btRigidBody;
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "btSolverConstraint.h" #include "btSolverConstraint.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
struct btSolverBody;
class btSerializer; class btSerializer;
enum btTypedConstraintType enum btTypedConstraintType
@@ -122,7 +122,7 @@ public:
}; };
///internal method used by the constraint solver, don't use them directly ///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 ///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep) 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 ///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 const btRigidBody& getRigidBodyA() const

33
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -88,6 +88,15 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
m_rigidbodyFlags = 0; 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.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)); 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; 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) void btRigidBody::addConstraintRef(btTypedConstraint* c)
{ {
int index = m_constraintRefs.findLinearSearch(c); int index = m_constraintRefs.findLinearSearch(c);
@@ -355,3 +387,4 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali
return btRigidBodyDataName; return btRigidBodyDataName;
} }

110
src/BulletDynamics/Dynamics/btRigidBody.h
View File

@@ -59,7 +59,6 @@ class btRigidBody : public btCollisionObject
btVector3 m_linearVelocity; btVector3 m_linearVelocity;
btVector3 m_angularVelocity; btVector3 m_angularVelocity;
btScalar m_inverseMass; btScalar m_inverseMass;
btVector3 m_angularFactor;
btVector3 m_linearFactor; btVector3 m_linearFactor;
btVector3 m_gravity; btVector3 m_gravity;
@@ -91,8 +90,20 @@ class btRigidBody : public btCollisionObject
int m_debugBodyId; 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: public:
///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///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) ///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. ///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_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor; btScalar m_additionalAngularDampingFactor;
btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
m_mass(mass), m_mass(mass),
m_motionState(motionState), m_motionState(motionState),
@@ -244,6 +254,7 @@ public:
void setLinearFactor(const btVector3& linearFactor) void setLinearFactor(const btVector3& linearFactor)
{ {
m_linearFactor = linearFactor; m_linearFactor = linearFactor;
m_invMass = m_linearFactor*m_inverseMass;
} }
btScalar getInvMass() const { return m_inverseMass; } btScalar getInvMass() const { return m_inverseMass; }
const btMatrix3x3& getInvInertiaTensorWorld() const { 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() void clearForces()
{ {
m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
@@ -521,6 +519,88 @@ public:
return m_rigidbodyFlags; 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; virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure) ///fills the dataBuffer and returns the struct name (and 0 on failure)

12
src/LinearMath/btAlignedObjectArray.h
View File

@@ -205,6 +205,18 @@ class btAlignedObjectArray
m_size = newsize; 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()) SIMD_FORCE_INLINE T& expand( const T& fillValue=T())
{ {

7
src/LinearMath/btScalar.h
View File

@@ -42,6 +42,7 @@ inline int btGetVersion()
#define SIMD_FORCE_INLINE inline #define SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a #define ATTRIBUTE_ALIGNED128(a) a
#else #else
//#define BT_HAS_ALIGNED_ALLOCATOR //#define BT_HAS_ALIGNED_ALLOCATOR
@@ -52,6 +53,7 @@ inline int btGetVersion()
#define SIMD_FORCE_INLINE __forceinline #define SIMD_FORCE_INLINE __forceinline
#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a #define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a #define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
#ifdef _XBOX #ifdef _XBOX
#define BT_USE_VMX128 #define BT_USE_VMX128
@@ -87,6 +89,7 @@ inline int btGetVersion()
#if defined (__CELLOS_LV2__) #if defined (__CELLOS_LV2__)
#define SIMD_FORCE_INLINE inline #define SIMD_FORCE_INLINE inline
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #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_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert #ifndef assert
#include <assert.h> #include <assert.h>
@@ -108,6 +111,7 @@ inline int btGetVersion()
#define SIMD_FORCE_INLINE __inline #define SIMD_FORCE_INLINE __inline
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #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_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert #ifndef assert
#include <assert.h> #include <assert.h>
@@ -135,6 +139,7 @@ inline int btGetVersion()
#define SIMD_FORCE_INLINE inline #define SIMD_FORCE_INLINE inline
///@todo: check out alignment methods for other platforms/compilers ///@todo: check out alignment methods for other platforms/compilers
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) #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_ALIGNED128(a) a __attribute__ ((aligned (128)))
#ifndef assert #ifndef assert
#include <assert.h> #include <assert.h>
@@ -156,8 +161,10 @@ inline int btGetVersion()
#define SIMD_FORCE_INLINE inline #define SIMD_FORCE_INLINE inline
///@todo: check out alignment methods for other platforms/compilers ///@todo: check out alignment methods for other platforms/compilers
///#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16))) ///#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_ALIGNED128(a) a __attribute__ ((aligned (128)))
#define ATTRIBUTE_ALIGNED16(a) a #define ATTRIBUTE_ALIGNED16(a) a
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a #define ATTRIBUTE_ALIGNED128(a) a
#ifndef assert #ifndef assert
#include <assert.h> #include <assert.h>