Beispiel #1
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.command = &command;
    fn.stop = 0;
    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

    // create world
    dInitODE();
    world = dWorldCreate();

#if 1
    space = dHashSpaceCreate (0);
#elif 0
    dVector3 center = {0,0,0}, extents = { 100, 100, 100};
    space = dQuadTreeSpaceCreate(0, center, extents, 5);
#elif 0
    space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
#else
    space = dSimpleSpaceCreate(0);
#endif

    contactgroup = dJointGroupCreate (0);
    dWorldSetGravity (world,0,0,-0.5);
    dWorldSetCFM (world,1e-5);
    
    dWorldSetLinearDamping(world, 0.00001);
    dWorldSetAngularDamping(world, 0.005);
    dWorldSetMaxAngularSpeed(world, 200);

    dWorldSetContactSurfaceLayer (world,0.001);
    ground = dCreatePlane (space,0,0,1,0);
    
    memset (obj,0,sizeof(obj));

    // create lift platform
    platform = dCreateBox(space, 4, 4, 1);

    dGeomSetCategoryBits(ground, 1ul);
    dGeomSetCategoryBits(platform, 2ul);
    dGeomSetCollideBits(ground, ~2ul);
    dGeomSetCollideBits(platform, ~1ul);

    // run simulation
    dsSimulationLoop (argc,argv,352,288,&fn);

    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}
Beispiel #2
0
// 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);
}
Beispiel #3
0
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 Primitive::attachGeomAndSetColliderFlags(){
   if(mode & Body){
     // geom is assigned to body and is set into category Dyn
     dGeomSetBody (geom, body);
     dGeomSetCategoryBits (geom, Dyn);
     dGeomSetCollideBits (geom, ~0x0); // collides with everything
   } else {
     // geom is static, so it is member of the static category and will collide not with other statics
     dGeomSetCategoryBits (geom, Stat);
     dGeomSetCollideBits (geom, ~Stat);
   }
   if(mode & _Child){ // in case of a child object it is always dynamic
     dGeomSetCategoryBits (geom, Dyn);
     dGeomSetCollideBits (geom, ~0x0); // collides with everything
   }
   dGeomSetData(geom, (void*)this); // set primitive as geom data
 }
