示例#1
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.02);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }


  if (doFeedback)
  {
    if (fbnum>MAX_FEEDBACKNUM)
      printf("joint feedback buffer overflow!\n");
    else
    {
      dVector3 sum = {0, 0, 0};
      printf("\n");
      for (int i=0; i<fbnum; i++) {
        dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
        printf("%f %f %f\n", f[0], f[1], f[2]);
        sum[0] += f[0];
        sum[1] += f[1];
        sum[2] += f[2];
      }
      printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
      dMass m;
      dBodyGetMass(obj[selected].body, &m);
      printf("Object G=%f\n", GRAVITY*m.mass);
    }
    doFeedback = 0;
    fbnum = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
示例#2
0
void Object::UpdateDisableState()
{
	bool disabled = true;
	if (iBody == 0)
		return;

	if (dBodyIsEnabled(iBody) == 0)
		return;

	const dReal *lv = dBodyGetLinearVel(iBody);
	const dReal *av = dBodyGetAngularVel(iBody);

	
		if ((lv[0]*lv[0]+lv[1]*lv[1]+lv[2]*lv[2]) > DISABLE_THRESHOLD)
			disabled = false;

	
		if ((av[0]*av[0]+av[1]*av[1]+av[2]*av[2]) > DISABLE_THRESHOLD)
			disabled = false;

	if (disabled = false)
		disabledSteps++;

	if (disabledSteps > AUTO_DISABLE_STEPS)
	{
		dBodyDisable(iBody);
	}


};
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldQuickStep (world,0.05);

  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }
  
  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0.8,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
