/**
     * \brief Destroyes a body from a node:
     *
     * This function checks if not more than one geom is connected to the
     * body and destroyes the body in that case. In the other case the counter
     * of the connected geoms is decreased.
     *
     * pre:
     *     - the body exists in the physical world
     *
     * post:
     *     - if more than one geoms are connected to the body, decrease the
     *       counter of connected geoms
     *     - otherwise, if only one geom is connected to the body, destroy the body
     */
    void WorldPhysics::destroyBody(dBodyID theBody, NodePhysics* node) {
      std::vector<body_nbr_tupel>::iterator iter;
      std::vector<NodePhysics*>::iterator jter;

      for(iter = comp_body_list.begin(); iter != comp_body_list.end(); iter++) {
        if((*iter).body == theBody) {
          if((*iter).connected_geoms > 1) {
            (*iter).connected_geoms--;
            for(jter = (*iter).comp_nodes.begin();
                jter != (*iter).comp_nodes.end(); jter++) {
              if((*jter) == node) {
                (*iter).comp_nodes.erase(jter);
                break;
              }
            }
            resetCompositeMass(theBody);
            return;
          }
          else {
            dBodyDestroy(theBody);
            comp_body_list.erase(iter);
            return;
          }
        }
      }
      // if we get here in the code, the body is not in the list and can
      // be removed from the world
      dBodyDestroy(theBody);
    }
Пример #2
0
void SkidSteeringVehicle::destroy() {
    dBodyDestroy(this->vehicleBody);
    dGeomDestroy(this->vehicleGeom);
    
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            dBodyDestroy(this->wheelBody[fr][lr]);
            dGeomDestroy(this->wheelGeom[fr][lr]);
            dJointDestroy(this->wheelJoint[fr][lr]);
        }
    }
    
    dRigidBodyArrayDestroy(this->bodyArray);
}
Пример #3
0
void Machine::destroy(void)
{
	int i;
	dBodyDestroy(body[0]);
	dBodyDestroy(body[1]);
	dGeomDestroy(geom[0]);
	dJointDestroy(joint);
	for(i=0; i<2; i++)
		dGeomDestroy(geom[i+2]);
	for(i=0; i<3; i++)
	{
		dBodyDestroy(wheel[i]);
		dGeomDestroy(sphere[i]);
	}
}
Пример #4
0
 void cPhysicsObject::DestroyBody()
 {
   if (body != NULL) {
     dBodyDestroy(body);
     body = NULL;
   }
 }
Пример #5
0
	void YGEBodyAsset::setParent(YGETimeSpace::YGEEntity* entity){
		parent = entity;
		if(hasBody){
			dBodyDestroy(bodyId);
			hasBody = false;
		}
	}
Пример #6
0
void stop()
{
    dGeomDestroy(mesh_geom);
    dGeomTriMeshDataDestroy(mesh_data);

    dBodyDestroy(ball1_body);
    dBodyDestroy(ball2_body);

    dGeomDestroy(ground);

    dJointGroupDestroy(contact_group);

    dSpaceDestroy(space); // will destroy all geoms

    dWorldDestroy(world);
}
Пример #7
0
LaserBeam::~LaserBeam()
{
    dGeomDestroy(geom);
    dBodyDestroy(me);

    printf("Good bye....\n");
}
Пример #8
0
/**
 * This method is called if the joint should be detached. It stores
 * the actual Joint-axis, which can change their orientation at
 * Hinge2Joints, and destroys the helper body and joint.
 **/
