C++ dGeomSetPosition函数代码示例

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.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  dInitODE2(0);
  world = dWorldCreate();
 
  space = dSimpleSpaceCreate(0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-0.5);
  dWorldSetCFM (world,1e-5);
  dCreatePlane (space,0,0,1,0);
  memset (obj,0,sizeof(obj));

  // note: can't share tridata if intending to trimesh-trimesh collide
  TriData1 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData1, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  TriData2 = dGeomTriMeshDataCreate();
  dGeomTriMeshDataBuildSingle(TriData2, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
  
  TriMesh1 = dCreateTriMesh(space, TriData1, 0, 0, 0);
  TriMesh2 = dCreateTriMesh(space, TriData2, 0, 0, 0);
  dGeomSetData(TriMesh1, TriData1);
  dGeomSetData(TriMesh2, TriData2);
  
  {dGeomSetPosition(TriMesh1, 0, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh1, Rotation);}

  {dGeomSetPosition(TriMesh2, 1, 0, 0.9);
  dMatrix3 Rotation;
  dRFromAxisAndAngle(Rotation, 1, 0, 0, M_PI / 2);
  dGeomSetRotation(TriMesh2, Rotation);}
  
  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

void CManipulator::CreateManipulator(dWorldID world,dSpaceID space)
{
	if(baseManipulator.paraBase.typeJoint=CYLINDER)
		baseManipulator.geomBase=dCreateCylinder(space,baseManipulator.paraBase.paraManipulatorCylinder.r,baseManipulator.paraBase.paraManipulatorCylinder.length);
	else
		baseManipulator.geomBase=dCreateBox(space,baseManipulator.paraBase.paraManipulatorBox.x,baseManipulator.paraBase.paraManipulatorBox.y,baseManipulator.paraBase.paraManipulatorBox.z);
	if(baseManipulator.paraBase.typeJoint=CYLINDER)
		dGeomSetPosition(baseManipulator.geomBase,0,0,baseManipulator.paraBase.paraManipulatorCylinder.length/2.0);
	else
		dGeomSetPosition(baseManipulator.geomBase,0,0,baseManipulator.paraBase.paraManipulatorBox.z/2.0);

	int i;
	SPoint tempptAnchor;
	dBodyID tempbodyLast=NULL;
	tempptAnchor.x=tempptAnchor.y=0;
	if(baseManipulator.paraBase.typeJoint=CYLINDER)
		tempptAnchor.z=baseManipulator.paraBase.paraManipulatorCylinder.length;
	else
		tempptAnchor.z=baseManipulator.paraBase.paraManipulatorBox.z;
	//创建第一轴
	CreateJoint(tempptAnchor,jointManipulator[0].paraJoint,BORDER,tempbodyLast,
			jointManipulator[0].bodyJoint,jointManipulator[0].geomJoint,jointManipulator[0].joint,world,space);
	SetStop(jointManipulator[0].joint,stopparaJoint[0]);

	//创建第二轴	
	if(jointManipulator[0].paraJoint.typeJoint==CYLINDER)
	{
		tempptAnchor.x=tempptAnchor.x;
		tempptAnchor.z=tempptAnchor.z;
		tempptAnchor.y=tempptAnchor.y+jointManipulator[0].paraJoint.paraManipulatorCylinder.r;
	}
	else
	{
		tempptAnchor.x=tempptAnchor.x;
		tempptAnchor.z=tempptAnchor.z;
		tempptAnchor.y=tempptAnchor.y+jointManipulator[0].paraJoint.paraManipulatorBox.y/2;
	}
	CreateJoint(tempptAnchor,jointManipulator[1].paraJoint,BORDER,tempbodyLast,
			jointManipulator[1].bodyJoint,jointManipulator[1].geomJoint,jointManipulator[1].joint,world,space);
	SetStop(jointManipulator[1].joint,stopparaJoint[1]);
	
	//创建其他轴
	for( i=2;i<NUMOFLEGJOINTS;i++)
	{
		CreateJoint(tempptAnchor,
			jointManipulator[i].paraJoint,BORDER,
			tempbodyLast,
			jointManipulator[i].bodyJoint,
			jointManipulator[i].geomJoint,
			jointManipulator[i].joint,
			world,space);
		SetStop(jointManipulator[i].joint,stopparaJoint[i]);
	}

	return;
}

int test_dBoxTouchesBox()
{
  int k,bt1,bt2;
  dVector3 p1,p2,side1,side2;
  dMatrix3 R1,R2;

  dSimpleSpace space(0);
  dGeomID box1 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box1);
  dGeomID box2 = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box2);

  dMakeRandomVector (p1,3,0.5);
  dMakeRandomVector (p2,3,0.5);
  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);

  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
  dGeomSetRotation (box1,R1);
  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
  dGeomSetRotation (box2,R2);
  draw_all_objects (space);

  int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
  int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
  bt1 = t1 || t2;
  bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);

  if (bt1 != bt2) FAILED();

  /*
    // some more debugging info if necessary
    if (bt1 && bt2) printf ("agree - boxes touch\n");
    if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
    if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
			     "says no\n");
    if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
			     "says yes\n");
  */

  PASSED();
}

