void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
{
//fix the 3 linear degrees of freedom
const btTransform& transA = m_rbA.getCenterOfMassTransform();
const btTransform& transB = m_rbB.getCenterOfMassTransform();
const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
const btMatrix3x3& worldOrnB = m_rbB.getCenterOfMassTransform().getBasis();
info->m_J1linearAxis[0] = 1;
info->m_J1linearAxis[info->rowskip+1] = 1;
info->m_J1linearAxis[2*info->rowskip+2] = 1;
btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
btVector3 a1neg = -a1;
a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
if (info->m_J2linearAxis)
{
info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[info->rowskip+1] = -1;
info->m_J2linearAxis[2*info->rowskip+2] = -1;
}
btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
{
btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
}
// set right hand side for the linear dofs
btScalar k = info->fps * info->erp;
btVector3 linearError = k*(a2+worldPosB-a1-worldPosA);
int j;
for (j=0; j<3; j++)
{
info->m_constraintError[j*info->rowskip] = linearError[j];
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
btVector3 target;
btScalar x = ivB.dot(ivA);
btScalar y = ivB.dot(jvA);
btScalar z = ivB.dot(kvA);
btVector3 swingAxis(0,0,0);
{
if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
{
swingAxis = -ivB.cross(ivA);
}
}
btVector3 vTwist(1,0,0);
// compute rotation of A wrt B (in constraint space)
btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
btQuaternion qAB = qB.inverse() * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
int row = 3;
int srow = row * info->rowskip;
btVector3 ax1;
// angular limits
{
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
btTransform trA = transA*m_frameInA;
btVector3 twistAxis = trA.getBasis().getColumn(0);
btVector3 p = trA.getBasis().getColumn(1);
btVector3 q = trA.getBasis().getColumn(2);
int srow1 = srow + info->rowskip;
J1[srow+0] = p[0];
J1[srow+1] = p[1];
J1[srow+2] = p[2];
J1[srow1+0] = q[0];
J1[srow1+1] = q[1];
J1[srow1+2] = q[2];
J2[srow+0] = -p[0];
J2[srow+1] = -p[1];
J2[srow+2] = -p[2];
J2[srow1+0] = -q[0];
J2[srow1+1] = -q[1];
J2[srow1+2] = -q[2];
btScalar fact = info->fps;
info->m_constraintError[srow] = fact * swingAxis.dot(p);
info->m_constraintError[srow1] = fact * swingAxis.dot(q);
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
info->m_lowerLimit[srow1] = -SIMD_INFINITY;
info->m_upperLimit[srow1] = SIMD_INFINITY;
srow = srow1 + info->rowskip;
{
btQuaternion qMinTwist = qABTwist;
btScalar twistAngle = qABTwist.getAngle();
if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
{
qMinTwist = -(qABTwist);
twistAngle = qMinTwist.getAngle();
}
if (twistAngle > SIMD_EPSILON)
{
twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
twistAxis.normalize();
twistAxis = quatRotate(qB, -twistAxis);
}
ax1 = twistAxis;
btScalar *J1 = info->m_J1angularAxis;
btScalar *J2 = info->m_J2angularAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
btScalar k = info->fps;
info->m_constraintError[srow] = k * twistAngle;
info->m_lowerLimit[srow] = -SIMD_INFINITY;
info->m_upperLimit[srow] = SIMD_INFINITY;
}
}
}
void RotationConstraintTests::testSwingTwistConstraint() {
// referenceRotation is the default rotation
float referenceAngle = 1.23f;
glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f));
glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis);
// the angle limits of the constriant about the hinge axis
float minTwistAngle = -PI / 2.0f;
float maxTwistAngle = PI / 2.0f;
// build the constraint
SwingTwistConstraint shoulder;
shoulder.setReferenceRotation(referenceRotation);
shoulder.setTwistLimits(minTwistAngle, maxTwistAngle);
float lowDot = 0.25f;
float highDot = 0.75f;
// The swing constriants are more interesting: a vector of minimum dot products
// as a function of theta around the twist axis. Our test function will be shaped
// like the square wave with amplitudes 0.25 and 0.75:
//
// |
// 0.75 - o---o---o---o
// | / '
// | / '
// | / '
// 0.25 o---o---o---o o
// |
// +-------+-------+-------+-------+---
// 0 pi/2 pi 3pi/2 2pi
int numDots = 8;
std::vector<float> minDots;
int dotIndex = 0;
while (dotIndex < numDots / 2) {
++dotIndex;
minDots.push_back(lowDot);
}
while (dotIndex < numDots) {
minDots.push_back(highDot);
++dotIndex;
}
shoulder.setSwingLimits(minDots);
const SwingTwistConstraint::SwingLimitFunction& shoulderSwingLimitFunction = shoulder.getSwingLimitFunction();
{ // test interpolation of SwingLimitFunction
const float ACCEPTABLE_ERROR = 1.0e-5f;
float theta = 0.0f;
float minDot = shoulderSwingLimitFunction.getMinDot(theta);
float expectedMinDot = lowDot;
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
theta = PI;
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = highDot;
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
// test interpolation on upward slope
theta = PI * (7.0f / 8.0f);
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = 0.5f * (highDot + lowDot);
QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
// test interpolation on downward slope
theta = PI * (15.0f / 8.0f);
minDot = shoulderSwingLimitFunction.getMinDot(theta);
expectedMinDot = 0.5f * (highDot + lowDot);
}
float smallAngle = PI / 100.0f;
// Note: the twist is always about the yAxis
glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
{ // test INSIDE both twist and swing
int numSwingAxes = 7;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle + smallAngle;
float endTwist = maxTwistAngle - smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float swing = acosf(shoulderSwingLimitFunction.getMinDot(theta)) - smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == false);
QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test INSIDE twist but OUTSIDE swing
int numSwingAxes = 7;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle + smallAngle;
float endTwist = maxTwistAngle - smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle + smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
glm::quat expectedRotation = expectedSwingRotation * twistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test OUTSIDE twist but INSIDE swing
int numSwingAxes = 6;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle - smallAngle;
float endTwist = maxTwistAngle + smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle - smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
glm::quat expectedRotation = swingRotation * expectedTwistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
{ // test OUTSIDE both twist and swing
int numSwingAxes = 5;
float deltaTheta = TWO_PI / numSwingAxes;
int numTwists = 2;
float startTwist = minTwistAngle - smallAngle;
float endTwist = maxTwistAngle + smallAngle;
float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
float twist = startTwist;
for (int i = 0; i < numTwists; ++i) {
glm::quat twistRotation = glm::angleAxis(twist, yAxis);
float clampedTwistAngle = glm::clamp(twist, minTwistAngle, maxTwistAngle);
for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
float swing = maxSwingAngle + smallAngle;
glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
glm::quat outputRotation = inputRotation;
shoulder.clearHistory();
bool updated = shoulder.apply(outputRotation);
QVERIFY(updated == true);
glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
glm::quat expectedRotation = expectedSwingRotation * expectedTwistRotation * referenceRotation;
QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
}
twist += deltaTwist;
}
}
}
