C++ dGeomSetBody函数代码示例

// Create a ball and a pole
void createBallandPole() {
	dMass m1;
	dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;

	// ball
	ball.radius = 0.2;
	ball.mass = 1.0;
	ball.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetSphereTotal(&m1, ball.mass, ball.radius);
	dBodySetMass(ball.body, &m1);
	dBodySetPosition(ball.body, x0, y0, z0);

	ball.geom = dCreateSphere(space, ball.radius);
	dGeomSetBody(ball.geom, ball.body);

	// pole
	pole.radius = 0.025;
	pole.length = 1.0;
	pole.mass = 1.0;
	pole.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
	dBodySetMass(pole.body, &m1);
	dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);

	pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
	dGeomSetBody(pole.geom, pole.body);

	// hinge joint
	joint = dJointCreateHinge(world, 0);
	dJointAttach(joint, ball.body, pole.body);
	dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
	dJointSetHingeAxis(joint, 1, 0, 0);
}

int main (int argc, char **argv)
{
    // setup pointers to drawstuff callback functions
    dsFunctions fn;
    fn.version = DS_VERSION;
    fn.start = &start;
    fn.step = &simLoop;
    fn.stop = 0;
    fn.command = 0;
    fn.path_to_textures = "../../drawstuff/textures";
 
    dInitODE ();
    // create world
    world = dWorldCreate ();
    space = dHashSpaceCreate (0);
    dWorldSetGravity (world,0,0,0); //Original Gravity = -0.2
    dWorldSetCFM (world,1e-5);
    dCreatePlane (space,0,0,1,0);
    contactgroup = dJointGroupCreate (0);

    // create object
    sphere0 = dBodyCreate (world);
    sphere0_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere0,&m);
    dGeomSetBody (sphere0_geom,sphere0);
 
    sphere1 = dBodyCreate (world);
    sphere1_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere1,&m);
    dGeomSetBody (sphere1_geom,sphere1);
 
    sphere2 = dBodyCreate (world);
    sphere2_geom = dCreateSphere (space,0.5);
    dMassSetSphere (&m,1,0.5);
    dBodySetMass (sphere2,&m);
    dGeomSetBody (sphere2_geom,sphere2);
  
    // set initial position
    dBodySetPosition (sphere0,0,0,4);
    dBodySetPosition (sphere1,5,0,4);
    dBodySetPosition (sphere2,-2,0,4);

// run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);
    // clean up
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}

void RigidBodyEnvironment::createWorld(void)
{
    // BEGIN SETTING UP AN OPENDE ENVIRONMENT
    // ***********************************

    bodyWorld = dWorldCreate();
    space = dHashSpaceCreate(0);

    dWorldSetGravity(bodyWorld, 0, 0, -0.981);

    double lx = 0.2;
    double ly = 0.2;
    double lz = 0.1;

    dMassSetBox(&m, 1, lx, ly, lz);

    boxGeom = dCreateBox(space, lx, ly, lz);
    boxBody = dBodyCreate(bodyWorld);
    dBodySetMass(boxBody, &m);
    dGeomSetBody(boxGeom, boxBody);

    // *********************************
    // END SETTING UP AN OPENDE ENVIRONMENT

    setPlanningParameters();
}

void TrackedVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    //dMassAdjust(&this->vehicleMass, 2.40);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);

    this->leftTrack->create();
    this->rightTrack->create();

    dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
    dRigidBodyArraySetPosition(leftTrack->bodyArray,  -wheelBase/2, -(w - trackWidth)/2, 0);
    dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2,  (w - trackWidth)/2, 0);

    this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
    this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
    dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
    dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
    dJointSetFixed(this->leftTrackJoint);
    dJointSetFixed(this->rightTrackJoint);

    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
    dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}

