Beispiel #1
0
void CRigidBox::setMass(F32 mass){
	if(!mBodyID)
		return;
	dMass m;
	dMassSetBoxTotal(&m, mass, TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y), TO_PHYSICS(mDimentions.z));
	dBodySetMass(mBodyID, &m);
}
Beispiel #2
0
 void trimesh::set_mass(double mass)
 {
     if(body_id)
     {   
         dMass dmass;
         dMassSetBoxTotal (&dmass, mass, size[0], size[1], size[2]);
         dBodySetMass (body_id, &dmass);
         material.mass = mass;
     }
 }
Beispiel #3
0
IoObject *IoODEMass_setBoxMass(IoODEMass *self, IoObject *locals, IoMessage *m)
{
	const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
	const double lx        = IoMessage_locals_doubleArgAt_(m, locals, 1);
	const double ly        = IoMessage_locals_doubleArgAt_(m, locals, 2);
	const double lz        = IoMessage_locals_doubleArgAt_(m, locals, 3);

	dMassSetBoxTotal(DATA(self), totalMass, lx, ly, lz);
	return self;
}
Beispiel #4
0
void OscPrismODE::on_mass()
{
    ODEObject *ode_object = static_cast<ODEObject*>(special());
    dMassSetBoxTotal(&ode_object->mass(), m_mass.m_value,
                     m_size.x, m_size.y, m_size.z);
    dBodySetMass(ode_object->body(), &ode_object->mass());

    dReal volume = m_size.x * m_size.y * m_size.z;
    m_density.m_value = m_mass.m_value / volume;
}
Beispiel #5
0
    void cPhysicsObject::CreateBox(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot)
    {
      geom = dCreateBox(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(),
        2.0f * fWidth, 2.0f * fLength, 2.0f * fHeight);

      InitCommon(pWorld, pos, rot);

      if (bBody) {
        dMass mass;
        dMassSetBoxTotal(&mass, fMassKg, 2.0f * fWidth, 2.0f * fLength, 2.0f * fHeight);
        dBodySetMass(body, &mass);
      }
    }
Beispiel #6
0
TouchSensor::TouchSensor(dSpaceID odeSpace, dBodyID sensorBody, float mass,
		osg::Vec3 pos, float x, float y, float z, std::string label) :
		collideSpace_(odeSpace), label_(label) {
	// create own space to avoid physical collisions with other objects
	sensorSpace_ = dHashSpaceCreate(0);
	// create Sensor geom in this different space
	dMass massOde;
	dMassSetBoxTotal(&massOde, mass, x, y, z);
	dBodySetMass(sensorBody, &massOde);
	sensorGeometry_ = dCreateBox(sensorSpace_, x, y, z);
	dBodySetPosition(sensorBody, pos.x(), pos.y(), pos.z());
	dGeomSetPosition(sensorGeometry_, pos.x(), pos.y(), pos.z());
	dGeomSetBody(sensorGeometry_, sensorBody);
}
Beispiel #7
0
//set mass and sides by available dimensions for box objects
void ODE_Particle::setMassTotal(dReal total_mass, dReal side1,dReal side2,dReal side3)
{
    dMass m;
    dMassSetZero(&m);       
    if (getShapeType() == dBoxClass)
    {
        dMassSetBoxTotal(&m,total_mass,side1,side2,side3);
        dBodySetMass(body,&m);
    }
    else
    {
        printf("ERROR in ODE_Particle.cpp: Setting Mass using total mass for non box object");
        exit(0);
    }
}
Beispiel #8
0
/*** 台車の生成 ***/
static void makeBase()
{
    dMass mass;
    base.x = base.y = 0.0;
    base.z = Z;
    base.lx = SIDE[0]; base.ly = SIDE[1]; base.lz = SIDE[2];

    base.body  = dBodyCreate(world);
    base.geom = dCreateBox(space,base.lx, base.ly, base.lz);
    dGeomSetBody(base.geom, base.body);

    dMassSetZero(&mass);
    dMassSetBoxTotal(&mass,M,base.lx, base.ly, base.lz);
    dBodySetMass(base.body,&mass);

    dBodySetPosition(base.body, base.x, base.y, base.z);
}
Beispiel #9
0
/*** ロボットアームの生成 ***/
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]);
  }
}
 CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
                                CBoxEntity& c_box) :
    CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
    m_cBoxEntity(c_box),
    m_sGeomData(GEOM_NORMAL) {
    /* Check whether the box is movable or not */
    if(c_box.GetEmbodiedEntity().IsMovable()) {
       /* Movable box */
       /* Set the body to its initial position and orientation */
       const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
       dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
       dBodySetQuaternion(m_tBody, tQuat);
       const CVector3& cPos = GetEmbodiedEntity().GetPosition();
       dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
       /* Create the geometry and the mass */
       const CVector3& cBoxSize = c_box.GetSize();
       m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       /* Set Geom gripping properties. */
       m_sGeomData.Type = GEOM_GRIPPABLE;
       dGeomSetData(m_tGeom, &m_sGeomData);
       /* Create its mass */
       dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       /* Associate the body to the geom */
       dGeomSetBody(m_tGeom, m_tBody);
       /* Set the parent body total mass */
       dBodySetMass(m_tBody, &m_tMass);
    }
    else {
       /* Unmovable box, get rid of the body and add only the geometry */
       dBodyDestroy(m_tBody);
       /* Create the geometry */
       const CVector3& cBoxSize = c_box.GetSize();
       m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
       dGeomSetData(m_tGeom, &m_sGeomData);
       /* Set the geom to its position and orientation */
       const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
       dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
       dGeomSetQuaternion(m_tGeom, tQuat);
       const CVector3& cPos = GetEmbodiedEntity().GetPosition();
       dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
       /* Associate the geom to null body (this makes it static) */
       dGeomSetBody(m_tGeom, 0);
    }
 }
