人間 - wendyz @ ウィキ

概要

足、腕、胴体、頭から成る人間型のオブジェクトを生成する。 足は、足首、膝、股関節に関節を持ち可動範囲を人間に近づくよう設定する。 腕も同様に肘、方に関節を設定する。 頭は簡易的にただの球体をジョイントで接続した。

試しにボールをぶつけてみる

定義

static dSpaceID human_space;
static dBodyID  hLeg_R_body[3];
static dGeomID  hLeg_R_geom[3][10];
static dJointID hLeg_R_joint[3];
static dBodyID  hLeg_L_body[3];
static dGeomID  hLeg_L_geom[3][10];
static dJointID hLeg_L_joint[3];
static dBodyID  trunk_body;
static dGeomID  trunk_geom[3];
static dBodyID  head_body;
static dGeomID  head_geom;
static dJointID head_joint;
static dJointID hArm_L_joint[3];
static dBodyID  hArm_L_body[3];
static dGeomID  hArm_L_geom[3][10];
static dJointID hArm_R_joint[3];
static dBodyID  hArm_R_body[3];
static dGeomID  hArm_R_geom[3][10];
static dJointID hHand_R_joint;
static dBodyID  hHand_R_body;
static dGeomID  hHand_R_geom;
static dJointID hHand_L_joint;
static dBodyID  hHand_L_body;
static dGeomID  hHand_L_geom;

オブジェクト生成

#define HUMAN
#ifdef HUMAN

