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

1
Demos/OpenGL/DemoApplication.h
View File

@@ -102,6 +102,7 @@ public:
}
void setOrthographicProjection();
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_facc.setValue(0,0,0,0);
body->m_tacc.setValue(0,0,0,0);
body->m_facc.setValue(0,0,0);
body->m_tacc.setValue(0,0,0);
body->m_linearVelocity = orgBody->getLinearVelocity();
body->m_angularVelocity = orgBody->getAngularVelocity();

34
Extras/quickstep/btOdeTypedJoint.cpp
View File

@@ -96,16 +96,6 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{
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;
@@ -113,23 +103,8 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{
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.
int s = info->rowskip;
// set jacobian
@@ -160,16 +135,17 @@ void OdeP2PJoint::GetInfo2(Info2 *info)
{
for (int j=0; j<3; j++)
{
info->m_constraintError[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
a1[j] - body0_trans.getOrigin()[j]);
info->m_constraintError[j] = 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]);
}
}
else
{
for (int j=0; j<3; j++)
{
info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
body0_trans.getOrigin()[j]);
info->m_constraintError[j] = k * (p2pconstraint->getPivotInB()[j] - a1[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)
{
case 0:
plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.));
plane[3] = -halfExtents.x();
plane.setValue(btScalar(1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 1:
plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.));
plane[3] = -halfExtents.x();
plane.setValue(btScalar(-1.),btScalar(0.),btScalar(0.),-halfExtents.x());
break;
case 2:
plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.));
plane[3] = -halfExtents.y();
plane.setValue(btScalar(0.),btScalar(1.),btScalar(0.),-halfExtents.y());
break;
case 3:
plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.));
plane[3] = -halfExtents.y();
plane.setValue(btScalar(0.),btScalar(-1.),btScalar(0.),-halfExtents.y());
break;
case 4:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.));
plane[3] = -halfExtents.z();
plane.setValue(btScalar(0.),btScalar(0.),btScalar(1.),-halfExtents.z());
break;
case 5:
plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.));
plane[3] = -halfExtents.z();
plane.setValue(btScalar(0.),btScalar(0.),btScalar(-1.),-halfExtents.z());
break;
default:
assert(0);

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

@@ -23,7 +23,8 @@ Written by: Marcus Hennix
#include <new>
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,
const btTransform& rbAFrame,const btTransform& 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_swingSpan2 = btScalar(1e30);
@@ -46,7 +48,8 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
m_angularOnly(false)
m_angularOnly(false),
m_useSolveConstraintObsolete(true)
{
m_rbBFrame = m_rbAFrame;
@@ -61,7 +64,28 @@ 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()
{
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
@@ -199,10 +223,12 @@ void btConeTwistConstraint::buildJacobian()
}
}
}
}
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();
@@ -214,8 +240,10 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep)
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
@@ -229,16 +257,21 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep)
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());
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
}
}
{
///solve angular part
const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
// solve swing limit
if (m_solveSwingLimit)
@@ -253,9 +286,8 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep)
btVector3 impulse = m_swingAxis * impulseMag;
m_rbA.applyTorqueImpulse(impulse);
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
@@ -271,12 +303,13 @@ void btConeTwistConstraint::solveConstraint(btScalar timeStep)
btVector3 impulse = m_twistAxis * impulseMag;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
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_solveSwingLimit;
bool m_useSolveConstraintObsolete;
public:
@@ -74,7 +75,12 @@ public:
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);

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

@@ -22,7 +22,9 @@ enum btSolverMode
SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4,
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
@@ -72,7 +74,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_splitImpulsePenetrationThreshold = -0.02f;
m_linearSlop = btScalar(0.0);
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
}
};

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

