ブランコ - wendyz @ ウィキ

定義

static dSpaceID swings_space;
static dBodyID  swings_base_body[5];
static dGeomID  swings_base_geom[100];
static dBodyID  swings_chain_body[100];
static dGeomID  swings_chain_geom[100];
static dGeomID  swings_base_trans[100];
static dBodyID  swings_base_trans_body;

オブジェクト生成

#define SWINGS
#ifdef SWINGS

#undef SCALE
#define SCALE 5
#define SWINGS_BASE_RAD 0.07*SCALE
#define SWINGS_BASE_LEN 2.5*SCALE
#define SWINGS_BASE_POS_X  0
#define SWINGS_BASE_POS_Y -10

	{
 
 
		swings_base_trans_body = dBodyCreate(world);
 
		swings_base_trans[0] = dCreateGeomTransform(swings_space);
		dGeomTransformSetCleanup(swings_base_trans[0],1); 		
		swings_base_geom[0] = dCreateCapsule (0,SWINGS_BASE_RAD,SWINGS_BASE_LEN);
		dGeomTransformSetGeom(swings_base_trans[0],swings_base_geom[0]);
		dGeomSetPosition (swings_base_geom[0],SWINGS_BASE_POS_X,SWINGS_BASE_POS_Y,SWINGS_BASE_LEN*0.4);
		{
			dMatrix3 R;
			dRFromAxisAndAngle(R, 1.0, 0.0, 0.0,  M_PI/ 6.0);
			dGeomSetRotation(swings_base_geom[0],R);
		}
 
		swings_base_trans[1] = dCreateGeomTransform(swings_space);
		dGeomTransformSetCleanup(swings_base_trans[1],1); 		
		swings_base_geom[1] = dCreateCapsule (0,SWINGS_BASE_RAD,SWINGS_BASE_LEN);
		dGeomTransformSetGeom(swings_base_trans[1],swings_base_geom[1]);
		dGeomSetPosition (swings_base_geom[1],SWINGS_BASE_POS_X,SWINGS_BASE_POS_Y-1.3*SCALE,SWINGS_BASE_LEN*0.4);
		{
			dMatrix3 R;
			dRFromAxisAndAngle(R, -1.0, 0.0, 0.0,  M_PI/ 6.0);
			dGeomSetRotation(swings_base_geom[1],R);
		}
 
		swings_base_trans[2] = dCreateGeomTransform(swings_space);
		dGeomTransformSetCleanup(swings_base_trans[2],1); 		
		swings_base_geom[2] = dCreateCapsule (0,SWINGS_BASE_RAD,SWINGS_BASE_LEN);
		dGeomTransformSetGeom(swings_base_trans[2],swings_base_geom[2]);
		dGeomSetPosition (swings_base_geom[2],SWINGS_BASE_POS_X+3.0*SCALE,SWINGS_BASE_POS_Y,SWINGS_BASE_LEN*0.4);
		{
			dMatrix3 R;
			dRFromAxisAndAngle(R, 1.0, 0.0, 0.0,  M_PI/ 6.0);
			dGeomSetRotation(swings_base_geom[2],R);
		}
 
		swings_base_trans[3] = dCreateGeomTransform(swings_space);
		dGeomTransformSetCleanup(swings_base_trans[3],1); 		
		swings_base_geom[3] = dCreateCapsule (0,SWINGS_BASE_RAD,SWINGS_BASE_LEN);
		dGeomTransformSetGeom(swings_base_trans[3],swings_base_geom[3]);
		dGeomSetPosition (swings_base_geom[3],SWINGS_BASE_POS_X+3.0*SCALE,SWINGS_BASE_POS_Y-1.3*SCALE,SWINGS_BASE_LEN*0.4);
		{
			dMatrix3 R;
			dRFromAxisAndAngle(R, -1.0, 0.0, 0.0,  M_PI/ 6.0);
			dGeomSetRotation(swings_base_geom[3],R);
		}
 
		swings_base_trans[4] = dCreateGeomTransform(swings_space);
		dGeomTransformSetCleanup(swings_base_trans[4],1); 		
		swings_base_geom[4] = dCreateCapsule (0,SWINGS_BASE_RAD,SWINGS_BASE_LEN*1.2);
		dGeomTransformSetGeom(swings_base_trans[4],swings_base_geom[4]);
		dGeomSetPosition (swings_base_geom[4],SWINGS_BASE_POS_X+1.5*SCALE,SWINGS_BASE_POS_Y-0.66*SCALE,SWINGS_BASE_LEN*0.85);
		{
			dMatrix3 R;
			dRFromAxisAndAngle(R, 0.0, 1.0, 0.0,  M_PI/2.0);
			dGeomSetRotation(swings_base_geom[4],R);
		}
 
		dGeomSetBody(swings_base_trans[0],swings_base_trans_body);
		dGeomSetBody(swings_base_trans[1],swings_base_trans_body);
		dGeomSetBody(swings_base_trans[2],swings_base_trans_body);
		dGeomSetBody(swings_base_trans[3],swings_base_trans_body);
		dGeomSetBody(swings_base_trans[4],swings_base_trans_body);		
 
	}
#endif

Geom描画関数

static void drawGeom(dGeomID g, const dReal *pos, const dReal *R)
{
  if (!g) return;
  if (!pos) pos = dGeomGetPosition(g);
  if (!R) R = dGeomGetRotation(g);
 
  int type = dGeomGetClass(g);
 
  if (type == dCCylinderClass) {
    dReal radius,length;
    dGeomCCylinderGetParams(g,&radius,&length);
    dsDrawCappedCylinderD(pos,R,length,radius);
  }
  else if (type == dSphereClass) {
    dsDrawSphereD(pos,R,dGeomSphereGetRadius (g));
  }
  else if (type == dGeomTransformClass) {
    dGeomID g2 = dGeomTransformGetGeom(g);
    const dReal *pos2 = dGeomGetPosition(g2);
    const dReal *R2   = dGeomGetRotation(g2);
    dVector3 actual_pos;
    dMatrix3 actual_R;
    dMULTIPLY0_331(actual_pos,R,pos2);
    actual_pos[0] += pos[0];
    actual_pos[1] += pos[1];
    actual_pos[2] += pos[2];
    dMULTIPLY0_333 (actual_R,R,R2);
    drawGeom(g2,actual_pos,actual_R);
  }
}

描画部分

#ifdef SWINGS
	{
		drawGeom(swings_base_geom[0],0,0);
		drawGeom(swings_base_geom[1],0,0);
		drawGeom(swings_base_geom[2],0,0);
		drawGeom(swings_base_geom[3],0,0);
		drawGeom(swings_base_geom[4],0,0);
	}
#endif