void Hinge2Joint::detachJoint() {
	if (active && joint)
	{
		TransformationData entityTrans;
		dVector3 vec;
		dJointGetHinge2Axis1(joint, vec);
		axis1[0] = vec[0];
		axis1[1] = vec[1];
		axis1[2] = vec[2];
		dJointGetHinge2Axis2(joint, vec);
		axis2[0] = vec[0];
		axis2[1] = vec[1];
		axis2[2] = vec[2];
		if (mainEntity != NULL)
		{
			entityTrans = mainEntity->getEnvironmentTransformation();
			gmtl::Quatf entityRot;
			gmtl::Vec3f scaleVec;
			gmtl::AxisAnglef axAng;

	// get the inverse scale values of the mainEntity
/*			scaleVec[0] = 1.0f/mainEntity->getXScale();
			scaleVec[1] = 1.0f/mainEntity->getYScale();
			scaleVec[2] = 1.0f/mainEntity->getZScale();*/
			scaleVec[0] = 1.0f/entityTrans.scale[0];
			scaleVec[1] = 1.0f/entityTrans.scale[1];
			scaleVec[2] = 1.0f/entityTrans.scale[2];
	// get the inverse Rotation of the mainEntity
// 			axAng[0] = -mainEntity->getRotAngle();
// 			axAng[1] = mainEntity->getXRot();
// 			axAng[2] = mainEntity->getYRot();
// 			axAng[3] = mainEntity->getZRot();
// 			gmtl::set(entityRot, axAng);
			entityRot = entityTrans.orientation;
			gmtl::invert(entityRot);

			axis1 *= entityRot;
			axis2 *= entityRot;

			axis1[0] *= scaleVec[0];
			axis1[1] *= scaleVec[1];
			axis1[2] *= scaleVec[2];

			axis2[0] *= scaleVec[0];
			axis2[1] *= scaleVec[1];
			axis2[2] *= scaleVec[2];

			gmtl::normalize(axis1);
			gmtl::normalize(axis2);
		} // if
	} // if

	if (usedHelperJoint)
	{
		dJointDestroy(helperJoint);
		dBodyDestroy(helperBody);
		usedHelperJoint = false;
	} // if
} // detachJoint
Пример #9
0
MyODEGeom::~MyODEGeom() {
    if (mBody) {
        dBodyDestroy(mBody);
    }
    if (mODEGeom) {
        dGeomDestroy(mODEGeom);
    }
}
Пример #10
0
void PhysicsBody::onDestroy()
{
    if(_BodyID)
    {
        dBodyDestroy(_BodyID);
        _BodyID =0;
    }
}
Пример #11
0
ODE_Particle::~ODE_Particle()
{
    /*printf("Particle=%u destructor!\n",id);
    fflush(stdout);*/
        
    dBodyDestroy(body);
    dGeomDestroy(geom);   
}
Пример #12
0
 Primitive::~Primitive () {
   QMP_CRITICAL(8);
   // 20091023; guettler:
   // hack for tasked simulations; there are some problems if running in parallel mode,
   // if you do not destroy the geom, everything is fine (should be no problem because world is destroying geoms too)
   if(destroyGeom && geom) dGeomDestroy( geom );
   if(body && ((mode & _Transform) == 0) ) dBodyDestroy( body );
   QMP_END_CRITICAL(8);
 }
