I want to detect collision and read the collision force vector between two bodies in a 3D physics engine like ODE. Could someone please tell me how to achieve this?

```
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#define NUM 4
dWorldID world;
dBodyID link(NUM);
dJointID joint(NUM);
static double THETA(NUM) = { 0.0, 0.0, 0.0, 0.0 };
static double l(NUM) = { 0.10, 0.90, 1.00, 1.00 };
static double r(NUM) = { 0.20, 0.04, 0.04, 0.04 };
//static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointGroupID contactgroup;
//dsFunctions fn;
static void nearCallback(void* data, dGeomID o1, dGeomID o2)
{
const int N = 10;
dContact contact(N);
int isGround = ((ground == o1) || (ground == o2));
int n = dCollide(o1, o2, N, &contact(0).geom, sizeof(dContact));
if (isGround) {
for (int i = 0; i < n; i++) {
contact(i).surface.mode = dContactBounce;
contact(i).surface.bounce = 0.1; // (0.0~1.0) restitution parameter
contact(i).surface.bounce_vel = 0.1; // minimum incoming velocity for bounce
dJointID c = dJointCreateContact(world, contactgroup, &contact(i));
dJointAttach(c, dGeomGetBody(contact(i).geom.g1),
dGeomGetBody(contact(i).geom.g2));
//if (contact(i).geom.depth > 0.000001)
{
printf("Collision points %d , force %fn", n, contact(i).geom.depth);
}
}
}
}
void control() {
static int step = 0;
double k1 = 10.0, fMax = 100.0;
printf("r%6d:", step++);
for (int j = 1; j < NUM; j++) {
double tmpAngle = dJointGetHingeAngle(joint(j));
double z = THETA(j) - tmpAngle;
dJointSetHingeParam(joint(j), dParamVel, k1 * z);
dJointSetHingeParam(joint(j), dParamFMax, fMax);
}
```

}

void start() {

float xyz(3) = { 3.04, 1.28, 0.76 };

float hpr(3) = { -160.0, 4.50, 0.00 };

dsSetViewpoint(xyz, hpr);

}

void command(int cmd) {

switch (cmd) {

case ‘j’: THETA(1) += 0.05; break;

case ‘f’: THETA(1) -= 0.05; break;

case ‘k’: THETA(2) += 0.05; break;

case ‘d’: THETA(2) -= 0.05; break;

case ‘l’: THETA(3) += 0.05; break;

case ‘s’: THETA(3) -= 0.05; break;

}

```
if (THETA(1) < -M_PI) THETA(1) = -M_PI;
if (THETA(1) > M_PI) THETA(1) = M_PI;
if (THETA(2) < -2 * M_PI / 3) THETA(2) = -2 * M_PI / 3;
if (THETA(2) > 2 * M_PI / 3) THETA(2) = 2 * M_PI / 3;
if (THETA(3) < -2 * M_PI / 3) THETA(3) = -2 * M_PI / 3;
if (THETA(3) > 2 * M_PI / 3) THETA(3) = 2 * M_PI / 3;
```

}

void simLoop(int pause) {

control();

dWorldStep(world, 0.02);

dSpaceCollide(space, 0, &nearCallback);

dsSetColor(1.0, 1.0, 1.0);

for (int i = 0; i < NUM; i++)

dsDrawCapsuleD(dBodyGetPosition(link(i)), dBodyGetRotation(link(i)), l(i), r(i));

}

int main(int argc, char* argv()) {

dsFunctions fn;

dMass mass;

double x(NUM) = { 0.00 }, y(NUM) = { 0.00 };

double z(NUM) = { 0.05, 0.50, 1.50, 2.55 };

double m(NUM) = { 10.00, 2.00, 2.00, 2.00 };

double anchor_x(NUM) = { 0.00 }, anchor_y(NUM) = { 0.00 };

double anchor_z(NUM) = { 0.00, 0.10, 1.00, 2.00 };

double axis_x(NUM) = { 0.00, 0.00, 0.00, 0.00 };

double axis_y(NUM) = { 0.00, 0.00, 1.00, 1.00 };

double axis_z(NUM) = { 1.00, 1.00, 0.00, 0.00 };

dInitODE2(0);

fn.version = DS_VERSION; fn.start = &start;

fn.step = &simLoop; fn.command = &command;

fn.path_to_textures = “../../drawstuff/textures”;

```
dInitODE();
world = dWorldCreate();
dWorldSetGravity(world, 0, 0, -9.8);
//world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dJointGroupEmpty(contactgroup);
//dWorldSetGravity(world, 0, 0, -0.5);
// Create a ground
ground = dCreatePlane(space, 0, 0, 1, 0);
for (int i = 0; i < NUM; i++) {
link(i) = dBodyCreate(world);
dBodySetPosition(link(i), x(i), y(i), z(i));
dMassSetZero(&mass);
dMassSetCappedCylinderTotal(&mass, m(i), 3, r(i), l(i));
dBodySetMass(link(i), &mass);
}
joint(0) = dJointCreateFixed(world, 0);
dJointAttach(joint(0), link(0), 0);
dJointSetFixed(joint(0));
for (int j = 1; j < NUM; j++) {
joint(j) = dJointCreateHinge(world, 0);
dJointAttach(joint(j), link(j - 1), link(j));
dJointSetHingeAnchor(joint(j), anchor_x(j), anchor_y(j), anchor_z(j));
dJointSetHingeAxis(joint(j), axis_x(j), axis_y(j), axis_z(j));
}
dsSimulationLoop(argc, argv, 640, 570, &fn);
dCloseODE();
return 0;
```

}