示例#4
0
void CPHShellSplitterHolder::PhDataUpdate(dReal step)
{
	SPLITTER_I i=m_splitters.begin(),e=m_splitters.end();
	for(;i!=e;++i)
	{
		switch(i->m_type) 
		{
		case CPHShellSplitter::splElement:
			{
				CPHElement* element=m_pShell->elements[i->m_element];
				dBodyID body=element->get_body();//!element->EnabledStateOnStep()
				if(!dBodyIsEnabled(body)) return;//
				i->m_breaked=(element->FracturesHolder()->PhDataUpdate(element))||i->m_breaked;
				break;
			}
		case CPHShellSplitter::splJoint:
			{
				CPHJoint *j=m_pShell->joints[i->m_joint];
				//if(j->bActive)
				i->m_breaked=j->JointDestroyInfo()->Update()||i->m_breaked;
				break;
			}
		default: NODEFAULT;
		}
		m_has_breaks=m_has_breaks||i->m_breaked;
	}
}
示例#5
0
void Scene::step( void )
{
	//Our internal units are pixels, and at 160dpi there are 6300 pixels per meter
	float gravity[3];
	gravity[0] = (m_motion_lopass[0]*1 + m_motion_hipass[0]*10) * 6300;
	gravity[1] = (m_motion_lopass[1]*1 + m_motion_hipass[1]*10) * 6300;
	gravity[2] = (m_motion_lopass[2]*1 + m_motion_hipass[2]*10) * 6300;
	
	float hisq = m_motion_hipass[0]*m_motion_hipass[0] + m_motion_hipass[1]*m_motion_hipass[1] + m_motion_hipass[2]*m_motion_hipass[2];
	if( hisq > 1.0f )
	{
		int num_woken = 0;
		for( SceneObj *obj_p=m_obj_p; obj_p; obj_p=obj_p->m_next_p )
		{
			if( !dBodyIsEnabled(obj_p->m_dbody) )
			{
				dBodyEnable( obj_p->m_dbody );
				num_woken++;
				if( num_woken == 3 )
					break;
			}
		}
	}

	dWorldSetGravity( m_dworld, gravity[0], gravity[1], gravity[2] );

	do_collision();
	dWorldQuickStep( m_dworld, 1.0f/60.0f );
	dJointGroupEmpty( m_colljoints );
}
示例#6
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }
}
示例#7
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldStepFast (world,0.05, 1);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (i==selected) {
	dsSetColor (0,0.7,1);
      }
      else if (! dBodyIsEnabled (obj[i].body)) {
	dsSetColor (1,0,0);
      }
      else {
	dsSetColor (1,1,0);
      }
      drawGeom (obj[i].geom[j],0,0,show_aabb);
    }
  }

  /*{
    for (int i = 1; i < IndexCount; i++) {
      dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
    }
  }*/

  {const dReal* Pos = dGeomGetPosition(TriMesh);
  const dReal* Rot = dGeomGetRotation(TriMesh);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
	const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
	const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
	dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
  }}}

  if (Ray){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Ray, Origin, Direction);
	  
	  dReal Length = dGeomRayGetLength(Ray);
	  
	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
	  
	  dsDrawLine(Origin, End);
  }
}
示例#8
0
void BaseWidget::setPosition(const dReal *position)
{
  if(! dBodyIsEnabled(body)) return;

  //Update position
  int xPos = ABSOLUTE(position[0]);
  int yPos = ABSOLUTE(position[1]);

  // Update Qt's position
  qDebug("Position: %i, %i", xPos, yPos);
  move(xPos, yPos);
}
示例#9
0
bool Construction::IsActive()
{
	
	for(int i=0; i<nObjects; i++)
	{
		if(dBodyIsEnabled( ObjectList[i].Body ) == false )
			return false;
	}


	return true;
}
示例#10
0
static void simLoop (int pause)
{
    const dReal stepsize = 0.02;

    dsSetColor (0,0,2);
    dSpaceCollide (space,0,&nearCallback);
    if (!pause) {
        
        if (mov_type == 1)
            moveplat_1(stepsize);
        else
            moveplat_2(stepsize);

        dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
        updatecam();
        dWorldQuickStep (world,stepsize);
        //dWorldStep (world,stepsize);
    }

    if (write_world) {
        FILE *f = fopen ("state.dif","wt");
        if (f) {
            dWorldExportDIF (world,f,"X");
            fclose (f);
        }
        write_world = 0;
    }
  
    // remove all contact joints
    dJointGroupEmpty (contactgroup);

    dsSetColor (1,1,0);
    dsSetTexture (DS_WOOD);
    for (int i=0; i<num; i++) {
        for (int j=0; j < GPB; j++) {
            if (! dBodyIsEnabled (obj[i].body)) {
                dsSetColor (1,0.8,0);
            }
            else {
                dsSetColor (1,1,0);
            }
            drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
    }
    dsSetColor (1,0,0);
    drawGeom (platform,0,0,show_aabb);
    //usleep(5000);
}
示例#11
0
void Sphere::Update()
{
   const dReal *p,*r;

   UpdateDisableState();

   if(dBodyIsEnabled(iBody) == 0)
     return;

   p = dBodyGetPosition(iBody);
   iPosition.x = p[0];
   iPosition.y = p[1];
   iPosition.z = p[2];
   r = dGeomGetRotation(iGeom);
   iRotate.x = p[0];
   iRotate.y = p[1];
   iRotate.z = p[2];
//   v = dBodyGetLinearVel(iBody);
//   dBodyAddForce(iBody,v[0]*v[0]+v[1]*v[1]+v[2]*v[2],0,0);
//   printf("x: %f, y: %f z: %f\n",p[0],p[1],p[2]);
};
示例#12
0
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);
  if (!pause) dWorldStep (world,0.05);

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    int color_changed = 0;
    if (i==selected) {
      dsSetColor (0,0.7,1);
      color_changed = 1;
    }
    else if (! dBodyIsEnabled (obj[i].body)) {
      dsSetColor (1,0,0);
      color_changed = 1;
    }
    for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
    if (color_changed) dsSetColor (1,1,0);
  }

  {for (int i = 0; i < RayCount; i++){
	  dVector3 Origin, Direction;
	  dGeomRayGet(Rays[i], Origin, Direction);
  
	  dReal Length = dGeomRayGetLength(Rays[i]);

	  dVector3 End;
	  End[0] = Origin[0] + (Direction[0] * Length);
	  End[1] = Origin[1] + (Direction[1] * Length);
	  End[2] = Origin[2] + (Direction[2] * Length);
	  End[3] = Origin[3] + (Direction[3] * Length);
  
	  dsDrawLine(Origin, End);
  }}
}
示例#13
0
		void internal_collisionCallback(void *data, dGeomID o0, dGeomID o1)
		{
			if (dGeomIsSpace(o0) || dGeomIsSpace(o1))
			{
				// Colliding a space with either a geom or another space.
				dSpaceCollide2(o0, o1, data, &internal_collisionCallback);

				if (dGeomIsSpace(o0))
				{
					// Colliding all geoms internal to the space.
					dSpaceCollide((dSpaceID) o0, data,
					               &internal_collisionCallback);
				}

				if (dGeomIsSpace(o1))
				{
					// Colliding all geoms internal to the space.
					dSpaceCollide((dSpaceID) o1, data,
					               &internal_collisionCallback);
				}
			}
			else
			{
				// Colliding two geoms.

				// The following is a set of special cases where we might
				// want to skip collision detection (but not all are
				// enforced here for various reasons):
				// 1. Two static Solids (neither geom has a body) AND
				//    neither Solid has a CollisionEventHandler AND there are
				//    not global handlers: this is enforced.
				// 2. Two Shapes that are part of the same Solid (they
				//    share a body): this is not enforced because ODE
				//    already takes care of it.
				// 3. Two sleeping Solids (note: both must have bodies to
				//    check this): this is enforced.  (ODE should handle
				//    this, but it doesn't.)
				// 4. Two Solids connected by a fixed Joint: this is
				//    enforced.
				// 5. Two Solids connected by a Joint (besides ODE
				//    contact joints, which should never generate more
				//    contacts) with contacts disabled (note: both must have
				//    bodies to check this): this is enforced.
				// 6. Solid0 is static, Solid1 is dynamic and is sleeping,
				//    static-to-sleeping contacts are ignored by the
				//    Simulator, and neither Solid has a
				//    CollisionEventHandler AND there are no global handlers:
				//    this is enforced.
				// 7. Solid1 is static, Solid0 is dynamic and is sleeping,
				//    static-to-sleeping contacts are ignored by the
				//    Simulator, and neither Solid has a
				//    CollisionEventHandler AND there are no global handlers:
				//    this is enforced.
				// 8. The two Solids' contact groups do not generate
				//    contacts when they collide AND neither Solid has a
				//    CollisionEventHandler AND there are no global handlers.

				// Get the geoms' ODE body IDs.
				dBodyID o0BodyID = dGeomGetBody(o0);
				dBodyID o1BodyID = dGeomGetBody(o1);
				bool solid0Static = (0 == o0BodyID);
				bool solid1Static = (0 == o1BodyID);

				// Check if both Solids are dynamic (i.e. have ODE bodies).
				bool bothHaveBodies = true;
				if (0 == o0BodyID || 0 == o1BodyID)
				{
					bothHaveBodies = false;
				}

				// If the two Solids are connected by a common Joint, get
				// a pointer to that Joint.
				Joint* commonJoint = NULL;
				if (bothHaveBodies && dAreConnectedExcluding(o0BodyID,
				        o1BodyID, dJointTypeContact))
				{
					// This will become non-NULL only if there exists an ODE
					// joint connecting the two bodies.
					commonJoint = internal_getCommonJoint(o0BodyID, o1BodyID);
				}

				// Get pointers to the geoms' GeomData structures.
				GeomData* geomData0 = (GeomData*) dGeomGetData(o0);
				GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));

				// Get pointers to the geoms' ShapeData structures.
				const ShapeData* shape0 = geomData0->shape;
				const ShapeData* shape1 = geomData1->shape;

				// Get a pointer to the ODESimulator.
				ODESimulator* sim = (ODESimulator*)data;

				// Check if the two Solids' contact groups generate contacts
				// when they collide.
				bool makeContacts = sim->groupsMakeContacts(
					shape0->contactGroup, shape1->contactGroup);

				// Find out whether the Simulator has static-to-sleeping
				// contacts disabled.
				bool ignoreStaticSleepingContacts =
				    !sim->areStaticSleepingContactsEnabled();

				// Get pointers to the geoms' Solids.
				Solid* solid0 = geomData0->solid;
				Solid* solid1 = geomData1->solid;

				// Get pointers to the two Solids' CollisionEventHandlers.
				// These will be NULL if the Solids don't use
				// CollisionEventHandlers.
				CollisionEventHandler* handler0 =
				    solid0->getCollisionEventHandler();
				CollisionEventHandler* handler1 =
				    solid1->getCollisionEventHandler();

				bool neitherHasCollisionHandler = !(handler0 || handler1);

				bool noGlobalCollisionHandlers = 
					sim->getNumGlobalCollisionEventHandlers() == 0;

				// Now do the actual tests to see if we should return early.
				// It is important here that we don't call dBodyIsEnabled on
				// a static body because that crashes ODE.

				bool case1 = neitherHasCollisionHandler && 
					noGlobalCollisionHandlers && solid0Static && solid1Static;
				//bool case2= o0BodyID == o1BodyID;
				bool case3 = bothHaveBodies && !dBodyIsEnabled(o0BodyID)
					&& !dBodyIsEnabled(o1BodyID);
				bool case4 = commonJoint &&
					commonJoint->getType() == FIXED_JOINT;
				bool case5 = commonJoint
					&& !commonJoint->areContactsEnabled();
				bool case6 = solid0Static && 0 != o1BodyID
					&& !dBodyIsEnabled(o1BodyID)
					&& ignoreStaticSleepingContacts
					&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
				bool case7 = solid1Static && 0 != o0BodyID
					&& !dBodyIsEnabled(o0BodyID)
					&& ignoreStaticSleepingContacts
					&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
				bool case8 = !makeContacts && neitherHasCollisionHandler
					&& noGlobalCollisionHandlers;

				if (case1 || case3 || case4 || case5 || case6 || case7 || case8)
				{
					return;
				}

				// Now actually test for collision between the two geoms.
				// This is one of the more expensive operations.
				dWorldID theWorldID = sim->internal_getWorldID();
				dJointGroupID theJointGroupID =
				    sim->internal_getJointGroupID();
				dContactGeom contactArray[globals::maxMaxContacts];
				int numContacts = dCollide(o0, o1, sim->getMaxContacts(),
					contactArray, sizeof(dContactGeom));

				// If the two objects didn't make any contacts, they weren't
				// touching, so just return.
				if (0 == numContacts)
				{
					return ;
				}

				// If at least one of the Solids has a CollisionEventHandler,
				// send it a CollisionEvent.
				if (handler0 || handler1 || !noGlobalCollisionHandlers)
				{
					// Call the CollisionEventHandlers.  Note that we only
					// use one contact point per collision: just the first one
					// in the contact array.  The order of the Solids
					// passed to the event handlers is important: the first
					// one should be the one whose event handler is
					// getting called.

					CollisionEvent e;
					e.thisSolid = solid0;
					e.otherSolid = solid1;
					e.pos[0] = (real) contactArray[0].pos[0];
					e.pos[1] = (real) contactArray[0].pos[1];
					e.pos[2] = (real) contactArray[0].pos[2];
					e.normal[0] = (real) contactArray[0].normal[0];
					e.normal[1] = (real) contactArray[0].normal[1];
					e.normal[2] = (real) contactArray[0].normal[2];
					e.depth = (real) contactArray[0].depth;

					if (handler0)
					{
						handler0->internal_pushCollisionEvent(e);
					}

					if (handler1)
					{
						// For the other Solid's CollisionEventHandler, we need
						// to invert the normal and swap the Solid pointers.
						e.normal *= -1;
						e.thisSolid = solid1;
						e.otherSolid = solid0;
						handler1->internal_pushCollisionEvent(e);
					}

					sim->internal_recordCollision(e);
				}

				// Old version...
				//// Early check to save some time.
				//if (solid0->getCollisionEventHandler()
				//  || solid1->getCollisionEventHandler())
				//{
				//  // Call the event handlers.  Note: we only use one
				//  // contact point per collision; just use the first one
				//  // in the contact array.  The order of the Solids
				//  // passed to the event handlers is important: the first
				//  // one should be the one whose event handler is
				//  // getting called.

				//  CollisionEvent e;
				//  e.solid0 = solid0;
				//  e.solid1 = solid1;
				//  e.pos[0] = contactArray[0].pos[0];
				//  e.pos[1] = contactArray[0].pos[1];
				//  e.pos[2] = contactArray[0].pos[2];
				//  e.normal[0] = contactArray[0].normal[0];
				//  e.normal[1] = contactArray[0].normal[1];
				//  e.normal[2] = contactArray[0].normal[2];
				//  e.depth = contactArray[0].depth;

				//  EventHandler* eventHandler =
				//      solid0->getCollisionEventHandler();
				//  if (eventHandler)
				//  {
				//      generateContacts0 =
				//          eventHandler->handleCollisionEvent(e);
				//  }

				//  e.normal *= -1; // Invert normal.
				//  e.solid0 = solid1; // Swap solid pointers.
				//  e.solid1 = solid0;

				//  eventHandler = solid1->getCollisionEventHandler();
				//  if (eventHandler)
				//  {
				//      generateContacts1 =
				//          eventHandler->handleCollisionEvent(e);
				//  }
				//}

				if (makeContacts)
				{
					// Invalidate the "freely-spinning" parameters.
					((ODESolid*)solid0)->internal_setFreelySpinning(false);
					((ODESolid*)solid1)->internal_setFreelySpinning(false);

					for (int i = 0; i < numContacts; ++i)
					{
						const Material* m0 = &(shape0->material);
						const Material* m1 = &(shape1->material);

						dContact tempContact;
						tempContact.surface.mode = dContactBounce |
							dContactSoftERP; // | dContactSoftCFM;

						// Average the hardness of the two materials.
						assert(m0->hardness >= 0 && m0->hardness <= 1
						        && m1->hardness >= 0 && m1->hardness <= 1);
						real hardness = (m0->hardness + m1->hardness) *
						                (real) 0.5;

						// Convert hardness to ERP.  As hardness goes from
						// 0.0 to 1.0, ERP goes from min to max.
						tempContact.surface.soft_erp = hardness *
							(defaults::ode::maxERP - defaults::ode::minERP) +
							defaults::ode::minERP;

						// Don't use contact CFM anymore.  Just let it use
						// the global value set in the ODE world.
						//tempContact.surface.soft_cfm =
						//  defaults::ode::minCFM;

						// As friction goes from 0.0 to 1.0, mu goes from 0.0
						// to max, though it is set to dInfinity when
						// friction == 1.0.
						assert(m0->friction >= 0 && m0->friction <= 1
						        && m1->friction >= 0 && m1->friction <= 1);
						if (1.0 == m0->friction && 1.0 == m1->friction)
						{
							tempContact.surface.mu = dInfinity;
						}
						else
						{
							tempContact.surface.mu =
							    sqrt(m0->friction * m1->friction) *
							    defaults::ode::maxFriction;
						}

						// Average the bounciness of the two materials.
						assert(m0->bounciness >= 0 && m0->bounciness <= 1
							&& m1->bounciness >= 0 && m1->bounciness <= 1);
						real bounciness = (m0->bounciness + m1->bounciness) *
							(real) 0.5;

						// ODE's bounce parameter, a.k.a. restitution.
						tempContact.surface.bounce = bounciness;

						// ODE's bounce_vel parameter is a threshold:
						// the relative velocity of the two objects must be
						// greater than this for bouncing to occur at all.
						tempContact.surface.bounce_vel =
						    defaults::bounceThreshold;

						// Old way to calculate bounce threshold; threshold
						// was scaled by the collision bounciness, but this
						// makes all objects with non-zero bounciness
						// always bounce.  This is undesirable because it
						// takes a while for bouncing to totally diminish.
						//tempContact.surface.bounce_vel =
						//  defaults::ode::maxBounceVel - bounciness *
						//  defaults::ode::maxBounceVel;

						tempContact.geom = contactArray[i];
						dJointID contactJoint = dJointCreateContact(
							theWorldID, theJointGroupID, &tempContact);

						// Note: the following line of code replaces the
						// rest of this function which is commented out.
						// TODO: test this and make sure the mass ratio
						// issues are unimportant and that we never need
						// "one-sided" contacts between two Solids.
						dJointAttach(contactJoint, o0BodyID, o1BodyID);

						//if (!bothHaveBodies)
						//{
						//  // at least one object is static, so just handle
						//  // things like normal
						//  dJointAttach(contactJoint, o0BodyID, o1BodyID);
						//}
						//else
						//{
						//  // TODO: We probably need to remove the following chunk of
						//  // code.  The first case is obsolete since now both sides
						//  // always get contacts, if at all.  (There isn't really a
						//  // good reason to have one side use contacts but not the
						//  // other; the side not wanting contacts would appear to be
						//  // static, so just make it static in the first place.  On
						//  // the other hand, this may end up being desirable if
						//  // an object should be moved by some objects but not
						//  // others, and the "others" *do* collid with the first
						//  // object... but this situation may never come up.  The
						//  // second case, using mass ratios to determine whether two
						//  // objects should collide, might not be an issue.  Mass
						//  // ratios might only be a problem when the two objects are
						//  // constantly connected with a Joint.

						//  // Handle one-sided contacts for two cases: 1) only one of
						//  // the above event handlers actually wants contacts generated,
						//  // 2) if the mass ratio is above some threshold, treat it as
						//  // a one-sided collision solution: treat the more massive
						//  // object as static, calculate the collision like normal
						//  // (with the massive one being static), then also add the
						//  // massive object's velocity to the smaller one (velocity
						//  // calculated at the point of collision).

						//  // calculate mass ratio (use mass1 / mass2); if
						//  // the ratio is too high, mass1 is too large; if
						//  // the ratio is too low, mass2 is too large
						//  real massRatio = 0;
						//  dMass mass0, mass1;
						//  dBodyGetMass(o0BodyID, &mass0);
						//  dBodyGetMass(o1BodyID, &mass1);
						//  massRatio = mass0.mass / mass1.mass;

						//  // here we handle all the different collision
						//  // cases: one solid or the other or both may want
						//  // contacts generated; also, the mass ratio may
						//  // be outside the acceptable range

						//  if (true == generateContacts0 && true ==
						//      generateContacts1)
						//  {
						//      // both want contacts, neither wants to be
						//      // static
						//      if (massRatio > defaults::ode::maxMassRatio)
						//      {
						//          // ratio is too high - mass0 is too large,
						//          // treat solid0 as static
						//          dBodyEnable(o1BodyID);
						//          dJointAttach(contactJoint, 0, o1BodyID);
						//      }
						//      else if (massRatio <
						//          defaults::ode::minMassRatio)
						//      {
						//          // ratio is too low - mass1 is too large,
						//          // treat solid1 as static
						//          dBodyEnable(o0BodyID);
						//          dJointAttach(contactJoint, o0BodyID, 0);
						//      }
						//      else
						//      {
						//          //ratio is good - no static objects
						//          dJointAttach(contactJoint, o0BodyID,
						//              o1BodyID);
						//      }
						//  }
						//  else if (true == generateContacts0)
						//  {
						//      // solid0 wants contacts, solid1 wants to be
						//      // static
						//      if (massRatio > defaults::ode::maxMassRatio)
						//      {
						//          // ratio is too high - mass0 is too large,
						//          // treat solid0 and solid1 as static
						//          // i.e. don't generate a contact joint
						//      }
						//      else
						//      {
						//          // this block handles two cases which have
						//          // the same result:
						//          // 1. ratio is too low - mass1 is too
						//          //  large, treat solid1 as static
						//          // 2. ratio is good - treat solid1 as
						//          //  static
						//          dBodyEnable(o0BodyID);
						//          dJointAttach(contactJoint, o0BodyID, 0);
						//      }
						//  }
						//  else //generateContacts1 must be true
						//  {
						//      // solid1 wants contacts, solid0 wants to be
						//      // static
						//      if (massRatio < defaults::ode::minMassRatio)
						//      {
						//          // ratio is too low - mass1 is too large,
						//          // treat solid0 and solid1 as static
						//          // i.e. don't generate a contact joint
						//      }
						//      else
						//      {
						//          // this block handles two cases which have
						//          // the same result:
						//          // 1. ratio is too high - mass0 is too
						//          //  large, treat solid0 as static
						//          // 2. ratio is good - treat solid0 as
						//          //  static
						//          dBodyEnable(o1BodyID);
						//          dJointAttach(contactJoint, 0, o1BodyID);
						//      }
						//  }
						//}
					}
				}
			}
		}
