예제 #1
0
  /** The call to ragdoll.destroy() used to cause an NPE. */
  @Test
  public void testIssue31() {
    DWorld world = OdeHelper.createWorld();
    world.setGravity(0, 0, -9.8);
    world.setDamping(1e-4, 1e-5);
    //    dWorldSetERP(world, 1);
    DSpace space = OdeHelper.createSimpleSpace();
    OdeHelper.createPlane(space, 0, 0, 1, 0);

    DxRagdoll ragdoll = new DxRagdoll(world, space, new DxDefaultHumanRagdollConfig());
    ragdoll.setAngularDamping(0.1);
    DQuaternion q = new DQuaternion(1, 0, 0, 0);
    Rotation.dQFromAxisAndAngle(q, new DVector3(1, 0, 0), -0.5 * Math.PI);
    for (int i = 0; i < ragdoll.getBones().size(); i++) {
      DxRagdollBody bone = ragdoll.getBones().get(i);
      DGeom g = OdeHelper.createCapsule(space, bone.getRadius(), bone.getLength());
      DBody body = bone.getBody();
      DQuaternion qq = new DQuaternion();
      OdeMath.dQMultiply1(qq, q, body.getQuaternion());
      body.setQuaternion(qq);
      DMatrix3 R = new DMatrix3();
      OdeMath.dRfromQ(R, q);
      DVector3 v = new DVector3();
      OdeMath.dMultiply0_133(v, body.getPosition(), R);
      body.setPosition(v.get0(), v.get1(), v.get2());
      g.setBody(body);
    }

    ragdoll.destroy();
  }
예제 #2
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 //	void setAnchors( dxJoint j, double x, double y, double z,
 //	dVector3 anchor1, dVector3 anchor2 )
 final void setAnchors(DVector3C xyz, DVector3 anchor1, DVector3 anchor2) {
   if (node[0].body != null) {
     /// double[] q = new double[4];
     DVector3 q = new DVector3();
     //			q.v[0] = x - node[0].body._posr.pos.v[0];
     //			q.v[1] = y - node[0].body._posr.pos.v[1];
     //			q.v[2] = z - node[0].body._posr.pos.v[2];
     //			q.v[3] = 0;
     q.eqDiff(xyz, node[0].body.posr().pos());
     dMultiply1_331(anchor1, node[0].body.posr().R(), q);
     if (node[1].body != null) {
       //				q.v[0] = x - node[1].body._posr.pos.v[0];
       //				q.v[1] = y - node[1].body._posr.pos.v[1];
       //				q.v[2] = z - node[1].body._posr.pos.v[2];
       //				q.v[3] = 0;
       q.eqDiff(xyz, node[1].body.posr().pos());
       dMultiply1_331(anchor2, node[1].body.posr().R(), q);
     } else {
       //				anchor2.v[0] = x;
       //				anchor2.v[1] = y;
       //				anchor2.v[2] = z;
       anchor2.set(xyz);
     }
   }
   //		anchor1.v[3] = 0;
   //		anchor2.v[3] = 0;
 }
예제 #3
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 //	void getAnchor2( dxJoint j, dVector3 result, dVector3 anchor2 )
 void getAnchor2(DVector3 result, DVector3 anchor2) {
   if (node[1].body != null) {
     dMultiply0_331(result, node[1].body.posr().R(), anchor2);
     //			result.v[0] += node[1].body.posr.pos.v[0];
     //			result.v[1] += node[1].body.posr.pos.v[1];
     //			result.v[2] += node[1].body.posr.pos.v[2];
     result.add(node[1].body.posr().pos());
   } else {
     //			result.v[0] = anchor2.v[0];
     //			result.v[1] = anchor2.v[1];
     //			result.v[2] = anchor2.v[2];
     result.set(anchor2);
   }
 }
예제 #4
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 //	void getAnchor( dxJoint j, dVector3 result, dVector3 anchor1 )
 void getAnchor(DVector3 result, DVector3 anchor1) {
   if (node[0].body != null) {
     dMultiply0_331(result, node[0].body.posr().R(), anchor1);
     //			result.v[0] += node[0].body._posr.pos.v[0];
     //			result.v[1] += node[0].body._posr.pos.v[1];
     //			result.v[2] += node[0].body._posr.pos.v[2];
     result.add(node[0].body.posr().pos());
   }
 }
