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); } } }
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); } } }
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; } }
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 ); }
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); } } }
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); } }
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); }
bool Construction::IsActive() { for(int i=0; i<nObjects; i++) { if(dBodyIsEnabled( ObjectList[i].Body ) == false ) return false; } return true; }
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); }
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]); };
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); }} }
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); }}} }
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); } } }
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); }
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]); };