0
\$\begingroup\$

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 %f\n", 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;

}

\$\endgroup\$
6
  • 1
    \$\begingroup\$ What you're asking is quite broad, you're asking us to 1) show you how to use a physics engine, 2) show you how to detect collisions and 3) detect the force. Do you know how to use a physics engine? Have you tried anything? \$\endgroup\$ – Vaillancourt Jun 22 '20 at 1:46
  • 1
    \$\begingroup\$ Presumably you started by researching the published documentation about how to work with collisions in ODE. Can you show us what you've done so far based on your research, or explain where specifically you're stuck? \$\endgroup\$ – DMGregory Jun 22 '20 at 1:49
  • \$\begingroup\$ I was working with sketchyphysics in google sketchup but in this case I do not know how to read the collision force vector between two bodies. So now I am looking for a physics engine where I can read the collision force vector between two bodies. I installed ODE and I am trying to work with it. \$\endgroup\$ – threewire Jun 22 '20 at 2:07
  • \$\begingroup\$ What do you need it for? This might help figure out what to tell you. \$\endgroup\$ – Vaillancourt Jun 22 '20 at 3:38
  • \$\begingroup\$ I am trying to get the collision force vector from the tip of the robotic arm when ever it collides with the ground or some other object. I have edited my question and added the code that I am working with. \$\endgroup\$ – threewire Jun 22 '20 at 5:48

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.