//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
btRigidBody& body2, const btVector3& pos2,
btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
(void)timeStep;
(void)distance;
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1));
if (normalLenSqr > btScalar(1.1))
{
impulse = btScalar(0.);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
btVector3 vel = vel1 - vel2;
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
body2.getInvInertiaDiagLocal(),body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity(
body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity());
btScalar a;
a=jacDiagABInv;
rel_vel = normal.dot(vel);
//todo: move this into proper structure
btScalar contactDamping = btScalar(0.2);
#ifdef ONLY_USE_LINEAR_MASS
btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
impulse = - contactDamping * rel_vel * massTerm;
#else
btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
#endif
}
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
{
//calculate transforms
m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
m_realPivotAInW = m_calculatedTransformA.getOrigin();
m_realPivotBInW = m_calculatedTransformB.getOrigin();
m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
m_delta = m_realPivotBInW - m_realPivotAInW;
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
btVector3 normalWorld;
int i;
//linear part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacLin[i]) btJacobianEntry(
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
m_relPosA,
m_relPosB,
normalWorld,
rbA.getInvInertiaDiagLocal(),
rbA.getInvMass(),
rbB.getInvInertiaDiagLocal(),
rbB.getInvMass()
);
m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
m_depth[i] = m_delta.dot(normalWorld);
}
testLinLimits();
// angular part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacAng[i]) btJacobianEntry(
normalWorld,
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
rbA.getInvInertiaDiagLocal(),
rbB.getInvInertiaDiagLocal()
);
}
testAngLimits();
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
// clear accumulator for motors
m_accumulatedLinMotorImpulse = btScalar(0.0);
m_accumulatedAngMotorImpulse = btScalar(0.0);
}
//bilateral constraint between two dynamic objects
void RaycastCar::resolveSingleBilateral(btRigidBody & body1,
const btVector3 & pos1,
btRigidBody & body2,
const btVector3 & pos2,
const btVector3 & normal,
btScalar & impulse)
{
btScalar normalLenSqr = normal.length2();
btAssert(btFabs(normalLenSqr) < btScalar(1.1f));
if (normalLenSqr > btScalar(1.1f))
{
impulse = btScalar(0.0f);
return;
}
btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
body2.getCenterOfMassTransform().getBasis().transpose(),
rel_pos1,
rel_pos2,
normal,
body1.getInvInertiaDiagLocal(),
body1.getInvMass(),
body2.getInvInertiaDiagLocal(),
body2.getInvMass());
btScalar jacDiagAB = jac.getDiagonal();
btScalar jacDiagABInv = btScalar(1.0f) / jacDiagAB;
btScalar rel_vel = jac.getRelativeVelocity
(body1.getLinearVelocity(),
body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(),
body2.getLinearVelocity(),
body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity());
btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
{
//calculate transforms
m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
m_realPivotAInW = m_calculatedTransformA.getOrigin();
m_realPivotBInW = m_calculatedTransformB.getOrigin();
m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
m_delta = m_realPivotBInW - m_realPivotAInW;
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
btVector3 normalWorld;
int i;
//linear part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacLin[i]) btJacobianEntry(
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
m_relPosA,
m_relPosB,
normalWorld,
rbA.getInvInertiaDiagLocal(),
rbA.getInvMass(),
rbB.getInvInertiaDiagLocal(),
rbB.getInvMass()
);
m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
m_depth[i] = m_delta.dot(normalWorld);
}
m_solveLinLim = false;
if(m_lowerLinLimit <= m_upperLinLimit)
{
if(m_depth[0] > m_upperLinLimit)
{
m_depth[0] -= m_upperLinLimit;
m_solveLinLim = true;
}
else if(m_depth[0] < m_lowerLinLimit)
{
m_depth[0] -= m_lowerLinLimit;
m_solveLinLim = true;
}
else
{
m_depth[0] = btScalar(0.);
}
}
else
{
m_depth[0] = btScalar(0.);
}
// angular part
for(i = 0; i < 3; i++)
{
normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
new (&m_jacAng[i]) btJacobianEntry(
normalWorld,
rbA.getCenterOfMassTransform().getBasis().transpose(),
rbB.getCenterOfMassTransform().getBasis().transpose(),
rbA.getInvInertiaDiagLocal(),
rbB.getInvInertiaDiagLocal()
);
}
m_angDepth = btScalar(0.);
m_solveAngLim = false;
if(m_lowerAngLimit <= m_upperAngLimit)
{
const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
if(rot < m_lowerAngLimit)
{
m_angDepth = rot - m_lowerAngLimit;
m_solveAngLim = true;
}
else if(rot > m_upperAngLimit)
{
m_angDepth = rot - m_upperAngLimit;
m_solveAngLim = true;
}
}
btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
} // btSliderConstraint::buildJacobianInt()
