Esempio n. 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();
  }
Esempio n. 2
0
  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()));
    }
  }
Esempio n. 3
0
  /**
   * 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());
  }
  // 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);
  }
  // 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);
  }