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

Big work-in-progress refactoring of the constraint solver:

Browse Source 
1) Add fast branchless SIMD support for constraint solver (Windows only until we get other contributions).
See resolveSingleConstraintRowGenericSIMD in Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
resolveSingleConstraintRowGenericSIMD can be used for all constraints, including contact, point 2 point, hinge, generic etc.

2) During this refactoring, all constraints support the obsolete 'solveConstraintObsolete' while we add 'getInfo1' and 'getInfo2' support.
This interface is almost identical interface to Open Dynamics Engine, to make it easier to port Dantzig LCP solver.

3) Some minor refactoring to reduce huge constructor overhead in math classes.
This commit is contained in:
erwin.coumans
2008-12-01 06:41:25 +00:00
parent 7e93be739b
commit e80feca36b
25 changed files with 2099 additions and 1315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Demos/MultiThreadedDemo/MultiThreadedDemo.cpp
View File

@@ -102,7 +102,7 @@ void MultiThreadedDemo::createStack( btCollisionShape* boxShape, float halfCubeS
int rowSize = size - i; int rowSize = size - i;
for(int j=0; j< rowSize; j++) for(int j=0; j< rowSize; j++)
{ {
btVector4 pos; btVector3 pos;
pos.setValue( pos.setValue(
-rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize, -rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize,
halfCubeSize + i * halfCubeSize * 2.0f, halfCubeSize + i * halfCubeSize * 2.0f,
@@ -148,9 +148,10 @@ void MultiThreadedDemo::clientMoveAndDisplay()
if (m_dynamicsWorld) if (m_dynamicsWorld)
{ {
//#define FIXED_STEP 1 #define FIXED_STEP 1
#ifdef FIXED_STEP #ifdef FIXED_STEP
m_dynamicsWorld->stepSimulation(1.0f/60.f,0); m_dynamicsWorld->stepSimulation(1.0f/60.f,0);
//CProfileManager::dumpAll();
#else #else
//during idle mode, just run 1 simulation step maximum //during idle mode, just run 1 simulation step maximum
@@ -354,6 +355,7 @@ m_threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32Threa
m_dynamicsWorld = world; m_dynamicsWorld = world;
world->getSolverInfo().m_numIterations = 4; world->getSolverInfo().m_numIterations = 4;
world->getSolverInfo().m_solverMode = SOLVER_SIMD+SOLVER_USE_WARMSTARTING;
m_dynamicsWorld->getDispatchInfo().m_enableSPU = true; m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
m_dynamicsWorld->setGravity(btVector3(0,-10,0)); m_dynamicsWorld->setGravity(btVector3(0,-10,0));

1
Demos/OpenGL/DemoApplication.h
View File

@@ -102,6 +102,7 @@ public:
} }
void setOrthographicProjection(); void setOrthographicProjection();
void resetPerspectiveProjection(); void resetPerspectiveProjection();

4
Extras/quickstep/btOdeQuickstepConstraintSolver.cpp
View File

@@ -206,8 +206,8 @@ int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedOb
body->m_originalBody = orgBody; body->m_originalBody = orgBody;
body->m_facc.setValue(0,0,0,0); body->m_facc.setValue(0,0,0);
body->m_tacc.setValue(0,0,0,0); body->m_tacc.setValue(0,0,0);
body->m_linearVelocity = orgBody->getLinearVelocity(); body->m_linearVelocity = orgBody->getLinearVelocity();
body->m_angularVelocity = orgBody->getAngularVelocity(); body->m_angularVelocity = orgBody->getAngularVelocity();

36
Extras/quickstep/btOdeTypedJoint.cpp
View File

@@ -96,16 +96,6 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{ {
body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
} }
// btScalar body0_mat[12];
// body0_mat[0] = body0_trans.getBasis()[0][0];
// body0_mat[1] = body0_trans.getBasis()[0][1];
// body0_mat[2] = body0_trans.getBasis()[0][2];
// body0_mat[4] = body0_trans.getBasis()[1][0];
// body0_mat[5] = body0_trans.getBasis()[1][1];
// body0_mat[6] = body0_trans.getBasis()[1][2];
// body0_mat[8] = body0_trans.getBasis()[2][0];
// body0_mat[9] = body0_trans.getBasis()[2][1];
// body0_mat[10] = body0_trans.getBasis()[2][2];
btTransform body1_trans; btTransform body1_trans;
@@ -113,23 +103,8 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{ {
body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
} }
// btScalar body1_mat[12];
// body1_mat[0] = body1_trans.getBasis()[0][0];
// body1_mat[1] = body1_trans.getBasis()[0][1];
// body1_mat[2] = body1_trans.getBasis()[0][2];
// body1_mat[4] = body1_trans.getBasis()[1][0];
// body1_mat[5] = body1_trans.getBasis()[1][1];
// body1_mat[6] = body1_trans.getBasis()[1][2];
// body1_mat[8] = body1_trans.getBasis()[2][0];
// body1_mat[9] = body1_trans.getBasis()[2][1];
// body1_mat[10] = body1_trans.getBasis()[2][2];
// anchor points in global coordinates with respect to body PORs. // anchor points in global coordinates with respect to body PORs.
int s = info->rowskip; int s = info->rowskip;
// set jacobian // set jacobian
@@ -160,17 +135,18 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] - info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
a1[j] - body0_trans.getOrigin()[j]); printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
} }
} }
else else
{ {
for (int j=0; j<3; j++) for (int j=0; j<3; j++)
{ {
info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - body0_trans.getOrigin()[j]);
body0_trans.getOrigin()[j]); printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
}
} }
} }

18
src/BulletCollision/CollisionShapes/btBoxShape.h
View File

@@ -161,28 +161,22 @@ public:
switch (i) switch (i)
{ {
case 0: case 0:
plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.)); plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
plane[3] = -halfExtents.x();
break; break;
case 1: case 1:
plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.)); plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
plane[3] = -halfExtents.x();
break; break;
case 2: case 2:
plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.)); plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
plane[3] = -halfExtents.y();
break; break;
case 3: case 3:
plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.)); plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
plane[3] = -halfExtents.y();
break; break;
case 4: case 4:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.)); plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
plane[3] = -halfExtents.z();
break; break;
case 5: case 5:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.)); plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
plane[3] = -halfExtents.z();
break; break;
default: default:
assert(0); assert(0);

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

@@ -23,7 +23,8 @@ Written by: Marcus Hennix
#include <new> #include <new>
btConeTwistConstraint::btConeTwistConstraint() btConeTwistConstraint::btConeTwistConstraint()
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(true)
{ {
} }
@@ -31,7 +32,8 @@ btConeTwistConstraint::btConeTwistConstraint()
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame,const btTransform& rbBFrame) const btTransform& rbAFrame,const btTransform& rbBFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false) m_angularOnly(false),
m_useSolveConstraintObsolete(true)
{ {
m_swingSpan1 = btScalar(1e30); m_swingSpan1 = btScalar(1e30);
m_swingSpan2 = btScalar(1e30); m_swingSpan2 = btScalar(1e30);
@@ -46,7 +48,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
m_angularOnly(false) m_angularOnly(false),
m_useSolveConstraintObsolete(true)
{ {
m_rbBFrame = m_rbAFrame; m_rbBFrame = m_rbAFrame;
@@ -61,221 +64,251 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
} }
void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
btAssert(0);
}
}
void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(0);
}
void btConeTwistConstraint::buildJacobian() void btConeTwistConstraint::buildJacobian()
{ {
m_appliedImpulse = btScalar(0.); if (m_useSolveConstraintObsolete)
//set bias, sign, clear accumulator
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
if (!m_angularOnly)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); m_appliedImpulse = btScalar(0.);
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3]; //set bias, sign, clear accumulator
if (relPos.length2() > SIMD_EPSILON) m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
if (!m_angularOnly)
{ {
normal[0] = relPos.normalized(); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
} btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
else btVector3 relPos = pivotBInW - pivotAInW;
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]); btVector3 normal[3];
if (relPos.length2() > SIMD_EPSILON)
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
} else
if (twist > m_twistSpan*lockedFreeFactor)
{ {
m_twistCorrection = (twist - m_twistSpan); normal[0] = relPos.normalized();
}
else
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true; m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize(); m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
} } else
if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
}
} }
} }
void btConeTwistConstraint::solveConstraint(btScalar timeStep) void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{ {
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 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); btScalar tau = btScalar(0.3);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++) //linear part
{ if (!m_angularOnly)
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;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
}
}
{
///solve angular part
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
// solve swing limit
if (m_solveSwingLimit)
{ {
btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btScalar impulseMag = amplitude * m_kSwing; btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
// Clamp the accumulated impulse btVector3 vel1;
btScalar temp = m_accSwingLimitImpulse; bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); btVector3 vel2;
impulseMag = m_accSwingLimitImpulse - temp; bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
btVector3 impulse = m_swingAxis * impulseMag; for (int i=0;i<3;i++)
{
m_rbA.applyTorqueImpulse(impulse); const btVector3& normal = m_jac[i].m_linearJointAxis;
m_rbB.applyTorqueImpulse(-impulse); 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 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 twist limit
if (m_solveTwistLimit)
{ {
btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); ///solve angular part
btScalar impulseMag = amplitude * m_kTwist; btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
// Clamp the accumulated impulse // solve swing limit
btScalar temp = m_accTwistLimitImpulse; if (m_solveSwingLimit)
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); {
impulseMag = m_accTwistLimitImpulse - temp; btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
btScalar impulseMag = amplitude * m_kSwing;
btVector3 impulse = m_twistAxis * impulseMag; // Clamp the accumulated impulse
btScalar temp = m_accSwingLimitImpulse;
m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accSwingLimitImpulse - temp;
m_rbA.applyTorqueImpulse(impulse); btVector3 impulse = m_swingAxis * impulseMag;
m_rbB.applyTorqueImpulse(-impulse);
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_swingAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_swingAxis,-impulseMag);
}
// solve twist limit
if (m_solveTwistLimit)
{
btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
btScalar impulseMag = amplitude * m_kTwist;
// Clamp the accumulated impulse
btScalar temp = m_accTwistLimitImpulse;
m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
impulseMag = m_accTwistLimitImpulse - temp;
btVector3 impulse = m_twistAxis * impulseMag;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
}
} }
} }
} }

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

