private void nearCallback(Object data, DGeom o1, DGeom o2) {
int i;
// only collide things with the ground
boolean g1 = (o1 == ground);
boolean g2 = (o2 == ground);
if (!(g1 ^ g2)) return;
DBody b1 = o1.getBody();
DBody b2 = o2.getBody();
DContactBuffer contacts = new DContactBuffer(3); // up to 3 contacts per box
for (i = 0; i < 3; i++) {
contacts.get(i).surface.mode = OdeConstants.dContactSoftCFM | OdeConstants.dContactApprox1;
contacts.get(i).surface.mu = MU;
contacts.get(i).surface.soft_cfm = 0.01;
}
int numc = OdeHelper.collide(o1, o2, 3, contacts.getGeomBuffer());
if (numc != 0) { // [0].geom,sizeof(dContact))) {
for (i = 0; i < numc; i++) {
DJoint c = OdeHelper.createContactJoint(world, contactgroup, contacts.get(i));
c.attach(b1, b2);
}
}
}
/** 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();
}