void ApproxDistanceSensor::DistanceSensor::updateValue()
{
  pose = physicalObject->pose;
  pose.conc(offset);
  invertedPose = pose.invert();
  Vector3<> boxPos = pose * Vector3<>(max * 0.5f, 0.f, 0.f);
  dGeomSetPosition(geom, boxPos.x, boxPos.y, boxPos.z);
  dMatrix3 matrix3;
  ODETools::convertMatrix(pose.rotation, matrix3);
  dGeomSetRotation(geom, matrix3);
  closestGeom = 0;
  closestSqrDistance = maxSqrDist;
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
  dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
  if(closestGeom)
  {
    const dReal* pos = dGeomGetPosition(closestGeom);
    Geometry* geometry = (Geometry*)dGeomGetData(closestGeom);
    data.floatValue = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - pose.translation).abs() - geometry->innerRadius;
    if(data.floatValue < min)
      data.floatValue = min;
  }
  else
    data.floatValue = max;
}

// 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 command (int cmd)
{
  // note: 0.0174532925 radians = 1 degree
  dQuaternion q;
  dMatrix3 m;
  switch(cmd)
    {
    case 'w':
      geom1pos[0]+=0.05;
      break;
    case 'a':
      geom1pos[1]-=0.05;
      break;
    case 's':
      geom1pos[0]-=0.05;
      break;
    case 'd':
      geom1pos[1]+=0.05;
      break;
    case 'e':
      geom1pos[2]-=0.05;
      break;
    case 'q':
      geom1pos[2]+=0.05;
      break;
    case 'i':
      dQFromAxisAndAngle (q, 0, 0, 1,0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'j':
      dQFromAxisAndAngle (q, 1, 0, 0,0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'k':
      dQFromAxisAndAngle (q, 0, 0, 1,-0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'l':
      dQFromAxisAndAngle (q, 1, 0, 0,-0.0174532925);
      dQMultiply0(geom1quat,geom1quat,q);
      break;
    case 'm':
		(drawmode!=DS_POLYFILL)? drawmode=DS_POLYFILL:drawmode=DS_WIREFRAME;
      break;
    case 'n':
		(geoms!=convex)? geoms=convex:geoms=boxes;
      break;
    default:
      dsPrint ("received command %d (`%c')\n",cmd,cmd);     
    }
#if 0
  dGeomSetPosition (geoms[1],
		    geom1pos[0],
		    geom1pos[1],
		    geom1pos[2]);
  dQtoR (geom1quat, m);
  dGeomSetRotation (geoms[1],m);
#endif
  DumpInfo=true;
}

void construirCirculoCentral()
{
    // Círculo central (sendo desenhado como um cilindro)
    // Para melhor visualização, desabilitar sombras (CTRL+S)
    circle = dCreateCylinder(0,CIRCLE_RADIUS,0.01);
    dGeomSetPosition(circle,0,0,0.005);
}

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 Compound::addGeometry(const Pose3<>& parentPose, Geometry& geometry, SimRobotCore2::CollisionCallback* callback)
{
  // compute pose
  Pose3<> geomPose = parentPose;
  if(geometry.translation)
    geomPose.translate(*geometry.translation);
  if(geometry.rotation)
    geomPose.rotate(*geometry.rotation);

  // create geometry
  dGeomID geom = geometry.createGeometry(Simulation::simulation->staticSpace);
  if(geom)
  {
    dGeomSetData(geom, &geometry);

    // set pose
    dGeomSetPosition(geom, geomPose.translation.x, geomPose.translation.y, geomPose.translation.z);
    dMatrix3 matrix3;
    ODETools::convertMatrix(geomPose.rotation, matrix3);
    dGeomSetRotation(geom, matrix3);
  }

  // handle nested geometries
  for(std::list< ::PhysicalObject*>::const_iterator iter = geometry.physicalDrawings.begin(), end = geometry.physicalDrawings.end(); iter != end; ++iter)
  {
    Geometry* geometry = dynamic_cast<Geometry*>(*iter);
    if(geometry)
      addGeometry(geomPose, *geometry, callback);
  }
}

    void box::create_physical_body(
                    double x, 
                    double y, 
                    double z, 
                    double size_x, 
                    double size_y, 
                    double size_z, 
                    double mass, 
                    manager& mgr)
    {
         world_id = mgr.ode_world();
         space_id = mgr.ode_space();

        //create and position the geom to represent the pysical shape of the rigid body   
        geom_id = dCreateBox (mgr.ode_space(), size_x, size_y, size_z);
        object::set_geom_data(geom_id);
        dGeomSetPosition (geom_id, x, y, z); 		
		size[0] = size_x;
        size[1] = size_y;
        size[2] = size_z;

		if(mass > 0)
		{
			object::create_rigid_body(x, y, z, mgr);
			set_mass(mass);
			dGeomSetBody (geom_id, body_id);  
		}
    }

void Obstacle::on_addToScene()
{
    node_ = SceneGraph::addModel(name_, model_);
    size_t batchCnt = 0;
    TriangleBatch const *triBatch = model_->batches(&batchCnt);
    size_t boneCnt = 0;
    Bone const *bone = model_->bones(&boneCnt);
    size_t vSize = model_->vertexSize();
    char const * vertex = (char const *)model_->vertices();
    unsigned int const *index = (unsigned int const *)model_->indices();
    for (size_t bi = 0; bi != batchCnt; ++bi)
    {
        dTriMeshDataID tmd = dGeomTriMeshDataCreate();
        dGeomTriMeshDataBuildSingle(tmd, vertex, vSize, triBatch[bi].maxVertexIndex + 1, 
            index + triBatch[bi].firstTriangle * 3, triBatch[bi].numTriangles * 3, 12);
        dGeomID geom = dCreateTriMesh(gStaticSpace, tmd, 0, 0, 0);
        tmd_.push_back(tmd);
        geom_.push_back(geom);
        Matrix bx;
        get_bone_transform(bone, triBatch[bi].bone, bx);
        Vec3 p(bx.translation());
        addTo(p, pos());
        dGeomSetPosition(geom, p.x, p.y, p.z);
        dGeomSetRotation(geom, bx.rows[0]);
    }
}

    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());
      }
    }

    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);
		}

    }