#undef SCALE
#define SCALE 5
#define HUMAN_LEG_BASE_X 0.11*SCALE
#define HUMAN_LEG_BASE_Y 0.25*SCALE
#define HUMAN_LEG_BASE_Z 0.05*SCALE
#define HUMAN_LEG_SHANK_X HUMAN_LEG_BASE_X*0.5
#define HUMAN_LEG_SHANK_Y HUMAN_LEG_BASE_X
#define HUMAN_LEG_SHANK_Z 0.25*SCALE
#define HUMAN_LEG_THIGH_X HUMAN_LEG_BASE_X*0.5
#define HUMAN_LEG_THIGH_Y HUMAN_LEG_BASE_X
#define HUMAN_LEG_THIGH_Z 0.15*SCALE
#define HUMAN_LEG_DISTANCE 0.15*SCALE
#define HUMAN_LEG_LENGTH (HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z)
#define MASS (1.0) 
#define HUMAN_HEAD_RAD 0.1*SCALE
	{
		dMass m;
 
		// R LEG
		hLeg_R_body[0] = dBodyCreate (world);
		dBodySetPosition(hLeg_R_body[0],HUMAN_LEG_DISTANCE,0,HUMAN_LEG_BASE_Z/2);
		dMassSetBox( &m, 1, HUMAN_LEG_BASE_X, HUMAN_LEG_BASE_Y, HUMAN_LEG_BASE_Z );
		dMassAdjust( &m, MASS );
		dBodySetMass( hLeg_R_body[0], &m );
		hLeg_R_geom[0][0] = dCreateBox (human_space,HUMAN_LEG_BASE_X,HUMAN_LEG_BASE_Y,HUMAN_LEG_BASE_Z);
		dGeomSetBody(hLeg_R_geom[0][0],hLeg_R_body[0]);
 
		hLeg_R_body[1] = dBodyCreate (world);
		hLeg_R_geom[1][0] = dCreateCapsule (human_space,HUMAN_LEG_SHANK_X,HUMAN_LEG_SHANK_Z);
		dGeomSetBody(hLeg_R_geom[1][0],hLeg_R_body[1]);
		dBodySetPosition(hLeg_R_body[1],HUMAN_LEG_DISTANCE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z/2+0.07*SCALE);
 
		hLeg_R_body[2] = dBodyCreate (world);
		hLeg_R_geom[2][0] = dCreateCapsule (human_space,HUMAN_LEG_THIGH_X,HUMAN_LEG_THIGH_Z);
		dGeomSetBody(hLeg_R_geom[2][0],hLeg_R_body[2]);
		dBodySetPosition(hLeg_R_body[2],HUMAN_LEG_DISTANCE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.2*SCALE);
 
		hLeg_R_joint[0] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_R_joint[0], hLeg_R_body[0], hLeg_R_body[1] );
		dJointSetHingeAnchor(hLeg_R_joint[0],HUMAN_LEG_DISTANCE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+0.03*SCALE);
 
		dJointSetHingeParam(hLeg_R_joint[0], dParamLoStop, -0.3 * M_PI);
		dJointSetHingeParam(hLeg_R_joint[0], dParamHiStop,  0 * M_PI);
 
		hLeg_R_joint[1] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_R_joint[1], hLeg_R_body[1], hLeg_R_body[2] );
		dJointSetHingeAnchor(hLeg_R_joint[1],HUMAN_LEG_DISTANCE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+0.14*SCALE);
 
		dJointSetHingeParam(hLeg_R_joint[1], dParamLoStop, -0.8 * M_PI);
		dJointSetHingeParam(hLeg_R_joint[1], dParamHiStop,  0 * M_PI);
 
		// L LEG
		hLeg_L_body[0] = dBodyCreate (world);
		dBodySetPosition(hLeg_L_body[0],0,0,HUMAN_LEG_BASE_Z/2);
		dMassSetBox( &m, 1, HUMAN_LEG_BASE_X, HUMAN_LEG_BASE_Y, HUMAN_LEG_BASE_Z );
		dMassAdjust( &m, MASS );
		dBodySetMass( hLeg_L_body[0], &m );
		hLeg_L_geom[0][0] = dCreateBox (human_space,HUMAN_LEG_BASE_X,HUMAN_LEG_BASE_Y,HUMAN_LEG_BASE_Z);
		dGeomSetBody(hLeg_L_geom[0][0],hLeg_L_body[0]);
 
		hLeg_L_body[1] = dBodyCreate (world);
		hLeg_L_geom[1][0] = dCreateCapsule (human_space,HUMAN_LEG_SHANK_X,HUMAN_LEG_SHANK_Z);
		dGeomSetBody(hLeg_L_geom[1][0],hLeg_L_body[1]);
		dBodySetPosition(hLeg_L_body[1],0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z/2+0.07*SCALE);
 
		hLeg_L_body[2] = dBodyCreate (world);
		hLeg_L_geom[2][0] = dCreateCapsule (human_space,HUMAN_LEG_THIGH_X,HUMAN_LEG_THIGH_Z);
		dGeomSetBody(hLeg_L_geom[2][0],hLeg_L_body[2]);
		dBodySetPosition(hLeg_L_body[2],0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.2*SCALE);
 
		hLeg_L_joint[0] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_L_joint[0], hLeg_L_body[0], hLeg_L_body[1] );
		dJointSetHingeAnchor(hLeg_L_joint[0],0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+0.02*SCALE);
 
		dJointSetHingeParam(hLeg_L_joint[0], dParamLoStop, -0.3 * M_PI);
		dJointSetHingeParam(hLeg_L_joint[0], dParamHiStop,  0 * M_PI);
 
		hLeg_L_joint[1] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_L_joint[1], hLeg_L_body[1], hLeg_L_body[2] );
		dJointSetHingeAnchor(hLeg_L_joint[1],0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+0.14*SCALE);
 
		dJointSetHingeParam(hLeg_L_joint[1], dParamLoStop, -0.8 * M_PI);
		dJointSetHingeParam(hLeg_L_joint[1], dParamHiStop,  0 * M_PI);
 
