예제 #1
0
void PhysicsBody::setCapsuleMassTotal( Real32 totalMass, Int32 direction, 
        Real32 radius, Real32 length )
{
    dMass mass;
    dMassSetCapsuleTotal(&mass, totalMass, direction, radius, length);
    setMassStruct(mass);
}
예제 #2
0
 void createPart(const dWorldID & _world , dPart & part, dReal * pos3){
     dMass mass;
     part.body = dBodyCreate(_world);
     dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position
     dMassSetZero(&mass);                        // Set mass parameter to zero
     dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length);  // Calculate mass parameter
     dBodySetMass(part.body, &mass);  // Set mass
 }
예제 #3
0
파일: ode_policies.cpp 프로젝트: mempko/ncc
 void capsule::set_mass(double mass)
 {
   if(body_id)
     {
         dMass dmass;
         dMassSetZero(&dmass); 
         dMassSetSphereTotal(&dmass,mass,radius);
         dMassSetCapsuleTotal(&dmass,mass,3,radius,length);
         dBodySetMass (body_id, &dmass);
         material.mass = mass;
     }
 }
예제 #4
0
    void cPhysicsObject::CreateCapsule(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot)
    {
      geom = dCreateCapsule(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(), fRadius, fLength);

      InitCommon(pWorld, pos, rot);

      if (bBody) {
        dMass mass;
        dMassSetCapsuleTotal(&mass, fMassKg, uiDirectionX, 2.0f*fRadius, fLength);
        dBodySetMass(body, &mass);
      }
    }
예제 #5
0
파일: 6axis4.cpp 프로젝트: Ry0/ODE
/*** ロボットアームの生成 ***/
void  makeArm()
{
  dMass mass;                                    // 質量パラメータ
  dReal x[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 x
  dReal y[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 y
  dReal z[NUM]      = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0};  // 重心 z
  dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50};  // 長さ
  dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50};  // 質量
  dReal r[NUM-1]      = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};  // 半径
  dReal c_x[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 x
  dReal c_y[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 y
  dReal c_z[NUM]    = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9};  // 関節中心点 z
  dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0};              // 関節回転軸 x
  dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0};              // 関節回転軸 y
  dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1};              // 関節回転軸 z

  // リンクの生成
  for (int i = 0; i < NUM-1; i++) {
    rlink[i].body = dBodyCreate(world);
    dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
    dMassSetZero(&mass);
    dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
    dBodySetMass(rlink[i].body, &mass);
    rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
    dGeomSetBody(rlink[i].geom,rlink[i].body);
  }
  rlink[NUM-1].body = dBodyCreate(world);
  dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
  dBodySetMass(rlink[NUM-1].body, &mass);
  rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
  dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);

  // ジョイントの生成とリンクへの取り付け
  joint[0] = dJointCreateFixed(world, 0);  // 固定ジョイント
  dJointAttach(joint[0], rlink[0].body, 0);
  dJointSetFixed(joint[0]);
  for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
    dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
    dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
  }
}
예제 #6
0
int main(int argc, char *argv[]) {
  dsFunctions fn;
  double x[NUM] = {0.00}, y[NUM] = {0.00};  // Center of gravity
  double z[NUM]         = { 0.05, 0.50, 1.50, 2.55};
  double m[NUM] = {10.00, 2.00, 2.00, 2.00};       // mass
  double anchor_x[NUM]  = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints  &nbsp;
  double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00};
  double axis_x[NUM]  = { 0.00, 0.00, 0.00, 0.00};  // axises of joints
  double axis_y[NUM]  = { 0.00, 0.00, 1.00, 1.00};
  double axis_z[NUM]  = { 1.00, 1.00, 0.00, 0.00};
  fn.version = DS_VERSION;  fn.start   = &start;   fn.step   = &simLoop;
  fn.command = &command;
  fn.path_to_textures = "../../drawstuff/textures";

  dInitODE();  // Initialize ODE
  world = dWorldCreate();  // Create a world
  dWorldSetGravity(world, 0, 0, -9.8);

  for (int i = 0; i < NUM; i++) {
    dMass mass;
    link[i] = dBodyCreate(world);
    dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position
    dMassSetZero(&mass);      // Set mass parameter to zero
    dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]);  // Calculate mass parameter
    dBodySetMass(link[i], &mass);  // Set mass
  }

  joint[0] = dJointCreateFixed(world, 0); // A fixed joint
  dJointAttach(joint[0], link[0], 0);     // Attach the joint between the ground and the base
  dJointSetFixed(joint[0]);               // Set the fixed joint

  for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint
    dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint
    dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]);
  }
  dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop
  dCloseODE();
  return 0;
}
예제 #7
0
파일: physics.c 프로젝트: ntoand/electro
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3],
                                              const float r[16])
{
    dVector3 v;
    dMatrix3 M;
    dReal  rad;
    dReal  len;
    dMass  add;

    if (r) set_rotation(M, r);

    if (dGeomGetClass(geom) != dPlaneClass)
    {
        dReal m = get_data(geom)->mass;

        /* Create a new mass for the given geom. */

        switch (dGeomGetClass(geom))
        {
        case dBoxClass:
            dGeomBoxGetLengths(geom, v);
            dMassSetBoxTotal(&add, m, v[0], v[1], v[2]);
            break;

        case dSphereClass:
            rad = dGeomSphereGetRadius(geom);
            dMassSetSphereTotal(&add, m, rad);
            break;

        case dCapsuleClass:
            dGeomCapsuleGetParams(geom, &rad, &len);
            dMassSetCapsuleTotal(&add, m, 3, rad, len);
            break;

        default:
            dMassSetZero(&add);
            break;
        }

        /* Transform the geom and mass to the given position and rotation. */

        if(dGeomGetBody(geom))
        {
            if (p)
            {
                dGeomSetOffsetPosition(geom, p[0], p[1], p[2]);
                dMassTranslate        (&add, p[0], p[1], p[2]);
            }
            if (r)
            {
                dGeomSetOffsetRotation(geom, M);
                dMassRotate           (&add, M);
            }
        }
        else
        {
            if (p) dGeomSetPosition(geom, p[0], p[1], p[2]);
            if (r) dGeomSetRotation(geom, M);
        }

        /* Accumulate the new mass with the body's existing mass. */

        dMassAdd(mass, &add);
    }
}
예제 #8
0
void  makeRobot_Nleg() 
{ 
	for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		dReal torso_m = 50.0;                    // Mass of body
		//	torso_m[segment] = 10.0;
		
		dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
		
		//for four legs
		//		dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		//																		{-cx1,-cx1,-cx1}};
		dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
			{0,0,0}};
		
		dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
			{-cy1,-cy1,-cy1}};
		dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
		
		dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
		dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
		dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
		
		//  //for one leg
		//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
		//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
		//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
		
		//for four legs
		dReal axis_x[num_legs][num_links];
		dReal axis_y[num_legs][num_links];
		dReal axis_z[num_legs][num_links];
		for(int i = 0; i < num_legs; ++i) {
			axis_x[i][0] = 0.0;
			axis_x[i][1] = 1.0;
			axis_x[i][2] = 0.0;
			
			axis_y[i][0] = 1.0;
			axis_y[i][1] = 0.0;
			axis_y[i][2] = 1.0;
			
			axis_z[i][0] = 0.0;
			axis_z[i][1] = 0.0;
			axis_z[i][2] = 0.0;
		}
		// For mation of the body
		dMass mass; 
		torso[segment].body  = dBodyCreate(world);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
		dBodySetMass(torso[segment].body,&mass);
		torso[segment].geom = dCreateBox(space,lx, ly, lz); 
		dGeomSetBody(torso[segment].geom, torso[segment].body);
		dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); 
		
		// Formation of leg
		dMatrix3 R;                          // Revolution queue
		dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) {
				THETA[segment][i][j] = 0;
				
				leg[segment][i][j].body = dBodyCreate(world);
				if (j == 0)
					dBodySetRotation(leg[segment][i][j].body,R);
				dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);				
				dMassSetZero(&mass);
				dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
				dBodySetMass(leg[segment][i][j].body, &mass);
				//if(i==1 and j==2)                                         //to set the length of one leg differently
				//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
				//else
				leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
				dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
			}
		}
		
		// Formation of joints (and connecting them up)
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) { 
				leg[segment][i][j].joint = dJointCreateHinge(world, 0);
				if (j == 0){ 
					dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
					dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
					dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop,  .50*M_PI);
				}
				else
					dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
				
				dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
				dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
			} 
		}
	}
	
	