@@ -26,6 +26,35 @@ http://gimpact.sf.net
#include <new>
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true),
m_useSolveConstraintObsolete(true)
{
}
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useSolveConstraintObsolete(true)
{
}
#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]; \
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
@@ -110,7 +139,7 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value)
btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
btRigidBody * body0, btRigidBody * body1)
btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
{
if (needApplyTorques()==false) return 0.0f;
@@ -127,11 +156,14 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
maxMotorForce *= timeStep;
// current velocity difference
btVector3 vel_diff = body0->getAngularVelocity();
if (body1)
{
vel_diff -= body1->getAngularVelocity();
}
btVector3 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 vel_diff;
vel_diff = angVelA-angVelB;
@@ -174,13 +206,14 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp = clippedMotorImpulse * axis;
//body0->applyTorqueImpulse(motorImp);
//body1->applyTorqueImpulse(-motorImp);
bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
body0->applyTorqueImpulse(motorImp);
if (body1) body1->applyTorqueImpulse(-motorImp);
return clippedMotorImpulse;
@@ -193,8 +226,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits(
btScalar btTranslationalLimitMotor::solveLinearAxis(
btScalar timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos)
@@ -206,8 +239,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel1;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
btVector3 vel2;
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel = axis_normal_on_a.dot(vel);
@@ -260,29 +295,23 @@ btScalar btTranslationalLimitMotor::solveLinearAxis(
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1.applyImpulse( impulse_vector, rel_pos1);
body2.applyImpulse(-impulse_vector, rel_pos2);
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
return normalImpulse;
}
//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
btGeneric6DofConstraint::btGeneric6DofConstraint()
:btTypedConstraint(D6_CONSTRAINT_TYPE),
m_useLinearReferenceFrameA(true)
{
}
btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
{
}
@@ -377,6 +406,8 @@ bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
}
void btGeneric6DofConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
{
// Clear accumulated impulses for the next simulation step
@@ -429,12 +460,279 @@ void btGeneric6DofConstraint::buildJacobian()
}
}
}
}
void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
info->m_numConstraintRows = 0;
info->nub = 0;
} else
{
//prepare constraint
calculateTransforms();
info->m_numConstraintRows = 3;
info->nub = 3;//??
//test angular limits
for (int i=0;i<3 ;i++ )
{
//if(i==2) continue;
if(testAngularLimitMotor(i))
{
info->m_numConstraintRows++;
}
}
}
}
void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
{
btAssert(!m_useSolveConstraintObsolete);
int row = setLinearLimits(info);
setAngularLimits(info, row);
}
int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
{
btGeneric6DofConstraint * d6constraint = this;
//retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// anchor points in global coordinates with respect to body PORs.
int s = info->rowskip;
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2;
a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
// set right hand side
btScalar k = info->fps * info->erp;
for (int j=0; j<3; j++)
{
info->m_constraintError[s*j] = k * (a2[j] + body1_trans.getOrigin()[j] -
a1[j] - body0_trans.getOrigin()[j]);
}
return 3;
}
void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
/*! \pre testLimitValue must be called on limot*/
int bt_get_limit_motor_info2(
btRotationalLimitMotor * limot,
btRigidBody * body0, btRigidBody * body1,
btTypedConstraint::btConstraintInfo2 *info, int row, btVector3& ax1, int rotational)
{
int srow = row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered = limot->m_enableMotor;
int limit = limot->m_currentLimit;
if (powered || limit)
{
btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
btScalar *J2 = rotational ? info->m_J2angularAxis : 0;//info->m_J2linearAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if (body1)
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
if (!rotational && body1)
{
btVector3 c;
c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
-body0->getCenterOfMassPosition()[0]);
c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
-body0->getCenterOfMassPosition()[1]);
c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
-body0->getCenterOfMassPosition()[2]);
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
if (powered)
{
info->cfm[srow] = 0.0f;//limot->m_normalCFM;
if (! limit)
{
info->m_constraintError[srow] = limot->m_targetVelocity;
info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
info->m_upperLimit[srow] = limot->m_maxMotorForce;
}
}
if (limit)
{
btScalar k = info->fps * limot->m_ERP;
info->m_constraintError[srow] = -k * limot->m_currentLimitError;
info->cfm[srow] = 0.0f;//limot->m_stopCFM;
if (limot->m_loLimit == limot->m_hiLimit)
{
// limited low and high simultaneously
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
if (limit == 1)
{
// low limit
info->m_lowerLimit[srow] = 0;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
else
{
// high limit
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = 0;
}
// deal with bounce
if (limot->m_bounce > 0)
{
// calculate joint velocity
btScalar vel;
if (rotational)
{
vel = body0->getAngularVelocity().dot(ax1);
if (body1)
vel -= body1->getAngularVelocity().dot(ax1);
}
else
{
vel = body0->getLinearVelocity().dot(ax1);
if (body1)
vel -= body1->getLinearVelocity().dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
// low limit
if (vel < 0)
{
btScalar newc = -limot->m_bounce* vel;
if (newc > info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
else
{
// high limit - all those computations are reversed
if (vel > 0)
{
btScalar newc = -limot->m_bounce * vel;
if (newc < info->m_constraintError[srow])
info->m_constraintError[srow] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset)
{
btGeneric6DofConstraint * d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i=0;i<3 ;i++ )
{
//if(i==2) continue;
if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
{
btVector3 axis = d6constraint->getAxis(i);
row += bt_get_limit_motor_info2(
d6constraint->getRotationalLimitMotor(i),
&m_rbA,
&m_rbB,
info,row,axis,1);
}
}
return row;
}
///////////////////limit motor support
void btGeneric6DofConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
{
m_timeStep = timeStep;
//calculateTransforms();
@@ -462,8 +760,8 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
m_linearLimits.solveLinearAxis(
m_timeStep,
jacDiagABInv,
m_rbA,pointInA,
m_rbB,pointInB,
m_rbA,bodyA,pointInA,
m_rbB,bodyB,pointInB,
i,linear_axis, m_AnchorPos);
}
@@ -482,7 +780,8 @@ void btGeneric6DofConstraint::solveConstraint(btScalar timeStep)
angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,bodyA,&m_rbB,bodyB);
}
}
}
}

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

@@ -110,7 +110,7 @@ public:
int testLimitValue(btScalar test_value);
//! 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 timeStep,
btScalar jacDiagABInv,
btRigidBody& body1,const btVector3 &pointInA,
btRigidBody& body2,const btVector3 &pointInB,
btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
int limit_index,
const btVector3 & axis_normal_on_a,
const btVector3 & anchorPos);
@@ -262,6 +262,9 @@ protected:
}
int setAngularLimits(btConstraintInfo2 *info, int row_offset);
int setLinearLimits(btConstraintInfo2 *info);
void buildLinearJacobian(
btJacobianEntry & jacLinear,const btVector3 & normalWorld,
@@ -276,6 +279,10 @@ protected:
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();
@@ -330,7 +337,11 @@ public:
//! performs Jacobian calculation, and also calculates angle differences and axis
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);

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

