ODE的buggy例程分析
ODE (Open Dynamic Engine) 是一个免费的具有工业品质的刚体动力学的库，一款优秀的开源物理引擎。它能很好地仿真现实环境中的可移动物体，而且它有内建的碰撞检测系统。
最近从网上看到了ODE，不禁有一种跃跃欲试的冲动，于是这两天就小试了一下。虽然ODE这个库已经使用了十几年了，但是资料还是比较少，中文的更是寥寥可数，只有几篇官网教程的翻译，也只有前几章的，不是很全。貌似这种日本做的东西，文档做的都不是很好呀。以下是几篇教程的链接：
ODE文档的部分翻译：
http://www.cnblogs.com/muxi/archive/2012/03/13/2394752.html
ODE 教程：
http://bbs.sjtu.edu.cn/bbscon,board,GNULinux,file,M.1274284081.A.html
http://hi.baidu.com/ujbiogeffebcmnd/item/86a926cbadea4227e80f2ef0
没办法，就开始看官方源码中的demo，今天花了一下午时间看了buggy的例程，对照着在线手册了解了程序中的API，简单做了一下中文注释。
/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * * All rights reserved. Email: [email protected] Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ /* buggy with suspension. this also shows you how to use geom groups. */ #define dSINGLE //定义为单精度 #include <ode/ode.h> #include <drawstuff/drawstuff.h> #define DRAWSTUFF_TEXTURE_PATH "textures" //纹理文件的路径 // select correct drawing functions #ifdef dDOUBLE #define dsDrawBox dsDrawBoxD #define dsDrawSphere dsDrawSphereD #define dsDrawCylinder dsDrawCylinderD #define dsDrawCapsule dsDrawCapsuleD #endif // some constants #define LENGTH 0.7 // 车壳长度 #define WIDTH 0.5 // 车壳宽度 #define HEIGHT 0.2 // 车壳高度 #define RADIUS 0.18 // 轮子半径 #define STARTZ 0.5 // 开始车壳的位置 #define CMASS 1 // 车壳的质量 #define WMASS 0.2 // 轮子的质量 // dynamics and collision objects (chassis, 3 wheels, environment) static dWorldID world; //动力学计算使用的world static dSpaceID space; //检测碰撞使用的space static dBodyID body[4]; //设置车体的ID(三个轮子，一个车体) static dJointID joint[3]; // joint[0]是前轮 static dJointGroupID contactgroup; static dGeomID ground; //大地 static dSpaceID car_space; static dGeomID box[1]; //车体geom的ID static dGeomID sphere[3]; //车轮geom的ID static dGeomID ground_box; //障碍物geom的ID // things that the user controls static dReal speed=0,steer=0; // 用户命令 // 碰撞检测的callback函数 static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,n; // 选择碰撞检测中的2个中的一个作为标志； // 如果有一个碰撞物为ground或者障碍物则把标志位g1\g2置为1 int g1 = (o1 == ground || o1 == ground_box); int g2 = (o2 == ground || o2 == ground_box); if (!(g1 ^ g2)) return; const int N = 10; //接触点的上限是10个 dContact contact[N]; n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); //n是碰撞的次数 if (n > 0) { for (i=0; i<n; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; //设定地面模式 contact[i].surface.mu = dInfinity; //库伦摩擦系数(dInfinity:无穷大) contact[i].surface.slip1 = 0.1; //滑动系数 contact[i].surface.slip2 = 0.1; contact[i].surface.soft_erp = 0.5; //这两个好像是设置柔软度的 contact[i].surface.soft_cfm = 0.3; //生成contactjoint dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); //用contactjoint对接触的两个geom进行约束 dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } } // 开始仿真，这是视角 static void start() { dAllocateODEDataForThread(dAllocateMaskAll); static float xyz[3] = {0.8317f,-0.9817f,0.8000f}; //视线的位置 static float hpr[3] = {121.0000f,-27.5000f,0.0000f}; //视线的方向 dsSetViewpoint (xyz,hpr); //设定视线 printf ("Press:\t'a' to increase speed.\n" "\t'z' to decrease speed.\n" "\t',' to steer left.\n" "\t'.' to steer right.\n" "\t' ' to reset speed and steering.\n" "\t'1' to save the current state to 'state.dif'.\n"); } //键盘操作的回调函数 static void command (int cmd) { switch (cmd) { case 'a': case 'A': speed += 0.3; break; case 'z': case 'Z': speed -= 0.3; break; case ',': steer -= 0.5; break; case '.': steer += 0.5; break; case ' ': speed = 0; steer = 0; break; case '1': { FILE *f = fopen ("state.dif","wt"); if (f) { dWorldExportDIF (world,f,""); fclose (f); } } } } // 仿真循环 // 电机驱动和转向驱动都是在前轮 static void simLoop (int pause) { int i; if (!pause) { // motor dJointSetHinge2Param (joint[0],dParamVel2,-speed); //设置期望速度 dJointSetHinge2Param (joint[0],dParamFMax2,0.1); //电机驱动的最大扭力 // steering dReal v = steer - dJointGetHinge2Angle1 (joint[0]); if (v > 0.1) v = 0.1; if (v < -0.1) v = -0.1; v *= 10.0; dJointSetHinge2Param (joint[0],dParamVel,v); dJointSetHinge2Param (joint[0],dParamFMax,0.2); dJointSetHinge2Param (joint[0],dParamLoStop,-0.75); //最小停止角度 dJointSetHinge2Param (joint[0],dParamHiStop,0.75); //最大停止角度 dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1); //防止启动跳动 dSpaceCollide (space,0,&nearCallback); //碰撞检测，最初写入的值 dWorldStep (world,0.05); //决定simulation的stepsize // remove all contact joints dJointGroupEmpty (contactgroup); //将contactgroup置空 } //DrawStuff绘制 dsSetColor (0,1,1); //颜色切换 dsSetTexture (DS_WOOD); //纹理切换 dReal sides[3] = {LENGTH,WIDTH,HEIGHT}; //车壳的尺寸 dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides); //绘制车壳 dsSetColor (1,1,1); for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),0.02f,RADIUS); //绘制车轮 dVector3 ss; dGeomBoxGetLengths (ground_box,ss); dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss); //绘制障碍 } int main (int argc, char **argv) { int i; dMass m; // setup pointers to drawstuff callback functions dsFunctions fn; // drawstuff结构体 fn.version = DS_VERSION; // drawstuff的版本 fn.start = &start; // 仿真的前处理函数 fn.step = &simLoop; // 仿真的每一步被调用的函数 fn.command = &command; // 键盘输入 fn.stop = 0; // 没有函数，因此设定为NULL指针 fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; // 纹理文件的路径 // create world dInitODE2(0); //初始化ODE环境 world = dWorldCreate(); //创建世界 space = dHashSpaceCreate (0); //创建碰撞检测用space，返回它的ID。 contactgroup = dJointGroupCreate (0); //生成jointGroup dWorldSetGravity (world,0,0,-0.5); //重力加速度 ground = dCreatePlane (space,0,0,1,0); //创建一个ground // 创建车体 body[0] = dBodyCreate (world); dBodySetPosition (body[0],0,0,STARTZ); //设置初始位置 dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT); dMassAdjust (&m,CMASS); dBodySetMass (body[0],&m); //设置质量 box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT); //生成车体的geom dGeomSetBody (box[0],body[0]); //设置车体中的geom，给物体的两个属性形状geom和刚体body添加关联。 // wheel bodies for (i=1; i<=3; i++) { body[i] = dBodyCreate (world); //创建车轮的body dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); //计算车轮的旋转角度 dBodySetQuaternion (body[i],q); //设置车轮的旋转角度 dMassSetSphere (&m,1,RADIUS); dMassAdjust (&m,WMASS); dBodySetMass (body[i],&m); //设置质量 sphere[i-1] = dCreateSphere (0,RADIUS); dGeomSetBody (sphere[i-1],body[i]); //设置geom } dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5); //设置初始位置 dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5); dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5); // front and back wheel hinges for (i=0; i<3; i++) { joint[i] = dJointCreateHinge2 (world,0); //创建joint dJointAttach (joint[i],body[0],body[i+1]); //设置joint的连接体 const dReal *a = dBodyGetPosition (body[i+1]); //获取位置 dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]); //设置joint的位置 dJointSetHinge2Axis1 (joint[i],0,0,1); //设置旋转轴 dJointSetHinge2Axis2 (joint[i],0,1,0); } // 设置joint的悬挂参数 // 如果不需要悬挂可以将参数设置为0 for (i=0; i<3; i++) { dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4); dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8); } // 固定后面两个轮子 for (i=1; i<3; i++) { // set stops to make sure wheels always stay in alignment dJointSetHinge2Param (joint[i],dParamLoStop,0); dJointSetHinge2Param (joint[i],dParamHiStop,0); } // 创建car的space，并且包含在总的space中 car_space = dSimpleSpaceCreate (space); dSpaceSetCleanup (car_space,0); dSpaceAdd (car_space,box[0]); dSpaceAdd (car_space,sphere[0]); dSpaceAdd (car_space,sphere[1]); dSpaceAdd (car_space,sphere[2]); // 设置障碍 ground_box = dCreateBox (space,2,1.5,1); dMatrix3 R; dRFromAxisAndAngle (R,0,1,0,-0.15); dGeomSetPosition (ground_box,2,0,-0.34); dGeomSetRotation (ground_box,R); // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dGeomDestroy (box[0]); dGeomDestroy (sphere[0]); dGeomDestroy (sphere[1]); dGeomDestroy (sphere[2]); dJointGroupDestroy (contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
运行效果：
程序中比较难理解的是一些参数的设置和API的使用。
当发生碰撞时，调用回调函数nearCallback，其中需要设置一些碰撞点的参数，具体的参数可以参考用户手册中的说明：
joint的配置中也有一些参数的设置：
以上均参考在线文档：
API手册：http://robotics.naist.jp/~akihiko-y/doxy/ode0.9/index.html
用户手册：http://www.ode.org/ode-latest-userguide.html