static void simLoop (int pause)
{
  dsSetColor (0,0,2);
  dSpaceCollide (space,0,&nearCallback);


#if 1
  // What is this for??? - Bram
  if (!pause) 
  {
    for (int i=0; i<num; i++)
      for (int j=0; j < GPB; j++)
        if (obj[i].geom[j])
          if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
            setCurrentTransform(obj[i].geom[j]);
 
    setCurrentTransform(TriMesh1);
    setCurrentTransform(TriMesh2);
  }
#endif

  //if (!pause) dWorldStep (world,0.05);
  if (!pause) dWorldQuickStep (world,0.05);

  for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
	  dSpaceGetGeom(space, j);
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,0);
  dsSetTexture (DS_WOOD);
  for (int i=0; i<num; i++) {
    for (int j=0; j < GPB; j++) {
      if (obj[i].geom[j]) {
        if (i==selected) {
          dsSetColor (0,0.7,1);
        }
        else if (! dBodyIsEnabled (obj[i].body)) {
          dsSetColor (1,0,0);
        }
        else {
          dsSetColor (1,1,0);
        }
      
        if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
          dTriIndex* Indices = (dTriIndex*)::Indices;

          // assume all trimeshes are drawn as bunnies
          const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
          const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
        
          for (int ii = 0; ii < IndexCount / 3; ii++) {
            const dReal v[9] = { // explicit conversion from float to dReal
              Vertices[Indices[ii * 3 + 0] * 3 + 0],
              Vertices[Indices[ii * 3 + 0] * 3 + 1],
              Vertices[Indices[ii * 3 + 0] * 3 + 2],
              Vertices[Indices[ii * 3 + 1] * 3 + 0],
              Vertices[Indices[ii * 3 + 1] * 3 + 1],
              Vertices[Indices[ii * 3 + 1] * 3 + 2],
              Vertices[Indices[ii * 3 + 2] * 3 + 0],
              Vertices[Indices[ii * 3 + 2] * 3 + 1],
              Vertices[Indices[ii * 3 + 2] * 3 + 2]
            };
            dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
          }

          // tell the tri-tri collider the current transform of the trimesh --
          // this is fairly important for good results.
          
		  // Fill in the (4x4) matrix.
		  dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

		  p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
		  p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
		  p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
		  p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

		  // Flip to other matrix.
		  obj[i].last_matrix_index = !obj[i].last_matrix_index;

		  dGeomTriMeshSetLastTransform( obj[i].geom[j], 
			  *(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
  
        } else {
          drawGeom (obj[i].geom[j],0,0,show_aabb);
        }
      }
    }
  }

  dTriIndex* Indices = (dTriIndex*)::Indices;

  {const dReal* Pos = dGeomGetPosition(TriMesh1);
  const dReal* Rot = dGeomGetRotation(TriMesh1);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
  }}}

  {const dReal* Pos = dGeomGetPosition(TriMesh2);
  const dReal* Rot = dGeomGetRotation(TriMesh2);

  {for (int i = 0; i < IndexCount / 3; i++){
    const dReal v[9] = { // explicit conversion from float to dReal
      Vertices[Indices[i * 3 + 0] * 3 + 0],
      Vertices[Indices[i * 3 + 0] * 3 + 1],
      Vertices[Indices[i * 3 + 0] * 3 + 2],
      Vertices[Indices[i * 3 + 1] * 3 + 0],
      Vertices[Indices[i * 3 + 1] * 3 + 1],
      Vertices[Indices[i * 3 + 1] * 3 + 2],
      Vertices[Indices[i * 3 + 2] * 3 + 0],
      Vertices[Indices[i * 3 + 2] * 3 + 1],
      Vertices[Indices[i * 3 + 2] * 3 + 2]
    };
    dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
  }}}
}
示例#15
0
static void command (int cmd)
{
  size_t i;
  int j,k;
  dReal sides[3];
  dMass m;
  int setBody;
  
  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
  {
    setBody = 0;
    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);
	dRSetIdentity (R);
	//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/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;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
    //<---- Convex Object    
    else if (cmd == 'v') 
      {
	dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
	obj[i].geom[0] = dCreateConvex (space,
					planes,
					planecount,
					points,
					pointcount,
					polygons);
#else
	obj[i].geom[0] = dCreateConvex (space,
					Sphere_planes,
					Sphere_planecount,
					Sphere_points,
					Sphere_pointcount,
					Sphere_polygons);
#endif
      }
    //----> Convex Object
    else if (cmd == 'y') {
      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 == 'x' && USE_GEOM_OFFSET) {
      setBody = 1;
      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      dReal dpos[GPB][3];	// delta-positions for encapsulated 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 == '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]);
	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);

	// Translation *after* rotation
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

	// 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<GPB; 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]);
    }

    if (!setBody)
     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;
  }
  else if (cmd == 'p'&& selected >= 0)
  {
    const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
    const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
    printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
    printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
           rot[0],rot[1],rot[2],rot[3],
           rot[4],rot[5],rot[6],rot[7],
           rot[8],rot[9],rot[10],rot[11]);
  }
  else if (cmd == 'f' && selected >= 0 && selected < num) {
          if (dBodyIsEnabled(obj[selected].body))
            doFeedback = 1;
  }
}
static void simLoop (int pause)