Пример #13
0
void BoxObstacle::remove() {
	dGeomDestroy(boxGeom_);
	if (box_ != 0) {
		for(int i=0; i< dBodyGetNumJoints(box_); i++) {
			dJointDestroy(dBodyGetJoint(box_, i));
		}
		dBodyDestroy(box_);
	}
}
Пример #14
0
void IoODEBody_free(IoODEBody *self)
{
	if(BODYID && WORLD)
	{
		IoODEWorld_removeBody(WORLD, self);
		dBodyDestroy(BODYID);
	}
	free(IoObject_dataPointer(self));
}
Пример #15
0
PhysicsActor::~PhysicsActor(){

if (joint>0)
    dJointDestroy(joint);
if (geom>0)
    dGeomDestroy(geom);
if (body>0)
    dBodyDestroy(body);

}
Пример #16
0
dBodyID set_phys_body_type(dBodyID body, int b)
{
    if (body)
        dBodyDestroy(body);

    if (b)
        return dBodyCreate(world);

    return 0;
}
Пример #17
0
void Wheel::reset() {
  dSpaceID space = dGeomGetSpace(ph.geom);
  dGeomDestroy(ph.geom);
  dBodyDestroy(ph.body);

  Utils::Xml x(cst.xmlFile, "wheel");
  
  createPhysics(x, space);
  disposePhysics(x);
}
Пример #18
0
TSRODERigidBody::~TSRODERigidBody()
{
    if ( m_BodyID )
    {
        dBodyDestroy( m_BodyID );
    }
    for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
    {
        dGeomDestroy( m_GeomIDs[ i ] );
    }
}
Пример #19
0
void TrackedVehicle::destroy() {
    this->leftTrack->destroy();
    dJointDestroy(this->leftTrackJoint);
    this->rightTrack->destroy();
    dJointDestroy(this->rightTrackJoint);

    dBodyDestroy(this->vehicleBody);
    dGeomDestroy(this->vehicleGeom);
    
    dRigidBodyArrayDestroy(this->bodyArray);
}
Пример #20
0
void CPHActivationShape::	Destroy	()
{
	VERIFY(m_geom&&m_body)				;
	spatial_unregister		()			;
	CPHObject::deactivate	()			;
	dGeomDestroyUserData	(m_geom)	;
	dGeomDestroy			(m_geom)	;
	m_geom					=NULL		;
	dBodyDestroy			(m_body)	;
	m_body					=NULL		;
}
Пример #21
0
SceneObj::~SceneObj( void )
{
	if( m_dgeom )
		dGeomDestroy( m_dgeom );

	if( m_dbody )
		dBodyDestroy( m_dbody );

	if( m_prev_p )		m_prev_p->m_next_p = m_next_p;
	if( m_next_p )		m_next_p->m_prev_p = m_prev_p;

	if( m_scene_p->m_obj_p==this )
		m_scene_p->m_obj_p = m_next_p;
}
Пример #22
0
void ODE_Link::destroy()
{
    if(jointType!=FREE_JOINT)
        dJointDestroy(odeJointId);
    for(int i=0; i<geomIds.size(); i++)
        dGeomDestroy(geomIds.at(i));
    if(triMeshDataId)
        dGeomTriMeshDataDestroy(triMeshDataId);
    dBodyDestroy(bodyId);
    ODE_Link* link = static_cast<ODE_Link*>(child);
    while(link){
        ODE_Link* linkToDelete = (ODE_Link*)link;
        link = static_cast<ODE_Link*>(link->sibling);
        linkToDelete->destroy();
    }
}
Пример #23
0
void CPhysicManager::DelNode(INode* node){

    std::vector<CPhysicObject*>::iterator _it=PhysicObjectList.begin();
    for (int i=PhysicObjectList.size();i>0;i--){
        CPhysicObject* my = (*_it);
        if (my->node==node) {
            dBodyDestroy(my->myBody);
            dGeomDestroy(my->myGeom);
            my->Release();
            PhysicObjectList.erase(_it);
            break;
            }
        _it++;
    };

};
Пример #24
0
 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);
    }
 }
Пример #25
0
	void YGEBodyAsset::createBody(){
		YGETimeSpace::YGESpace* parentSpace = parent->getSpace();
		if(parentSpace != NULL && parent->getHasAbsPosition()){

			if(hasBody) {
				dBodyDestroy(bodyId);
				dGeomDestroy(geomId);
			}
			bodyId = dBodyCreate(parentSpace->getWorldId());


			mass = new dMass();
			dMassSetBox(mass,1,1,1,1);

			dMassAdjust(mass,bodyMass);

			dBodySetMass(bodyId, mass);

			dBodySetAutoDisableFlag(bodyId, 0);

			YGEMath::Vector3 pos = parent->getAbsPosition();
			YGEMath::Quaternion rot = parent->getAbsOrientation();

			dBodySetPosition(bodyId,pos.x,pos.y,pos.z);
			dQuaternion q;
			q[0]=rot.w;
			q[1]=rot.x;
			q[2]=rot.y;
			q[3]=rot.z;
			dBodySetQuaternion(bodyId, q);
			//dBodyAddForce(bodyId, 0.5, 0, 0);

			// a hull, remove this
			geomId = dCreateBox(parentSpace->getDSpaceId(), bodySize.x, bodySize.y, bodySize.z);
			dGeomSetData(geomId, this);
			
			dGeomSetCategoryBits(geomId, YGEPhysics::ENTITIES );
			dGeomSetCollideBits(geomId, YGEPhysics::ENTITIES | YGEPhysics::STATIC_OBJECTS );
			dGeomSetBody(geomId, bodyId);

			//			dBodySetAuto
			hasBody = true;
		}
	}