@@ -63,6 +63,7 @@ public:
bool m_solveTwistLimit; bool m_solveTwistLimit;
bool m_solveSwingLimit; bool m_solveSwingLimit;
bool m_useSolveConstraintObsolete;
public: public:
@@ -74,7 +75,12 @@ public:
virtual void buildJacobian(); virtual void buildJacobian();
virtual void solveConstraint(btScalar timeStep); virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep); void updateRHS(btScalar timeStep);

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

@@ -22,7 +22,9 @@ enum btSolverMode
SOLVER_FRICTION_SEPARATE = 2, SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4, SOLVER_USE_WARMSTARTING = 4,
SOLVER_USE_FRICTION_WARMSTARTING = 8, SOLVER_USE_FRICTION_WARMSTARTING = 8,
SOLVER_CACHE_FRIENDLY = 16 SOLVER_CACHE_FRIENDLY = 16,
SOLVER_SIMD = 32,//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version
SOLVER_CUDA = 64 //will be open sourced during Game Developers Conference 2009. Much faster.
}; };
struct btContactSolverInfoData struct btContactSolverInfoData
@@ -72,7 +74,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_splitImpulsePenetrationThreshold = -0.02f; m_splitImpulsePenetrationThreshold = -0.02f;
m_linearSlop = btScalar(0.0); m_linearSlop = btScalar(0.0);
m_warmstartingFactor=btScalar(0.85); m_warmstartingFactor=btScalar(0.85);
m_solverMode = SOLVER_CACHE_FRIENDLY | SOLVER_RANDMIZE_ORDER | SOLVER_USE_WARMSTARTING; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;//SOLVER_RANDMIZE_ORDER
m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution
} }
}; };

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

File diff suppressed because it is too large Load Diff

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

@@ -110,7 +110,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, btRigidBody * body1); btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB);
}; };
@@ -168,8 +168,8 @@ public:
btScalar solveLinearAxis( btScalar solveLinearAxis(
btScalar timeStep, btScalar timeStep,
btScalar jacDiagABInv, btScalar jacDiagABInv,
btRigidBody& body1,const btVector3 &pointInA, btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB, btRigidBody& body2,btSolverBody& bodyB,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);
@@ -262,6 +262,9 @@ protected:
} }
int setAngularLimits(btConstraintInfo2 *info, int row_offset);
int setLinearLimits(btConstraintInfo2 *info);
void buildLinearJacobian( void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld, btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -276,6 +279,10 @@ protected:
public: public:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
btGeneric6DofConstraint(); btGeneric6DofConstraint();
@@ -330,7 +337,11 @@ public:
//! performs Jacobian calculation, and also calculates angle differences and axis //! performs Jacobian calculation, and also calculates angle differences and axis
virtual void buildJacobian(); virtual void buildJacobian();
virtual void solveConstraint(btScalar timeStep); virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep); void updateRHS(btScalar timeStep);

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

