Open Dynamics engine(2)

一、主函数

1.初始化：

dInitODE();
1

2.创建世界

world = dWorldCreate();
1

3.设置重力加速度

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

2.创建$space$：

space = dSimpleSpaceCreate(0);
1

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

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

5.创建几何体：

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

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

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

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

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

7.连接关节与物体：

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

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);
1

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

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

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

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

碰撞处理

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;
1
2
3
4
5

2.更新世界

dWorldStep(world, 0.002);
1

3.可视化

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

二、代码

#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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155