void BodyVerifier::collisionCallback(void *data, dGeomID o1, dGeomID o2) { CollisionData *collisionData = (CollisionData *) data; const int MAX_CONTACTS = 32; // maximum number of contact points per body // TODO will it work with just 1 point? Probably yes. // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) { return; } dContact contact[MAX_CONTACTS]; for (int i = 0; i < MAX_CONTACTS; i++) { contact[i].surface.mode = 0; } if (dGeomGetClass(o1) == dCylinderClass && dGeomGetClass(o2) == dCylinderClass) { collisionData->cylinders.push_back(o1); collisionData->cylinders.push_back(o2); } int collisionCounts = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); if (collisionCounts != 0) { collisionData->offendingBodies.push_back(std::pair<dBodyID, dBodyID>(b1, b2)); } }
void CProtoHapticDoc::nearCallback(dGeomID o1, dGeomID o2) { int index1= (int)dGeomGetData(o1); int index2= (int)dGeomGetData(o2); if(m_shapes[index1]->isCollisionDynamic() || m_shapes[index2]->isCollisionDynamic()) { int n, i; const int N = 50; dContact contact[N]; n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { OutputDebugString("Collision\n"); for (i=0; i<n; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass) contact[i].surface.mu = 20; else contact[i].surface.mu = 0.5; contact[i].surface.slip1 = 1.0 - (m_shapes[index1]->getSurfaceFriction()); contact[i].surface.slip2 = 1.0 - (m_shapes[index2]->getSurfaceFriction()); contact[i].surface.soft_erp = 0.8; contact[i].surface.soft_cfm = 0.01; dJointID c = dJointCreateContact (m_worldID,m_jointGroup,contact+i); dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2)); } } } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,n; dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnected(b1, b2)) return; const int N = 4; dContact contact[N]; n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { for (i=0; i<n; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass) contact[i].surface.mu = 20; else contact[i].surface.mu = 0.5; contact[i].surface.slip1 = 0.0; contact[i].surface.slip2 = 0.0; contact[i].surface.soft_erp = 0.8; contact[i].surface.soft_cfm = 0.01; dJointID c = dJointCreateContact (world,contactgroup,contact+i); dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2)); } } }
void draw_phys_geom(dGeomID geom) { glPushAttrib(GL_ENABLE_BIT); { glDisable(GL_LIGHTING); glDisable(GL_TEXTURE_2D); glEnable(GL_COLOR_MATERIAL); glColor4f(1.0f, 1.0f, 0.0f, 0.5f); if (dGeomGetClass(geom) == dPlaneClass) draw_phys_plane(geom); else { switch (dGeomGetClass(geom)) { case dBoxClass: draw_phys_box (geom); break; case dSphereClass: draw_phys_sphere (geom); break; case dCapsuleClass: draw_phys_ccylinder(geom); break; case dRayClass: draw_phys_ray (geom); break; } } } glPopAttrib(); }
//Verena ? void Test_ODEBodies::testODEBody() { Core::resetCore(); ODE_SimulationAlgorithm *algorithm = new ODE_SimulationAlgorithm(); Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm); ODE_BodyAdapter *body = new ODE_BodyAdapter("Body1"); algorithm->resetPhysics(); QVERIFY(body->getRigidBodyID() == 0); body->setup(); QVERIFY(body->getRigidBodyID() != 0); QCOMPARE(body->getCollisionObjects().size(), 0); body->addCollisionObject(new CollisionObject(BoxGeom(body, 1, 1, 1), body)); QCOMPARE(body->getCollisionObjects().size(), 1); // Test create Collision Objects: Creates a dGeomID if a valid Geom exists QVERIFY(body->createODECollisionObjects()); CollisionObject *boxCollisionObject = body->getCollisionObjects().at(0); QVERIFY(boxCollisionObject->getNativeCollisionObject() != 0); // A valid dGeomID was created and added to the collision object as native collision object. QVERIFY(dGeomGetBody(static_cast<dGeomID>(boxCollisionObject->getNativeCollisionObject())) == body->getRigidBodyID()); // create fails, if an invalid SimulationAlgorithm is used Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(0); QVERIFY(!body->createODECollisionObjects()); Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm); QVERIFY(body->createODECollisionObjects()); body->clear(); // create fails, if no dRigidBody exists and the object is dynamic. QVERIFY(body->getRigidBodyID() == 0); QVERIFY(!body->createODECollisionObjects()); // create succeeds, if the object is static. body->getParameter("Dynamic")->setValueFromString("False"); QVERIFY(body->createODECollisionObjects()); QVERIFY(body->getCollisionObjects().at(0)->getNativeCollisionObject() != 0); QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 1); body->removeCollisionObject(boxCollisionObject); // Check whether the ode-type of the created objects is correct. body->addCollisionObject(new CollisionObject(SphereGeom(body, 1), body)); QVERIFY(body->createODECollisionObjects()); CollisionObject *sphereCollisionObject = body->getCollisionObjects().at(0); QVERIFY(sphereCollisionObject->getNativeCollisionObject() != 0); QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 0); body->removeCollisionObject(sphereCollisionObject); body->addCollisionObject(new CollisionObject(CylinderGeom(body, 1, 2), body)); QVERIFY(body->createODECollisionObjects()); CollisionObject *cylinderCollisionObject = body->getCollisionObjects().at(0); QVERIFY(cylinderCollisionObject ->getNativeCollisionObject() != 0); QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 3); body->removeCollisionObject(cylinderCollisionObject); body->addCollisionObject(new CollisionObject(TriangleGeom(body), body)); QVERIFY(body->createODECollisionObjects()); CollisionObject *trimeshCollisionObject = body->getCollisionObjects().at(0); QVERIFY(trimeshCollisionObject->getNativeCollisionObject() != 0); QCOMPARE(dGeomGetClass(static_cast<dGeomID>(body->getCollisionObjects().at(0)->getNativeCollisionObject())), 8); }
void TurbulentDispersionScene::Collide(dGeomID g1, dGeomID g2) { dBodyID b1 = dGeomGetBody(g1); dBodyID b2 = dGeomGetBody(g2); dContact contact[MAX_CONTACTS]; int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); if(n>0) { //Particle g1 collides with ground g2 if ((dGeomGetClass(g1) == dSphereClass)&&(g2==ground_wall)) { unsigned int index=(size_t) dGeomGetData(g1); //bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index)); Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index)); return; } //Particle g2 collides with ground g1 if((dGeomGetClass(g2) == dSphereClass)&&(g1==ground_wall)) { unsigned int index=(size_t) dGeomGetData(g2); //bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index)); Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index)); return; } } for (int i=0; i<n; ++i) { // contact[i].surface.mode = dContactBounce | dContactSoftCFM; contact[i].surface.mode = dContactBounce; //contact[i].surface.mu = dInfinity; contact[i].surface.mu = 0.5; //contact[i].surface.mu2 = 0.5; contact[i].surface.bounce = 0.000999990; //contact[i].surface.bounce_vel = 0.1; //contact[i].surface.soft_cfm = 0.001; //contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM; contact[i].surface.soft_erp = 1.0; //1.0; contact[i].surface.soft_cfm = 1e-10; dJointID j = dJointCreateContact (domain->getWorld(), domain->getContactGroup(), contact+i); dJointAttach(j, b1, b2); } }
void DrawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { // If the geom ID is missing then return immediately. if (!g) { return; } // If there was no position vector supplied then get the existing position. if (!pos) { pos = dGeomGetPosition (g); } // If there was no rotation matrix given then get the existing rotation. if (!R) { R = dGeomGetRotation (g); } // Get the geom's class type. int type = dGeomGetClass (g); if (type == dBoxClass) { // Create a temporary array of floats to hold the box dimensions. dReal sides[3]; dGeomBoxGetLengths(g, sides); // Now to actually render the box we make a call to DrawBox, passing the geoms dimensions, position vector and // rotation matrix. And this function is the subject of our next discussion. // DrawBox(sides, pos, R); } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCCylinderClass) { dReal radius,length; dGeomCCylinderGetParams (g,&radius,&length); dsDrawCappedCylinder (pos,R,length,radius); } else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R); } }
bool PObject::isPlaceable() { int c = dGeomGetClass(geom); if (c == dPlaneClass) return false; return true; }
void drawGeom( dGeomID g, int colored = 0 ) { if( !g ) //If the geometry object is missing, end the function. return; const dReal *position; //Define pointers to internal positions and orientations. const dReal *orientation; //Pointers to constant objects (so the objects will not change). int type = dGeomGetClass( g ); //Get the type of geometry. position = dGeomGetPosition( g ); //Then, get the geometry position. orientation = dGeomGetRotation( g ); //And get existing geometry orientation. if( type == dBoxClass ) //Is it a box? { dReal sides[3]; dGeomBoxGetLengths( g, sides ); //Get length of sides. renderBox( sides, position, orientation, colored ); //Render the actual box in environment. } else if (type == dCylinderClass) { dReal radius, length; dGeomCylinderGetParams(g, &radius, &length); renderCylinder(radius, length, position, orientation); } else if (type == dCapsuleClass) { dReal radius, length; dGeomCapsuleGetParams(g, &radius, &length); renderCapsule(radius, length, position, orientation); } }
void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,j,n; const int N = 100; dContactGeom contact[N]; if (dGeomGetClass (o2) == dRayClass) { n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom)); } else { n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom)); } if (n > 0) { dMatrix3 RI; dRSetIdentity (RI); const dReal ss[3] = {0.01,0.01,0.01}; for (i=0; i<n; i++) { contact[i].pos[2] += Z_OFFSET; dsDrawBox (contact[i].pos,RI,ss); dVector3 n; for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j]; dsDrawLine (contact[i].pos,n); } } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!draw_geom){ return; } if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ else if (type == dGeomTransformClass) { dGeomID g2 = dGeomTransformGetGeom (g); const dReal *pos2 = dGeomGetPosition (g2); const dReal *R2 = dGeomGetRotation (g2); dVector3 actual_pos; dMatrix3 actual_R; dMULTIPLY0_331 (actual_pos,R,pos2); actual_pos[0] += pos[0]; actual_pos[1] += pos[1]; actual_pos[2] += pos[2]; dMULTIPLY0_333 (actual_R,R,R2); drawGeom (g2,actual_pos,actual_R,0); } if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
//Auxiliary function to print info on a space void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name) { int num_geoms = 0; dGeomID geom_temp; int geom_class_temp = 0; dReal aabb[6]; dBodyID body_temp; num_geoms = dSpaceGetNumGeoms(my_space); printf("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms); for (int i=0;i<=(num_geoms-1);i++){ geom_temp = dSpaceGetGeom (my_space, i); geom_class_temp = dGeomGetClass(geom_temp); printGeomClassAndNr(geom_class_temp,i); if (!dGeomIsSpace(geom_temp)){ if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom printf(" ID: %p, Coordinates:",geom_temp); ICubSim::printPositionOfGeom(geom_temp); dGeomGetAABB(geom_temp,aabb); printf(" Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); if (geom_class_temp==8){ // trimesh body_temp = dGeomGetBody(geom_temp); printf(" Coordinates of associated body are:"); ICubSim::printPositionOfBody(body_temp); } } else { dGeomGetAABB(geom_temp,aabb); printf(" ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]); } } } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i; // if (o1->body && o2->body) return; // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box for (i=0; i<MAX_CONTACTS; i++) { contact[i].surface.mode = dContactBounce | dContactSoftCFM; contact[i].surface.mu = dInfinity; contact[i].surface.mu2 = 0; contact[i].surface.bounce = 0.1; contact[i].surface.bounce_vel = 0.1; contact[i].surface.soft_cfm = 0.01; } if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, sizeof(dContact))) { dMatrix3 RI; dRSetIdentity (RI); const dReal ss[3] = {0.02,0.02,0.02}; for (i=0; i<numc; i++) { if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){ dMatrix3 Rotation; dRSetIdentity(Rotation); dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01)); dVector3 End; End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth); End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth); End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth); End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth); dsDrawLine(contact[i].geom.pos, End); continue; } dJointID c = dJointCreateContact (world,contactgroup,contact+i); dJointAttach (c,b1,b2); if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); } } }
void TSRODERigidBody::DebugRender() { Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState ); TSRMatrix4 bodyTransform; TSRMatrix4 geomTransform; const float* pBodyPosition = dBodyGetPosition( m_BodyID ); const float* pBodyRotation = dBodyGetRotation( m_BodyID ); ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); TSRDebugDraw::RenderAxis( 1.0f ); TSRDebugDraw::RenderSphere( 0.25f ); for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ ) { dGeomID currGeomTransformID = m_GeomIDs[ i ]; dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID ); const float* pGeomPosition = dGeomGetPosition( geomID ); const float* pGeomRotation = dGeomGetRotation( geomID ); ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation ); TSRGlobalConstants.PushMatrix(); TSRGlobalConstants.MultMatrix( bodyTransform.d ); switch( dGeomGetClass( geomID ) ) { case dBoxClass: { dVector3 extents; dGeomBoxGetLengths( geomID, extents ); TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] ); TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] ); aabbMin *= 0.5f; aabbMax *= 0.5f; TSRDebugDraw::RenderAABB( aabbMin, aabbMax ); } break; case dSphereClass: { float radius; radius = dGeomSphereGetRadius( geomID ); TSRDebugDraw::RenderSphere( radius ); } break; case dCylinderClass: { float radius,length; dGeomCylinderGetParams( geomID, &radius, &length ); TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) ); } break; } TSRGlobalConstants.PopMatrix(); } TSRGlobalConstants.PopMatrix(); Graphics()->SetRasterizerState( Graphics()->m_FillSolidState ); }
void GetSphereExtensions(dGeomID sphere,const dReal* axis, const dReal *pos, float center_prg,dReal* lo_ext,dReal* hi_ext) { R_ASSERT2(dGeomGetClass(sphere)==dSphereClass,"is not a sphere"); dReal radius=dGeomSphereGetRadius(sphere); dReal dif=dDOT(pos,axis)-center_prg; *lo_ext=-radius+dif; *hi_ext=radius+dif; }
/******************************************************************************* Function to draw a geometry object. *******************************************************************************/ void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const { if( !g ) //If the geometry object is missing, end the function. return; if( !position ) //Position was not passed? position = dGeomGetPosition( g ); //Then, get the geometry position. if( !orientation ) //Orientation was not given? orientation = dGeomGetRotation( g ); //And get existing geometry orientation. int type = dGeomGetClass( g ); //Get the type of geometry. if( type == dBoxClass ) //Is it a box? { dReal sides[3]; dGeomBoxGetLengths( g, sides ); //Get length of sides. renderBox( sides, position, orientation ); //Render the actual box in environment. } if( type == dSphereClass ) //Is it a sphere? { dReal radius; radius = dGeomSphereGetRadius( g ); //Get the radius. renderSphere( radius, position, orientation ); //Render sphere in environment. } if( type == dCapsuleClass ) { dReal radius; dReal length; dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length. renderCapsule( radius, length, position, orientation ); //Render capsule in environment. } if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body. { dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper. const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry. const dReal *orientation2 = dGeomGetRotation( g2 ); dVector3 actualPosition; //Real world coordinated position and orientation dMatrix3 actualOrientation; //of the wrapped geometry. dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position. actualPosition[0] += position[0]; actualPosition[1] += position[1]; actualPosition[2] += position[2]; dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation. drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry. } }
void TransformedGeometryExtensionLocalParams(dGeomID geom_transform,const dReal* axis,float center_prg,dReal* local_axis,dReal& local_center_prg) { R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform"); const dReal* rot=dGeomGetRotation(geom_transform); const dReal* pos=dGeomGetPosition(geom_transform); dVector3 local_pos; dMULTIPLY1_331(local_axis,rot,axis); dMULTIPLY1_331(local_pos,rot,pos); local_center_prg=center_prg-dDOT(local_pos,local_axis); }
Particle::Particle(dBodyID &b, dGeomID &g):ODE_Particle(b,g) { shape_type=dGeomGetClass(g); enabled=true; position[0]=0;position[1]=0;position[2]=0; velocity[0]=0;velocity[1]=0;velocity[2]=0; ang_velocity[0]=0;ang_velocity[1]=0;ang_velocity[2]=0; //printf("Particle constructor!\n"); //fflush(stdout); }
static void computeFinalTx(dGeomID geom_transform,dReal* final_pos,dReal* final_R) { R_ASSERT2(dGeomGetClass(geom_transform)==dGeomTransformClass,"is not a geom transform"); dGeomID obj=dGeomTransformGetGeom(geom_transform); const dReal *R =dGeomGetRotation(geom_transform); const dReal *pos=dGeomGetPosition(geom_transform); dMULTIPLY0_331 (final_pos,R,dGeomGetPosition(obj)); final_pos[0] += pos[0]; final_pos[1] += pos[1]; final_pos[2] += pos[2]; dMULTIPLY0_333 (final_R,R,dGeomGetRotation(obj)); }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i; // if (o1->body && o2->body) return; // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnected (b1,b2)) return; dContact contact[32]; // up to 3 contacts per box for (i=0; i<32; i++) { contact[i].surface.mode = dContactBounce; //dContactMu2; contact[i].surface.mu = dInfinity; contact[i].surface.mu2 = 0; contact[i].surface.bounce = 0.5; contact[i].surface.bounce_vel = 0.1; } if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) { dMatrix3 RI; dRSetIdentity (RI); const dReal ss[3] = {0.02,0.02,0.02}; for (i=0; i<numc; i++) { if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){ dMatrix3 Rotation; dRSetIdentity(Rotation); dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01)); continue; } dJointID c = dJointCreateContact (world,contactgroup,contact+i); dJointAttach (c,b1,b2); //dsDrawBox (contact[i].geom.pos,RI,ss); } } }
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) { if (!g) return; if (!pos) pos = dGeomGetPosition (g); if (!R) R = dGeomGetRotation (g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,R,sides); } else if (type == dSphereClass) { dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); } else if (type == dCapsuleClass) { dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,R,length,radius); } else if (type == dConvexClass) { //dVector3 sides={0.50,0.50,0.50}; dsDrawConvex(pos,R,planes, planecount, points, pointcount, polygons); } /* // cylinder option not yet implemented else if (type == dCylinderClass) { dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,R,length,radius); } */ if (show_aabb) { // draw the bounding box for this geom dReal aabb[6]; dGeomGetAABB (g,aabb); dVector3 bbpos; for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); dVector3 bbsides; for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2]; dMatrix3 RI; dRSetIdentity (RI); dsSetColorAlpha (1,0,0,0.5); dsDrawBox (bbpos,RI,bbsides); } }
void GetBoxExtensions(dGeomID box,const dReal* axis, const dReal *pos, const dReal *rot, float center_prg,dReal* lo_ext,dReal* hi_ext) { R_ASSERT2(dGeomGetClass(box)==dBoxClass,"is not a box"); dVector3 length; dGeomBoxGetLengths(box,length); dReal dif=dDOT(pos,axis)-center_prg; dReal ful_ext=dFabs(dDOT14(axis,rot+0))*length[0] +dFabs(dDOT14(axis,rot+1))*length[1] +dFabs(dDOT14(axis,rot+2))*length[2]; ful_ext/=2.f; *lo_ext=-ful_ext+dif; *hi_ext=ful_ext+dif; }
void drawGeom(dGeomID g) { int gclass = dGeomGetClass(g); const dReal *pos = dGeomGetPosition(g); const dReal *rot = dGeomGetRotation(g); switch (gclass) { case dSphereClass: dsSetColorAlpha(0, 0.75, 0.5, 0.5); dsSetTexture (DS_CHECKERED); dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); break; case dBoxClass: { dVector3 lengths; dsSetColorAlpha(1, 1, 0, 0.5); dsSetTexture (DS_WOOD); dGeomBoxGetLengths(g, lengths); dsDrawBox(pos, rot, lengths); break; } case dTriMeshClass: { int numi = dGeomTriMeshGetTriangleCount(g); for (int i=0; i<numi; ++i) { dVector3 v0, v1, v2; dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2); dsSetTexture (DS_WOOD); dsSetDrawMode(DS_WIREFRAME); dsSetColorAlpha(0, 0, 0, 1.0); dsDrawTriangle(pos, rot, v0, v1, v2, true); dsSetDrawMode(DS_POLYFILL); dsSetColorAlpha(1, 1, 0, 0.5); dsDrawTriangle(pos, rot, v0, v1, v2, true); } break; } default: {} } }
void GetCylinderExtensions(dGeomID cyl,const dReal* axis, const dReal *pos, const dReal *rot, float center_prg,dReal* lo_ext,dReal* hi_ext) { R_ASSERT2(dGeomGetClass(cyl)==dCylinderClassUser,"is not a cylinder"); dReal radius,length; dGeomCylinderGetParams(cyl,&radius,&length); dReal dif=dDOT(pos,axis)-center_prg; dReal _cos=dFabs(dDOT14(axis,rot+1)); dReal cos1=dDOT14(axis,rot+0); dReal cos3=dDOT14(axis,rot+2); dReal _sin=_sqrt(cos1*cos1+cos3*cos3); length/=2.f; dReal ful_ext=_cos*length+_sin*radius; *lo_ext=-ful_ext+dif; *hi_ext=ful_ext+dif; }
void dmDraw(dmObject *obj) /*** 物体の描画 ***/ { if (!obj->geom) return; obj->p = (double *) dGeomGetPosition(obj->geom); obj->R = (double *) dGeomGetRotation(obj->geom); dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b) // printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]); int type = dGeomGetClass(obj->geom); switch (type) { case dBoxClass: { dVector3 sides; dGeomBoxGetLengths(obj->geom,sides); dsDrawBox(obj->p,obj->R,sides); } break; case dSphereClass: dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom)); break; case dCapsuleClass: { dReal radius,length; dGeomCapsuleGetParams(obj->geom,&radius,&length); dsDrawCapsule(obj->p,obj->R,length,radius); } break; case dCylinderClass: { dReal radius,length; dGeomCylinderGetParams(obj->geom,&radius,&length); dsDrawCylinder(obj->p,obj->R,length,radius); } break; default: printf("Bad geometry type \n"); } }
void drawGeom (dGeomID g) { const dReal *pos = dGeomGetPosition(g); const dReal *R = dGeomGetRotation(g); int type = dGeomGetClass (g); if (type == dBoxClass) { dVector3 sides; dGeomBoxGetLengths (g, sides); dsDrawBox (pos,R,sides); } if (type == dCylinderClass) { dReal r,l; dGeomCylinderGetParams(g, &r, &l); dsDrawCylinder (pos, R, l, r); } }
//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~ CollisionShape::CollisionShapeType CollisionShape::getType() { switch(dGeomGetClass(m_geometryId)) { case dSphereClass: return SPHERE_SHAPE; case dBoxClass: return BOX_SHAPE; case dCapsuleClass: return CAPSULE_SHAPE; case dCylinderClass: return CYLINDER_SHAPE; case dHeightfieldClass: return HEIGHTFIELD_SHAPE; case dPlaneClass: case dRayClass: case dConvexClass: case dGeomTransformClass: case dTriMeshClass: default: return NULL_SHAPE; }; }
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 callback(void *data, dGeomID o1, dGeomID o2) { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return; /* Two geoms associated with the same body do not collide. */ if (b1 == 0 || b2 == 0 || b1 != b2) { dContact contact[MAX_CONTACTS]; size_t sz = sizeof (dContact); int i, n; /* Find and enumerate all collision points. */ if ((n = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sz))) { struct geom_data *d1 = get_data(o1); struct geom_data *d2 = get_data(o2); /* Compute collision parameters from geom parameters. */ dReal bounce = MAX(d1->bounce, d2->bounce); dReal friction = MIN(d1->friction, d2->friction); dReal soft_erp = MIN(d1->soft_erp, d2->soft_erp); dReal soft_cfm = MAX(d1->soft_cfm, d2->soft_cfm); unsigned long category1 = dGeomGetCategoryBits(o1); unsigned long category2 = dGeomGetCategoryBits(o2); if (b1) dBodyEnable(b1); if (b2) dBodyEnable(b2); /* Create a contact joint for each collision, if requsted. */ if ((category1 & d2->response) || (category2 & d1->response)) for (i = 0; i < n; ++i) if (dGeomGetClass(contact[0].geom.g1) != dRayClass && dGeomGetClass(contact[0].geom.g2) != dRayClass) { dJointID c; contact[i].surface.mode = dContactBounce | dContactSoftCFM | dContactSoftERP; contact[i].surface.mu = friction; contact[i].surface.bounce = bounce; contact[i].surface.soft_cfm = soft_cfm; contact[i].surface.soft_erp = soft_erp; contact[i].surface.bounce_vel = (dReal) 0.10; c = dJointCreateContact(world, group, contact + i); dJointAttach(c, b1, b2); } /* Report contacts to the Lua script, if requested. */ if ((category1 & d2->callback) || (category2 & d1->callback)) for (i = 0; i < n; ++i) { float p[3]; float v[3]; p[0] = (float) contact[i].geom.pos[0]; p[1] = (float) contact[i].geom.pos[1]; p[2] = (float) contact[i].geom.pos[2]; v[0] = (float) contact[i].geom.normal[0]; v[1] = (float) contact[i].geom.normal[1]; v[2] = (float) contact[i].geom.normal[2]; do_contact_script(d1->entity, d2->entity, p, v, (float) contact[i].geom.depth); } } } }