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