#define HUMAN_TRUNK_RAD 0.11*SCALE
#define HUMAN_TRUNK_LEN 0.2*SCALE
		// TRUNK＆HEAD
		trunk_body = dBodyCreate (world);
		trunk_geom[0] = dCreateCapsule (human_space,HUMAN_TRUNK_RAD,HUMAN_TRUNK_LEN);
		dGeomSetBody(trunk_geom[0],trunk_body);
		dBodySetPosition(trunk_body,HUMAN_LEG_DISTANCE/2,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+(HUMAN_TRUNK_LEN/2)+0.25*SCALE);
 
		head_body = dBodyCreate (world);
		head_geom = dCreateSphere (human_space,HUMAN_HEAD_RAD);
		dGeomSetBody(head_geom,head_body);
		dBodySetPosition(head_body,HUMAN_LEG_DISTANCE/2,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+HUMAN_HEAD_RAD+0.35*SCALE);
 
		hLeg_R_joint[2] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_R_joint[2], hLeg_R_body[2], trunk_body );
		dJointSetHingeAnchor(hLeg_R_joint[2],HUMAN_LEG_DISTANCE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+0.2*SCALE);
		dJointSetHingeParam(hLeg_R_joint[2], dParamLoStop, -0.2 * M_PI);
		dJointSetHingeParam(hLeg_R_joint[2], dParamHiStop,  0.65 * M_PI);
 
		hLeg_L_joint[2] = dJointCreateHinge( world, 0 );
		dJointAttach( hLeg_L_joint[2], hLeg_L_body[2], trunk_body );
		dJointSetHingeAnchor(hLeg_L_joint[2],0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+0.2*SCALE);
		dJointSetHingeParam(hLeg_L_joint[2], dParamLoStop, -0.2 * M_PI);
		dJointSetHingeParam(hLeg_L_joint[2], dParamHiStop,  0.65 * M_PI);
 
		head_joint = dJointCreateHinge( world, 0 );
		dJointAttach( head_joint, trunk_body, head_body );
		dJointSetHingeAnchor(head_joint,0,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+0.07*SCALE);
		dJointSetHingeParam(head_joint, dParamLoStop, 0 * M_PI);
		dJointSetHingeParam(head_joint, dParamHiStop,  0 * M_PI);
 
