// Move 1st body offset unit in the X direction
//
// X-------> X---------> <-- Axis
// B1 => B1
// B2 B2
//
// Start with a Offset of offset unit
//
// X-------> X---------> <-- Axis
// B1 => B1
// B2 B2
// TEST_FIXTURE (Fixture_dxJointPiston_B1_and_B2_At_Zero_Axis_Inverse_of_X,
@Test
public void test_dJointSetPistonAxisOffset_B1_3Unit() {
dJointSetPistonAnchor(jId, 0, 0, 0);
CHECK_CLOSE(0.0, dJointGetPistonPosition(jId), 1e-4);
dBodySetPosition(bId1, offset, 0, 0);
CHECK_CLOSE(-offset, dJointGetPistonPosition(jId), 1e-4);
dJointSetPistonAnchorOffset(
jId, 0, 0, 0, -offset * axis.get0(), -offset * axis.get1(), -offset * axis.get2());
CHECK_CLOSE(-offset, dJointGetPistonPosition(jId), 1e-4);
dBodySetPosition(bId1, 0, 0, 0);
CHECK_CLOSE(0.0, dJointGetPistonPosition(jId), 1e-4);
}
double getHingeAngleFromRelativeQuat(DQuaternionC qrel, DVector3C axis) {
// the angle between the two bodies is extracted from the quaternion that
// represents the relative rotation between them. recall that a quaternion
// q is:
// [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
// where s is a scalar and v is a 3-vector. u is a unit length axis and
// theta is a rotation along that axis. we can get theta/2 by:
// theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
// but we can't get sin(theta/2) directly, only its absolute value, i.e.:
// |v| = |sin(theta/2)| * |u|
// = |sin(theta/2)|
// using this value will have a strange effect. recall that there are two
// quaternion representations of a given rotation, q and -q. typically as
// a body rotates along the axis it will go through a complete cycle using
// one representation and then the next cycle will use the other
// representation. this corresponds to u pointing in the direction of the
// hinge axis and then in the opposite direction. the result is that theta
// will appear to go "backwards" every other cycle. here is a fix: if u
// points "away" from the direction of the hinge (motor) axis (i.e. more
// than 90 degrees) then use -q instead of q. this represents the same
// rotation, but results in the cos(theta/2) value being sign inverted.
// extract the angle from the quaternion. cost2 = cos(theta/2),
// sint2 = |sin(theta/2)|
double cost2 = qrel.get0();
double sint2 =
dSqrt(qrel.get1() * qrel.get1() + qrel.get2() * qrel.get2() + qrel.get3() * qrel.get3());
// double theta = ( dDOT( qrel.v, 1, axis.v, 0 ) >= 0 ) ? // @@@ padding assumptions
double theta =
((qrel.get1() * axis.get0() + qrel.get2() * axis.get1() + qrel.get3() * axis.get2()) >= 0)
? // @@@ padding assumptions
(2 * dAtan2(sint2, cost2))
: // if u points in direction of axis
(2 * dAtan2(sint2, -cost2)); // if u points in opposite direction
// the angle we get will be between 0..2*pi, but we want to return angles
// between -pi..pi
if (theta > M_PI) theta -= 2 * M_PI;
// the angle we've just extracted has the wrong sign
theta = -theta;
return theta;
}