@@ -19,11 +19,14 @@ subject to the following restrictions:
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btMinMax.h"
#include <new>
#include "btSolverBody.h"
btHingeConstraint::btHingeConstraint()
: 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)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{
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)
: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
@@ -115,7 +119,8 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
const btTransform& rbAFrame, const btTransform& rbBFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
m_angularOnly(false),
m_enableAngularMotor(false)
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{
// flip axis
m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
@@ -136,7 +141,8 @@ m_enableAngularMotor(false)
btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
m_angularOnly(false),
m_enableAngularMotor(false)
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(true)
{
///not providing rigidbody B means implicitly using worldspace for body B
@@ -157,6 +163,8 @@ m_enableAngularMotor(false)
}
void btHingeConstraint::buildJacobian()
{
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
@@ -258,8 +266,32 @@ void btHingeConstraint::buildJacobian()
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)
{
///for backwards compatibility during the transition to 'getInfo/getInfo2'
if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
@@ -273,8 +305,9 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
btVector3 vel1,vel2;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
for (int i=0;i<3;i++)
@@ -289,8 +322,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
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());
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);
}
}
@@ -302,8 +337,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
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 angVelA;
bodyA.getAngularVelocity(angVelA);
btVector3 angVelB;
bodyB.getAngularVelocity(angVelB);
btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
@@ -312,6 +349,8 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
btVector3 velrelOrthog = angAorthog-angBorthog;
{
//solve orthogonal angular velocity correction
btScalar relaxation = btScalar(1.);
btScalar len = velrelOrthog.length();
@@ -321,7 +360,11 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
getRigidBodyB().computeAngularImpulseDenominator(normal);
// scale for mass and relaxation
velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
//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
@@ -332,11 +375,16 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
btVector3 normal2 = angularError.normalized();
btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
getRigidBodyB().computeAngularImpulseDenominator(normal2);
angularError *= (btScalar(1.)/denom2) * relaxation;
//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));
}
m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
// solve limit
if (m_solveLimit)
@@ -351,9 +399,10 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
impulseMag = m_accLimitImpulse - temp;
btVector3 impulse = axisA * impulseMag * m_limitSign;
m_rbA.applyTorqueImpulse(impulse);
m_rbB.applyTorqueImpulse(-impulse);
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));
}
}
@@ -375,11 +424,12 @@ void btHingeConstraint::solveConstraint(btScalar timeStep)
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
btVector3 motorImp = clippedMotorImpulse * axisA;
m_rbA.applyTorqueImpulse(motorImp+angularLimit);
m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
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_enableAngularMotor;
bool m_solveLimit;
bool m_useSolveConstraintObsolete;
public:
@@ -73,7 +74,11 @@ public:
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);

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