#define HUMAN_ARM_RAD 0.035*SCALE
#define HUMAN_ARM_LEN 0.15*SCALE
#define HUMAN_ARM2_LEN 0.15*SCALE
#define HUMAN_HAND_RAD 0.05*SCALE
		
		// R Arm
		hArm_R_body[0] = dBodyCreate (world);
		hArm_R_geom[0][0] = dCreateCapsule (human_space,HUMAN_ARM_RAD,HUMAN_ARM_LEN);
		dGeomSetBody(hArm_R_geom[0][0],hArm_R_body[0]);
		dBodySetPosition(hArm_R_body[0],HUMAN_TRUNK_RAD+0.13*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+0.15*SCALE);
 
		hArm_R_body[1] = dBodyCreate (world);
		hArm_R_geom[1][0] = dCreateCapsule (human_space,HUMAN_ARM_RAD,HUMAN_ARM2_LEN);
		dGeomSetBody(hArm_R_geom[1][0],hArm_R_body[0]);
		dBodySetPosition(hArm_R_body[1],HUMAN_TRUNK_RAD+0.13*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN-HUMAN_ARM_LEN+0.15*SCALE);
 
		hArm_R_joint[0] = dJointCreateBall( world, 0 );
		dJointAttach( hArm_R_joint[0], trunk_body, hArm_R_body[0] );
		dJointSetBallAnchor(hArm_R_joint[0],HUMAN_TRUNK_RAD+0.1*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+0.27*SCALE);
		dJointSetBallParam(hArm_R_joint[0], dParamLoStop, 0 * M_PI);
		dJointSetBallParam(hArm_R_joint[0], dParamHiStop,  0 * M_PI);
 
		hArm_R_joint[1] = dJointCreateHinge( world, 0 );
		dJointAttach( hArm_R_joint[1], hArm_R_body[1], hArm_R_body[0] );
		dJointSetHingeAnchor(hArm_R_joint[1],HUMAN_TRUNK_RAD+0.13*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN-HUMAN_ARM_LEN+0.23*SCALE);
		dJointSetHingeParam(hArm_R_joint[1], dParamLoStop, -0.2 * M_PI);
		dJointSetHingeParam(hArm_R_joint[1], dParamHiStop,  0.65 * M_PI);
 
		hHand_R_body = dBodyCreate (world);
		hHand_R_geom = dCreateSphere (human_space,HUMAN_HAND_RAD);
		dGeomSetBody(hHand_R_geom,hHand_R_body);
		dBodySetPosition(hHand_R_body,HUMAN_TRUNK_RAD+0.13*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.15*SCALE);
 
		hHand_R_joint = dJointCreateHinge( world, 0 );
		dJointAttach( hHand_R_joint, hHand_R_body, hArm_R_body[1] );
		dJointSetHingeAnchor(hHand_R_joint,HUMAN_TRUNK_RAD+0.13*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.15*SCALE);
		dJointSetHingeParam(hHand_R_joint, dParamLoStop, 0 * M_PI);
		dJointSetHingeParam(hHand_R_joint, dParamHiStop,  0 * M_PI);
 
		// L Arm
		hArm_L_body[0] = dBodyCreate (world);
		hArm_L_geom[0][0] = dCreateCapsule (human_space,HUMAN_ARM_RAD,HUMAN_ARM_LEN);
		dGeomSetBody(hArm_L_geom[0][0],hArm_L_body[0]);
		dBodySetPosition(hArm_L_body[0],-0.095*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+0.15*SCALE);
 
		hArm_L_body[1] = dBodyCreate (world);
		hArm_L_geom[1][0] = dCreateCapsule (human_space,HUMAN_ARM_RAD,HUMAN_ARM2_LEN);
		dGeomSetBody(hArm_L_geom[1][0],hArm_L_body[0]);
		dBodySetPosition(hArm_L_body[1],-0.095*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN-HUMAN_ARM_LEN+0.15*SCALE);
 
		hArm_L_joint[0] = dJointCreateBall( world, 0 );
		dJointAttach( hArm_L_joint[0], trunk_body, hArm_L_body[0] );
		dJointSetBallAnchor(hArm_L_joint[0],-0.075*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN+0.27*SCALE);
		dJointSetBallParam(hArm_L_joint[0], dParamLoStop, 0 * M_PI);
		dJointSetBallParam(hArm_L_joint[0], dParamHiStop,  0 * M_PI);
 
		hArm_L_joint[1] = dJointCreateHinge( world, 0 );
		dJointAttach( hArm_L_joint[1], hArm_L_body[1], hArm_L_body[0] );
		dJointSetHingeAnchor(hArm_L_joint[1],-0.095*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_LENGTH+HUMAN_TRUNK_LEN-HUMAN_ARM_LEN+0.23*SCALE);
		dJointSetHingeParam(hArm_L_joint[1], dParamLoStop, -0.2 * M_PI);
		dJointSetHingeParam(hArm_L_joint[1], dParamHiStop,  0.65 * M_PI);
 
		hHand_L_body = dBodyCreate (world);
		hHand_L_geom = dCreateSphere (human_space,HUMAN_HAND_RAD);
		dGeomSetBody(hHand_L_geom,hHand_L_body);
		dBodySetPosition(hHand_L_body,-0.095*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.15*SCALE);
 
		hHand_L_joint = dJointCreateHinge( world, 0 );
		dJointAttach( hHand_L_joint, hHand_L_body, hArm_L_body[1] );
		dJointSetHingeAnchor(hHand_L_joint,-0.095*SCALE*SCALE,-HUMAN_LEG_BASE_Y/4,HUMAN_LEG_BASE_Z+HUMAN_LEG_SHANK_Z+HUMAN_LEG_THIGH_Z/2+0.15*SCALE);
		dJointSetHingeParam(hHand_L_joint, dParamLoStop, 0 * M_PI);
		dJointSetHingeParam(hHand_L_joint, dParamHiStop,  0 * M_PI);
 
 
	}
#endif

描画部分

