/** 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(); }
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())); } }
/** * 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); }