예제 #5
0
파일: DxJoint.java 프로젝트: kephale/ode4j
  /**
   * set three orientation rows in the constraint equation, and the corresponding right hand side.
   *
   * @param joint
   * @param info
   * @param qrel
   * @param start_row
   */
  void setFixedOrientation(DxJoint joint, Info2 info, DQuaternion qrel, int start_row) {
    int s = info.rowskip();
    int start_index = start_row * s;
    // 3 rows to make body rotations equal
    info._J[info.J1ap + start_index] = 1;
    info._J[info.J1ap + start_index + s + 1] = 1;
    info._J[info.J1ap + start_index + s * 2 + 2] = 1;
    if (joint.node[1].body != null) {
      info._J[info.J2ap + start_index] = -1;
      info._J[info.J2ap + start_index + s + 1] = -1;
      info._J[info.J2ap + start_index + s * 2 + 2] = -1;
    }

    // compute the right hand side. the first three elements will result in
    // relative angular velocity of the two bodies - this is set to bring them
    // back into alignment. the correcting angular velocity is
    //   |angular_velocity| = angle/time = erp*theta / stepsize
    //                      = (erp*fps) * theta
    //    angular_velocity  = |angular_velocity| * u
    //                      = (erp*fps) * theta * u
    // where rotation along unit length axis u by theta brings body 2's frame
    // to qrel with respect to body 1's frame. using a small angle approximation
    // for sin(), this gives
    //    angular_velocity  = (erp*fps) * 2 * v
    // where the quaternion of the relative rotation between the two bodies is
    //    q = [cos(theta/2) sin(theta/2)*u] = [s v]

    // get qerr = relative rotation (rotation error) between two bodies
    DQuaternion qerr = new DQuaternion();
    DVector3 e = new DVector3();
    if (joint.node[1].body != null) {
      DQuaternion qq = new DQuaternion();
      dQMultiply1(qq, joint.node[0].body._q, joint.node[1].body._q);
      dQMultiply2(qerr, qq, qrel);
    } else {
      dQMultiply3(qerr, joint.node[0].body._q, qrel);
    }
    if (qerr.get0() < 0) {
      qerr.set1(-qerr.get1()); // adjust sign of qerr to make theta small
      qerr.set2(-qerr.get2());
      qerr.set3(-qerr.get3());
    }
    // TZ:
    //		dMULTIPLY0_331( e, joint.node[0].body.posr.R, qerr + 1 );  // @@@ bad SIMD padding!
    DVector3 qerr2 = new DVector3();
    qerr2.set0(qerr.get1());
    qerr2.set1(qerr.get2());
    qerr2.set2(qerr.get3());
    dMultiply0_331(e, joint.node[0].body.posr().R(), qerr2); // @@@ bad SIMD padding!
    double k = info.fps * info.erp;
    info.setC(start_row, 2 * k * e.get0());
    info.setC(start_row + 1, 2 * k * e.get1());
    info.setC(start_row + 2, 2 * k * e.get2());
  }
예제 #6
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 void getAxis2(DVector3 result, DVector3C axis2) {
   if (node[1].body != null) {
     dMultiply0_331(result, node[1].body.posr().R(), axis2);
   } else {
     //			result.v[0] = axis2.v[0];
     //			result.v[1] = axis2.v[1];
     //			result.v[2] = axis2.v[2];
     result.set(axis2);
   }
 }
예제 #7
0
 //	#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];
 private final void FOO2(
     int i,
     int j,
     int op,
     DContactGeomBuffer contacts,
     int skip,
     DVector3 p,
     DMatrix3C R,
     DVector3 side) {
   contacts.get(i * skip).pos.eqSum(p, R.viewCol(j), op * side.get(j));
 }
  // Rotate the body by -0.23rad around X then back to original position.
  // The body is attached at the second position of the joint:
  // dJointAttache(jId, 0, bId);
  //
  //   ^         ^
  //   |  =>    /
  //   |       /
  //  B1      B1
  //
  // Start with a Delta of -0.23rad
  //     ^     ^
  //    /  =>  |
  //   /       |
  //   B1     B1
  // TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes,
  @Test
  public void test_dJointSetUniversalAxisOffset_1Body_B1_Minus0_23rad() {
    CHECK_CLOSE(dJointGetUniversalAngle1(jId), 0, 1e-4);

    DMatrix3 R = new DMatrix3();
    dRFromAxisAndAngle(R, 1, 0, 0, -(0.23));
    dBodySetRotation(bId1, R);

    CHECK_CLOSE(-(0.23), dJointGetUniversalAngle1(jId), 1e-4);

    DVector3 axis = new DVector3();
    dJointGetUniversalAxis1(jId, axis);
    dJointSetUniversalAxis1Offset(jId, axis.get0(), axis.get1(), axis.get2(), -(0.23), 0);
    CHECK_CLOSE(-(0.23), dJointGetUniversalAngle1(jId), 1e-4);

    dRFromAxisAndAngle(R, 1, 0, 0, 0);
    dBodySetRotation(bId1, R);

    CHECK_CLOSE(0, dJointGetUniversalAngle1(jId), 1e-4);
  }