{


	dsSetColor (0,0,2);


	dSpaceCollide (space,0,&nearCallback);


	if (!pause) dWorldStep (world,0.05);





	dAASSERT(terrainY);


	dAASSERT(terrainZ);


	dsSetColor (0,1,0);


	dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));


	dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));





	if (show_aabb) 


	{


		dReal aabb[6];


		dGeomGetAABB (terrainY,aabb);


		dVector3 bbpos;


		int i;


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		dVector3 bbsides;


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dMatrix3 RI;


		dRSetIdentity (RI);


		dsSetColorAlpha (1,0,0,0.5);


		dsDrawBox (bbpos,RI,bbsides);





		dGeomGetAABB (terrainZ,aabb);


		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);


		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];


		dsDrawBox (bbpos,RI,bbsides);


	}





	dsSetColor (1,1,0);


	


	// remove all contact joints


	dJointGroupEmpty (contactgroup);


	


	dsSetColor (1,1,0);


	dsSetTexture (DS_WOOD);


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


		for (int j=0; j < GPB; j++) {


			if (i==selected) {


				dsSetColor (0,0.7,1);


			}


			else if (! dBodyIsEnabled (obj[i].body)) {


				dsSetColor (1,0,0);


			}


			else {


				dsSetColor (1,1,0);


			}


			drawGeom (obj[i].geom[j],0,0,show_aabb);


		}


	}


}
示例#17
0
bool ODE_Particle::Is_enabled()
{
    return dBodyIsEnabled (body);
}
static void simLoop (int pause)
{
	int i, j;
		
	dsSetTexture (DS_WOOD);

	if (!pause) {
#ifdef BOX
		dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
		for (j = 0; j < joints; j++)
		{
			dReal curturn = dJointGetHinge2Angle1 (joint[j]);
			//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
			dJointSetHinge2Param(joint[j],dParamVel2,speed);
			dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
			dBodyEnable(dJointGetBody(joint[j],0));
			dBodyEnable(dJointGetBody(joint[j],1));
		}		
		if (doFast)
		{
			dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
			dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
			dWorldStepFast1 (world,0.05,ITERS);
#endif
			dJointGroupEmpty (contactgroup);
		}
		else
		{
			dSpaceCollide (space,0,&nearCallback);
			dWorldStep (world,0.05);
			dJointGroupEmpty (contactgroup);
		}
		
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b)) 
			{
				bool disable = true;
				const dReal *lvel = dBodyGetLinearVel(b);
				dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
				if (lspeed > DISABLE_THRESHOLD)
					disable = false;
				const dReal *avel = dBodyGetAngularVel(b);
				dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
				if (aspeed > DISABLE_THRESHOLD)
					disable = false;
				
				if (disable)
					wb_stepsdis[i]++;
				else
					wb_stepsdis[i] = 0;
				
				if (wb_stepsdis[i] > DISABLE_STEPS)
				{
					dBodyDisable(b);
					dsSetColor(0.5,0.5,1);
				}
				else
					dsSetColor(1,1,1);

			}
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	else
	{
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b))
				dsSetColor(1,1,1);
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	
	dsSetColor (0,1,1);
	dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
	for (i = 0; i < boxes; i++)
		dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
	dsSetColor (1,1,1);
	for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
				   dGeomGetRotation(sphere[i]),RADIUS);
	
	// draw the cannon
	dsSetColor (1,1,0);
	dMatrix3 R2,R3,R4;
	dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
	dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
	dMultiply0 (R4,R2,R3,3,3,3);
	dReal cpos[3] = {CANNON_X,CANNON_Y,1};
	dReal csides[3] = {2,2,2};
	dsDrawBox (cpos,R2,csides);
	for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
	dsDrawCylinder (cpos,R4,3,0.5);
	
	// draw the cannon ball
	dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
		      CANNON_BALL_RADIUS);
}
bool PhysicsBody::getEnable(void) const
{
    return dBodyIsEnabled(_BodyID);
}
示例#20
0
static void simLoop (int pause)
{
  int i,j;
  
  dsSetColor (0,0,2);
  
  dSpaceCollide (space,0,&nearCallback);
  
  //if (!pause) dWorldStep (world,0.05);
  //if (!pause) dWorldQuickStep (world,0.05);
  if (!pause) dWorldStepFast1 (world,0.05, 5);


  if (write_world) {
    FILE *f = fopen ("state.dif","wt");
    if (f) {
      dWorldExportDIF (world,f,"X");
      fclose (f);
    }
    write_world = 0;
  }

  // remove all contact joints
  dJointGroupEmpty (contactgroup);



	const dReal* pReal = dGeomGetPosition( gheight );

	const dReal* RReal = dGeomGetRotation( gheight );

	//
	// Draw Heightfield
	//

	// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
	int ox = (int) ( -HFIELD_WIDTH/2 );
	int oz = (int) ( -HFIELD_DEPTH/2 );

//	for ( int tx = -1; tx < 2; ++tx )
//	for ( int tz = -1; tz < 2; ++tz )
	{
		dsSetColorAlpha (0.5,1,0.5,0.5);
		dsSetTexture( DS_WOOD );

		for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
		for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
		{
			dReal a[3], b[3], c[3], d[3];

			a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			a[ 1 ] = heightfield_callback( NULL, i, j );
			a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			b[ 1 ] = heightfield_callback( NULL, i + 1, j );
			b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;

			c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
			c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
			c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
			d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
			d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;

			dsDrawTriangle( pReal, RReal, a, c, b, 1 );
			dsDrawTriangle( pReal, RReal, b, c, d, 1 );
		}
	}





	dsSetColor (1,1,0);
	dsSetTexture (DS_WOOD);
	for (i=0; i<num; i++)
	{
		for (j=0; j < GPB; j++)
		{
			if (i==selected)
			{
				dsSetColor (0,0.7,1);
			}
			else if (! dBodyIsEnabled (obj[i].body))
			{
				dsSetColor (1,0.8,0);
			}
			else 
			{
				dsSetColor (1,1,0);
			}

			if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
			{
				dTriIndex* Indices = (dTriIndex*)::Indices;

				// assume all trimeshes are drawn as bunnies
				const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
				const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);

				for (int ii = 0; ii < IndexCount / 3; ii++)
				{
					const dReal v[9] = { // explicit conversion from float to dReal
						Vertices[Indices[ii * 3 + 0] * 3 + 0],
							Vertices[Indices[ii * 3 + 0] * 3 + 1],
							Vertices[Indices[ii * 3 + 0] * 3 + 2],
							Vertices[Indices[ii * 3 + 1] * 3 + 0],
							Vertices[Indices[ii * 3 + 1] * 3 + 1],
							Vertices[Indices[ii * 3 + 1] * 3 + 2],
							Vertices[Indices[ii * 3 + 2] * 3 + 0],
							Vertices[Indices[ii * 3 + 2] * 3 + 1],
							Vertices[Indices[ii * 3 + 2] * 3 + 2]
					};
					dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
				}

				// tell the tri-tri collider the current transform of the trimesh --
				// this is fairly important for good results.

				// Fill in the (4x4) matrix.
				dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );

				p_matrix[ 0 ] = Rot[ 0 ];	p_matrix[ 1 ] = Rot[ 1 ];	p_matrix[ 2 ] = Rot[ 2 ];	p_matrix[ 3 ] = 0;
				p_matrix[ 4 ] = Rot[ 4 ];	p_matrix[ 5 ] = Rot[ 5 ];	p_matrix[ 6 ] = Rot[ 6 ];	p_matrix[ 7 ] = 0;
				p_matrix[ 8 ] = Rot[ 8 ];	p_matrix[ 9 ] = Rot[ 9 ];	p_matrix[10 ] = Rot[10 ];	p_matrix[11 ] = 0;
				p_matrix[12 ] = Pos[ 0 ];	p_matrix[13 ] = Pos[ 1 ];	p_matrix[14 ] = Pos[ 2 ];	p_matrix[15 ] = 1;

				// Flip to other matrix.
				obj[i].last_matrix_index = !obj[i].last_matrix_index;

				// Apply the 'other' matrix which is the oldest.
				dGeomTriMeshSetLastTransform( obj[i].geom[j], 
					*(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
			}
			else
			{
				drawGeom (obj[i].geom[j],0,0,show_aabb);
			}
		}
	}

	if ( show_aabb )
	{
		// draw the bounding box for this geom
		dReal aabb[6];
		dGeomGetAABB (gheight,aabb);
		dVector3 bbpos;
		for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
		dVector3 bbsides;
		for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
		dMatrix3 RI;
		dRSetIdentity (RI);
		dsSetColorAlpha (1,0,0,0.5);
		dsDrawBox (bbpos,RI,bbsides);
	}
}
void CPhysicObject::addForce(const CVector& force){
    if ( !dBodyIsEnabled (myBody) )
        dBodyEnable(myBody);
     dBodyAddForce(myBody, force.v[0], force.v[1], force.v[2]);
};