@@ -19,11 +19,14 @@ subject to the following restrictions:
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h" #include "LinearMath/btMinMax.h"
#include <new> #include <new>
#include "btSolverBody.h"
btHingeConstraint::btHingeConstraint() btHingeConstraint::btHingeConstraint()
: btTypedConstraint (HINGE_CONSTRAINT_TYPE), : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
m_enableAngularMotor(false) m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{ {
} }
@@ -31,7 +34,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
btVector3& axisInA,btVector3& axisInB) btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false) m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{ {
m_rbAFrame.getOrigin() = pivotInA; m_rbAFrame.getOrigin() = pivotInA;
@@ -77,7 +81,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(true)
{ {
// since no frame is given, assume this to be zero angle and just pick rb transform axis // since no frame is given, assume this to be zero angle and just pick rb transform axis
@@ -115,7 +119,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame, const btTransform& rbBFrame) const btTransform& rbAFrame, const btTransform& rbBFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false) m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{ {
// flip axis // flip axis
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.); m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
@@ -136,7 +141,8 @@ m_enableAngularMotor(false)
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
m_angularOnly(false), m_angularOnly(false),
m_enableAngularMotor(false) m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{ {
///not providing rigidbody B means implicitly using worldspace for body B ///not providing rigidbody B means implicitly using worldspace for body B
@@ -158,226 +164,270 @@ m_enableAngularMotor(false)
void btHingeConstraint::buildJacobian() void btHingeConstraint::buildJacobian()
{ {
m_appliedImpulse = btScalar(0.); if (m_useSolveConstraintObsolete)
if (!m_angularOnly)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); m_appliedImpulse = btScalar(0.);
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3]; if (!m_angularOnly)
if (relPos.length2() > SIMD_EPSILON)
{ {
normal[0] = relPos.normalized(); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
} btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
else btVector3 relPos = pivotBInW - pivotAInW;
{
normal[0].setValue(btScalar(1.0),0,0); btVector3 normal[3];
if (relPos.length2() > SIMD_EPSILON)
{
normal[0] = relPos.normalized();
}
else
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
} }
btPlaneSpace1(normal[0], normal[1], normal[2]); //calculate two perpendicular jointAxis, orthogonal to hingeAxis
//these two jointAxis require equal angular velocities for both bodies
for (int i=0;i<3;i++) //this is unused for now, it's a todo
{ btVector3 jointAxis0local;
new (&m_jac[i]) btJacobianEntry( btVector3 jointAxis1local;
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
//these two jointAxis require equal angular velocities for both bodies
//this is unused for now, it's a todo
btVector3 jointAxis0local;
btVector3 jointAxis1local;
btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
new (&m_jacAng[0]) btJacobianEntry(jointAxis0, btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[1]) btJacobianEntry(jointAxis1, getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
m_rbA.getCenterOfMassTransform().getBasis().transpose(), btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
m_rbB.getCenterOfMassTransform().getBasis().transpose(), btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
m_rbA.getInvInertiaDiagLocal(), btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
m_rbA.getCenterOfMassTransform().getBasis().transpose(), m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(), m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(), m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal()); m_rbB.getInvInertiaDiagLocal());
new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
m_rbA.getInvInertiaDiagLocal(),
m_rbB.getInvInertiaDiagLocal());
// Compute limit information // Compute limit information
btScalar hingeAngle = getHingeAngle(); btScalar hingeAngle = getHingeAngle();
//set bias, sign, clear accumulator //set bias, sign, clear accumulator
m_correction = btScalar(0.); m_correction = btScalar(0.);
m_limitSign = btScalar(0.); m_limitSign = btScalar(0.);
m_solveLimit = false; m_solveLimit = false;
m_accLimitImpulse = btScalar(0.); m_accLimitImpulse = btScalar(0.);
// if (m_lowerLimit < m_upperLimit) // if (m_lowerLimit < m_upperLimit)
if (m_lowerLimit <= m_upperLimit) if (m_lowerLimit <= m_upperLimit)
{
// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
if (hingeAngle <= m_lowerLimit)
{ {
m_correction = (m_lowerLimit - hingeAngle); // if (hingeAngle <= m_lowerLimit*m_limitSoftness)
m_limitSign = 1.0f; if (hingeAngle <= m_lowerLimit)
m_solveLimit = true; {
} m_correction = (m_lowerLimit - hingeAngle);
// else if (hingeAngle >= m_upperLimit*m_limitSoftness) m_limitSign = 1.0f;
else if (hingeAngle >= m_upperLimit) m_solveLimit = true;
{ }
m_correction = m_upperLimit - hingeAngle; // else if (hingeAngle >= m_upperLimit*m_limitSoftness)
m_limitSign = -1.0f; else if (hingeAngle >= m_upperLimit)
m_solveLimit = true; {
m_correction = m_upperLimit - hingeAngle;
m_limitSign = -1.0f;
m_solveLimit = true;
}
} }
//Compute K = J*W*J' for hinge axis
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
getRigidBodyB().computeAngularImpulseDenominator(axisA));
} }
//Compute K = J*W*J' for hinge axis
btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
getRigidBodyB().computeAngularImpulseDenominator(axisA));
} }
void btHingeConstraint::solveConstraint(btScalar timeStep)
void btHingeConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
btAssert(0);
}
}
void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(0);
}
void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); ///for backwards compatibility during the transition to 'getInfo/getInfo2'
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); if (m_useSolveConstraintObsolete)
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 = m_rbA.getVelocityInLocalPoint(rel_pos1); btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++) btScalar tau = btScalar(0.3);
{
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
btScalar rel_vel; //linear part
rel_vel = normal.dot(vel); if (!m_angularOnly)
//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;
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
}
}
{
///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);
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
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 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btScalar relaxation = btScalar(1.); btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
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;
}
//solve angular positional correction btVector3 vel1,vel2;
btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
btScalar len2 = angularError.length(); bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
if (len2>btScalar(0.00001)) btVector3 vel = vel1 - vel2;
{
btVector3 normal2 = angularError.normalized();
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
getRigidBodyB().computeAngularImpulseDenominator(normal2);
angularError *= (btScalar(1.)/denom2) * relaxation;
}
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError); for (int i=0;i<3;i++)
m_rbB.applyTorqueImpulse(velrelOrthog-angularError); {
const btVector3& normal = m_jac[i].m_linearJointAxis;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
// solve limit btScalar rel_vel;
if (m_solveLimit) rel_vel = normal.dot(vel);
{ //positional error (zeroth order error)
btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
btScalar impulseMag = amplitude * m_kHinge; m_appliedImpulse += impulse;
btVector3 impulse_vector = normal * impulse;
// Clamp the accumulated impulse btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btScalar temp = m_accLimitImpulse; btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
impulseMag = m_accLimitImpulse - temp; bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
btVector3 impulse = axisA * impulseMag * m_limitSign;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
} }
} }
//apply motor
if (m_enableAngularMotor)
{ {
//todo: add limits too ///solve angular part
btVector3 angularLimit(0,0,0);
btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; // get axes in world space
btScalar projRelVel = velrel.dot(axisA); btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
btScalar desiredMotorVel = m_motorTargetVelocity; btVector3 angVelA;
btScalar motor_relvel = desiredMotorVel - projRelVel; bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
//todo: should clip against accumulated impulse btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
btVector3 motorImp = clippedMotorImpulse * axisA;
m_rbA.applyTorqueImpulse(motorImp+angularLimit); btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
m_rbB.applyTorqueImpulse(-motorImp-angularLimit); 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;;
//todo: should clip against accumulated impulse
btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
btVector3 motorImp = clippedMotorImpulse * axisA;
bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
}
} }
} }

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

@@ -57,6 +57,7 @@ public:
bool m_angularOnly; bool m_angularOnly;
bool m_enableAngularMotor; bool m_enableAngularMotor;
bool m_solveLimit; bool m_solveLimit;
bool m_useSolveConstraintObsolete;
public: public:
@@ -73,7 +74,11 @@ public:
virtual void buildJacobian(); virtual void buildJacobian();
virtual void solveConstraint(btScalar timeStep); virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep); void updateRHS(btScalar timeStep);

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

@@ -21,100 +21,197 @@ subject to the following restrictions:
btPoint2PointConstraint::btPoint2PointConstraint() btPoint2PointConstraint::btPoint2PointConstraint()
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE),
m_useSolveConstraintObsolete(false)
{ {
} }
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_useSolveConstraintObsolete(false)
{ {
} }
btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) :btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
m_useSolveConstraintObsolete(false)
{ {
} }
void btPoint2PointConstraint::buildJacobian() void btPoint2PointConstraint::buildJacobian()
{ {
m_appliedImpulse = btScalar(0.); if (m_useSolveConstraintObsolete)
btVector3 normal(0,0,0);
for (int i=0;i<3;i++)
{ {
normal[i] = 1; m_appliedImpulse = btScalar(0.);
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(), btVector3 normal(0,0,0);
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal, for (int i=0;i<3;i++)
m_rbA.getInvInertiaDiagLocal(), {
m_rbA.getInvMass(), normal[i] = 1;
m_rbB.getInvInertiaDiagLocal(), new (&m_jac[i]) btJacobianEntry(
m_rbB.getInvMass());
normal[i] = 0; m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
normal,
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
normal[i] = 0;
}
} }
} }
void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
{ {
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA; if (m_useSolveConstraintObsolete)
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB; {
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
info->m_numConstraintRows = 3;
info->nub = 3;
}
}
#define dCROSSMAT(A,a,skip,plus,minus) \
{ \
(A)[1] = minus (a)[2]; \
(A)[2] = plus (a)[1]; \
(A)[(skip)+0] = plus (a)[2]; \
(A)[(skip)+2] = minus (a)[0]; \
(A)[2*(skip)+0] = minus (a)[1]; \
(A)[2*(skip)+1] = plus (a)[0]; \
}
#include <stdio.h>
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(!m_useSolveConstraintObsolete);
btVector3 normal(0,0,0); //retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); // anchor points in global coordinates with respect to body PORs.
// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); int s = info->rowskip;
for (int i=0;i<3;i++) // set jacobian
{ info->m_J1linearAxis[0] = 1;
normal[i] = 1; info->m_J1linearAxis[s+1] = 1;
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); info->m_J1linearAxis[2*s+2] = 1;
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); btVector3 a1,a2;
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
rel_vel = normal.dot(vel);
/* a1 = body0_trans.getBasis()*getPivotInA();
//velocity error (first order error) //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
m_rbB.getLinearVelocity(),angvelB);
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/ */
//positional error (zeroth order error)
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar impulseClamp = m_setting.m_impulseClamp; a2 = body1_trans.getBasis()*getPivotInB();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
//dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
// set right hand side
btScalar k = info->fps * info->erp;
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btScalar impulseClamp = m_setting.m_impulseClamp;
for (j=0; j<3; j++)
{
if (impulseClamp > 0) if (impulseClamp > 0)
{ {
if (impulse < -impulseClamp) info->m_lowerLimit[j*s] = -impulseClamp;
impulse = -impulseClamp; info->m_upperLimit[j*s] = impulseClamp;
if (impulse > impulseClamp)
impulse = impulseClamp;
} }
}
}
m_appliedImpulse+=impulse;
btVector3 impulse_vector = normal * impulse; void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); {
m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
btVector3 normal(0,0,0);
normal[i] = 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 impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
btScalar impulseClamp = m_setting.m_impulseClamp;
if (impulseClamp > 0)
{
if (impulse < -impulseClamp)
impulse = -impulseClamp;
if (impulse > impulseClamp)
impulse = impulseClamp;
}
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);
normal[i] = 0;
}
} }
} }

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

@@ -50,6 +50,9 @@ public:
public: public:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btConstraintSetting m_setting; btConstraintSetting m_setting;
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB); btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
@@ -60,8 +63,12 @@ public:
virtual void buildJacobian(); virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void solveConstraint(btScalar timeStep); virtual void getInfo2 (btConstraintInfo2* info);
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
void updateRHS(btScalar timeStep); void updateRHS(btScalar timeStep);

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