@@ -21,24 +21,29 @@ subject to the following restrictions:
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)
: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)
: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()
{
if (m_useSolveConstraintObsolete)
{
m_appliedImpulse = btScalar(0.);
@@ -58,10 +63,96 @@ void btPoint2PointConstraint::buildJacobian()
m_rbB.getInvMass());
normal[i] = 0;
}
}
}
void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
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);
//retrieve matrices
btTransform body0_trans;
body0_trans = m_rbA.getCenterOfMassTransform();
btTransform body1_trans;
body1_trans = m_rbB.getCenterOfMassTransform();
// anchor points in global coordinates with respect to body PORs.
int s = info->rowskip;
// set jacobian
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[s+1] = 1;
info->m_J1linearAxis[2*s+2] = 1;
btVector3 a1,a2;
a1 = body0_trans.getBasis()*getPivotInA();
//dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
dCROSSMAT (info->m_J1angularAxis,a1,s,-,+);
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
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)
{
info->m_lowerLimit[j*s] = -impulseClamp;
info->m_upperLimit[j*s] = impulseClamp;
}
}
}
void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
{
if (m_useSolveConstraintObsolete)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
@@ -82,8 +173,9 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
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 vel1,vel2;
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
btVector3 vel = vel1 - vel2;
btScalar rel_vel;
@@ -111,12 +203,17 @@ void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
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());
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;
}
}
}
void btPoint2PointConstraint::updateRHS(btScalar timeStep)
{

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

@@ -50,6 +50,9 @@ public:
public:
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btConstraintSetting m_setting;
btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB);
@@ -60,8 +63,12 @@ public:
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);

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

