// 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);
  }
コード例 #2
0
ファイル: DxJoint.java プロジェクト: kephale/ode4j
  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;
  }