Beispiel #11
0
Datei: dm6.cpp Projekt: Ry0/ODE
void dmCreateBox(dmObject *obj, double p[3], double R[12], double m, double side[3],  double color[3])
{
	obj->body = dBodyCreate(world);          // ボールの生成
	obj->m = m;
	obj->R = R;
	obj->p = p;
	obj->side = side;
	obj->color = color;

	dMass mass;                              // 構造体massの宣言
	dMassSetZero(&mass);                     // 構造体massの初期化
	dMassSetBoxTotal(&mass,obj->m,obj->side[0], obj->side[1], obj->side[2]);  // 構造体massに質量を設定
	dBodySetMass(obj->body,&mass);               // ボールにmassを設定

	dBodySetPosition(obj->body, obj->p[0], obj->p[1], obj->p[2]);  // ボールの位置(x,y,z)を設定
	dBodySetRotation(obj->body, obj->R);

	obj->geom = dCreateBox(space,obj->side[0], obj->side[1], obj->side[2]); // 球ジオメトリの生成
	dGeomSetBody(obj->geom, obj->body);      // ボディとジオメトリの関連付け
}
Beispiel #12
0
void HarrierSim::makeHarrier()
{
  dMass mass;
  itsHarrierBody = dBodyCreate(world);
  //pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;

  dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);

  dMatrix3 R;
  dRFromAxisAndAngle (R,1,0,0,0);
  dBodySetRotation(itsHarrierBody, R);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
  //dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
  dMassRotate(&mass, R);
  dBodySetMass(itsHarrierBody,&mass);
  itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
  dGeomSetRotation(itsHarrierGeom, R);
  dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
