btScalar btHingeConstraint::getHingeAngle()
{
const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
return m_referenceSign * angle;
}
void btSliderConstraint::testAngLimits(void)
{
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;
}
}
} // btSliderConstraint::testAngLimits()
void btConeTwistConstraint::calcAngleInfo()
{
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
btScalar swx=btScalar(0.),swy = btScalar(0.);
btScalar thresh = btScalar(10.);
btScalar fact;
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing1 *= fact;
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = btAtan2Fast(swy, swx);
fact = (swy*swy + swx*swx) * thresh * thresh;
fact = fact / (fact + btScalar(1.0));
swing2 *= fact;
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
m_twistAngle = twist;
// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
}
else if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
}
}
}
void btConeTwistConstraint::buildJacobian()
{
m_appliedImpulse = btScalar(0.);
//set bias, sign, clear accumulator
m_swingCorrection = btScalar(0.);
m_twistLimitSign = btScalar(0.);
m_solveTwistLimit = false;
m_solveSwingLimit = false;
m_accTwistLimitImpulse = btScalar(0.);
m_accSwingLimitImpulse = btScalar(0.);
if (!m_angularOnly)
{
btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
btVector3 relPos = pivotBInW - pivotAInW;
btVector3 normal[3];
if (relPos.length2() > SIMD_EPSILON)
{
normal[0] = relPos.normalized();
}
else
{
normal[0].setValue(btScalar(1.0),0,0);
}
btPlaneSpace1(normal[0], normal[1], normal[2]);
for (int i=0;i<3;i++)
{
new (&m_jac[i]) btJacobianEntry(
m_rbA.getCenterOfMassTransform().getBasis().transpose(),
m_rbB.getCenterOfMassTransform().getBasis().transpose(),
pivotAInW - m_rbA.getCenterOfMassPosition(),
pivotBInW - m_rbB.getCenterOfMassPosition(),
normal[i],
m_rbA.getInvInertiaDiagLocal(),
m_rbA.getInvMass(),
m_rbB.getInvInertiaDiagLocal(),
m_rbB.getInvMass());
}
}
btVector3 b1Axis1,b1Axis2,b1Axis3;
btVector3 b2Axis1,b2Axis2;
b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
// Get Frame into world space
if (m_swingSpan1 >= btScalar(0.05f))
{
b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
}
if (m_swingSpan2 >= btScalar(0.05f))
{
b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
}
btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
if (EllipseAngle > 1.0f)
{
m_swingCorrection = EllipseAngle-1.0f;
m_solveSwingLimit = true;
// Calculate necessary axis & factors
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
m_swingAxis.normalize();
btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
m_swingAxis *= swingAxisSign;
m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
}
// Twist limits
if (m_twistSpan >= btScalar(0.))
{
btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
if (twist <= -m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = -(twist + m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_twistAxis *= -1.0f;
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
} else
if (twist > m_twistSpan*lockedFreeFactor)
{
m_twistCorrection = (twist - m_twistSpan);
m_solveTwistLimit = true;
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
m_twistAxis.normalize();
m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
}
}
}
void 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()