@@ -37,24 +37,68 @@ subject to the following restrictions:
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_btSeed2(0)
{
}
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
{
}
// Project Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
#ifdef USE_SIMD
#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;
deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm;
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);
btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
__m128 result = _mm_mul_ps( vec0, vec1);
return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
}
#endif//USE_SIMD
// 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 += deltaVel2Dotn*c.m_jacDiagABInv;
btScalar sum = c.m_appliedImpulse + deltaImpulse;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
@@ -69,7 +113,70 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBod
{
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);
}
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)
{
btRigidBody* rb = btRigidBody::upcast(collisionObject);
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
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_linearVelocity = rb->getLinearVelocity();
solverBody->m_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor();
} 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_linearVelocity.setValue(0,0,0);
solverBody->m_originalBody = 0;
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;
}
//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)
{
btRigidBody* body0=btRigidBody::upcast(colObj0);
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_solverBodyIdA = solverBodyIdA;
@@ -310,8 +272,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
solverConstraint.m_friction = cp.m_combinedFriction;
solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = btScalar(0.);
solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_appliedImpulse = 0.f;
// solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f;
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -355,10 +317,57 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
body1->getInvInertiaDiagLocal(),
body1->getInvMass());
#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;
}
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)
{
@@ -372,16 +381,149 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
// printf("empty\n");
return 0.f;
}
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0;
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0);
//btRigidBody* rb0=0,*rb1=0;
//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;
btPersistentManifold* manifold = 0;
btCollisionObject* colObj0=0,*colObj1=0;
for (i=0;i<numManifolds;i++)
{
@@ -394,49 +536,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (manifold->getNumContacts())
{
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);
}
solverBodyIdA = getOrInitSolverBody(*colObj0);
solverBodyIdB = getOrInitSolverBody(*colObj1);
}
btVector3 rel_pos1;
@@ -463,10 +564,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar rel_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* rb1 = btRigidBody::upcast(colObj1);
@@ -555,7 +656,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_appliedImpulse = 0.f;
}
solverConstraint.m_appliedPushImpulse = 0.f;
// solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar rel_vel;
@@ -569,13 +670,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar positionalError = 0.f;
positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
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_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e30f;
solverConstraint.m_upperLimit = 1e10f;
}
@@ -594,7 +694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (1)
{
solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
if (!cp.m_lateralFrictionInitialized)
{
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
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_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
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;
}
}
@@ -632,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
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)
{
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)
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
@@ -672,6 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btContactSolverInfo info = infoGlobal;
if (1)
{
int j;
for (j=0;j<numConstraints;j++)
@@ -681,8 +782,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
}
}
int numConstraintPool = m_tmpSolverConstraintPool.size();
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
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*/)
{
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...
int iteration;
@@ -735,82 +837,96 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
}
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
///solve all joint constraints, using SIMD, if available
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
}
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
///todo: use solver bodies, so we don't need to copy from/to btRigidBody
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
{
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
}
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
{
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
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);
}
constraint->solveConstraint(infoGlobal.m_timeStep);
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
{
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
}
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
{
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
}
}
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
///solve all contact constraints using SIMD, if available
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRow(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
{
int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
totalImpulse);
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;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
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);
}
if (infoGlobal.m_splitImpulse)
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];
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
{
int numPoolConstraints = m_tmpSolverConstraintPool.size();
int j;
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_tmpSolverConstraintPool[m_orderTmpConstraintPool[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;
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
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;
}
@@ -830,23 +946,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
solveGroupCacheFriendlySetup( 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;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{
pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
}
//do a callback here?
}
if (infoGlobal.m_splitImpulse)
@@ -865,8 +981,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
m_tmpSolverBodyPool.resize(0);
m_tmpSolverConstraintPool.resize(0);
m_tmpSolverFrictionConstraintPool.resize(0);
m_tmpSolverContactConstraintPool.resize(0);
m_tmpSolverNonContactConstraintPool.resize(0);
m_tmpSolverContactFrictionConstraintPool.resize(0);
return 0.f;
}

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

@@ -23,6 +23,7 @@ class btIDebugDraw;
#include "btSolverConstraint.h"
///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
///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<btSolverConstraint> m_tmpSolverConstraintPool;
btAlignedObjectArray<btSolverConstraint> m_tmpSolverFrictionConstraintPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
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);
@@ -52,17 +53,16 @@ protected:
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo);
void resolveSingleConstraintRow(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(btCollisionObject& body);
void resolveSingleFrictionCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& contactConstraint,
const btContactSolverInfo& solverInfo,
btScalar appliedNormalImpulse);
void resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& contactConstraint);
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:

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

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