#ifdef USE_NLEG_ROBOT
	// link torsos
	for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		switch (hingeType) {
			case 1: //Hinge Joint, X axis  (back-breaker)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
				break;
			case 2: //Hinge Joint, Y axis (???)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 3: //Hinge Joint, Z axis (snake-like)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
				break;
			case 4: // Slider, Y axis  (??)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 5: // Slider, X axis  (extendo)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				break;
			case 6: //Universal Joint
				torso[segment].joint = dJointCreateUniversal(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
				dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);				
				break;
			case 7: //Ball Joint
				torso[segment].joint = dJointCreateBall(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				break;
			case 8:  // Fixed
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(torso[segment].joint, dParamHiStop,  0.00*M_PI);
				break;
			default:
				assert(false); // not a valid hinge type
				break;
		}
	}
#endif
}
예제 #9
0
/*** Formation of robot ***/
void  makeRobot() 
{ 

  dReal torso_m = 10.0;                    // Mass of body
  dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
	
  //for four legs
  dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		{-cx1,-cx1,-cx1},{ cx1, cx1, cx1}};
  dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate)
		{-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}};
  dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}};
	
	
  dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
  dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
  dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
	
	//  //for one leg
	//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
	//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
	//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
	
  //for four legs
  dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}};
  dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}};
  dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}};
  
  // For mation of the body
  dMass mass; 
  torso[0].body  = dBodyCreate(world);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
  dBodySetMass(torso[0].body,&mass);
  torso[0].geom = dCreateBox(space, lx, ly, lz); 
  dGeomSetBody(torso[0].geom, torso[0].body); 
  dBodySetPosition(torso[0].body, SX, SY, SZ); 
	
  // Formation of leg
  dMatrix3 R;                          // Revolution queue
  dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].body = dBodyCreate(world);
      if (j == 0) dBodySetRotation(leg[0][i][j].body,R);
      dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]);
      dMassSetZero(&mass);
      dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
      dBodySetMass(leg[0][i][j].body, &mass);
      //if(i==1 and j==2)                                         //to set the length of one leg differently
      //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
      //else
      leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
      dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body);
    }
  }
	
  // Formation of joints (and connecting them up)
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].joint = dJointCreateHinge(world, 0);
      if (j == 0){ 
				dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment
				dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop,  .5*M_PI);
      }
      else
        dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body);
      dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]);
      dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
    } 
  } 
  
}
예제 #10
0
파일: mass.cpp 프로젝트: Belxjander/Asuna
void dMassSetCappedCylinderTotal(dMass *a, dReal b, int c, dReal d, dReal e)
{
  return dMassSetCapsuleTotal(a,b,c,d,e);
}