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();
}
private void demo(String[] args) {
int i, j;
DMass m = OdeHelper.createMass();
// create world
OdeHelper.initODE2(0);
world = OdeHelper.createWorld();
space = OdeHelper.createHashSpace(null);
contactgroup = OdeHelper.createJointGroup();
world.setGravity(0, 0, -GRAVITY);
ground = OdeHelper.createPlane(space, 0, 0, 1, 0);
// bodies
for (i = 0; i < N1; i++) {
for (j = 0; j < N2; j++) {
body[i][j] = OdeHelper.createBody(world);
m.setBox(1, LENGTH, LENGTH, HEIGHT);
m.adjust(MASS * (j + 1));
body[i][j].setMass(m);
body[i][j].setPosition(i * 2 * LENGTH, j * 2 * LENGTH, HEIGHT * 0.5);
box[i][j] = OdeHelper.createBox(space, LENGTH, LENGTH, HEIGHT);
box[i][j].setBody(body[i][j]);
}
}
// run simulation
dsSimulationLoop(args, 352, 288, this);
contactgroup.destroy();
space.destroy();
world.destroy();
OdeHelper.closeODE();
}
private void simLoop(boolean pause) {
int i;
if (!pause) {
// apply forces to all bodies
for (i = 0; i < N1; i++) {
for (int j = 0; j < N2; j++) {
body[i][j].addForce(FORCE * (i + 1), 0, 0);
}
}
OdeHelper.spaceCollide(space, 0, nearCallback);
world.step(0.05);
// remove all contact joints
contactgroup.empty();
}
dsSetColor(1, 0, 1);
DVector3 sides = new DVector3(LENGTH, LENGTH, HEIGHT);
for (i = 0; i < N1; i++) {
for (int j = 0; j < N2; j++) {
dsDrawBox(box[i][j].getPosition(), box[i][j].getRotation(), sides);
}
}
}
// ODE_API
public static DWorld dWorldCreate() {
return OdeHelper.createWorld();
}