예제 #9
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 void setAxes(DVector3C axis, DVector3 axis1, DVector3 axis2) {
   if (node[0].body != null) {
     DVector3 q = new DVector3(axis);
     dNormalize3(q);
     if (axis1 != null) {
       dMultiply1_331(axis1, node[0].body.posr().R(), q);
     }
     if (axis2 != null) {
       if (node[1].body != null) {
         dMultiply1_331(axis2, node[1].body.posr().R(), q);
       } else {
         axis2.set(axis);
       }
     }
   }
 }
예제 #10
0
파일: DxJoint.java 프로젝트: kephale/ode4j
 //	void setAxes( dxJoint j, double x, double y, double z,
 void setAxes(double x, double y, double z, DVector3 axis1, DVector3 axis2) {
   if (node[0].body != null) {
     //        double[] q = new double[4];
     DVector3 q = new DVector3(x, y, z);
     dNormalize3(q);
     if (axis1 != null) {
       dMultiply1_331(axis1, node[0].body.posr().R(), q);
       //				axis1.v[3] = 0;
     }
     if (axis2 != null) {
       if (node[1].body != null) {
         dMultiply1_331(axis2, node[1].body.posr().R(), q);
       } else {
         //					axis2.v[0] = x;
         //					axis2.v[1] = y;
         //					axis2.v[2] = z;
         axis2.set(x, y, z);
       }
       //				axis2.v[3] = 0;
     }
   }
 }
예제 #11
0
  // 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();
  }
예제 #12
0
 // void dJointGetLMotorAxis( dJoint j, int anum, dVector3 result )
 void dJointGetLMotorAxis(int anum, DVector3 result) {
   dAASSERT(anum >= 0 && anum < 3);
   if (anum < 0) anum = 0;
   if (anum > 2) anum = 2;
   result.set(axis[anum]);
 }
