// 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);
  }
Example #2
0
  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;
  }
  // int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
  //    int flags, dContactGeom *contact, int skip)
  int dCollideBoxPlane(DxBox o1, DxPlane o2, int flags, DContactGeomBuffer contacts, int skip) {
    // dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT(skip == 1);
    //		dIASSERT (o1.type == dBoxClass);
    //		dIASSERT (o2.type == dPlaneClass);
    dIASSERT((flags & DxGeom.NUMC_MASK) >= 1);

    DxBox box = o1;
    DxPlane plane = o2;

    DContactGeom contact = contacts.get(0);
    contact.g1 = o1;
    contact.g2 = o2;
    contact.side1 = -1;
    contact.side2 = -1;

    // int ret = 0;
    RefInt ret = new RefInt();

    // @@@ problem: using 4-vector (plane.p) as 3-vector (normal).
    // final double *R = o1.final_posr.R;		// rotation of box
    // final double *n = plane.p;		// normal vector
    final DMatrix3C R = o1.final_posr().R(); // rotation of box
    // final double []n = plane._p;		// normal vector
    DVector3C n = plane.getNormal();

    // project sides lengths along normal vector, get absolute values
    double Q1 = dCalcVectorDot3_14(n, R, 0);
    double Q2 = dCalcVectorDot3_14(n, R, 1);
    double Q3 = dCalcVectorDot3_14(n, R, 2);
    //		double A1 = box.side.v[0] * Q1;
    //		double A2 = box.side.v[1] * Q2;
    //		double A3 = box.side.v[2] * Q3;
    double[] A = {box.side.get0() * Q1, box.side.get1() * Q2, box.side.get2() * Q3};
    //		double B1 = dFabs(A1);
    //		double B2 = dFabs(A2);
    //		double B3 = dFabs(A3);
    double[] B = {dFabs(A[0]), dFabs(A[1]), dFabs(A[2])};

    // early exit test
    //		double depth = plane._p[3] + (0.5)*(B1+B2+B3) - dDOT(n,o1._final_posr.pos);
    RefDouble depth =
        new RefDouble(
            plane.getDepth() + (0.5) * (B[0] + B[1] + B[2]) - n.dot(o1.final_posr().pos()));
    if (depth.get() < 0) return 0;

    // find number of contacts requested
    int maxc = flags & DxGeom.NUMC_MASK;
    // if (maxc < 1) maxc = 1; // an assertion is made on entry
    if (maxc > 4) maxc = 4; // not more than 4 contacts per box allowed

    // find deepest point
    DVector3 p = new DVector3();
    //		p.v[0] = o1._final_posr.pos.v[0];
    //		p.v[1] = o1._final_posr.pos.v[1];
    //		p.v[2] = o1._final_posr.pos.v[2];
    p.set(o1.final_posr().pos());
    //		#define FOO1(i,op) \
    //		p.v[0] op (0.5)*box.side[i] * R[0+i]; \
    //		p.v[1] op (0.5)*box.side[i] * R[4+i]; \
    //		p.v[2] op (0.5)*box.side[i] * R[8+i];
    //		#define BAR1(i,iinc) if (A ## iinc > 0) { FOO1(i,-=) } else { FOO1(i,+=) }
    //		BAR1(0,1);
    //		BAR1(1,2);
    //		BAR1(2,3);
    //		#undef FOO
    //		#undef BAR
    // substitute for FOO and BAR above (TZ)
    for (int i = 0; i < 3; i++) {
      if (A[i] > 0) p.eqSum(p, R.columnAsNewVector(i), -0.5 * box.side.get(i));
      else p.eqSum(p, R.columnAsNewVector(i), +0.5 * box.side.get(i));
    }

    // the deepest point is the first contact point
    //		contact.pos[0] = p[0];
    //		contact.pos[1] = p[1];
    //		contact.pos[2] = p[2];
    contact.pos.set(p);
    contact.depth = depth.get();
    ret.set(1); // ret = 1;		// ret is number of contact points found so far
    // TZ
    do {
      if (maxc == 1) {
        // goto done;
        done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n);
        return ret.get();
      }

      // get the second and third contact points by starting from `p' and going
      // along the two sides with the smallest projected length.

      //			#define FOO2(i,j,op) \
      //			CONTACT(contact,i*skip).pos[0] = p[0] op box.side[j] * R[0+j]; \
      //			CONTACT(contact,i*skip).pos[1] = p[1] op box.side[j] * R[4+j]; \
      //			CONTACT(contact,i*skip).pos[2] = p[2] op box.side[j] * R[8+j];
      // contacts.get(i*skip).pos.sum(p, R.columnAsNewVector(j), op*box.side(j));
      //			#define BAR2(ctact,side,sideinc) \
      //			depth -= B ## sideinc; \
      //			if (depth < 0) goto done; \
      //			if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO2(ctact,side,-); } \
      //			CONTACT(contact,ctact*skip).depth = depth; \
      //			ret++;

      int p1, p2, p3, go;
      //			if (B[0] < B[1]) {
      //				if (B[2] < B[0]) goto use_side_3;
      //				else {
      //					BAR2(1,0,1);	// use side 1
      //					if (maxc == 2) { //goto done;
      //						done(ret, contacts, skip, o1, o2);
      //						return ret;
      //					}
      //					if (B[1] < B[2]) goto contact2_2; else goto contact2_3;
      //				}
      //			} else {
      //				if (B[2] < B[1]) {
      //					use_side_3:	// use side 3
      //					BAR2(1,2,3);
      //					if (maxc == 2) { //goto done;
      //						done(ret, contacts, skip, o1, o2);
      //						return ret;
      //					}
      //					if (B[0] < B[1]) goto contact2_1; else goto contact2_2;
      //				} else {
      //					BAR2(1,1,2);	// use side 2
      //					if (maxc == 2) { //goto done;
      //						done(ret, contacts, skip, o1, o2);
      //						return ret;
      //					}
      //					if (B[0] < B[2]) goto contact2_1; else goto contact2_3;
      //				}
      //			}
      if (B[0] < B[1]) {
        if (B[2] < B[0]) {
          // TODO
          p1 = 1;
          p2 = 2;
          p3 = 3; // 1 2 3
          if (B[0] < B[1]) go = 21;
          else go = 22;
        } else {
          p1 = 1;
          p2 = 0;
          p3 = 1; // 1 0 1
          if (B[1] < B[2]) go = 22;
          else go = 23;
        }
      } else {
        if (B[2] < B[1]) {
          p1 = 1;
          p2 = 2;
          p3 = 3; // 1 2 3
          if (B[0] < B[1]) go = 21;
          else go = 22;
        } else {
          p1 = 1;
          p2 = 1;
          p3 = 2; // 1 1 2
          if (B[0] < B[2]) go = 21;
          else go = 23;
        }
      }

      BAR2(p1, p2, p3, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n);
      // TODO improve  (e.g. make depth a primitive int, reduce arguments, ...
      //			BAR2_(p1, p2, depth, ret, contacts, skip, A[p2], B[p2], o1, o2, p, R, box.side);

      if (maxc == 2) { // goto done;
        break;
      } else {
        if (go == 21) {
          if (!BAR2(2, 0, 1, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
            return ret.get();
          break;
        } else if (go == 22) {
          if (!BAR2(2, 1, 2, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
            return ret.get();
          break;
        } else if (go == 23) {
          if (!BAR2(2, 2, 3, depth, ret, contacts, skip, A, B, o1, o2, p, R, box.side, maxc, n))
            return ret.get();
          break;
        } else {
          throw new IllegalStateException("go=" + go);
        }
      }
      // TZ
      //			throw new IllegalStateException();
      //			contact2_1: BAR2(2,0,1); goto done;
      //			contact2_2: BAR2(2,1,2); goto done;
      //			contact2_3: BAR2(2,2,3); goto done;
      //			#undef FOO
      //			#undef BAR
    } while (false);
    // done:
    //			for (int i=0; i<ret; i++) {
    //				CONTACT(contact,i*skip).g1 = o1;
    //				CONTACT(contact,i*skip).g2 = o2;
    //			}
    done(ret, contacts, skip, o1, o2, maxc, depth.get(), p, n);
    return ret.get();
  }
Example #4
0
 public void setC(int i, DVector3C v) {
   _cA[_cP + i] = v.get0();
 }