void CProtoHapticDoc::UpdateDynamics()
{
	for(int i= 0; i<m_shapeCount; i++) {

		dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
									    m_shapes[i]->getSizeY(),
									    m_shapes[i]->getSizeZ());

		dGeomSetPosition   (m_geoms[i], m_shapes[i]->getLocationX(),
									    m_shapes[i]->getLocationY(),
									    m_shapes[i]->getLocationZ());
		dGeomSetRotation   (m_geoms[i], dBodyGetRotation(bodies[i]));

		dBodySetPosition   (bodies[i], m_shapes[i]->getLocationX(),
									   m_shapes[i]->getLocationY(),
									   m_shapes[i]->getLocationZ());

		float *rotation= m_shapes[i]->getRotation();

		const dReal rot[12]=
		{ rotation[0], rotation[4], rotation[8], rotation[12],
		  rotation[1], rotation[5], rotation[9], rotation[13],
		  rotation[2], rotation[6], rotation[10], rotation[14] };

		dBodySetRotation   (bodies[i], rot);

		dMass mass;
		dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
								                   m_shapes[i]->getSizeY(),
								                   m_shapes[i]->getSizeZ());

		dBodySetMass (bodies[i], &mass);

	}
}

ODEObject::ODEObject(OscObject *obj, dGeomID odeGeom, dWorldID odeWorld, dSpaceID odeSpace)
    : m_odeWorld(odeWorld), m_odeSpace(odeSpace)
{
    m_object = obj;

    m_odeGeom = odeGeom;
    m_odeBody = NULL;
    m_odeBody = dBodyCreate(m_odeWorld);

    assert(m_odeGeom!=NULL);

    dBodySetPosition(m_odeBody, 0, 0, 0);
    dGeomSetPosition(m_odeGeom, 0, 0, 0);

    // note: owners must override this by setting the density. can't
    //       do it here because obj->m_pSpecial is not yet
    //       initialized.
    dMassSetSphere(&m_odeMass, 1, 1);
    dBodySetMass(m_odeBody, &m_odeMass);

    dGeomSetBody(m_odeGeom, m_odeBody);
    dGeomSetData(m_odeGeom, obj);

    if (!obj) return;

    obj->m_rotation.setSetCallback(ODEObject::on_set_rotation, this);
    obj->m_position.setSetCallback(ODEObject::on_set_position, this);
    obj->m_velocity.setSetCallback(ODEObject::on_set_velocity, this);
    obj->m_accel.setSetCallback(ODEObject::on_set_accel, this);
    obj->m_force.setSetCallback(ODEObject::on_set_force, this);

    obj->addHandler("push", "ffffff", ODEObject::push_handler);
}