Beispiel #13
0
SceneObj::SceneObj( Scene *scene_p )
{
	m_scene_p = scene_p;
	m_prev_p = 0;
	m_next_p = m_scene_p->m_obj_p;
	if( m_next_p )
		m_next_p->m_prev_p = this;
	m_scene_p->m_obj_p = this;
	
	m_dbody = dBodyCreate( m_scene_p->m_dworld );
	dBodySetData( m_dbody, this );
	dBodyDisable( m_dbody );

	dMass mass;
	dMassSetBoxTotal( &mass, 0.1f, 57, 57, 20 );
	dBodySetMass( m_dbody, &mass );

	m_dgeom = dCreateBox( m_scene_p->m_dspace, 57, 57, 20 );
	dGeomSetBody( m_dgeom, m_dbody );
	
	m_texid = 0;
}
Beispiel #14
0
/* ------------------------
* 箱の生成,描画
------------------------ */
void MakeBox()
{
    /* ローカル変数の定義 */
    dMass mass;
    for(int i = 0; i < BOX_NUM; i++)
    {
        /* ボディを生成 */
        box[i].body = dBodyCreate(world);
        dMassSetZero(&mass);
        /* 質量を設定 */
        dMassSetBoxTotal(&mass, boxM[i], boxSize[i][0], boxSize[i][1], boxSize[i][2]);
        dBodySetMass(box[i].body, &mass);
        /* ジオメトリ生成 */
        box[i].geom = dCreateBox(space, boxSize[i][0], boxSize[i][1], boxSize[i][2]);
        /* ジオメトリをセット */
        dGeomSetBody(box[i].geom, box[i].body);
        dBodySetPosition(box[i].body, boxPos[i][0], boxPos[i][1], boxPos[i][2]);
        /* 箱と地面の結合 */
        fixed[i] = dJointCreateFixed(world, 0);
        dJointAttach(fixed[i], box[i].body, 0);
        dJointSetFixed(fixed[i]);
    }
}
Beispiel #15
0
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);
    }
}
Beispiel #16
0
/* --------------------------------
** 全方向移動ロボットの生成, 描画
----------------------------------- */
void MakeOmni()
{
    /* ローカル変数の定義 */
    dMass mass;
    double r, w, d, m_w, x, y, z, lx, ly, lz, m_b;
    /* ロボットの個数分だけループ */
    for (int i=0; i< OMNI_NUM; i++)
    {
        /* 車輪 */
        r = wheelSize[i][0];
        w = wheelSize[i][1];
        d = wheelSize[i][2];
        m_w = wheelM[i];
        /* 土台 */
        x = basePos[i][0];
        y = basePos[i][1];
        z = basePos[i][2];
        lx = baseSize[i][0];
        ly = baseSize[i][1];
        lz = baseSize[i][2];
        m_b = baseM[i];
        /* 土台の生成 */
        base[i].body  = dBodyCreate(world);
        dMassSetZero(&mass);
        dMassSetBoxTotal(&mass, m_b, lx, ly, lz);
        dBodySetMass(base[i].body,&mass);
        base[i].geom = dCreateBox(space, lx, ly, lz);
        dGeomSetBody(base[i].geom, base[i].body);
        dBodySetPosition(base[i].body, x, y, z); /* 座標 */
        /* 車輪の生成 */
        double wx[WHEEL_NUM] = {lx/2+w/2+d, - (lx/2+w/2+d), 0, 0};
        double wy[WHEEL_NUM] = {0, 0, ly/2+w/2+d, -(ly/2+w/2+d)};
        double wz[WHEEL_NUM] = {z, z, z, z};
        double jx[WHEEL_NUM] = {lx/2, -lx/2, 0, 0};
        double jy[WHEEL_NUM] = {0, 0, ly/2, -ly/2};
        double jz[WHEEL_NUM] = {z, z, z, z};
      
        for (int j = 0; j < WHEEL_NUM; j++)
        {
            wheel[i][j].body = dBodyCreate(world);
            dMatrix3 R;
            if (j >= 2)
            {
                dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
                dBodySetRotation(wheel[i][j].body,R);
            }
            else
            {
                dRFromAxisAndAngle(R, 0, 1, 0, M_PI/2.0);
                dBodySetRotation(wheel[i][j].body, R);
            }
            dMassSetZero(&mass);
            if (j < 2)
            {
                dMassSetCylinderTotal(&mass, m_w, 1, r, w);
            }
            else
            {
                dMassSetCylinderTotal(&mass, m_w, 2, r, w);
            }
            dBodySetMass(wheel[i][j].body,&mass);
            wheel[i][j].geom = dCreateCylinder(space, r, w);
            dGeomSetBody(wheel[i][j].geom, wheel[i][j].body);
            dBodySetPosition(wheel[i][j].body, wx[j], wy[j], wz[j]);
            wheel[i][j].joint = dJointCreateHinge(world, 0);
            dJointAttach(wheel[i][j].joint, base[i].body, wheel[0][j].body);
            if (j < 2)
            {
                dJointSetHingeAxis(wheel[i][j].joint, 1, 0, 0);
            }
            else
            {
                dJointSetHingeAxis(wheel[i][j].joint, 0, -1, 0);
            }
            dJointSetHingeAnchor(wheel[i][j].joint, jx[j], jy[j], jz[j]);
        }
    }
}
Beispiel #17
0
void dMassSetBox (dMass *m, dReal density,
		  dReal lx, dReal ly, dReal lz)
{
  dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
}
Beispiel #18
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
}
Beispiel #19
0
State* init() {
    State* state = new State();
    dInitODE();

    SDL_Init(SDL_INIT_EVERYTHING);

    state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL);
    SDL_WM_SetCaption("Physics", NULL);
    SDL_Flip(state->screen);

    SDL_ShowCursor(SDL_DISABLE);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    GLfloat light_ambient[] = { 1, 1, 1, 1 };
    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
    GLfloat lightpos[] = {0, 4, 0, 1};
    glLightfv(GL_LIGHT0, GL_POSITION, lightpos);

    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
    glShadeModel(GL_SMOOTH);

    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    glEnable(GL_DEPTH_TEST);

    glEnable(GL_CULL_FACE);

    glClearColor(0, 0, 0, 1);

    state->posx = 0;//21;
    state->posy = 4;//8;
    state->posz = 5;//21;
    state->rotx = 0;
    state->roty = 0;//-40;
    state->rotz = 0;

    state->wkey = false;
    state->akey = false;
    state->skey = false;
    state->dkey = false;
    state->gkey = false;

    state->simSpeed = 60;

    state->carcam = false;

    state->carbodydrawable = new Drawable("objs/carbody.obj");
    state->carwheeldrawable = new Drawable("objs/carwheel.obj");
    state->map = new Drawable("objs/jump2.obj");
    state->cube = new Drawable("objs/cube.obj");
