Open Dynamics engine(2)

一、主函数

1.初始化：

dInitODE();

2.创建世界

world = dWorldCreate();

3.设置重力加速度

dWorldSetGravity(world, 0, 0, -9.8);

2.创建$space$：

space = dSimpleSpaceCreate(0);

4.设置质量、质心、惯性张量：

dMass mass;
dMassSetBox(&mass, 1, 1, 1, 1);
dBodySetMass(obj[0].body, &mass);

5.创建几何体：

obj[0].geom = dCreateBox(space, 1, 1, 1);

6.将几何体与物体关联：

dGeomSetBody(obj[0].geom, obj[0].body);

6.创建关节、设置轴与锚点：

joint = dJointCreateHinge(world, 0);
dJointSetHingeAxis(joint, 0, 0, 1);
dJointSetHingeAnchor(joint, 0, 0, 0.5);

7.连接关节与物体：

dJointAttach(joint, obj[0].body, obj[1].body);

8.设置物体初始速度：
dReal x1 = 0.0;
dReal y1 = 0.0;
dReal z1 = 0.0;
dReal x2 = 0.5;
dReal y2 = 0.5;
dReal z2 = 0.0;
dBodySetLinearVel(obj[0].body, x1, y1, z1);
dBodySetLinearVel(obj[1].body, x2, y2, z2);

二、模拟循环

1.首先进行碰撞检测，对整个$space$内的物体首先进行粗略碰撞检测，再进行精细检测

dSpaceCollide(space, 0, &nearCallback);

$dCollide$函数对候选几何体再做一次检测，返回碰撞点的个数

n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));

若检测到碰撞点，创建碰撞关节并连接两个几何体

dJointID c = dJointCreateContact(world, contactgroup, contact + i);
dJointAttach(c, dGeomGetBody(o1), dGeomGetBody(o2));

碰撞处理

contact[i].surface.mu = 0.5;
contact[i].surface.slip1 = 0.0;
contact[i].surface.slip2 = 0.0;
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 0.01;

2.更新世界

dWorldStep(world, 0.002);

3.可视化

for (int i = 0; i < 2; i++)
	 {
	     drawGeom(obj[i].geom);
	 }

二、代码

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include "texturepath.h"


#define dsDrawBox dsDrawBoxD

struct Object {
    dBodyID body;			// the body
    dGeomID geom;		// geometries representing this body
};
dWorldID world;
dSpaceID space;

dJointID joint;
Object obj[2];
static dJointGroupID contactgroup;

static void nearCallback(void*, dGeomID o1, dGeomID o2)
{
    int i, n;

    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    if (b1 && b2 && dAreConnected(b1, b2))
        return;

    const int N = 4;
    dContact contact[N];
    n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
    if (n > 0) {
        for (i = 0; i < n; i++) {
            contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;

            contact[i].surface.mu = 0.5;
            contact[i].surface.slip1 = 0.0;
            contact[i].surface.slip2 = 0.0;
            contact[i].surface.soft_erp = 0.8;
            contact[i].surface.soft_cfm = 0.01;
            dJointID c = dJointCreateContact(world, contactgroup, contact + i);
            dJointAttach(c, dGeomGetBody(o1), dGeomGetBody(o2));
        }
    }
}

void start()
{
    // initial camera position
    float xyz[3] = { 3.8966, -2.0614, 4.0300 };
    float hpr[3] = { 153.5, -16.5, 0 };
    dsSetViewpoint(xyz, hpr);
}

void stop()
{
    dSpaceDestroy(space);

    dWorldDestroy(world);
}


void drawGeom(dGeomID g)
{
    int gclass = dGeomGetClass(g);
    const dReal* pos = dGeomGetPosition(g);
    const dReal* rot = dGeomGetRotation(g);

   
    dVector3 lengths;
    dsSetColorAlpha(1, 1, 0, 1);
    dsSetTexture(DS_WOOD);
    dGeomBoxGetLengths(g, lengths);
    dsDrawBox(pos, rot, lengths);
      
}

void simLoop(int pause)
{
    dSpaceCollide(space, 0, &nearCallback);
   
    if (!pause)
        /*可选择添加外力*/
        //dBodyAddForce(obj[0].body, 1, 0, 0);
        dWorldStep(world, 0.002);
    
    // now we draw everything
    for (int i = 0; i < 2; i++)
    {
        drawGeom(obj[i].geom);
    }

}
int main(int argc, char** argv)
{
     //setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.command = 0;
    fn.stop = stop;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

     //create world
    dInitODE();
    world = dWorldCreate();

    dWorldSetGravity(world, 0, 0, -0.5);

    dWorldSetDamping(world, 1e-4, 1e-5);
        dWorldSetERP(world, 1);

    space = dSimpleSpaceCreate(0);
    dCreatePlane(space, 0, 0, 1, 0);
    obj[0].body = dBodyCreate(world);
    obj[1].body = dBodyCreate(world);

    dMass mass;

    dBodySetPosition(obj[0].body, -1, 0, 0.5);
    dBodySetPosition(obj[1].body, 1, 0, 0.5);

    obj[0].geom = dCreateBox(space, 1, 1, 1);
    dGeomSetBody(obj[0].geom, obj[0].body);
    dMassSetBox(&mass, 1, 1, 1, 1);
    dBodySetMass(obj[0].body, &mass);

    obj[1].geom = dCreateBox(space, 1, 1, 1);
    dGeomSetBody(obj[1].geom, obj[1].body);
    dMassSetBox(&mass, 1, 1, 1, 1);
    dBodySetMass(obj[1].body, &mass);


    joint = dJointCreateHinge(world, 0);
    dJointSetHingeAxis(joint, 0, 0, 1);
    dJointSetHingeAnchor(joint, 0, 0, 0.5);
    dJointAttach(joint, obj[0].body, obj[1].body);
    

    dReal x1 = 0.0;
    dReal y1 = 0.0;
    dReal z1 = 0.0;
    dReal x2 = 0.5;
    dReal y2 = 0.5;
    dReal z2 = 0.0;
    dBodySetLinearVel(obj[0].body, x1, y1, z1);
    dBodySetLinearVel(obj[1].body, x2, y2, z2);
    

     //run demo
    dsSimulationLoop(argc, argv, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &fn);

    dCloseODE();
    return 0;
}