ODE仿真引擎使用(九)

  这里，首先解释了一个单位系统和ODE的坐标。其次，解释如何设置和获得body的位置和旋转。
Unit system
   ODE不介意任何单位系统。而角是弧度。在本例中，将使用SI单位系统。这对我们(不是美国)来说很熟悉。长度为[m]，重量为[kg]，时间为[s]。

Coordinate system
  ODE的坐标系如图所示，是直角坐标系。它被广泛应用于物理和数学中。如图所示，共有9座小金字塔。坐标系的原点是金字塔的中心，x轴是从中心到红色金字塔的方向。y轴是从中心到蓝色金字塔的方向。 z轴是从中心到天空的方向。这些教程采用SI单位制，所以金字塔之间的距离是1[m]。
Setting position and posture
   A position and an orientation of a body must be set. The following APIs are used for the purpose.
   void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z);
  Set a position (x, y, z) in the absolute coordinate system.
   void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle);
  Calculate a rotation matrix from a rotation axis vector (ax, ay, az) and rotational angle [rad] which rotates counterclockwisely.
   void dBodySetRotation (dBodyID, const dMatrix3 R);
  Set an orientation of the body with the rotation matrix R.
  In addition, Quaternion can be used in ODE. The API is
   dBodySetQuaternion (dBodyID, const dQuaternion q).
   Getting the position and posture
   const dReal *dBodyGetPosition (dBodyID);
   Get a positon of the body. The return value is a pointer to an array.
   const dReal *dBodyGetRotation (dBodyID);
   Get a ratation matrix of the body. The return value is a pointer to an array.
  在下面的示例代码中，一个圆柱体位于(0.0,0.0,1.0)，并设置了圆柱体沿x轴旋转45度(M_PI/4.0)的方向。此外，身体的重心位置由printPos()显示。

typedef struct {
  dBodyID body;
  dGeomID geom;
} MyObject;

MyObject pillar;

void createPillar()
{
  dMass m1;
  dReal radius = 0.1, length = 0.5, mass = 1.0; // radius, length, mass

  pillar.body = dBodyCreate(world);
  dMassSetZero(&m1);
  dMassSetCylinderTotal(&m1,mass,3,radius,length);
  dBodySetMass(pillar.body,&m1);
  dBodySetPosition(pillar.body,0.0,0.0,1.0); // Set a position to (0.0,0.0,1.0)[m]

  dMatrix3 R;
  dRFromAxisAndAngle(R,1.0,0.0,0.0, M_PI/4.0);　// Rotate PI/4[rad] along X axis
  dBodySetRotation(pillar.body,R);
  pillar.geom = dCreateCylinder(space,radius,length);
  dGeomSetBody(pillar.geom,pillar.body);
}

void printPos(dBodyID obj_body) // print a position
{
  const dReal *pos;　　// Do not forget const to prevent an error
  dReal x, y, z;
  pos = dBodyGetPosition(obj_body);　// Return value is an pointer to a structure
  x = pos[0];　   y = pos[1];   z = pos[2];
  printf("x=%f y=%f z=%f \n", x, y, z);
}