void createInvisibleHead( float* pos )
{
	dMatrix3 head_orientation;
	dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);

	//position and orientation
	head.Body = dBodyCreate(World);
	dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
	dBodySetRotation(head.Body, head_orientation);
	dBodySetLinearVel(head.Body, 0, 0, 0);
	dBodySetData(head.Body, (void *)0);

	//mass
	dMass head_mass;
	dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
	dBodySetMass(head.Body, &head_mass);

	//geometry
	head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
	dGeomSetBody(head.Geom, head.Body);

	//fixed joint
	invis_box_joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(invis_box_joint, body.Body, head.Body);
	dJointSetFixed(invis_box_joint);
}

/*
=================================================================================
createFixedLeg

	Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
	ODEObject &bodyAttachedTo,
	dJointID& joint,
	dReal xPos, dReal yPos, dReal zPos,
	dReal xRot, dReal yRot, dReal zRot,
	dReal radius,
	dReal length)
{
	dMatrix3 legOrient;
	dRFromEulerAngles(legOrient, xRot, yRot, zRot);

	//position and orientation
	leg.Body = dBodyCreate(World);
	dBodySetPosition(leg.Body, xPos, yPos, zPos);
	dBodySetRotation(leg.Body, legOrient);
	dBodySetLinearVel(leg.Body, 0, 0, 0);
	dBodySetData(leg.Body, (void *)0);

	//mass
	dMass legMass;
	dMassSetCapsule(&legMass, 1, 3, radius, length);
	dBodySetMass(leg.Body, &legMass);

	//geometry
	leg.Geom = dCreateCapsule(Space, radius, length);
	dGeomSetBody(leg.Geom, leg.Body);

	//fixed joint
	joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
	dJointSetFixed(joint);
}

void Balaenidae::embody(dWorldID world, dSpaceID space)
{
    me = dBodyCreate(world);
    embody(me);
    geom = dCreateBox( space, 100.0f, 40, 500.0f);   // scale 50
    dGeomSetBody(geom, me);
}

 BarrelGeom2(Barrel *b) :
     OdeGeom(dCreateBox(gDynamicSpace, b->radius_ * 1.4f, b->radius_ * 1.4f, b->height_)),
     b_(b)
 {
     dGeomSetBody(id_, b->body_->id_);
     dGeomSetData(id_, this);
 }

 BarrelGeom(Barrel *b) :
     OdeGeom(dCreateCylinder(gDynamicSpace, b->radius_, b->height_)),
     b_(b)
 {
     dGeomSetBody(id_, b->body_->id_);
     dGeomSetData(id_, this);
 }

void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
	TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
    dMass totalMass;
    dBodyGetMass( m_BodyID, &totalMass );
    if ( m_GeomIDs.size() == 0 )
    {
        dMassSetZero( &totalMass );
    }
    dMatrix4 R;
    dVector3 P;
    Matrix4ToODE( _bodyToGeomTransform, R, P );
    dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
    dGeomID encapsulatedGeom = 0;
    dMass currMass;
    dMassSetZero( &currMass );

    encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );

    dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
    dMassRotate( &currMass, R );
    //dMassTranslate(&currMass,P[0],P[1],P[2]);
    dMassAdd( &totalMass, &currMass );
    dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
    dGeomSetRotation( encapsulatedGeom, R );
    dGeomTransformSetCleanup( geomTransform, 1 );
    dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
    dGeomSetBody( geomTransform, m_BodyID );

    m_GeomIDs.push_back( geomTransform );
    dBodySetMass( m_BodyID, &totalMass );

}

    void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
    {
      math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);

      rotation.LoadIdentity();
      rotation.SetFromAngles(math::DegreesToRadians(rot));

      const math::cMat4 m = rotation.GetMatrix();

      dMatrix3 r;
      r[0] = m[0];    r[1] = m[4];    r[2] = m[8];    r[3] = 0;
      r[4] = m[1];    r[5] = m[5];    r[6] = m[9];    r[7] = 0;
      r[8] = m[2];    r[9] = m[6];    r[10] = m[10];  r[11] = 0;

      position = pos;

      dGeomSetPosition(geom, position.x, position.y, position.z);
      dGeomSetRotation(geom, r);

      if (bBody) {
        body = dBodyCreate(pWorld->GetWorld());
        dBodySetPosition(body, position.x, position.y, position.z);
        dBodySetRotation(body, r);
        dBodySetAutoDisableFlag(body, 1);

        dGeomSetBody(geom, body);

        pWorld->AddPhysicsObject(shared_from_this());
      }
    }

CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
    body = dBodyCreate(world);
    geom = dCreateBox(space, sides[0], sides[1], sides[2]);
    dGeomSetBody(geom, body);
    dGeomSetData(geom, this);
    dMass mass;
    mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
    dBodySetMass(body, &mass);

    const dMatrix3 rotationMatrix = {1,0,0,0,
                                     0,1,0,0,
                                     0,0,1,0};

    dBodySetRotation(body, rotationMatrix);

    dBodySetPosition(body,x,y,z);

    for(int i = 0 ; i < 6 ; i++)
        attachedPieces[i] = NULL; // initialize attached piece array to all null pointers

    color[0] = 1;
    color[1] = 1;
    color[2] = 1;
}

// Universal method for all specific ODE geom types, which add the
// geom to the collide space, using an ODE proxy geom to offset the
// geom by the provided transformation matrix. The geom will also
// be attached to the rigid body, if any is set.
void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID)
{
	n_assert(GeomId);
	n_assert(!IsAttached());

	// set the geom's local Transform
	const vector3& Pos = Transform.pos_component();
	dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z);
	dMatrix3 ODERotation;
	CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
	dGeomSetRotation(GeomId, ODERotation);

	// if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body
	// else directly set Transform and rotation
	if (pRigidBody)
	{
		ODEGeomID = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(ODEGeomID, 1);
		dGeomTransformSetGeom(ODEGeomID, GeomId);
		dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID());
	}
	else ODEGeomID = GeomId;

	dGeomSetCategoryBits(ODEGeomID, CatBits);
	dGeomSetCollideBits(ODEGeomID, CollBits);
	dGeomSetData(ODEGeomID, this);
	AttachToSpace(SpaceID);
}

void Robots::construirChassi(dWorldID world)
{
    for (int i=0; i < 2; i++)
    {
        // Cria objeto e geometria
        this->body[i] = dBodyCreate(world);
        this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);

        // Define a posição do objeto
        dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);

		// Se o robô for do segundo time, deve ser rotacionado em 180 graus
        if ((this->id == 3) || (this->id == 4) || (this->id == 5))
        {
			dQuaternion q;
			dQFromAxisAndAngle(q,0,0,1,M_PI);
			dBodySetQuaternion(this->body[i],q);
		}

        // Define a massa do objeto
        dMass m;
        dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT);	// O segundo bloco é mais curto
        dMassAdjust(&m,CMASS*(1+i*2));					// O segundo bloco é mais pesado
        dBodySetMass(this->body[i],&m);
                
        // Associa o objeto à sua geometria
        dGeomSetBody(this->box[i],this->body[i]);
    }
		
    // O chassis é composto por dois blocos que são fixos entre si
    dJointID fixed = dJointCreateFixed(world,0);
    dJointAttach(fixed,this->body[1],this->body[0]);
    dJointSetFixed(fixed); 
}

    void cylinder::create_physical_body(
                    double x, 
                    double y, 
                    double z, 
                    double radius, 
                    double length, 
                    double mass, 
                    manager& mgr)
    {
		this->radius = radius;
		this->length = length;

		world_id = mgr.ode_world();
		space_id = mgr.ode_space();
        //create and position the geom to represent the physical shape of the rigid body   
        geom_id = dCreateCylinder(mgr.ode_space(),radius, length);
        object::set_geom_data(geom_id);
        dGeomSetPosition (geom_id, x, y, z); 		
		
		if(mass > 0)
		{
			object::create_rigid_body(x, y, z, mgr);
			dGeomSetBody (geom_id, body_id);  
			set_mass(mass);
		}

    }