@@ -37,29 +37,73 @@ subject to the following restrictions:
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_btSeed2(0) :m_btSeed2(0)
{ {
} }
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
{ {
} }
// Project Gauss Seidel or the equivalent Sequential Impulse #ifdef USE_SIMD
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) #include <emmintrin.h>
#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
{ {
float deltaImpulse; __m128 result = _mm_mul_ps( vec0, vec1);
deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm; return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); }
btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); #endif//USE_SIMD
btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
// Project Gauss Seidel or the equivalent Sequential Impulse
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
__m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_add_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass));
__m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else
resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv; deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
btScalar sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit) if (sum < c.m_lowerLimit)
{ {
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit; c.m_appliedImpulse = c.m_lowerLimit;
} }
else if (sum > c.m_upperLimit) else if (sum > c.m_upperLimit)
{ {
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse; deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
@@ -69,8 +113,71 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBod
{ {
c.m_appliedImpulse = sum; c.m_appliedImpulse = sum;
} }
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse); if (body1.m_invMass)
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse); body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
if (body2.m_invMass)
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
}
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
__m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_add_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
__m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass));
//body1.applyImpulse(c.m_contactNormal*body1.m_invMass.m_vec128,c.m_angularComponentA,deltaImpulse);
//body2.applyImpulse(c.m_contactNormal*body2.m_invMass.m_vec128,c.m_angularComponentB,-deltaImpulse);
__m128 impulseMagnitude = deltaImpulse;
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else
resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
if (body1.m_invMass)
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
if (body2.m_invMass)
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
} }
@@ -113,39 +220,24 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
{ {
btRigidBody* rb = btRigidBody::upcast(collisionObject); btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
if (rb) if (rb)
{ {
solverBody->m_angularVelocity = rb->getAngularVelocity() ;
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = rb->getInvMass(); solverBody->m_invMass = rb->getInvMass();
solverBody->m_linearVelocity = rb->getLinearVelocity();
solverBody->m_originalBody = rb; solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_angularFactor = rb->getAngularFactor();
} else } else
{ {
solverBody->m_angularVelocity.setValue(0,0,0);
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
solverBody->m_friction = collisionObject->getFriction();
solverBody->m_invMass = 0.f; solverBody->m_invMass = 0.f;
solverBody->m_linearVelocity.setValue(0,0,0);
solverBody->m_originalBody = 0; solverBody->m_originalBody = 0;
solverBody->m_angularFactor = 1.f; solverBody->m_angularFactor = 1.f;
} }
solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
} }
@@ -157,149 +249,19 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
return rest; return rest;
} }
//SIMD_FORCE_INLINE
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo)
{
(void)solverInfo;
if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
{
gNumSplitImpulseRecoveries++;
btScalar normalImpulse;
// Optimized version of projected relative velocity, use precomputed cross products with normal
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
// btVector3 vel = vel1 - vel2;
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
btScalar rel_vel;
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
// btScalar positionalError = contactConstraint.m_penetration;
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
normalImpulse = penetrationImpulse+velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
btScalar sum = oldNormalImpulse + normalImpulse;
contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
}
}
//SIMD_FORCE_INLINE
void btSequentialImpulseConstraintSolver::resolveSingleFrictionCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo,
btScalar appliedNormalImpulse)
{
(void)solverInfo;
const btScalar combinedFriction = contactConstraint.m_friction;
const btScalar limit = appliedNormalImpulse * combinedFriction;
if (appliedNormalImpulse>btScalar(0.))
//friction
{
btScalar j1;
{
#ifndef _USE_JACOBIAN
btScalar rel_vel;
const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
rel_vel = vel1Dotn-vel2Dotn;
#else
btScalar rel_vel = contactConstraint.m_jac.getRelativeVelocity(body1.m_linearVelocity1+body1.m_deltaLinearVelocity,body1.m_angularVelocity1+body1.m_deltaAngularVelocity,
body2.m_linearVelocity1+body2.m_deltaLinearVelocity,body2.m_angularVelocity1+body2.m_deltaAngularVelocity);
#endif //_USE_JACOBIAN
// calculate j that moves us to zero relative velocity
j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
if (limit < contactConstraint.m_appliedImpulse)
{
contactConstraint.m_appliedImpulse = limit;
} else
{
if (contactConstraint.m_appliedImpulse < -limit)
contactConstraint.m_appliedImpulse = -limit;
}
j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
#else
if (limit < j1)
{
j1 = limit;
} else
{
if (j1 < -limit)
j1 = -limit;
}
#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
}
body1.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
}
}
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) 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)
{ {
btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body0=btRigidBody::upcast(colObj0);
btRigidBody* body1=btRigidBody::upcast(colObj1); btRigidBody* body1=btRigidBody::upcast(colObj1);
btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand(); btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
solverConstraint.m_contactNormal = normalAxis; solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
@@ -310,8 +272,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = 0; solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = btScalar(0.); solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f; // solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f; solverConstraint.m_penetration = 0.f;
{ {
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -355,10 +317,57 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
body1->getInvInertiaDiagLocal(), body1->getInvInertiaDiagLocal(),
body1->getInvMass()); body1->getInvMass());
#endif //_USE_JACOBIAN #endif //_USE_JACOBIAN
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = 0.f;
positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
solverConstraint.m_restitution=0.f;
btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
return solverConstraint; return solverConstraint;
} }
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
{
int solverBodyIdA = -1;
if (body.getCompanionId() >= 0)
{
//body has already been converted
solverBodyIdA = body.getCompanionId();
} else
{
btRigidBody* rb = btRigidBody::upcast(&body);
if (rb && rb->getInvMass())
{
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,&body);
body.setCompanionId(solverBodyIdA);
} else
{
return 0;//assume first one is a fixed solver body
}
}
return solverBodyIdA;
}
#include <stdio.h>
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{ {
@@ -372,16 +381,149 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
// printf("empty\n"); // printf("empty\n");
return 0.f; return 0.f;
} }
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0; btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btRigidBody* rb0=0,*rb1=0; //btRigidBody* rb0=0,*rb1=0;
//if (1) //if (1)
{ {
{
int totalNumRows = 0;
//calculate the total number of contraint rows
for (int i=0;i<numConstraints;i++)
{
btTypedConstraint::btConstraintInfo1 info1;
constraints[i]->getInfo1(&info1);
totalNumRows += info1.m_numConstraintRows;
}
m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
btTypedConstraint::btConstraintInfo1 info1;
info1.m_numConstraintRows = 0;
if (m_tmpSolverNonContactConstraintPool.size()>3)
{
printf("dsadas\n");
}
///setup the btSolverConstraints
int currentRow = 0;
for (int i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
{
constraints[i]->getInfo1(&info1);
if (info1.m_numConstraintRows)
{
btAssert(currentRow<totalNumRows);
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint* constraint = constraints[i];
btRigidBody& rbA = constraint->getRigidBodyA();
btRigidBody& rbB = constraint->getRigidBodyB();
int solverBodyIdA = getOrInitSolverBody(rbA);
int solverBodyIdB = getOrInitSolverBody(rbB);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
for (int j=0;j<info1.m_numConstraintRows;j++)
{
memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
currentConstraintRow[j].m_upperLimit = FLT_MAX;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_penetration = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
}
bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
btTypedConstraint::btConstraintInfo2 info2;
info2.fps = 1.f/infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
info2.m_J2linearAxis = 0;
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
info2.m_constraintError = &currentConstraintRow->m_rhs;
info2.cfm = &currentConstraintRow->m_cfm;
info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
constraints[i]->getInfo2(&info2);
///finalize the constraint setup
for (int j=0;j<info1.m_numConstraintRows;j++)
{
btSolverConstraint& solverConstraint = currentConstraintRow[j];
{
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1;
}
{
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2;
}
{
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = 1./sum;
}
///fix rhs
///todo: add force/torque accelerators
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
solverConstraint.m_restitution = 0.f;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
}
}
{ {
int i; int i;
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0;
for (i=0;i<numManifolds;i++) for (i=0;i<numManifolds;i++)
{ {
@@ -394,49 +536,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (manifold->getNumContacts()) if (manifold->getNumContacts())
{ {
solverBodyIdA = getOrInitSolverBody(*colObj0);
solverBodyIdB = getOrInitSolverBody(*colObj1);
if (colObj0->getIslandTag() >= 0)
{
if (colObj0->getCompanionId() >= 0)
{
//body has already been converted
solverBodyIdA = colObj0->getCompanionId();
} else
{
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,colObj0);
colObj0->setCompanionId(solverBodyIdA);
}
} else
{
//create a static body
solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,colObj0);
}
if (colObj1->getIslandTag() >= 0)
{
if (colObj1->getCompanionId() >= 0)
{
solverBodyIdB = colObj1->getCompanionId();
} else
{
solverBodyIdB = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,colObj1);
colObj1->setCompanionId(solverBodyIdB);
}
} else
{
//create a static body
solverBodyIdB = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,colObj1);
}
} }
btVector3 rel_pos1; btVector3 rel_pos1;
@@ -463,10 +564,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar rel_vel; btScalar rel_vel;
btVector3 vel; btVector3 vel;
int frictionIndex = m_tmpSolverConstraintPool.size(); int frictionIndex = m_tmpSolverContactConstraintPool.size();
{ {
btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1); btRigidBody* rb1 = btRigidBody::upcast(colObj1);
@@ -541,7 +642,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}; };
} }
///warm starting (or zero if disabled) ///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{ {
@@ -555,7 +656,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
} }
solverConstraint.m_appliedPushImpulse = 0.f; // solverConstraint.m_appliedPushImpulse = 0.f;
{ {
btScalar rel_vel; btScalar rel_vel;
@@ -568,14 +669,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping; btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhs = (penetrationImpulse+velocityImpulse);
solverConstraint.m_cfm = 0.f; solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0; solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e30f; solverConstraint.m_upperLimit = 1e10f;
} }
@@ -594,7 +694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (1) if (1)
{ {
solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size(); solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
if (!cp.m_lateralFrictionInitialized) if (!cp.m_lateralFrictionInitialized)
{ {
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -615,9 +715,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
//re-calculate friction direction every frame, todo: check if this is really needed //re-calculate friction direction every frame, todo: check if this is really needed
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{ {
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true; cp.m_lateralFrictionInitialized = true;
} }
} }
@@ -632,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{ {
{ {
btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex]; btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{ {
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
@@ -646,7 +746,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
} }
} }
{ {
btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{ {
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
@@ -672,6 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btContactSolverInfo info = infoGlobal; btContactSolverInfo info = infoGlobal;
if (1)
{ {
int j; int j;
for (j=0;j<numConstraints;j++) for (j=0;j<numConstraints;j++)
@@ -681,8 +782,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
} }
} }
int numConstraintPool = m_tmpSolverConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
m_orderTmpConstraintPool.resize(numConstraintPool); m_orderTmpConstraintPool.resize(numConstraintPool);
@@ -706,8 +807,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
{ {
BT_PROFILE("solveGroupCacheFriendlyIterations"); BT_PROFILE("solveGroupCacheFriendlyIterations");
int numConstraintPool = m_tmpSolverConstraintPool.size();
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
//should traverse the contacts random order... //should traverse the contacts random order...
int iteration; int iteration;
@@ -735,82 +837,96 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
} }
} }
for (j=0;j<numConstraints;j++) if (infoGlobal.m_solverMode & SOLVER_SIMD)
{ {
btTypedConstraint* constraint = constraints[j]; ///solve all joint constraints, using SIMD, if available
///todo: use solver bodies, so we don't need to copy from/to btRigidBody for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
{ {
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity(); btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
} resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
{
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
} }
constraint->solveConstraint(infoGlobal.m_timeStep); for (j=0;j<numConstraints;j++)
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
{ {
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity(); int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
} int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0)) btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
{ btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity(); constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
} }
} ///solve all contact constraints using SIMD, if available
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRow(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
} ///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
{
int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+ btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], if (totalImpulse>btScalar(0))
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
totalImpulse);
}
}
}
if (infoGlobal.m_splitImpulse)
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]]; solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal); m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
} else
{
///solve all joint constraints
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
}
for (j=0;j<numConstraints;j++)
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
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
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
} }
} }
} }
} }
return 0.f; return 0.f;
} }
@@ -830,23 +946,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
int numPoolConstraints = m_tmpSolverConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j; int j;
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt); btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse; pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{ {
pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
} }
//do a callback here? //do a callback here?
} }
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
@@ -865,8 +981,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
m_tmpSolverBodyPool.resize(0); m_tmpSolverBodyPool.resize(0);
m_tmpSolverConstraintPool.resize(0); m_tmpSolverContactConstraintPool.resize(0);
m_tmpSolverFrictionConstraintPool.resize(0); m_tmpSolverNonContactConstraintPool.resize(0);
m_tmpSolverContactFrictionConstraintPool.resize(0);
return 0.f; return 0.f;
} }

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