#ifdef HUMAN
	// DRAW R LEG
	{
		dVector3 v3; 
		dGeomBoxGetLengths (hLeg_R_geom[0][0], v3);
		dsDrawBox(
			dBodyGetPosition(hLeg_R_body[0]),
			dBodyGetRotation(hLeg_R_body[0]),
			v3
			);
	}	
	for(int i=1;i<3;i++)
	{
		dReal  rad,len;
		dGeomCapsuleGetParams (hLeg_R_geom[i][0], &rad,&len);
		dsDrawCapsule(
			dBodyGetPosition(hLeg_R_body[i]),
			dBodyGetRotation(hLeg_R_body[i]),
			len,rad
			);
	}
 
	for(i=0;i<3;i++){
		dVector3 v3;
		dMatrix3 R1;
		dRSetIdentity(R1);
		dJointGetHingeAnchor(hLeg_L_joint[i], v3);
		dsDrawSphere(v3, R1, 0.05*SCALE);
	}
 
	// DRAW L LEG
	{
		dVector3 v3;
		dGeomBoxGetLengths (hLeg_L_geom[0][0], v3);
		dsDrawBox(
			dBodyGetPosition(hLeg_L_body[0]),
			dBodyGetRotation(hLeg_L_body[0]),
			v3
			);
	}	
	for(int i=1;i<3;i++)
	{
		dReal  rad,len;
		dGeomCapsuleGetParams (hLeg_L_geom[i][0], &rad,&len);
		dsDrawCapsule(
			dBodyGetPosition(hLeg_L_body[i]),
			dBodyGetRotation(hLeg_L_body[i]),
			len,rad
			);
	}
	for(i=0;i<3;i++){
		dVector3 v3;
		dMatrix3 R1;
		dRSetIdentity(R1);
		dJointGetHingeAnchor(hLeg_R_joint[i], v3);
		dsDrawSphere(v3, R1, 0.05*SCALE);
	}
 
	//DRAW TRUNK
	{
		dReal  rad,len;
		dGeomCapsuleGetParams (trunk_geom[0], &rad,&len);
		dsDrawCapsule(
			dBodyGetPosition(trunk_body),
			dBodyGetRotation(trunk_body),
			len,rad
			);
 
		dsDrawSphere (dBodyGetPosition(head_body),dBodyGetRotation(head_body),
			HUMAN_HEAD_RAD);
	}
 
	//DRAW ARM
	for(int i=0;i<2;i++)
	{
		dReal  rad,len;
		dGeomCapsuleGetParams (hArm_R_geom[i][0], &rad,&len);
		dsDrawCapsule(
			dBodyGetPosition(hArm_R_body[i]),
			dBodyGetRotation(hArm_R_body[i]),
			len,rad
			);
		dGeomCapsuleGetParams (hArm_L_geom[i][0], &rad,&len);
		dsDrawCapsule(
			dBodyGetPosition(hArm_L_body[i]),
			dBodyGetRotation(hArm_L_body[i]),
			len,rad
			);
	}
 
	//DRAW HAND
	{
		dsDrawSphere(
			dBodyGetPosition(hHand_R_body),
			dBodyGetRotation(hHand_R_body),
			dGeomSphereGetRadius (hHand_R_geom)
			);
 
		dsDrawSphere(
			dBodyGetPosition(hHand_L_body),
			dBodyGetRotation(hHand_L_body),
			dGeomSphereGetRadius (hHand_L_geom)
			);
	}
 
	for(i=0;i<2;i++){
		dVector3 v3;
		dMatrix3 R1;
		dRSetIdentity(R1);
 
		float rad=0.0f;
		if(i==0){
			rad = 0.07*SCALE;
			dJointGetBallAnchor(hArm_R_joint[i], v3);
		}
		else	{
			rad = 0.04*SCALE;
			dJointGetHingeAnchor(hArm_R_joint[i], v3);
		}
		dsDrawSphere(v3, R1, rad);
 
		if(i==0){
			rad = 0.07*SCALE;
			dJointGetBallAnchor(hArm_L_joint[i], v3);
		}
		else	{
			dJointGetHingeAnchor(hArm_L_joint[i], v3);
			rad = 0.04*SCALE;
		}
		dsDrawSphere(v3, R1, rad);
	}
#endif HUMAN