예제 #13
0
파일: DxJoint.java 프로젝트: kephale/ode4j
  void setBall2(
      DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2, DVector3 axis, double erp1) {
    // anchor points in global coordinates with respect to body PORs.
    DVector3 a1 = new DVector3(), a2 = new DVector3();

    int s = info.rowskip();

    // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
    // [0 1 0] and [0 0 1], which makes everything much easier.
    DVector3 q1 = new DVector3(), q2 = new DVector3();
    dPlaneSpace(axis, q1, q2);

    // set jacobian
    //		for ( i = 0; i < 3; i++ ) info._J[info.J1lp+i] = axis.get(i);
    //		for ( i = 0; i < 3; i++ ) info._J[info.J1lp+s+i] = q1.get(i);
    //		for ( i = 0; i < 3; i++ ) info._J[info.J1lp+2*s+i] = q2.get(i);
    axis.wrapSet(info._J, info.J1lp);
    q1.wrapSet(info._J, info.J1lp + s);
    q2.wrapSet(info._J, info.J1lp + 2 * s);
    dMultiply0_331(a1, joint.node[0].body.posr().R(), anchor1);
    dCalcVectorCross3(info._J, info.J1ap, a1, axis);
    dCalcVectorCross3(info._J, info.J1ap + s, a1, q1);
    dCalcVectorCross3(info._J, info.J1ap + 2 * s, a1, q2);
    if (joint.node[1].body != null) {
      //			for ( i = 0; i < 3; i++ ) info._J[info.J2lp+i] = -axis.v[i];
      //			for ( i = 0; i < 3; i++ ) info._J[info.J2lp+s+i] = -q1.v[i];
      //			for ( i = 0; i < 3; i++ ) info._J[info.J2lp+2*s+i] = -q2.v[i];
      axis.wrapSub(info._J, info.J2lp);
      q1.wrapSub(info._J, info.J2lp + s);
      q2.wrapSub(info._J, info.J2lp + 2 * s);
      //	        dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
      //	        dReal *J2a = info->J2a;
      //	        dCalcVectorCross3( J2a, a2, axis );
      //	        dNegateVector3( J2a );
      //	        dReal *J2a_plus_s = J2a + s;
      //	        dCalcVectorCross3( J2a_plus_s, a2, q1 );
      //	        dNegateVector3( J2a_plus_s );
      //	        dReal *J2a_plus_2s = J2a_plus_s + s;
      //	        dCalcVectorCross3( J2a_plus_2s, a2, q2 );
      //	        dNegateVector3( J2a_plus_2s );
      dMultiply0_331(a2, joint.node[1].body._posr.R, anchor2);
      double[] J = info._J; // TZ
      int J2ap = info.J2ap; // TZ
      // dReal *J2a = info->J2a;
      dCalcVectorCross3(J, J2ap, a2, axis);
      dNegateVector3(J, J2ap);
      // dReal *J2a_plus_s = J2a + s;
      dCalcVectorCross3(J, J2ap + s, a2, q1);
      dNegateVector3(J, J2ap + s);
      // dReal *J2a_plus_2s = J2a_plus_s + s;
      dCalcVectorCross3(J, J2ap + 2 * s, a2, q2);
      dNegateVector3(J, J2ap + 2 * s);
    }

    // set right hand side - measure error along (axis,q1,q2)
    double k1 = info.fps * erp1;
    double k = info.fps * info.erp;

    // for ( i = 0; i < 3; i++ ) a1.v[i] += joint.node[0].body._posr.pos.v[i];
    a1.add(joint.node[0].body.posr().pos());
    if (joint.node[1].body != null) {
      //			for ( i = 0; i < 3; i++ ) a2.v[i] += joint.node[1].body._posr.pos.v[i];
      a2.add(joint.node[1].body.posr().pos());
      DVector3 a2_minus_a1 = new DVector3();
      a2_minus_a1.eqDiff(a2, a1);
      info.setC(0, k1 * (dCalcVectorDot3(axis, a2_minus_a1)));
      info.setC(1, k * (dCalcVectorDot3(q1, a2_minus_a1)));
      info.setC(2, k * (dCalcVectorDot3(q2, a2_minus_a1)));
    } else {
      DVector3 anchor2_minus_a1 = new DVector3();
      anchor2_minus_a1.eqDiff(anchor2, a1);
      info.setC(0, k1 * (dCalcVectorDot3(axis, anchor2_minus_a1)));
      info.setC(1, k * (dCalcVectorDot3(q1, anchor2_minus_a1)));
      info.setC(2, k * (dCalcVectorDot3(q2, anchor2_minus_a1)));
    }
  }