//    state->monkey = new Drawable("objs/monkey.obj");

    state->world = dWorldCreate();
    dWorldSetGravity(state->world, 0, -9.8, 0);

    state->worldSpace = dHashSpaceCreate(0);

    const double carHeight = 0.65;
    const double carZ = 90;
    const double carX = 0;
    const float speed = -1000;
    const float force = 200;

    state->carbodybody = dBodyCreate(state->world);
    dBodySetPosition(state->carbodybody, carX, carHeight, carZ);
    dMass bodymass;
    dMassSetBoxTotal(&bodymass, 100, 2, 4, 1);
    dBodySetMass(state->carbodybody, &bodymass);
    dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4);
    dGeomSetBody(carbodygeom, state->carbodybody);

    state->flcarwheelbody = dBodyCreate(state->world);
    dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2);
    const dMatrix3 m = { 0, 0, 1, 0
                       , 0, 1, 0, 0
                       , 0, 0, 1, 0 };
    dBodySetRotation(state->flcarwheelbody, m);
    dMass wheelmass;
    dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2);
    dBodySetMass(state->flcarwheelbody, &wheelmass);
    dJointID joint = dJointCreateHinge(state->world, 0);
    dJointAttach(joint, state->carbodybody, state->flcarwheelbody);
    dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2);
    dJointSetHingeAxis(joint, 1, 0, 0);
    dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
    dGeomSetBody(flcarwheelgeom, state->flcarwheelbody);

    dJointID motor = dJointCreateAMotor(state->world, 0);
    dJointAttach(motor, state->flcarwheelbody, state->carbodybody);
    dJointSetAMotorNumAxes(motor, 1);
    dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
    dJointSetAMotorParam(motor, dParamVel, speed);
    dJointSetAMotorParam(motor, dParamFMax, force);

    state->frcarwheelbody = dBodyCreate(state->world);
    dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2);
    dBodySetRotation(state->frcarwheelbody, m);
    dBodySetMass(state->frcarwheelbody, &wheelmass);
    joint = dJointCreateHinge(state->world, 0);
    dJointAttach(joint, state->carbodybody, state->frcarwheelbody);
    dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2);
    dJointSetHingeAxis(joint, 1, 0, 0);
    dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
    dGeomSetBody(frcarwheelgeom, state->frcarwheelbody);

    motor = dJointCreateAMotor(state->world, 0);
    dJointAttach(motor, state->frcarwheelbody, state->carbodybody);
    dJointSetAMotorNumAxes(motor, 1);
    dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
    dJointSetAMotorParam(motor, dParamVel, speed);
    dJointSetAMotorParam(motor, dParamFMax, force);

    state->blcarwheelbody = dBodyCreate(state->world);
    dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2);
    dBodySetRotation(state->blcarwheelbody, m);
    dBodySetMass(state->blcarwheelbody, &wheelmass);
    joint = dJointCreateHinge(state->world, 0);
    dJointAttach(joint, state->carbodybody, state->blcarwheelbody);
    dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2);
    dJointSetHingeAxis(joint, 1, 0, 0);
    dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
    dGeomSetBody(blcarwheelgeom, state->blcarwheelbody);

    motor = dJointCreateAMotor(state->world, 0);
    dJointAttach(motor, state->blcarwheelbody, state->carbodybody);
    dJointSetAMotorNumAxes(motor, 1);
    dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
    dJointSetAMotorParam(motor, dParamVel, speed);
    dJointSetAMotorParam(motor, dParamFMax, force);

    state->brcarwheelbody = dBodyCreate(state->world);
    dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2);
    dBodySetRotation(state->brcarwheelbody, m);
    dBodySetMass(state->brcarwheelbody, &wheelmass);
    joint = dJointCreateHinge(state->world, 0);
    dJointAttach(joint, state->carbodybody, state->brcarwheelbody);
    dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2);
    dJointSetHingeAxis(joint, 1, 0, 0);
    dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
    dGeomSetBody(brcarwheelgeom, state->brcarwheelbody);

    motor = dJointCreateAMotor(state->world, 0);
    dJointAttach(motor, state->brcarwheelbody, state->carbodybody);
    dJointSetAMotorNumAxes(motor, 1);
    dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
    dJointSetAMotorParam(motor, dParamVel, speed);
    dJointSetAMotorParam(motor, dParamFMax, force);

    state->var = new double[3];

    state->var = dBodyGetPosition(state->carbodybody);