Пример #26
0
void CProtoHapticDoc::RemoveShape(CShape* shape)
{
	int i= 0;
	while(m_shapes[i]!=shape) i++;

	dGeomDestroy (m_geoms[i]);
	dBodyDestroy (bodies[i]);

	int j;
	for(j= i; j<m_shapeCount; j++) {
		m_shapes[j]= m_shapes[j+1];

		bodies[j]= bodies[j+1];
		m_geoms[j]= m_geoms[j+1];
	
		if(j+1<m_shapeCount) { 
			dBodySetData(bodies[j], (void *)j);
			dGeomSetData(m_geoms[j], (void *)j);
		}
	}
	m_shapeCount--;
}
static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*)(size_t)i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
    else if (cmd == 'y') {
      sides[1] *= 0.5;
      dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
	else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'm') {
      dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
      dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, 
		  (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));

      obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);

      // remember the mesh's dTriMeshDataID on its userdata for convenience.
      dGeomSetData(obj[i].geom[0], new_tmdata);

      dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
      printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
      dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
      dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCapsule (0,radius,length);
	  dMassSetCapsule (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}
Пример #28
0
 ~Card()
 {
     dBodyDestroy(body);
     dGeomDestroy(geom);
 }
static void command (int cmd)
{
  size_t i;
  int j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCCylinder (0,radius,length);
	  dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
  else if (cmd == '1') {
    write_world = 1;
  }
}
Пример #30
0
static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;
  bool setBody = false;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' 
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*)(size_t)i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
 
            setBody = true;
            // start accumulating masses for the composite geometries
            dMass m2;
            dMassSetZero (&m);

            dReal dpos[GPB][3];	// delta-positions for composite geometries
            dMatrix3 drot[GPB];
      
            // set random delta positions
            for (j=0; j<GPB; j++)
                for (k=0; k<3; k++)
                    dpos[j][k] = dRandReal()*0.3-0.15;
    
            for (k=0; k<GPB; k++) {
                if (k==0) {
                    dReal radius = dRandReal()*0.25+0.05;
                    obj[i].geom[k] = dCreateSphere (space,radius);
                    dMassSetSphere (&m2,DENSITY,radius);
                } else if (k==1) {
                    obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
                    dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
                } else {
                    dReal radius = dRandReal()*0.1+0.05;
                    dReal length = dRandReal()*1.0+0.1;
                    obj[i].geom[k] = dCreateCapsule(space,radius,length);
                    dMassSetCapsule(&m2,DENSITY,3,radius,length);
                }

                dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
                                   dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
                dMassRotate(&m2,drot[k]);
		
                dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

                // add to the total mass
                dMassAdd(&m,&m2);

            }
            for (k=0; k<GPB; k++) {
                dGeomSetBody(obj[i].geom[k],obj[i].body);
                dGeomSetOffsetPosition(obj[i].geom[k],
                                       dpos[k][0]-m.c[0],
                                       dpos[k][1]-m.c[1],
                                       dpos[k][2]-m.c[2]);
                dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
            }
            dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
            dBodySetMass(obj[i].body,&m);

        } else if (cmd == 'v') {

            dMassSetBox (&m,DENSITY,0.25,0.25,0.25);

            obj[i].geom[0] = dCreateConvex(space,
                                           planes,
                                           planecount,
                                           points,
                                           pointcount,
                                           polygons);
        }

        if (!setBody) { // avoid calling for composite geometries
            for (k=0; k < GPB; k++)
                if (obj[i].geom[k])
                    dGeomSetBody(obj[i].geom[k],obj[i].body);

            dBodySetMass(obj[i].body,&m);
        }
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}