예제 #14
0
파일: DxJoint.java 프로젝트: kephale/ode4j
  void setBall(DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2) {
    // anchor points in global coordinates with respect to body PORs.
    DVector3 a1 = new DVector3(), a2 = new DVector3();

    int s = info.rowskip();

    // set jacobian
    info._J[info.J1lp + 0] = 1;
    info._J[info.J1lp + s + 1] = 1;
    info._J[info.J1lp + 2 * s + 2] = 1;
    dMultiply0_331(a1, joint.node[0].body.posr().R(), anchor1);
    //    dCROSSMAT( info.J1a, a1, s, -, + );
    dSetCrossMatrixMinus(info._J, info.J1ap, a1, s);
    if (joint.node[1].body != null) {
      info._J[info.J2lp + 0] = -1;
      info._J[info.J2lp + s + 1] = -1;
      info._J[info.J2lp + 2 * s + 2] = -1;
      dMultiply0_331(a2, joint.node[1].body.posr().R(), anchor2);
      dSetCrossMatrixPlus(info._J, info.J2ap, a2, s);
    }

    DxBody b0 = joint.node[0].body;
    DxBody b1 = joint.node[1].body;

    // set right hand side
    double k = info.fps * info.erp;
    if (joint.node[1].body != null) {
      //			for ( int j = 0; j < 3; j++ )
      //			{
      //				info.setC(j, k * ( a2.v[j] + b1.posr().pos().v[j] -
      //						a1.v[j] - b0._posr.pos.v[j] ));
      //			}
      info.setC(0, k * (a2.get0() + b1.posr().pos().get0() - a1.get0() - b0.posr().pos().get0()));
      info.setC(1, k * (a2.get1() + b1.posr().pos().get1() - a1.get1() - b0.posr().pos().get1()));
      info.setC(2, k * (a2.get2() + b1.posr().pos().get2() - a1.get2() - b0.posr().pos().get2()));
    } else {
      //			for ( int j = 0; j < 3; j++ )
      //			{
      //				info.setC(j, k * ( anchor2.v[j] - a1.v[j] -
      //						b0._posr.pos.v[j] ));
      //			}
      info.setC(0, k * (anchor2.get0() - a1.get0() - b0.posr().pos().get0()));
      info.setC(1, k * (anchor2.get1() - a1.get1() - b0.posr().pos().get1()));
      info.setC(2, k * (anchor2.get2() - a1.get2() - b0.posr().pos().get2()));
    }
  }
  // Test when there is only one body at position one on the joint
  // TEST_FIXTURE (Fixture_dxJointUniversal_B1_At_Zero_Default_Axes,
  @Test
  public void test_dJointGetUniversalAngle1_1Body_B1() {
    CHECK_CLOSE(0, dJointGetUniversalAngle1(jId), 1e-4);
    CHECK_CLOSE(0, dJointGetUniversalAngle2(jId), 1e-4);

    RefDouble angle1 = new RefDouble(), angle2 = new RefDouble();
    dJointGetUniversalAngles(jId, angle1, angle2);
    CHECK_CLOSE(0, angle1.d, 1e-4);
    CHECK_CLOSE(0, angle2.d, 1e-4);

    DVector3 axis1 = new DVector3();
    dJointGetUniversalAxis1(jId, axis1);
    DVector3 axis2 = new DVector3();
    dJointGetUniversalAxis2(jId, axis2);

    DMatrix3 R = new DMatrix3();

    double ang1 = (0.23);
    dRFromAxisAndAngle(R, axis1.get0(), axis1.get1(), axis1.get2(), ang1);
    dBodySetRotation(bId1, R);

    double ang2 = (0.0);

    CHECK_CLOSE(ang1, dJointGetUniversalAngle1(jId), 1e-4);
    CHECK_CLOSE(-ang2, dJointGetUniversalAngle2(jId), 1e-4);

    dJointGetUniversalAngles(jId, angle1, angle2);
    CHECK_CLOSE(ang1, angle1.d, 1e-4);
    CHECK_CLOSE(-ang2, angle2.d, 1e-4);

    DMatrix3 I = new DMatrix3();
    I.setIdentity(); // Set the rotation of the body to be the Identity (i.e. zero)
    dBodySetRotation(bId1, I);

    CHECK_CLOSE(0, dJointGetUniversalAngle1(jId), 1e-4);
    CHECK_CLOSE(0, dJointGetUniversalAngle2(jId), 1e-4);

    // Test the same rotation, when axis1 is inverted
    dJointSetUniversalAxis1(jId, -axis1.get0(), -axis1.get1(), -axis1.get2());

    dBodySetRotation(bId1, R);

    CHECK_CLOSE(-ang1, dJointGetUniversalAngle1(jId), 1e-4);
    CHECK_CLOSE(-ang2, dJointGetUniversalAngle2(jId), 1e-4);

    dJointGetUniversalAngles(jId, angle1, angle2);
    CHECK_CLOSE(-ang1, angle1.d, 1e-4);
    CHECK_CLOSE(-ang2, angle2.d, 1e-4);

    // Test the same rotation, when axis1 is default and axis2 is inverted
    dBodySetRotation(bId1, I);

    dJointSetUniversalAxis1(jId, axis1.get0(), axis1.get1(), axis1.get2());
    dJointSetUniversalAxis2(jId, -axis2.get0(), -axis2.get1(), -axis2.get2());

    dBodySetRotation(bId1, R);

    CHECK_CLOSE(ang1, dJointGetUniversalAngle1(jId), 1e-4);
    CHECK_CLOSE(ang2, dJointGetUniversalAngle2(jId), 1e-4);

    dJointGetUniversalAngles(jId, angle1, angle2);
    CHECK_CLOSE(ang1, angle1.d, 1e-4);
    CHECK_CLOSE(ang2, angle2.d, 1e-4);
  }