//    cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
    //TODO check if translation matrix from dBody can be used.

    dSpaceID cubespace = dHashSpaceCreate(state->worldSpace);

    for(int i = 0; i < NUM_CUBES/10; i++) {
        for(int k = 0; k < 10; k++) {
            state->cubebody[i*10+k] = dBodyCreate(state->world);
            dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1);
            dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70);
            dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2);
            dGeomSetBody(cubegeom, state->cubebody[i*10+k]);
        }
    }

    {
        int indexSize = state->map->vertices.size()/3;
        unsigned int* index = new unsigned int[indexSize];
        for(int i = 0; i < indexSize; i++)
            index[i] = i;

        dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
        dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12);
        dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
        dGeomSetPosition(mapGeom, 0, 0, 0);
    }
//    state->monkeyBody = dBodyCreate(state->world);
//    {
//        int indexSize = state->monkey->vertices.size()/3;
//        unsigned int* index = new unsigned int[indexSize];
//        for(int i = 0; i < indexSize; i++)
//            index[i] = i;

//        dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
//        dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12);
//        dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
//        dGeomSetPosition(monkeyGeom, 0, 2, 0);
//        dGeomSetBody(monkeyGeom, state->monkeyBody);
//    }

    state->physicsContactgroup = dJointGroupCreate(0);

    return state;
}
Beispiel #20
0
void odCable::setupBody(dWorldID *world, dSpaceID space){
  int i;
  double pi=4.*atan(1.);
  double segLen, dL, x;
  double dd, s, rs;
  dVector3 OP;
  odPoint loc1, loc2, d, P, c;
  dMass *m;
  dGeomID geometry[nSegments];
//  dQuaternion align;
  dReal align[4];
  
  printf("Setting up cable model\n");
  
  if (body1!=NULL){
    loc1=body1->globalPosition(pos1);
  }else{
    loc1=pos1;
  }
  if (body2!=NULL){
    loc2=body2->globalPosition(pos2);
  }else{
    loc2=pos2;
  }
  
  d=loc2-loc1;
  dL=d.mag()/(double)nSegments;
  segLen=length/(double)nSegments;
  printf("Distance = %lf m\n",d.mag());
  printf("Segment length = %lf m\n",segLen);
  
  printf("Allocating memory\n");
  element = new dBodyID[nSegments];
  joint = new dJointID[nSegments-1];
  
  printf("Creating elements\n");
  for (i=0;i<nSegments;i++){
    element[i] = dBodyCreate (*world);
    dBodySetDamping(element[i], linearDamping, angularDamping);
    m=new dMass;
    dMassSetBoxTotal(m, weight*segLen, segLen, diameter, diameter);
    dMassAdjust (m, weight*segLen);
    dBodySetMass (element[i], m);
    x=((double)i+.5)*segLen;
    dBodySetPosition(element[i], x, 0, 0);
//    geometry[i] = dCreateBox(space, length, diameter, diameter);
//    dGeomSetBody(geometry[i], element[i]);
  }
  printf("Creating internal joints\n");
  for (i=0;i<nSegments-1;i++){
    joint[i] = dJointCreateBall(*world,0);
    dJointAttach (joint[i],element[i],element[i+1]);
    x = ((double)i+1.)*segLen;
    dJointSetBallAnchor (joint[i],x,0,0);
  }
/*
  c = d.crossProduct(odPoint(0,0,1)); 
  dd = d.dotProduct_natural(odPoint(0,0,1)); 
  s = sqrt((1.0 + dd)*2.0); 
  rs = 1.0/s;
  align[0]=s*0.5; align[1]=c.x*rs; align[2]=c.y*rs; align[3]=c.z*rs;

  printf("Moving segments to world positions...\n");
  for (i=0;i<nSegments;i++){
    P=loc1+d*((double)i)/(nSegments-1);
    if (i==0) P.x+=segLen/2.;
    if (i==nSegments-1) P.x-=segLen/2.;
    dBodySetPosition(element[i], P.x, P.y, P.z);
//    dBodySetQuaternion(element[i], align);
  }
*/  
  printf("Creating end joint 1\n");
  end1 = dJointCreateBall(*world,0);
  if (body1!=NULL){
    dJointAttach(end1, body1->odeBody, element[0]);
  }else{
    dJointAttach(end1, 0, element[0]);
  }
//  dJointSetBallAnchor (end1, loc1.x, loc1.y, loc1.z);
  dJointSetBallAnchor (end1, 0, 0, 0);
  end1Feedback = new dJointFeedback;
  dJointSetFeedback (end1, end1Feedback);

/*  
  printf("Creating end joint 2\n");  
  end2 = dJointCreateBall(*world,0);
  if (body2!=NULL){
    dJointAttach(end2, body2->odeBody, element[nSegments-1]);
  }else{
    dJointAttach(end2, 0, element[nSegments-1]);
  }
//  dJointSetBallAnchor (end2, loc2.x, loc2.y, loc2.z);
  dJointSetBallAnchor (end2, nSegments*segLen, 0, 0);
  */
  end2Feedback = new dJointFeedback;
  dJointSetFeedback (joint[0], end2Feedback);
}
Beispiel #21
0
// ロボットの生成
void create() {

  // SHIELDの生成(空間に固定)
  rod[0].body = dBodyCreate(world);
  dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
  dMassSetZero(&mass);
  dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH);
  dBodySetMass(rod[0].body, &mass);
  rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH);
  dGeomSetBody(rod[0].geom, rod[0].body);
  dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI);  // x軸に90度回転
  dBodySetRotation(rod[0].body, R);

  rod_joint[0] = dJointCreateFixed(world, 0);  // 固定ジョイント
  dJointAttach(rod_joint[0], rod[0].body, 0);
  dJointSetFixed(rod_joint[0]);

  // RODの生成(回転ジョイントy軸に回転軸)
  rod[1].body = dBodyCreate(world);
  dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
  dBodySetMass(rod[1].body, &mass);
  rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
  dGeomSetBody(rod[1].geom, rod[1].body);
  dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI);  // z軸に45度回転
  dBodySetRotation(rod[1].body, R);

  rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント
  dJointAttach(rod_joint[1], rod[1].body, rod[0].body);
  dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z);
  dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント

  // BODYの生成(たてておくだけ)
  rod[2].body = dBodyCreate(world);
  dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
  dBodySetMass(rod[2].body, &mass);
  rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
  dGeomSetBody(rod[2].geom, rod[2].body);

  // BULLETの生成(CANNON中心に初期座標)
  bullet.body = dBodyCreate(world);
  dMassSetZero(&mass);
  dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS);
  dBodySetMass(bullet.body,&mass);
  dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z);
  bullet.geom = dCreateSphere(space, BULLET_RADIUS);
  dGeomSetBody(bullet.geom, bullet.body);

  // TARGETの生成