Beispiel #5
0
void ODESimulator::AddObject(const RigidObject& object)
{
  objects.push_back(new ODERigidObject(object));
  objects.back()->Create(worldID,envSpaceID);
  dGeomSetData(objects.back()->geom(),(void*)(objects.size()-1));
  dGeomSetCategoryBits(objects.back()->geom(),0x2);
  dGeomSetCollideBits(objects.back()->geom(),0xffffffff);
}
Beispiel #6
0
void ODESimulator::AddRobot(Robot& robot)
{
  robots.push_back(new ODERobot(robot));
  //For some reason, self collisions don't work with hash spaces
  robots.back()->Create(worldID);
  //robotStances.resize(robots.size());
  for(size_t i=0;i<robot.links.size();i++)
    if(robots.back()->triMesh(i) && robots.back()->geom(i)) {
      if(robots.back()->robot.parents[i] == -1) { //treat as part of the terrain
	dGeomSetCategoryBits(robots.back()->geom(i),0x1);
	dGeomSetCollideBits(robots.back()->geom(i),0xffffffff ^ 0x1);
      }
      else {
	dGeomSetCategoryBits(robots.back()->geom(i),0x4);
	dGeomSetCollideBits(robots.back()->geom(i),0xffffffff);
      }
    }
}
Beispiel #7
0
void set_phys_geom_attr_i(dGeomID geom, int p, int i)
{
    switch (p)
    {
    case GEOM_ATTR_CATEGORY: dGeomSetCategoryBits(geom, i); break;
    case GEOM_ATTR_COLLIDER: dGeomSetCollideBits (geom, i); break;
    case GEOM_ATTR_RESPONSE: get_data(geom)->response = i;  break;
    case GEOM_ATTR_CALLBACK: get_data(geom)->callback = i;  break;
    }
}
Beispiel #8
0
void SkidSteeringVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);

    dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
            this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
            dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
            dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
            dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
            this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
            dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
            dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
            dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
            dMatrix3 wheelR;
            dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
            dBodySetRotation(this->wheelBody[fr][lr], wheelR);
            this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
            dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
            dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
            dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
            dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
        }
    }
    
    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
        }
    }
}
        void Solid::createGeometry(const dSpaceID& i_space)
        {
          /// need to get the correct geometry from volume.
          std::vector< ::Ogre::Vector3> vertices ;
          std::vector<unsigned long>    indices ;
          Ogre::Vector3                 scale(1,1,1) ;        
          
          m_vertices = NULL ;
          m_indices = NULL ;

          InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#0") ;
          getObject()
            ->getTrait<Model::Solid>()
            ->getMesh().getMeshInformation(vertices,indices,scale) ;

          if (vertices.size()>0 && indices.size() > 0)
          {
            m_vertices = new dVector3[vertices.size()];
            m_indices = new dTriIndex[indices.size()];
            
            for(unsigned int vertex_index = 0 ; 
                vertex_index < vertices.size() ; 
                ++vertex_index)
            {
              m_vertices[vertex_index][0] = (dReal)(vertices[vertex_index].x) ;
              m_vertices[vertex_index][1] = (dReal)(vertices[vertex_index].y) ;
              m_vertices[vertex_index][2] = (dReal)(vertices[vertex_index].z) ;
              m_vertices[vertex_index][3] = 0 ;
            }

            for(unsigned int index = 0 ; 
                index < indices.size() ; 
                ++index)
            {
              m_indices[index] = (int)indices[index] ;
            }

            m_data = dGeomTriMeshDataCreate() ;

            dGeomTriMeshDataBuildSimple(m_data,
                                        (dReal*)m_vertices,(int)vertices.size(),
                                        m_indices,(int)indices.size()) ;

            m_geometry1 = dCreateTriMesh(i_space,m_data,0,0,0);
            dGeomSetData(m_geometry1,m_data) ;
            dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Solid) ;
            createApproximatedGeometry(i_space) ;
            InternalMessage("Physic","Physic::Implementation::Ode::Solid::createGeometry trace#5") ;
          }
        }
Beispiel #10
0
void ODESimulator::AddEnvironment(const Environment& env)
{
  envs.push_back(&env);
  envMeshes.resize(envMeshes.size()+1);
  envMeshes.back() = new ODETriMesh;
  envMeshes.back()->Create(env.mesh,envSpaceID);
  envMeshes.back()->surf() = settings.defaultEnvSurface;
  envMeshes.back()->SetPadding(settings.defaultEnvPadding);
  if(!env.kFriction.empty())
    envMeshes.back()->surf().kFriction = env.kFriction[0];
  //the index of the environment is encoded as -1-index
  dGeomSetData(envMeshes.back()->geom(),(void*)(-(int)envs.size()));
  dGeomSetCategoryBits(envMeshes.back()->geom(),0x1);
  dGeomSetCollideBits(envMeshes.back()->geom(),0xffffffff ^ 0x1);
}
	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;
		}
	}
        void LaserBeam::createGeometry(const dSpaceID& space)
        {
          /// build a ray
          Model::LaserBeam* beam = getTrait<Model::LaserBeam>() ;
          Model::Mobile* mobile = getTrait<Model::Mobile>() ;

          float collision_beam_length =
              std::max(mobile->getSpeed().MeterPerSecond().length()*getControlerSet()->getTimeStep(),
                       beam->getLength().Meter()) ;

          m_ray = dCreateRay(0,2*collision_beam_length) ;

          /// we use -z as forward @see Model::Orientation
          dGeomRaySet(m_ray,0,0,0,0,0,-1) ;
          dGeomRaySetParams(m_ray,false,false) ;
          dGeomRaySetClosestHit(m_ray,true) ;

          /// a geometry transform allows to have a local z axis inversion
          m_geometry1 = dCreateGeomTransform(space) ;
          dGeomTransformSetGeom(m_geometry1,m_ray) ;
          dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Laser) ;
        }