@@ -23,6 +23,7 @@ class btIDebugDraw;
#include "btSolverConstraint.h" #include "btSolverConstraint.h"
///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses ///The btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com ///The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com
///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) ///Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP)
@@ -31,12 +32,12 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
{ {
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool; btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btAlignedObjectArray<btSolverConstraint> m_tmpSolverConstraintPool; btConstraintArray m_tmpSolverContactConstraintPool;
btAlignedObjectArray<btSolverConstraint> m_tmpSolverFrictionConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool;
protected: protected:
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); 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);
@@ -52,18 +53,17 @@ protected:
const btSolverConstraint& contactConstraint, const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo); const btContactSolverInfo& solverInfo);
void resolveSingleConstraintRow( //internal method
btSolverBody& body1, int getOrInitSolverBody(btCollisionObject& body);
btSolverBody& body2,
const btSolverConstraint& contactConstraint);
void resolveSingleFrictionCacheFriendly( void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo,
btScalar appliedNormalImpulse);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
public: public:

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

@@ -153,27 +153,41 @@ void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, co
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void btSliderConstraint::solveConstraint(btScalar timeStep) void btSliderConstraint::getInfo1 (btConstraintInfo1* info)
{
info->m_numConstraintRows = 0;
info->nub = 0;
}
void btSliderConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(0);
}
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{ {
m_timeStep = timeStep; m_timeStep = timeStep;
if(m_useLinearReferenceFrameA) if(m_useLinearReferenceFrameA)
{ {
solveConstraintInt(m_rbA, m_rbB); solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
} }
else else
{ {
solveConstraintInt(m_rbB, m_rbA); solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
} }
} // btSliderConstraint::solveConstraint() } // btSliderConstraint::solveConstraint()
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB) void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{ {
int i; int i;
// linear // linear
btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA); btVector3 velA;
btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB); bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
btVector3 velB;
bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
btVector3 vel = velA - velB; btVector3 vel = velA - velB;
for(i = 0; i < 3; i++) for(i = 0; i < 3; i++)
{ {
@@ -188,8 +202,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
// calcutate and apply impulse // calcutate and apply impulse
btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i]; btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
btVector3 impulse_vector = normal * normalImpulse; btVector3 impulse_vector = normal * normalImpulse;
rbA.applyImpulse( impulse_vector, m_relPosA);
rbB.applyImpulse(-impulse_vector, m_relPosB); //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)) if(m_poweredLinMotor && (!i))
{ // apply linear motor { // apply linear motor
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce) if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
@@ -215,8 +239,18 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
m_accumulatedLinMotorImpulse = new_acc; m_accumulatedLinMotorImpulse = new_acc;
// apply clamped impulse // apply clamped impulse
impulse_vector = normal * normalImpulse; impulse_vector = normal * normalImpulse;
rbA.applyImpulse( impulse_vector, m_relPosA); //rbA.applyImpulse( impulse_vector, m_relPosA);
rbB.applyImpulse(-impulse_vector, m_relPosB); //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);
}
} }
} }
} }
@@ -225,8 +259,10 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0); btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0); btVector3 axisB = m_calculatedTransformB.getBasis().getColumn(0);
const btVector3& angVelA = rbA.getAngularVelocity(); btVector3 angVelA;
const btVector3& angVelB = rbB.getAngularVelocity(); bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA); btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB); btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
@@ -236,24 +272,38 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
btVector3 velrelOrthog = angAorthog-angBorthog; btVector3 velrelOrthog = angAorthog-angBorthog;
//solve orthogonal angular velocity correction //solve orthogonal angular velocity correction
btScalar len = velrelOrthog.length(); btScalar len = velrelOrthog.length();
btScalar orthorImpulseMag = 0.f;
if (len > btScalar(0.00001)) if (len > btScalar(0.00001))
{ {
btVector3 normal = velrelOrthog.normalized(); btVector3 normal = velrelOrthog.normalized();
btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal); btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng; //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
} }
//solve angular positional correction //solve angular positional correction
btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep); btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
btVector3 angularAxis = angularError;
btScalar angularImpulseMag = 0;
btScalar len2 = angularError.length(); btScalar len2 = angularError.length();
if (len2>btScalar(0.00001)) if (len2>btScalar(0.00001))
{ {
btVector3 normal2 = angularError.normalized(); btVector3 normal2 = angularError.normalized();
btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2); btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng; angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
angularError *= angularImpulseMag;
} }
// apply impulse // apply impulse
rbA.applyTorqueImpulse(-velrelOrthog+angularError); //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
rbB.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; btScalar impulseMag;
//solve angular limits //solve angular limits
if(m_solveAngLim) if(m_solveAngLim)
@@ -267,8 +317,14 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
impulseMag *= m_kAngle * m_softnessDirAng; impulseMag *= m_kAngle * m_softnessDirAng;
} }
btVector3 impulse = axisA * impulseMag; btVector3 impulse = axisA * impulseMag;
rbA.applyTorqueImpulse(impulse); //rbA.applyTorqueImpulse(impulse);
rbB.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 //apply angular motor
if(m_poweredAngMotor) if(m_poweredAngMotor)
{ {
@@ -299,8 +355,11 @@ void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
m_accumulatedAngMotorImpulse = new_acc; m_accumulatedAngMotorImpulse = new_acc;
// apply clamped impulse // apply clamped impulse
btVector3 motorImp = angImpulse * axisA; btVector3 motorImp = angImpulse * axisA;
m_rbA.applyTorqueImpulse(motorImp); //rbA.applyTorqueImpulse(motorImp);
m_rbB.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);
} }
} }
} // btSliderConstraint::solveConstraint() } // btSliderConstraint::solveConstraint()

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