//  target.body = dBodyCreate(world);
//  dMassSetZero(&mass);
//  dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS);
//  dBodySetMass(target.body,&mass);
//  dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z);
//  target.geom = dCreateSphere(space, BULLET_RADIUS);
//  dGeomSetBody(target.geom, target.body);

}
Beispiel #22
0
//! add ODE parts with fixed joint
void SSimEntity::setMass(double mass)
{
	m_parts.mass = mass;
	int geomNum = getGeomNum();

	if (geomNum != 0) {
		// if a part is already added
		// mass per part
		double ms = mass / geomNum;

		// refer geometry and body
		dBodyID body = m_parts.body;

		dMass m;
		dMass m2;
		dMassSetZero(&m); dMassSetZero(&m2);

		for (int i = 0; i < geomNum; i++) {

			// refer the type of geometory
			dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
			int type = dGeomGetClass(geom);

			// setting of mass
			// sphere
			if (type == 0) {
				dReal radius = dGeomSphereGetRadius(geom);
				dMassSetSphereTotal(&m2, ms, radius);
			}

			// box
			else if (type == 1) {
				dVector3 size;
				dGeomBoxGetLengths(geom, size);
				dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
			}

			// cylinder
			else if (type == 3) {
				dReal radius = 0.0;
				dReal length = 0.0;
				dGeomCylinderGetParams(geom, &radius, &length);
				// TODO: confirm: Is 2 suitable for long axis?
				dMassSetCylinderTotal(&m2, ms, 2, radius, length);
			}
			const dReal *pos = dGeomGetPosition(geom);
			dMassTranslate(&m2, pos[0], pos[1], pos[2]);
			dMassAdd(&m, &m2);
		}

		/*
		  for (int i = 0; i < geomNum; i++) {
		    dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
		    const dReal *pos = dGeomGetPosition(geom);      
			// Change to a relative position from CoG
		    dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]);
		  }
		  // keep the CoG position
		  //m_parts.pos.set(m.c[0], m.c[1], m.c[2]);
		  */

		// Adjustment of the gap from CoG
		//const dReal *p = dBodyGetPosition(body);
		//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);    
		dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);

		// Setting of mass
		dBodySetMass(body, &m);
		dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
	} // if (partsNum != 0) {
}
void PhysicsBody::setBoxMassTotal( Real32 totalMass, Real32 lx, Real32 ly, Real32 lz )
{
    dMass mass;
    dMassSetBoxTotal(&mass, totalMass, lx, ly, lz);
    setMassStruct(mass);
}
Beispiel #24
0
void BoxMass::assembleMass()
{
  dMassSetBoxTotal(&mass, value, depth, width, height);
}
Beispiel #25
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]);
    } 
  } 
  
}
Beispiel #26
0
//! setting of mass
void SSimRobotEntity::setMass(SSimObjParts *parts, double mass)
{
	// if a part is already added
	int geomNum = parts->geoms.size();
	if (geomNum != 0) {
		// mass per each part
		double ms = mass / geomNum;

		// refer geometry and body
		dBodyID body = parts->body;

		dMass m;
		dMass m2;
		dMassSetZero(&m);
		dMassSetZero(&m2);

		for (int i = 0; i < geomNum; i++) {

			// Refer the type of geometry
			dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]);
			int type = dGeomGetClass(geom);

			// setting of mass
			// sphere
			if (type == 0) {
				dReal radius = dGeomSphereGetRadius(geom);
				dMassSetSphereTotal(&m2, ms, radius);
			}

			// box
			else if (type == 1) {
				dVector3 size;
				dGeomBoxGetLengths(geom, size);
				dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
			}

			// cylinder
			else if (type == 3) {
				dReal radius = 0.0;
				dReal length = 0.0;
				dGeomCylinderGetParams(geom, &radius, &length);
				// TODO: confirm: Is 2 suitable for long axis?
				dMassSetCylinderTotal(&m2, ms, 2, radius, length);
			}
			// 
			const dReal *pos = dGeomGetPosition(geom);
			//LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2]));
			dMassTranslate(&m2, pos[0], pos[1], pos[2]);
			dMassAdd(&m, &m2);
		}

		// adjustment of the gap between CoG
		//const dReal *p = dBodyGetPosition(body);
		//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);    
		dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);

		// seeting of mass
		dBodySetMass(body, &m);
		dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
	} // if (partsNum != 0) {
}
Beispiel #27
0
void BoxInfo::createMass(dMass * mass, float massVal)
{
    dMassSetBoxTotal(mass, massVal, lx, ly, lz);
}