@@ -126,7 +126,13 @@ public:
btSliderConstraint();
// overrides
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
const btRigidBody& getRigidBodyA() const { return m_rbA; }
const btRigidBody& getRigidBodyB() const { return m_rbB; }
@@ -202,7 +208,7 @@ public:
btScalar getAngDepth() { return m_angDepth; }
// internal
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
void calculateTransforms(void);
void testLinLimits(void);

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

@@ -23,193 +23,157 @@ class btRigidBody;
#include "LinearMath/btAlignedAllocator.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();
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
SIMD_FORCE_INLINE btSimdScalar()
{
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 void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
SIMD_FORCE_INLINE btSimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
//if (m_invMass)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
m_linearVelocity += linearComponent*impulseMagnitude;
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
}
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
:m_vec128(v128)
{
//if (m_invMass)
{
m_pushVelocity += linearComponent*impulseMagnitude;
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
}
union
{
__m128 m_vec128;
float m_floats[4];
int m_ints[4];
};
SIMD_FORCE_INLINE __m128 get128()
{
return m_vec128;
}
void writebackVelocity()
SIMD_FORCE_INLINE const __m128 get128() const
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity);
//m_originalBody->setCompanionId(-1);
}
return m_vec128;
}
void writebackVelocity(btScalar timeStep)
SIMD_FORCE_INLINE void set128(__m128 v128)
{
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);
}
m_vec128 = v128;
}
void readVelocity()
SIMD_FORCE_INLINE operator __m128()
{
if (m_invMass)
{
m_linearVelocity = m_originalBody->getLinearVelocity();
m_angularVelocity = m_originalBody->getAngularVelocity();
return m_vec128;
}
SIMD_FORCE_INLINE operator const __m128() const
{
return m_vec128;
}
SIMD_FORCE_INLINE operator float() const
{
return m_floats[0];
}
};
///@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
#define btSimdScalar btScalar
#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
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btMatrix3x3 m_worldInvInertiaTensor;
btVector3 m_angularVelocity1;
btVector3 m_linearVelocity1;
btVector3 m_deltaAngularVelocity;
btVector3 m_deltaLinearVelocity;
btVector3 m_centerOfMassPosition;
btVector3 m_pushVelocity;
btVector3 m_turnVelocity;
float m_angularFactor;
float m_invMass;
float m_friction;
btVector3 m_deltaAngularVelocity;
btScalar m_angularFactor;
btScalar m_invMass;
btScalar m_friction;
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 internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const
{
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_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()
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//m_originalBody->setCompanionId(-1);
}
}
*/
void writebackVelocity(btScalar timeStep)
void writebackVelocity(btScalar timeStep=0)
{
if (m_invMass)
{
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_angularVelocity1+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->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
//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

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

@@ -21,18 +21,16 @@ class btRigidBody;
#include "LinearMath/btMatrix3x3.h"
#include "btJacobianEntry.h"
#include <emmintrin.h>
//#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.
ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
#ifdef _USE_JACOBIAN
btJacobianEntry m_jac;
#endif
btVector3 m_relpos1CrossNormal;
btVector3 m_contactNormal;
@@ -41,8 +39,8 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btVector3 m_angularComponentB;
mutable btScalar m_appliedPushImpulse;
mutable btScalar m_appliedImpulse;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
int m_solverBodyIdA;
int m_solverBodyIdB;
@@ -52,8 +50,6 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btScalar m_jacDiagABInv;
btScalar m_penetration;
int m_constraintType;
int m_frictionIndex;
void* m_originalContactPoint;
@@ -69,9 +65,7 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
};
};
typedef btAlignedObjectArray<btSolverConstraint> btConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H

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