@@ -126,7 +126,13 @@ public:
btSliderConstraint(); btSliderConstraint();
// overrides // overrides
virtual void buildJacobian(); virtual void buildJacobian();
virtual void solveConstraint(btScalar timeStep); virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info);
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; }
const btRigidBody& getRigidBodyB() const { return m_rbB; } const btRigidBody& getRigidBodyB() const { return m_rbB; }
@@ -202,7 +208,7 @@ public:
btScalar getAngDepth() { return m_angDepth; } btScalar getAngDepth() { return m_angDepth; }
// internal // internal
void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB); void buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
void solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB); void solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB);
// shared code used by ODE solver // shared code used by ODE solver
void calculateTransforms(void); void calculateTransforms(void);
void testLinLimits(void); void testLinLimits(void);

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

@@ -23,193 +23,157 @@ class btRigidBody;
#include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btAlignedAllocator.h"
#include "LinearMath/btTransformUtil.h" #include "LinearMath/btTransformUtil.h"
#ifndef _USE_JACOBIAN ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
#define USE_SIMD 1
#endif //
///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED16 (struct) btSolverBody #ifdef USE_SIMD
#include <emmintrin.h>
struct btSimdScalar
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); SIMD_FORCE_INLINE btSimdScalar()
btMatrix3x3 m_worldInvInertiaTensor;
btVector3 m_angularVelocity;
btVector3 m_linearVelocity;
btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
float m_angularFactor;
float m_invMass;
float m_friction;
btRigidBody* m_originalBody;
SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
{ {
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
} }
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position SIMD_FORCE_INLINE btSimdScalar(float fl)
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) :m_vec128 (_mm_set1_ps(fl))
{ {
//if (m_invMass) }
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude; SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); :m_vec128(v128)
m_linearVelocity += linearComponent*impulseMagnitude; {
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); }
} union
{
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
};
SIMD_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
SIMD_FORCE_INLINE const __m128 get128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
{
m_vec128 = v128;
}
SIMD_FORCE_INLINE operator __m128()
{
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
} }
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) SIMD_FORCE_INLINE operator float() const
{ {
//if (m_invMass) return m_floats[0];
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
} }
void writebackVelocity()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
void writebackVelocity(btScalar timeStep)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
m_originalBody->setWorldTransform(newTransform);
//m_originalBody->setCompanionId(-1);
}
}
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity = m_originalBody->getLinearVelocity();
m_angularVelocity = m_originalBody->getAngularVelocity();
}
}
}; };
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}
#else #else
#define btSimdScalar btScalar
#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_ALIGNED16 (struct) btSolverBody
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
btMatrix3x3 m_worldInvInertiaTensor;
btVector3 m_angularVelocity1;
btVector3 m_linearVelocity1;
btVector3 m_deltaAngularVelocity;
btVector3 m_deltaLinearVelocity; btVector3 m_deltaLinearVelocity;
btVector3 m_deltaAngularVelocity;
btVector3 m_centerOfMassPosition; btScalar m_angularFactor;
btVector3 m_pushVelocity; btScalar m_invMass;
btVector3 m_turnVelocity; btScalar m_friction;
float m_angularFactor;
float m_invMass;
float m_friction;
btRigidBody* m_originalBody; btRigidBody* m_originalBody;
btVector3 m_pushVelocity;
//btVector3 m_turnVelocity;
SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
{ {
velocity = (m_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).cross(rel_pos); if (m_originalBody)
velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
else
velocity.setValue(0,0,0);
} }
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{ {
if (m_invMass) if (m_originalBody)
angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
else
angVel.setValue(0,0,0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
{
//if (m_invMass)
{ {
m_deltaLinearVelocity += linearComponent*impulseMagnitude; m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
} }
} }
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
{
if (m_invMass)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
/*
void writebackVelocity() void writebackVelocity()
{ {
if (m_invMass) if (m_invMass)
{ {
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1); //m_originalBody->setCompanionId(-1);
} }
} }
*/
void writebackVelocity(btScalar timeStep) void writebackVelocity(btScalar timeStep=0)
{ {
if (m_invMass) if (m_invMass)
{ {
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity); m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity); m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//correct the position/orientation based on push/turn recovery
btTransform newTransform;
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
m_originalBody->setWorldTransform(newTransform);
//m_originalBody->setCompanionId(-1); //m_originalBody->setCompanionId(-1);
} }
} }
void readVelocity()
{
if (m_invMass)
{
m_linearVelocity1 = m_originalBody->getLinearVelocity();
m_angularVelocity1 = m_originalBody->getAngularVelocity();
}
}
}; };
#endif //USE_JAC
#endif //BT_SOLVER_BODY_H #endif //BT_SOLVER_BODY_H

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

@@ -21,28 +21,26 @@ class btRigidBody;
#include "LinearMath/btMatrix3x3.h" #include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h" #include "btJacobianEntry.h"
#include <emmintrin.h>
//#define NO_FRICTION_TANGENTIALS 1 //#define NO_FRICTION_TANGENTIALS 1
#include "btSolverBody.h"
///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_ALIGNED16 (struct) btSolverConstraint
{ {
BT_DECLARE_ALIGNED_ALLOCATOR(); BT_DECLARE_ALIGNED_ALLOCATOR();
#ifdef _USE_JACOBIAN btVector3 m_relpos1CrossNormal;
btJacobianEntry m_jac; btVector3 m_contactNormal;
#endif
btVector3 m_relpos2CrossNormal;
btVector3 m_relpos1CrossNormal; btVector3 m_angularComponentA;
btVector3 m_contactNormal;
btVector3 m_relpos2CrossNormal;
btVector3 m_angularComponentA;
btVector3 m_angularComponentB; btVector3 m_angularComponentB;
mutable btScalar m_appliedPushImpulse; mutable btSimdScalar m_appliedPushImpulse;
mutable btScalar m_appliedImpulse; mutable btSimdScalar m_appliedImpulse;
int m_solverBodyIdA; int m_solverBodyIdA;
int m_solverBodyIdB; int m_solverBodyIdB;
@@ -51,16 +49,14 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btScalar m_restitution; btScalar m_restitution;
btScalar m_jacDiagABInv; btScalar m_jacDiagABInv;
btScalar m_penetration; btScalar m_penetration;
int m_constraintType; int m_constraintType;
int m_frictionIndex; int m_frictionIndex;
void* m_originalContactPoint; void* m_originalContactPoint;
btScalar m_rhs; btScalar m_rhs;
btScalar m_cfm; btScalar m_cfm;
btScalar m_lowerLimit; btScalar m_lowerLimit;
btScalar m_upperLimit; btScalar m_upperLimit;
enum btSolverConstraintType enum btSolverConstraintType
{ {
@@ -69,9 +65,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
}; };
}; };
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H #endif //BT_SOLVER_CONSTRAINT_H

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