Beispiel #13
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.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  if (argc >= 2 ) {
    for (int i=1; i < argc; ++i) {
      if (  0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
        Help (argv);

      if (  0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
        type = dJointTypePR;

      if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
        int j = i+1;
        if ( j+1 > argc      ||  // Check if we have enough arguments
             argv[j] == '\0' ||  // We should have a path here
             argv[j][0] == '-' ) // We should have a path not a command line
          Help (argv);
        else
          fn.path_to_textures = argv[++i]; // Increase i since we use this argument
      }
    }
  }

  dInitODE2(0);

  world.setERP (0.8);

  space = dSimpleSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  geom[GROUND] = dCreatePlane (space, 0,0,1,0);
  dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
  dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

  dMass m;

  // Create the body attached to the World
  body[W].create (world);
  // Main axis of cylinder is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[W], body[W]);
  dGeomSetCategoryBits (geom[W], catBits[W]);
  dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
  body[W].setMass(m);





  // Create the dandling body
  body[D].create(world);
  // Main axis of capsule is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[D], body[D]);
  dGeomSetCategoryBits (geom[D], catBits[D]);
  dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
  body[D].setMass(&m);


  // Create the external part of the slider joint
  geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
  dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
  dGeomSetCollideBits (geom[EXT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );

  // Create the internal part of the slider joint
  geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
                          INT_EXT_RATIO*extDim[Y],
                          INT_EXT_RATIO*extDim[Z]);
  dGeomSetCategoryBits (geom[INT], catBits[INT]);
  dGeomSetCollideBits (geom[INT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );


  dMatrix3 R;
  dGeomID id;
  // Create the first axis of the universal joi9nt
  geom[AXIS1] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS1], R);
  dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
  dGeomSetCollideBits (geom[AXIS1],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS1];
  dGeomTransformSetGeom (geom[AXIS1],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the second axis of the universal joint
  geom[AXIS2] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS2], R);
  dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
  dGeomSetCollideBits (geom[AXIS2],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS2];
  dGeomTransformSetGeom (geom[AXIS2],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the anchor
  geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
  dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
  dGeomSetCollideBits (geom[ANCHOR],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );



  if (body[W]) {
    body[W].setPosition(0, 0, 5);
  }


  if (geom[EXT]) {
    dGeomSetPosition (geom[EXT], 0,0,3.8);
  }
  if (geom[INT]) {
    dGeomSetPosition (geom[INT], 0,0,2.6);
  }
  if (geom[AXIS1]) {
    dGeomSetPosition (geom[AXIS1], 0,0,2.5);
  }
  if (geom[AXIS2]) {
    dGeomSetPosition (geom[AXIS2], 0,0,2.5);
  }

  if (geom[ANCHOR]) {
    dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
  }

  if (body[D]) {
    body[D].setPosition(0,0,1.5);
  }



  // Attache the upper box to the world
  dJointID fixed = dJointCreateFixed (world,0);
  dJointAttach (fixed , NULL, body[W]);
  dJointSetFixed (fixed );

  if (type == dJointTypePR) {
    dPRJoint *pr = new dPRJoint (world, 0);
    pr->attach (body[W], body[D]);
    pr->setAxis1 (0, 0, -1);
    pr->setAxis2 (1, 0, 0);
    joint = pr;

    dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
  }
  else {
    dPUJoint *pu = new dPUJoint (world, 0);
    pu->attach (body[W], body[D]);
    pu->setAxis1 (1, 0, 0);
    pu->setAxis2 (0, 1, 0);
    pu->setAxisP (0, 0, -1);
    joint = pu;

    dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
  }


  // run simulation
  dsSimulationLoop (argc,argv,400,300,&fn);

  delete joint;
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
void PhysicsGeom::changed(ConstFieldMaskArg whichField, 
                            UInt32            origin,
                            BitVector         details)
{
    Inherited::changed(whichField, origin, details);

    //Do not respond to changes that have a Sync origin
    if(origin & ChangedOrigin::Sync)
    {
        return;
    }

    if(whichField & BodyFieldMask)
    {
        if(getBody() != NULL)
        {
	        dGeomSetBody(_GeomID, getBody()->getBodyID());
        }
        else
        {
	        dGeomSetBody(_GeomID, 0);
        }
    }
    if(whichField & PositionFieldMask)
    {
	    dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
    }
    if(whichField & RotationFieldMask)
    {
	    dMatrix3 rotation;
	    Vec4f v1 =  getRotation()[0];
	    Vec4f v2 =  getRotation()[1];
	    Vec4f v3 =  getRotation()[2];
	    rotation[0]   = v1.x();
	    rotation[1]   = v1.y();
	    rotation[2]   = v1.z();
	    rotation[3]   = 0;
	    rotation[4]   = v2.x();
	    rotation[5]   = v2.y();
	    rotation[6]   = v2.z();
	    rotation[7]   = 0;
	    rotation[8]   = v3.x();
	    rotation[9]   = v3.y();
	    rotation[10]  = v3.z();
	    rotation[11]  = 0;
	    dGeomSetRotation(_GeomID, rotation);
    }
    if(whichField & QuaternionFieldMask)
    {
	    dQuaternion q;
	    q[0]=getQuaternion().w();
	    q[1]=getQuaternion().x();
	    q[2]=getQuaternion().y();
	    q[3]=getQuaternion().z();
	    dGeomSetQuaternion(_GeomID,q);
    }
    if(whichField & OffsetPositionFieldMask &&
        getBody() != NULL)
    {
	    dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
    }
    if(whichField & OffsetRotationFieldMask &&
        getBody() != NULL)
    {
	    dMatrix3 rotation;
	    Vec4f v1 =  getOffsetRotation()[0];
	    Vec4f v2 =  getOffsetRotation()[1];
	    Vec4f v3 =  getOffsetRotation()[2];
	    rotation[0]   = v1.x();
	    rotation[1]   = v1.y();
	    rotation[2]   = v1.z();
	    rotation[3]   = 0;
	    rotation[4]   = v2.x();
	    rotation[5]   = v2.y();
	    rotation[6]   = v2.z();
	    rotation[7]   = 0;
	    rotation[8]   = v3.x();
	    rotation[9]   = v3.y();
	    rotation[10]  = v3.z();
	    rotation[11]  = 0;
	    dGeomSetOffsetRotation(_GeomID, rotation);
    }
    if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
    {
	    dQuaternion q;
	    q[0]=getOffsetQuaternion().w();
	    q[1]=getOffsetQuaternion().x();
	    q[2]=getOffsetQuaternion().y();
	    q[3]=getOffsetQuaternion().z();
	    dGeomSetOffsetQuaternion(_GeomID,q);
    }
    if(whichField & CategoryBitsFieldMask)
    {
	    dGeomSetCategoryBits(_GeomID, getCategoryBits());
    }
    if(whichField & CollideBitsFieldMask)
    {
	    dGeomSetCollideBits(_GeomID, getCollideBits());
    }
    if(whichField & SpaceFieldMask)
    {
        dSpaceID CurSpace(dGeomGetSpace(_GeomID));
     
        if(CurSpace != 0 &&
           (getSpace() == NULL ||
            CurSpace != getSpace()->getSpaceID()))
        {
            dSpaceRemove(CurSpace,_GeomID);
        }

        if(getSpace() != NULL)
        {
	        dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
        }
    }
    if(whichField & EnableFieldMask)
    {
        if(getEnable())
        {
            dGeomEnable(_GeomID);
        }
        else
        {
            dGeomDisable(_GeomID);
        }
    }
}
Beispiel #15
0
void ODEGeom::setGeomCollideBits (unsigned long bits)
{
   dGeomSetCollideBits(geom,bits);
}
 void Solid::createApproximatedGeometry(const dSpaceID& space)
 {
   m_geometry2 = dCreateSphere(space,getObject()->getTrait<Model::Solid>()
                                                ->getRadius().Meter()) ;
   dGeomSetCollideBits(m_geometry2,(unsigned long)Collideable::ApproximatedSolid) ;
 }
int main (int argc, char **argv)
{
    dInitODE2(0);
    bool fixed  = true;

    // 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;

    dVector3 offset;
    dSetZero (offset, 4);

    // Default test case

    if (argc >= 2 )
    {
        for (int i=1; i < argc; ++i)
        {
            //static int tata = 0;

            if (1)
            {
                if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
                    Help (argv);

                if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
                    type = dJointTypeSlider;

                if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
                {
                    int j = i+1;
                    if ( j+1 > argc      ||  // Check if we have enough arguments
                            argv[j] == '\0' ||  // We should have a path here
                            argv[j][0] == '-' ) // We should have a path not a command line
                        Help (argv);
                    else
                        fn.path_to_textures = argv[++i]; // Increase i since we use this argument
                }
            }


            if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
                tc = 1;

            if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
                tc = 2;

            if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
                tc = 3;

            if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
                fixed = false;
        }
    }

    world = dWorldCreate();
    dWorldSetERP (world, 0.8);

    space = dSimpleSpaceCreate (0);
    contactgroup = dJointGroupCreate (0);
    geom[GROUND] = dCreatePlane (space, 0,0,1,0);
    dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
    dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

    dMass m;
    dMatrix3 R;


    // Create the Obstacle
    geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
    dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
    dGeomSetCollideBits (geom[OBS], catBits[ALL]);
    //Rotation of 45deg around y
    dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
    dGeomSetRotation (geom[OBS], R);
    dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);


    //Rotation of 90deg around y
    // Will orient the Z axis along X
    dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);


    // Create Body2 (Wiil be attached to the world)
    body[BODY2] = dBodyCreate (world);
    // Main axis of cylinder is along X=1
    dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dMassAdjust (&m, Mass1);
    geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
    dGeomSetBody (geom[BODY2], body[BODY2]);
    dGeomSetOffsetRotation (geom[BODY2], R);
    dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
    dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
    dBodySetMass (body[BODY2], &m);


    // Create Body 1 (Slider on the prismatic axis)
    body[BODY1] = dBodyCreate (world);
    // Main axis of capsule is along X=1
    dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
    dMassAdjust (&m, Mass1);
    geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
    dGeomSetBody (geom[BODY1], body[BODY1]);
    dGeomSetOffsetRotation (geom[BODY1], R);
    dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
    dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);

    dMass mRect;
    dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dMassAdd (&m, &mRect);
    // TODO: translate m?
    geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
    dGeomSetBody (geom[RECT], body[BODY1]);
    dGeomSetOffsetPosition (geom[RECT],
                            (BODY1_LENGTH-RECT_SIDES[0]) /2.0,
                            0.0,
                            -RADIUS -RECT_SIDES[2]/2.0);
    dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
    dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );

    dBodySetMass (body[BODY1], &m);



    setPositionBodies (tc);


    if ( fixed )
    {
        // Attache external cylinder to the world
        dJointID fixed = dJointCreateFixed (world,0);
        dJointAttach (fixed , NULL, body[BODY2]);
        dJointSetFixed (fixed );
        dWorldSetGravity (world,0,0,-0.8);
    }
    else
    {
        dWorldSetGravity (world,0,0,0);
    }




    // The static is here only to help debugging
    switch (type)
    {
    case dJointTypeSlider :
    {
        dSliderJoint *sj = new dSliderJoint (world, 0);
        sj->attach (body[BODY1], body[BODY2]);
        sj->setAxis (1, 0, 0);
        joint = sj;
    }
    break;

    case dJointTypePiston : // fall through default
    default:
    {
        dPistonJoint *pj = new dPistonJoint (world, 0);
        pj->attach (body[BODY1], body[BODY2]);
        pj->setAxis (1, 0, 0);

        dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);

        joint = pj;
    }
    break;
    };


    // run simulation
    dsSimulationLoop (argc,argv,400,300,&fn);

    delete joint;
    dJointGroupDestroy (contactgroup);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();
    return 0;
}
Beispiel #18
0
void ODE_Particle::setCollideBits (unsigned long bits)
{
   dGeomSetCollideBits(geom,bits);
}