@@ -18,6 +18,8 @@ subject to the following restrictions:
class btRigidBody;
#include "LinearMath/btScalar.h"
#include "btSolverConstraint.h"
struct btSolverBody;
enum btTypedConstraintType
{
@@ -55,12 +57,52 @@ public:
btTypedConstraint(btTypedConstraintType type);
virtual ~btTypedConstraint() {};
btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
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 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
{

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

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

18
src/LinearMath/btQuadWord.h
View File

@@ -24,13 +24,13 @@ subject to the following restrictions:
#include <altivec.h>
#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.
*/
#ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) btQuadWordStorage
ATTRIBUTE_ALIGNED16(class) btQuadWord
#else
class btQuadWordStorage
class btQuadWord
#endif
{
protected:
@@ -45,15 +45,11 @@ public:
{
return mVec128;
}
protected:
#else //__CELLOS_LV2__ __SPU__
btScalar m_floats[4];
#endif //__CELLOS_LV2__ __SPU__
};
/** @brief The btQuadWord is base-class for vectors, points */
class btQuadWord : public btQuadWordStorage
{
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.))
{
}
/**@brief Copy constructor */
SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q)
{
*((btQuadWordStorage*)this) = q;
}
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @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 "btQuadWord.h"
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
class btQuaternion : public btQuadWord {

208
src/LinearMath/btVector3.h
View File

@@ -17,24 +17,56 @@ subject to the following restrictions:
#ifndef 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.
* 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
*/
class btVector3 : public btQuadWord {
ATTRIBUTE_ALIGNED16(class) btVector3
{
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 */
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
* @param x X value
@@ -42,22 +74,20 @@ public:
* @param z Z value
*/
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
* @param The vector to add to this one */
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;
}
@@ -66,7 +96,7 @@ public:
* @param The vector to subtract */
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;
}
/**@brief Scale the vector
@@ -89,7 +119,7 @@ public:
* @param v The other vector in the dot product */
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 */
@@ -148,16 +178,16 @@ public:
SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
{
return btVector3(
m_floats[1] * v.z() - m_floats[2] * v.y(),
m_floats[2] * v.x() - m_floats[0] * v.z(),
m_floats[0] * v.y() - m_floats[1] * v.x());
m_floats[1] * v.m_floats[2] -m_floats[2] * v.m_floats[1],
m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
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
{
return m_floats[0] * (v1.y() * v2.z() - v1.z() * v2.y()) +
m_floats[1] * (v1.z() * v2.x() - v1.x() * v2.z()) +
m_floats[2] * (v1.x() * v2.y() - v1.y() * v2.x());
return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
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
@@ -187,9 +217,9 @@ public:
SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
{
btScalar s = btScalar(1.0) - rt;
m_floats[0] = s * v0.x() + rt * v1.x();
m_floats[1] = s * v0.y() + rt * v1.y();
m_floats[2] = s * v0.z() + rt * v1.z();
m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
//don't do the unused w component
// 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) */
SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const
{
return btVector3(m_floats[0] + (v.x() - m_floats[0]) * t,
m_floats[1] + (v.y() - m_floats[1]) * t,
m_floats[2] + (v.z() - m_floats[2]) * t);
return btVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] -m_floats[2]) * t);
}
/**@brief Elementwise multiply this vector by the other
* @param v The other vector */
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;
}
/**@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
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 */
SIMD_FORCE_INLINE btVector3
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 */
SIMD_FORCE_INLINE btVector3
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 */
SIMD_FORCE_INLINE btVector3
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 */
SIMD_FORCE_INLINE btVector3
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 */
@@ -269,7 +365,7 @@ operator/(const btVector3& v, const btScalar& s)
SIMD_FORCE_INLINE btVector3
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 */
@@ -325,11 +421,7 @@ lerp(const btVector3& v1, const btVector3& v2, const btScalar& 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
{
@@ -455,6 +547,40 @@ public:
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;
}
};