@@ -18,6 +18,8 @@ subject to the following restrictions:
class btRigidBody; class btRigidBody;
#include "LinearMath/btScalar.h" #include "LinearMath/btScalar.h"
#include "btSolverConstraint.h"
struct btSolverBody;
enum btTypedConstraintType enum btTypedConstraintType
{ {
@@ -55,13 +57,53 @@ public:
btTypedConstraint(btTypedConstraintType type); btTypedConstraint(btTypedConstraintType type);
virtual ~btTypedConstraint() {}; virtual ~btTypedConstraint() {};
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
struct btConstraintInfo1 {
int m_numConstraintRows,nub;
};
struct btConstraintInfo2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
btScalar fps,erp;
// for the first and second body, pointers to two (linear and angular)
// n*3 jacobian sub matrices, stored by rows. these matrices will have
// been initialized to 0 on entry. if the second body is zero then the
// J2xx pointers may be 0.
btScalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
// elements to jump from one row to the next in J's
int rowskip;
// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
// "constraint force mixing" vector. c is set to zero on entry, cfm is
// set to a constant value (typically very small or zero) value on entry.
btScalar *m_constraintError,*cfm;
// lo and hi limits for variables (set to -/+ infinity on entry).
btScalar *m_lowerLimit,*m_upperLimit;
// findex vector for variables. see the LCP solver interface for a
// description of what this does. this is set to -1 on entry.
// note that the returned indexes are relative to the first index of
// the constraint.
int *findex;
};
virtual void buildJacobian() = 0; virtual void buildJacobian() = 0;
virtual void solveConstraint(btScalar timeStep) = 0; virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, btScalar timeStep)
{
}
virtual void getInfo1 (btConstraintInfo1* info)=0;
virtual void getInfo2 (btConstraintInfo2* info)=0;
virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) = 0;
const btRigidBody& getRigidBodyA() const const btRigidBody& getRigidBodyA() const
{ {
return m_rbA; return m_rbA;
@@ -94,7 +136,7 @@ public:
{ {
m_userConstraintId = uid; m_userConstraintId = uid;
} }
int getUserConstraintId() const int getUserConstraintId() const
{ {
return m_userConstraintId; return m_userConstraintId;

20
src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h
View File

@@ -140,22 +140,22 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
} m_flags; } m_flags;
// Linear parts, used by all constraints // Linear parts, used by all constraints
btQuadWordStorage m_relPos1; btVector3 m_relPos1;
btQuadWordStorage m_relPos2; btVector3 m_relPos2;
btQuadWordStorage m_jacdiagABInv; //Jacobian inverse multiplied by gamma (damping) for each axis btVector3 m_jacdiagABInv; //Jacobian inverse multiplied by gamma (damping) for each axis
btQuadWordStorage m_linearBias; //depth*tau/(dt*gamma) along each axis btVector3 m_linearBias; //depth*tau/(dt*gamma) along each axis
// Joint-specific parts // Joint-specific parts
union union
{ {
struct struct
{ {
btQuadWordStorage m_frameAinW[3]; btVector3 m_frameAinW[3];
btQuadWordStorage m_frameBinW[3]; btVector3 m_frameBinW[3];
// For angular // For angular
btQuadWordStorage m_angJacdiagABInv; //1/j btVector3 m_angJacdiagABInv; //1/j
btQuadWordStorage m_angularBias; //error/dt, in x/y. limit error*bias factor / (dt * relaxation factor) in z btVector3 m_angularBias; //error/dt, in x/y. limit error*bias factor / (dt * relaxation factor) in z
// For limit // For limit
float m_limitAccumulatedImpulse; float m_limitAccumulatedImpulse;
@@ -168,8 +168,8 @@ ATTRIBUTE_ALIGNED16(struct) SpuSolverConstraint
struct struct
{ {
btQuadWordStorage m_swingAxis; btVector3 m_swingAxis;
btQuadWordStorage m_twistAxis; btVector3 m_twistAxis;
float m_swingError; float m_swingError;
float m_swingJacInv; float m_swingJacInv;

18
src/LinearMath/btQuadWord.h
View File

@@ -24,13 +24,13 @@ subject to the following restrictions:
#include <altivec.h> #include <altivec.h>
#endif #endif
/**@brief The btQuadWordStorage class is base class for btVector3 and btQuaternion. /**@brief The btQuadWord class is base class for btVector3 and btQuaternion.
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword. * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/ */
#ifndef USE_LIBSPE2 #ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) btQuadWordStorage ATTRIBUTE_ALIGNED16(class) btQuadWord
#else #else
class btQuadWordStorage class btQuadWord
#endif #endif
{ {
protected: protected:
@@ -45,15 +45,11 @@ public:
{ {
return mVec128; return mVec128;
} }
protected:
#else //__CELLOS_LV2__ __SPU__ #else //__CELLOS_LV2__ __SPU__
btScalar m_floats[4]; btScalar m_floats[4];
#endif //__CELLOS_LV2__ __SPU__ #endif //__CELLOS_LV2__ __SPU__
};
/** @brief The btQuadWord is base-class for vectors, points */
class btQuadWord : public btQuadWordStorage
{
public: public:
@@ -134,11 +130,7 @@ class btQuadWord : public btQuadWordStorage
// :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.)) // :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
{ {
} }
/**@brief Copy constructor */
SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
{
*((btQuadWordStorage*)this) = q;
}
/**@brief Three argument constructor (zeros w) /**@brief Three argument constructor (zeros w)
* @param x Value of x * @param x Value of x
* @param y Value of y * @param y Value of y

1
src/LinearMath/btQuaternion.h
View File

@@ -19,6 +19,7 @@ subject to the following restrictions:
#include "btVector3.h" #include "btVector3.h"
#include "btQuadWord.h"
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
class btQuaternion : public btQuadWord { class btQuaternion : public btQuadWord {

222
src/LinearMath/btVector3.h
View File

@@ -17,47 +17,77 @@ subject to the following restrictions:
#ifndef SIMD__VECTOR3_H #ifndef SIMD__VECTOR3_H
#define SIMD__VECTOR3_H #define SIMD__VECTOR3_H
#include "btQuadWord.h"
#include "btScalar.h"
#include "btScalar.h"
#include "btMinMax.h"
#include <emmintrin.h>
/**@brief btVector3 can be used to represent 3D points and vectors. /**@brief btVector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
* Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
*/ */
class btVector3 : public btQuadWord {
ATTRIBUTE_ALIGNED16(class) btVector3
{
public: public:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
union {
vec_float4 mVec128;
btScalar m_floats[4];
};
public:
vec_float4 get128() const
{
return mVec128;
}
public:
#else //__CELLOS_LV2__ __SPU__
#ifdef WIN32
union {
__m128 mVec128;
btScalar m_floats[4];
};
SIMD_FORCE_INLINE __m128 get128() const
{
return mVec128;
}
SIMD_FORCE_INLINE void set128(__m128 v128)
{
mVec128 = v128;
}
#else
btScalar m_floats[4];
#endif
#endif //__CELLOS_LV2__ __SPU__
public:
/**@brief No initialization constructor */ /**@brief No initialization constructor */
SIMD_FORCE_INLINE btVector3() {} SIMD_FORCE_INLINE btVector3() {}
/**@brief Constructor from btQuadWordStorage (btVector3 inherits from this so is also valid)
* Note: Vector3 derives from btQuadWordStorage*/
SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q)
: btQuadWord(q)
{
}
/**@brief Constructor from scalars /**@brief Constructor from scalars
* @param x X value * @param x X value
* @param y Y value * @param y Y value
* @param z Z value * @param z Z value
*/ */
SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z) SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)
:btQuadWord(x,y,z,btScalar(0.))
{ {
m_floats[0] = x;
m_floats[1] = y;
m_floats[2] = z;
m_floats[3] = btScalar(0.);
} }
// SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
// : btQuadWord(x,y,z,w)
// {
// }
/**@brief Add a vector to this one /**@brief Add a vector to this one
* @param The vector to add to this one */ * @param The vector to add to this one */
SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v) SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
{ {
m_floats[0] += v.x(); m_floats[1] += v.y(); m_floats[2] += v.z(); m_floats[0] += v.m_floats[0]; m_floats[1] += v.m_floats[1];m_floats[2] += v.m_floats[2];
return *this; return *this;
} }
@@ -66,14 +96,14 @@ public:
* @param The vector to subtract */ * @param The vector to subtract */
SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v)
{ {
m_floats[0] -= v.x(); m_floats[1] -= v.y(); m_floats[2] -= v.z(); m_floats[0] -= v.m_floats[0]; m_floats[1] -= v.m_floats[1];m_floats[2] -= v.m_floats[2];
return *this; return *this;
} }
/**@brief Scale the vector /**@brief Scale the vector
* @param s Scale factor */ * @param s Scale factor */
SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s) SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
{ {
m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[0] *= s; m_floats[1] *= s;m_floats[2] *= s;
return *this; return *this;
} }
@@ -89,7 +119,7 @@ public:
* @param v The other vector in the dot product */ * @param v The other vector in the dot product */
SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
{ {
return m_floats[0] * v.x() + m_floats[1] * v.y() + m_floats[2] * v.z(); return m_floats[0] * v.m_floats[0] + m_floats[1] * v.m_floats[1] +m_floats[2] * v.m_floats[2];
} }
/**@brief Return the length of the vector squared */ /**@brief Return the length of the vector squared */
@@ -148,30 +178,30 @@ public:
SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
{ {
return btVector3( return btVector3(
m_floats[1] * v.z() - m_floats[2] * v.y(), m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
m_floats[2] * v.x() - m_floats[0] * v.z(), m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
m_floats[0] * v.y() - m_floats[1] * v.x()); m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
} }
SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
{ {
return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) + return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) + m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x()); m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
} }
/**@brief Return the axis with the smallest value /**@brief Return the axis with the smallest value
* Note return values are 0,1,2 for x, y, or z */ * Note return values are 0,1,2 for x, y, or z */
SIMD_FORCE_INLINE int minAxis() const SIMD_FORCE_INLINE int minAxis() const
{ {
return m_floats[0] < m_floats[1] ? (m_floats[0] < m_floats[2] ? 0 : 2) : (m_floats[1] < m_floats[2] ? 1 : 2); return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
} }
/**@brief Return the axis with the largest value /**@brief Return the axis with the largest value
* Note return values are 0,1,2 for x, y, or z */ * Note return values are 0,1,2 for x, y, or z */
SIMD_FORCE_INLINE int maxAxis() const SIMD_FORCE_INLINE int maxAxis() const
{ {
return m_floats[0] < m_floats[1] ? (m_floats[1] < m_floats[2] ? 2 : 1) : (m_floats[0] < m_floats[2] ? 2 : 0); return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
} }
SIMD_FORCE_INLINE int furthestAxis() const SIMD_FORCE_INLINE int furthestAxis() const
@@ -187,9 +217,9 @@ public:
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt) SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
{ {
btScalar s = btScalar(1.0) - rt; btScalar s = btScalar(1.0) - rt;
m_floats[0] = s * v0.x() + rt * v1.x(); m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
m_floats[1] = s * v0.y() + rt * v1.y(); m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
m_floats[2] = s * v0.z() + rt * v1.z(); m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
//don't do the unused w component //don't do the unused w component
// m_co[3] = s * v0[3] + rt * v1[3]; // m_co[3] = s * v0[3] + rt * v1[3];
} }
@@ -199,20 +229,86 @@ public:
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */ * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
{ {
return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t, return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.y() - m_floats[1]) * t, m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.z() - m_floats[2]) * t); m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
} }
/**@brief Elementwise multiply this vector by the other /**@brief Elementwise multiply this vector by the other
* @param v The other vector */ * @param v The other vector */
SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v) SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
{ {
m_floats[0] *= v.x(); m_floats[1] *= v.y(); m_floats[2] *= v.z(); m_floats[0] *= v.m_floats[0]; m_floats[1] *= v.m_floats[1];m_floats[2] *= v.m_floats[2];
return *this; return *this;
} }
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar z) {m_floats[2] = z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
SIMD_FORCE_INLINE bool operator==(const btVector3& other) const
{
return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
}
SIMD_FORCE_INLINE bool operator!=(const btVector3& other) const
{
return !(*this == other);
}
/**@brief Set each element to the max of the current values and the values of another btVector3
* @param other The other btVector3 to compare with
*/
SIMD_FORCE_INLINE void setMax(const btVector3& other)
{
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.w());
}
/**@brief Set each element to the min of the current values and the values of another btVector3
* @param other The other btVector3 to compare with
*/
SIMD_FORCE_INLINE void setMin(const btVector3& other)
{
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.w());
}
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3] = 0.f;
}
}; };
@@ -220,34 +316,34 @@ public:
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator+(const btVector3& v1, const btVector3& v2) operator+(const btVector3& v1, const btVector3& v2)
{ {
return btVector3(v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z()); return btVector3(v1.m_floats[0] + v2.m_floats[0], v1.m_floats[1] + v2.m_floats[1], v1.m_floats[2] + v2.m_floats[2]);
} }
/**@brief Return the elementwise product of two vectors */ /**@brief Return the elementwise product of two vectors */
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v1, const btVector3& v2) operator*(const btVector3& v1, const btVector3& v2)
{ {
return btVector3(v1.x() * v2.x(), v1.y() * v2.y(), v1.z() * v2.z()); return btVector3(v1.m_floats[0] * v2.m_floats[0], v1.m_floats[1] * v2.m_floats[1], v1.m_floats[2] * v2.m_floats[2]);
} }
/**@brief Return the difference between two vectors */ /**@brief Return the difference between two vectors */
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v1, const btVector3& v2) operator-(const btVector3& v1, const btVector3& v2)
{ {
return btVector3(v1.x() - v2.x(), v1.y() - v2.y(), v1.z() - v2.z()); return btVector3(v1.m_floats[0] - v2.m_floats[0], v1.m_floats[1] - v2.m_floats[1], v1.m_floats[2] - v2.m_floats[2]);
} }
/**@brief Return the negative of the vector */ /**@brief Return the negative of the vector */
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator-(const btVector3& v) operator-(const btVector3& v)
{ {
return btVector3(-v.x(), -v.y(), -v.z()); return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
} }
/**@brief Return the vector scaled by s */ /**@brief Return the vector scaled by s */
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btScalar& s) operator*(const btVector3& v, const btScalar& s)
{ {
return btVector3(v.x() * s, v.y() * s, v.z() * s); return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
} }
/**@brief Return the vector scaled by s */ /**@brief Return the vector scaled by s */
@@ -269,7 +365,7 @@ operator/(const btVector3& v, const btScalar& s)
SIMD_FORCE_INLINE btVector3 SIMD_FORCE_INLINE btVector3
operator/(const btVector3& v1, const btVector3& v2) operator/(const btVector3& v1, const btVector3& v2)
{ {
return btVector3(v1.x() / v2.x(),v1.y() / v2.y(),v1.z() / v2.z()); return btVector3(v1.m_floats[0] / v2.m_floats[0],v1.m_floats[1] / v2.m_floats[1],v1.m_floats[2] / v2.m_floats[2]);
} }
/**@brief Return the dot product between two vectors */ /**@brief Return the dot product between two vectors */
@@ -325,11 +421,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
return v1.lerp(v2, t); return v1.lerp(v2, t);
} }
/**@brief Test if each element of the vector is equivalent */
SIMD_FORCE_INLINE bool operator==(const btVector3& p1, const btVector3& p2)
{
return p1.x() == p2.x() && p1.y() == p2.y() && p1.z() == p2.z();
}
SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
{ {
@@ -404,7 +496,7 @@ public:
if (m_floats[2] > maxVal) if (m_floats[2] > maxVal)
{ {
maxIndex = 2; maxIndex = 2;
maxVal = m_floats[2]; maxVal =m_floats[2];
} }
if (m_floats[3] > maxVal) if (m_floats[3] > maxVal)
{ {
@@ -437,7 +529,7 @@ public:
if (m_floats[2] < minVal) if (m_floats[2] < minVal)
{ {
minIndex = 2; minIndex = 2;
minVal = m_floats[2]; minVal =m_floats[2];
} }
if (m_floats[3] < minVal) if (m_floats[3] < minVal)
{ {
@@ -455,6 +547,40 @@ public:
return absolute4().maxAxis4(); return absolute4().maxAxis4();
} }
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
/* void getValue(btScalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] =m_floats[2];
}
*/
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
}
}; };