Big work-in-progress refactoring of the constraint solver:
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
@@ -21,100 +21,197 @@ 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()
|
||||
{
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
m_appliedImpulse = btScalar(0.);
|
||||
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
new (&m_jac[i]) btJacobianEntry(
|
||||
|
||||
m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
|
||||
m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
|
||||
normal,
|
||||
m_rbA.getInvInertiaDiagLocal(),
|
||||
m_rbA.getInvMass(),
|
||||
m_rbB.getInvInertiaDiagLocal(),
|
||||
m_rbB.getInvMass());
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void btPoint2PointConstraint::solveConstraint(btScalar timeStep)
|
||||
|
||||
void btPoint2PointConstraint::getInfo1 (btConstraintInfo1* info)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
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);
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
//retrieve matrices
|
||||
btTransform body0_trans;
|
||||
body0_trans = m_rbA.getCenterOfMassTransform();
|
||||
btTransform body1_trans;
|
||||
body1_trans = m_rbB.getCenterOfMassTransform();
|
||||
|
||||
// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
// anchor points in global coordinates with respect to body PORs.
|
||||
int s = info->rowskip;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
// set jacobian
|
||||
info->m_J1linearAxis[0] = 1;
|
||||
info->m_J1linearAxis[s+1] = 1;
|
||||
info->m_J1linearAxis[2*s+2] = 1;
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
btVector3 a1,a2;
|
||||
|
||||
/*
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
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;
|
||||
*/
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
|
||||
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
|
||||
|
||||
btScalar impulseClamp = m_setting.m_impulseClamp;
|
||||
a2 = body1_trans.getBasis()*getPivotInB();
|
||||
//dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
|
||||
//dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
|
||||
dCROSSMAT (info->m_J2angularAxis,a2,s,+,-);
|
||||
|
||||
|
||||
|
||||
// set right hand side
|
||||
btScalar k = info->fps * info->erp;
|
||||
int j;
|
||||
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
info->m_constraintError[j*s] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
|
||||
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
|
||||
}
|
||||
|
||||
btScalar impulseClamp = m_setting.m_impulseClamp;
|
||||
for (j=0; j<3; j++)
|
||||
{
|
||||
if (impulseClamp > 0)
|
||||
{
|
||||
if (impulse < -impulseClamp)
|
||||
impulse = -impulseClamp;
|
||||
if (impulse > impulseClamp)
|
||||
impulse = impulseClamp;
|
||||
info->m_lowerLimit[j*s] = -impulseClamp;
|
||||
info->m_upperLimit[j*s] = impulseClamp;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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());
|
||||
|
||||
void btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
|
||||
{
|
||||
if (m_useSolveConstraintObsolete)
|
||||
{
|
||||
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
|
||||
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
|
||||
|
||||
|
||||
btVector3 normal(0,0,0);
|
||||
|
||||
normal[i] = 0;
|
||||
|
||||
// btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
|
||||
// btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
btVector3 vel1,vel2;
|
||||
bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
|
||||
bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
/*
|
||||
//velocity error (first order error)
|
||||
btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
|
||||
m_rbB.getLinearVelocity(),angvelB);
|
||||
*/
|
||||
|
||||
//positional error (zeroth order error)
|
||||
btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
|
||||
btScalar impulse = depth*m_setting.m_tau/timeStep * jacDiagABInv - m_setting.m_damping * rel_vel * jacDiagABInv;
|
||||
|
||||
btScalar impulseClamp = m_setting.m_impulseClamp;
|
||||
if (impulseClamp > 0)
|
||||
{
|
||||
if (impulse < -impulseClamp)
|
||||
impulse = -impulseClamp;
|
||||
if (impulse > impulseClamp)
|
||||
impulse = impulseClamp;
|
||||
}
|
||||
|
||||
m_appliedImpulse+=impulse;
|
||||
btVector3 impulse_vector = normal * impulse;
|
||||
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
|
||||
btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
|
||||
bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
|
||||
bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
|
||||
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user