bool wheelCallBack(dGeomID o1,dGeomID o2,PSurface* s) { //s->id2 is ground const dReal* r; //wheels rotation matrix //const dReal* p; //wheels rotation matrix if ((o1==s->id1) && (o2==s->id2)) { r=dBodyGetRotation(dGeomGetBody(o1)); //p=dGeomGetPosition(o1);//never read } else if ((o1==s->id2) && (o2==s->id1)) { r=dBodyGetRotation(dGeomGetBody(o2)); //p=dGeomGetPosition(o2);//never read } else { //XXX: in this case we dont have the rotation // matrix, thus we must return return false; } s->surface.mode = dContactFDir1 | dContactMu2 | dContactApprox1 | dContactSoftCFM; s->surface.mu = fric(_w->cfg->robotSettings.WheelPerpendicularFriction); s->surface.mu2 = fric(_w->cfg->robotSettings.WheelTangentFriction); s->surface.soft_cfm = 0.002; dVector3 v={0,0,1,1}; dVector3 axis; dMultiply0(axis,r,v,4,3,1); dReal l = sqrt(axis[0]*axis[0] + axis[1]*axis[1]); s->fdir1[0] = axis[0]/l; s->fdir1[1] = axis[1]/l; s->fdir1[2] = 0; s->fdir1[3] = 0; s->usefdir1 = true; return true; }
void CDynamics3DEngine::ManageCloseGeomsAddingContactJoints(SGeomCheckData* ps_data, dGeomID t_geom1, dGeomID t_geom2) { /* * The passed geometries can be spaces or shapes. * Depending on the type of each passed geometry, we do something different */ if(dGeomIsSpace(t_geom1) || dGeomIsSpace(t_geom2)) { /* * At least one of the two geometries is a space. * We need to open up the spaces to get to the basic elements. */ dSpaceCollide2(t_geom1, t_geom2, ps_data, &ManageCloseGeomsAddingContactJointsCallback); } else { /* Both geoms are shapes. Let's check for collisions among them */ size_t unContacts = dCollide(t_geom1, t_geom2, m_unMaxContacts, &m_ptContacts->geom, sizeof(dContact)); /* If no collision is detected, we're done */ if(unContacts == 0) return; /* Otherwise, let's add contact joints in each contact point detected */ /* Get the body ids */ dBodyID tBody1 = dGeomGetBody(t_geom1); dBodyID tBody2 = dGeomGetBody(t_geom2); /* Create a buffer for the contact joints */ dJointID tContactJoint; /* Go through the contact points and create the joints */ for(UInt32 i = 0; i < unContacts; ++i) { tContactJoint = dJointCreateContact(m_tWorldID, m_tContactJointGroupID, m_ptContacts+i); dJointAttach(tContactJoint, tBody1, tBody2); } } }
void PhysicsWorld::collide(void *_data, dGeomID _o1, dGeomID _o2) { int i; //std::cout<<"collide\n"; // 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; for (i=0; i<m_maxContacts; i++) { m_contact[i].surface.mode = dContactBounce;// | dContactSoftCFM; m_contact[i].surface.mu = dInfinity; m_contact[i].surface.mu2 = 0; m_contact[i].surface.bounce = 0.3; m_contact[i].surface.bounce_vel = 0.1; m_contact[i].surface.soft_cfm = 0.01; } if (int numc = dCollide (_o1,_o2,m_maxContacts,&m_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++) { dJointID c = dJointCreateContact (m_world,m_contactgroup,m_contact+i); dJointAttach (c,b1,b2); } } }
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++) { dJointID c = dJointCreateContact (world,contactgroup,contact+i); dJointAttach (c,b1,b2); if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); } } }
///Will produce bogus o1 and o2 vectors void GetContacts(dBodyID a,vector<ODEContactList>& contacts) { if(a == 0) return; contacts.resize(0); for(list<ODEContactResult>::iterator i=gContacts.begin();i!=gContacts.end();i++) { if(a == dGeomGetBody(i->o1) || a == dGeomGetBody(i->o2)) { dBodyID b = dGeomGetBody(i->o2); bool reverse = false; if(b == a) { b = dGeomGetBody(i->o1); reverse = true; } contacts.resize(contacts.size()+1); contacts.back().points.resize(i->contacts.size()); contacts.back().forces.resize(i->feedback.size()); for(size_t j=0;j<i->feedback.size();j++) { CopyVector(contacts.back().forces[j],i->feedback[j].f1); CopyVector(contacts.back().points[j].x,i->contacts[j].pos); CopyVector(contacts.back().points[j].n,i->contacts[j].normal); //contacts.back().points[j].kFriction = i->contacts[j].surface.mu; contacts.back().points[j].kFriction = 0; if(reverse) { contacts.back().forces[j].inplaceNegative(); contacts.back().points[j].n.inplaceNegative(); } } } } }
void WorldPhysics::handleCollisionBetween(dGeomID o0, dGeomID o1) { int i,n; // only collide things with the ground //int g1 = (o1 == ground || o1 == ground_box); //int g2 = (o2 == ground || o2 == ground_box); //if (!(g1 ^ g2)) return; // for(int i=0;i<4;i++) { // if (((o0==bulldozer->geom) && (o1==wheels[i]->geom)) || // ((o0==wheels[i]->geom) && (o1==bulldozer->geom))) return; // } const int N = 100; dContact contact[N]; n = dCollide (o0,o1,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { for (i=0; i<n; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 1.7; contact[i].surface.slip1 = 0.1; contact[i].surface.slip2 = 0.1; contact[i].surface.soft_erp = 0.5; contact[i].surface.soft_cfm = 0.3; dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } }
void NodynamicsCollide(bool& do_colide,bool bo1,dContact& c,SGameMtl * /*material_1*/,SGameMtl * /*material_2*/) { dBodyID body1=dGeomGetBody(c.geom.g1); dBodyID body2=dGeomGetBody(c.geom.g2); if(!body1||!body2||(dGeomUserDataHasCallback(c.geom.g1,NodynamicsCollide)&&dGeomUserDataHasCallback(c.geom.g2,NodynamicsCollide)))return; do_colide=false; }
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 nearCallback(void *data, dGeomID o1, dGeomID o2) { State* state = (State*)data; if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) { dSpaceCollide2(o1, o2, data, &nearCallback); if(dGeomIsSpace(o1)) dSpaceCollide((dSpaceID)o1, data, &nearCallback); if(dGeomIsSpace(o2)) dSpaceCollide((dSpaceID)o2, data, &nearCallback); } else { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); const int MAX_CONTACTS = 18; dContact contact[MAX_CONTACTS]; for(int i = 0; i < MAX_CONTACTS; i++) { contact[i].surface.mode = dContactBounce; contact[i].surface.mu = 2000; contact[i].surface.bounce = 0.1; contact[i].surface.bounce_vel = 0.15; } if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) { for(int i = 0; i < numc; i++) { dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]); dJointAttach(c, b1, b2); } } } }
static void nearCallback(void *data, dGeomID o1, dGeomID o2) { // if a collision has not already been detected if (!reinterpret_cast<CallbackParam *>(data)->collision) { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if ((b1 != nullptr) && (b2 != nullptr) && (dAreConnectedExcluding(b1, b2, dJointTypeContact) != 0)) return; dContact contact[1]; // one contact is sufficient int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact)); // check if there is really a collision if (numc != 0) { // check if the collision is allowed bool valid = reinterpret_cast<CallbackParam *>(data)->env->isValidCollision(o1, o2, contact[0]); reinterpret_cast<CallbackParam *>(data)->collision = !valid; if (reinterpret_cast<CallbackParam *>(data)->env->verboseContacts_) { OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"), reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o1).c_str(), reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o2).c_str()); } } } }
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)); } } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { assert(o1); assert(o2); if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) { fprintf(stderr,"testing space %p %p\n", o1,o2); // colliding a space with something dSpaceCollide2(o1,o2,data,&nearCallback); // Note we do not want to test intersections within a space, // only between spaces. return; } const int N = 32; dContact contact[N]; int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact)); if (n > 0) { for (int i=0; i<n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1; contact[i].surface.mu = 100.0; contact[i].surface.soft_erp = 0.96; contact[i].surface.soft_cfm = 0.02; dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } }
/*********************************************************** hanlde collision ***********************************************************/ void ODEPhysicHandler::handleCollisionBetween(dGeomID o0, dGeomID o1) { // Create an array of dContact objects to hold the contact joints static const int MAX_CONTACTS = 10; dContact contact[MAX_CONTACTS]; for (int 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.8; contact[i].surface.bounce_vel = 0.1; contact[i].surface.soft_cfm = 0.01; } if (int numc = dCollide(o0, o1, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) { // Get the dynamics body for each geom dBodyID b1 = dGeomGetBody(o0); dBodyID b2 = dGeomGetBody(o1); // To add each contact point found to our joint group we call dJointCreateContact which is just one of the many // different joint types available. for (int i = 0; i < numc; i++) { // dJointCreateContact needs to know which world and joint group to work with as well as the dContact // object itself. It returns a new dJointID which we then use with dJointAttach to finally create the // temporary contact joint between the two geom bodies. dJointID c = dJointCreateContact(_world, _contactgroup, contact + i); dJointAttach(c, b1, b2); } } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { // for drawing the contact points dMatrix3 RI; dRSetIdentity (RI); const dReal ss[3] = {0.02,0.02,0.02}; int i; dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); dContact contact[MAX_CONTACTS]; int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, sizeof(dContact)); for (i=0; i<numc; i++) { contact[i].surface.mode = dContactApprox1; contact[i].surface.mu = 2; dJointID c = dJointCreateContact (*world,contactgroup,contact+i); dJointAttach (c,b1,b2); if (show_contacts) dsDrawBox (contact[i].geom.pos, RI, ss); } }
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)); } } } }
//description //Default behavior. //Default collision of bodies void ODEBaseScene::Collide(dGeomID g1, dGeomID g2) { int n; dBodyID b1 = dGeomGetBody(g1); dBodyID b2 = dGeomGetBody(g2); if (b1 && b2 && dAreConnected(b1, b2)) return; const int N = 4; dContact contact[N]; n = dCollide (g1,g2,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { // printf("Body %d hits body %d.\n", (size_t) dGeomGetData(g1),(size_t) dGeomGetData(g2)); 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 (m_domain->getWorld(), m_domain->getContactGroup(), contact+i); dJointAttach(j, b1, b2); } } }
static void nearCallback(void *data, dGeomID o1, dGeomID o2) //o1,o2: geometries likely to collide { // inner collision detecting innerCollision = innerCollision || (o1 != ground && o2 != ground); const int N = 30; dContact contact[N]; // Don’t do 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; int isGround = ((ground == o1) || (ground == o2)); //Collision with ground int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact)); //n: Number of collision points if (isGround) {//If there is a collision with the ground for (int i = 0; i < n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM; contact[i].surface.mu = dInfinity; //Coulomb friction coef contact[i].surface.soft_erp = 0.2; // 1.0 ideal contact[i].surface.soft_cfm = 1e-4; // 0.0 ideal dJointID c = dJointCreateContact(world,contactgroup,&contact[i]); dJointAttach(c,dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } }
/** * Returns a dBody id from its DEF name. * @param def DEF name of teh desired dBody * @return a pointer to the dBody if found NULL otherwise */ dBodyID getBodyFromDEF(const char *def) { //std::cout<<"Try to get body "<<def<<std::endl; dGeomID geom = dWebotsGetGeomFromDEF(def); if (! geom) { printf("Fatal error: did not find dGeom for DEF %s", def); s_data->disablePhysics(); return NULL; } dBodyID body; if (dGeomIsSpace(geom)){ body = dGeomGetBody(dSpaceGetGeom((dSpaceID)geom, 0)); std::cerr<<"I don't know why I have to do this"<<std::endl; s_data->disablePhysics(); }else body = dGeomGetBody(geom); if (! body) { printf("Fatal error: did not find dBody for DEF %s", def); s_data->disablePhysics(); return NULL; } return body; }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,n; // only collide things with the ground int g1 = (o1 == ground || o1 == ground_box); int g2 = (o2 == ground || o2 == ground_box); if (!(g1 ^ g2)) return; const int N = 10; 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; contact[i].surface.mu = dInfinity; contact[i].surface.slip1 = 0.1; contact[i].surface.slip2 = 0.1; contact[i].surface.soft_erp = 0.5; contact[i].surface.soft_cfm = 0.3; dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } }
//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]); } } } }
//collision detection static void nearCallback (void *data, dGeomID o1, dGeomID o2) { int i,n; dBodyID b1 = dGeomGetBody (o1); dBodyID b2 = dGeomGetBody (o2); if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact) ) return; const int N = 10; 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); contact[i].surface.mu = 0.1; contact[i].surface.slip1 = 0.02; contact[i].surface.slip2 = 0.02; contact[i].surface.soft_erp = 0.1; contact[i].surface.soft_cfm = 0.0001; dJointID c = dJointCreateContact (world,contactgroup,&contact[i]); dJointAttach (c,dGeomGetBody (contact[i].geom.g1),dGeomGetBody (contact[i].geom.g2) ); } } }
void CPHCollisionDamageReceiver::CollisionCallback(bool& do_colide,bool bo1,dContact& c,SGameMtl* material_1,SGameMtl* material_2) { if(material_1->Flags.test(SGameMtl::flPassable)||material_2->Flags.test(SGameMtl::flPassable))return; dBodyID b1 = dGeomGetBody(c.geom.g1) ; dBodyID b2 = dGeomGetBody(c.geom.g2) ; dxGeomUserData *ud_self = bo1 ? retrieveGeomUserData(c.geom.g1):retrieveGeomUserData(c.geom.g2); dxGeomUserData *ud_damager = bo1 ? retrieveGeomUserData(c.geom.g2):retrieveGeomUserData(c.geom.g1); SGameMtl *material_self = bo1 ? material_1:material_2; SGameMtl *material_damager = bo1 ? material_2:material_1; VERIFY (ud_self); CPhysicsShellHolder *o_self = ud_self->ph_ref_object; CPhysicsShellHolder *o_damager = NULL;if(ud_damager)o_damager=ud_damager->ph_ref_object; u16 source_id = o_damager ? o_damager->ID():u16(-1); CPHCollisionDamageReceiver *dr =o_self->PHCollisionDamageReceiver(); VERIFY2(dr,"wrong callback"); float damager_material_factor=material_damager->fBounceDamageFactor; if(ud_damager&&ud_damager->ph_object&&ud_damager->ph_object->CastType()==CPHObject::tpCharacter) { CCharacterPhysicsSupport* phs=o_damager->character_physics_support(); if(phs->IsSpecificDamager())damager_material_factor=phs->BonceDamageFactor(); } float dfs=(material_self->fBounceDamageFactor+damager_material_factor); if(fis_zero(dfs)) return; Fvector dir;dir.set(*(Fvector*)c.geom.normal); Fvector pos; pos.sub(*(Fvector*)c.geom.pos,*(Fvector*)dGeomGetPosition(bo1 ? c.geom.g1:c.geom.g2));//it is not true pos in bone space dr->Hit(source_id,ud_self->bone_id,E_NL(b1,b2,c.geom.normal)*damager_material_factor/dfs,dir,pos); }
/*** コールバック関数 ***/ static void nearCallback(void *data, dGeomID o1, dGeomID o2) { dVector3 tmp_fdir = {0, 0, 0, 0}; dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return; int wheel_flag = 0; for (int j = 0; j < WHEEL_NUM; j++) { if ((o1 == wheel[j].geom)||(o2 == wheel[j].geom)) { wheel_flag = 1; dJointGetHingeAxis(wheel[j].joint,tmp_fdir); break; } } static const int N = 10; dContact contact[N]; int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact)); if (n > 0) { if (wheel_flag == 1) { for (int i=0; i<n; i++) { contact[i].surface.mode = dContactFDir1| dContactMu2 | dContactSoftERP | dContactSoftCFM; contact[i].fdir1[0] = tmp_fdir[0]; // 第1摩擦方向の設定(x軸成分) contact[i].fdir1[1] = tmp_fdir[1]; // 第1摩擦方向の設定(y軸成分) contact[i].fdir1[2] = tmp_fdir[2]; // 第1摩擦方向の設定(z軸成分) contact[i].surface.mu = 0.1; // 車軸方向の摩擦係数 contact[i].surface.mu2 = dInfinity; // 車輪方向の摩擦係数 contact[i].surface.soft_erp = 0.9; contact[i].surface.soft_cfm = 0.001; dJointID c = dJointCreateContact(world,contactgroup,&contact[i]); dJointAttach(c,b1,b2); } } else { for (int i=0; i<n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM; contact[i].surface.mu = dInfinity; contact[i].surface.soft_erp = 0.8; contact[i].surface.soft_cfm = 1e-5; dJointID c = dJointCreateContact(world,contactgroup,&contact[i]); dJointAttach(c,b1,b2); } } } }
void ODEEngine::near_callback(dGeomID o1, dGeomID o2) { ODECollidable* c1 = (ODECollidable*) dGeomGetData(o1); ODECollidable* c2 = (ODECollidable*) dGeomGetData(o2); dContact contact[MAX_CONTACTS]; int collision_count = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); for(int32_t i = 0; i < collision_count; ++i) { //Calculate combined surface values to build contact joints contact[i].surface.mode = dContactBounce; contact[i].surface.bounce = c1->bounciness() * c2->bounciness(); if(c1->has_infinite_friction() || c2->has_infinite_friction()) { contact[i].surface.mu = dInfinity; } else { contact[i].surface.mu = c1->friction() * c2->friction(); } dBodyID body1 = dGeomGetBody(contact[i].geom.g1); dBodyID body2 = dGeomGetBody(contact[i].geom.g2); if(body1 == body2) { //Don't collide the same body against itself continue; } if(i == 0) { //Fire the collision signal on the first loop only c1->signal_collided_(*c2); c2->signal_collided_(*c1); //Fire any combined signals fire_collision_signals_for(*c1, *c2); } //Check to see if we should create response joints, or just ignore the collision if(c1->is_ghost() || c2->is_ghost()) { //Don't respond if we're not supposed to (e.g. we're a hit zone or something) continue; } if(c1->should_respond_callback_ && !c1->should_respond_callback_(*c2)) { continue; } if(c2->should_respond_callback_ && !c2->should_respond_callback_(*c1)) { continue; } dJointID c = dJointCreateContact(world(), contact_group_, &contact[i]); dJointAttach(c, body1, body2); } }
// main collision handling function void PhysicsServer::checkCollision(void *data, dGeomID o1, dGeomID o2) { int i,n; if( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) ) { dSpaceCollide2( o1, o2, data, &collisionCallback ); } else { int mode = 0; double slip1 = 0; double slip2 = 0; // get bodies dBodyID body1 = dGeomGetBody(o1); dBodyID body2 = dGeomGetBody(o2); // get geom data PhysGeomData* pData1 = (PhysGeomData*)dGeomGetData(o1); PhysGeomData* pData2 = (PhysGeomData*)dGeomGetData(o2); // set contact params according to geom data if (pData1 != NULL) slip1 = pData1->slip; if (pData2 != NULL) slip2 = pData2->slip; // set mode if (slip1 != 0) mode |= dContactSlip1; if (slip2 != 0) mode |= dContactSlip2; static const int N = 8; // max number of contact points 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 = mode; /*dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM;*/ contact[i].surface.mu = dInfinity; contact[i].surface.slip1 = slip1; contact[i].surface.slip2 = slip2; // contact[i].surface.soft_erp = 0.7; // contact[i].surface.soft_cfm = 0.1; dJointID c = dJointCreateContact (_world, _contactgroup, &contact[i]); dJointAttach (c, body1, body2); } } } }
static void nearCallback (void *, dGeomID o1, dGeomID o2) { // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); dContact contact; contact.surface.mode = 0; contact.surface.mu = dInfinity; if (dCollide (o1,o2,1,&contact.geom,sizeof(dContactGeom))) { dJointID c = dJointCreateContact (world,contactgroup,&contact); dJointAttach (c,b1,b2); } }
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); } }
//All pairs of geoms that are potentially intersecting will be passed by dSpaceCollide to this function. //Here we can determine which objects are actually colliding by making a call to dCollide and change the //behavior of the joints before adding them to the joint group. The first parameter would be the user data //pointer passed to dSpaceCollide if we had provided it. The second and third parameters are the two potentially intersecting geoms. void nearCallback (void *data, dGeomID o1, dGeomID o2) { //Temporary index for each contact int i; int numc = 0; //Get the dynamics body for each geom dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); //Create an array of dContact objects to hold the contact joints dContact contact[MAX_CONTACTS]; // Now we set the joint properties of each contact. Going into the full details here would require a tutorial of its // own. I'll just say that the members of the dContact structure control the joint behaviour, such as friction, // velocity and bounciness. See section 7.3.7 of the ODE manual and have fun experimenting to learn more. for (i = 0; i < MAX_CONTACTS; i++) { contact[i].surface.mode = dContactBounce | dContactSoftCFM; contact[i].surface.mu = dInfinity; contact[i].surface.mu2 = 5000; contact[i].surface.bounce = 0.98; contact[i].surface.bounce_vel = 0.5; contact[i].surface.soft_cfm = 0.01; } // Here we do the actual collision test by calling dCollide. It returns the number of actual contact points or zero // if there were none. As well as the geom IDs, max number of contacts we also pass the address of a dContactGeom // as the fourth parameter. dContactGeom is a substructure of a dContact object so we simply pass the address of // the first dContactGeom from our array of dContact objects and then pass the offset to the next dContactGeom // as the fifth paramater, which is the size of a dContact structure. That made sense didn't it? numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); if (numc > 0) { // To add each contact point found to our joint group we call dJointCreateContact which is just one of the many // different joint types available. for (i = 0; i < numc; i++) { //dJointCreateContact needs to know which world and joint group to work with as well as the dContact //object itself. It returns a new dJointID which we then use with dJointAttach to finally create the //temporary contact joint between the two geom bodies. dJointID c = dJointCreateContact(world, contactgroup, &contact[i]); dJointAttach(c, b1, b2); } } }
static void collideCB(void *data, dGeomID o1, dGeomID o2) { ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1); ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2); // const int N = 32; const int N = 10; // TODO: Magic number should be removed dContact contacts[N]; int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0])); if (n > 0) { ODEWorld *world = ODEWorld::get(); for (int i=0; i<n; i++) { dContact *c = &contacts[i]; c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce; // // Reflection of material parameters of the collided object // Fliction force should be regarded as average of contiguous material (???) // TODO: Calclation of fliction force sould be considered // if (odeobj1 && odeobj2) { c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0; c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0; c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0; c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0; c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0; c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0; c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0; } else { c->surface.mu = SPARTS_MU1; c->surface.mu2 = SPARTS_MU2; c->surface.slip1 = SPARTS_SLIP1; c->surface.slip2 = SPARTS_SLIP2; c->surface.soft_erp = SPARTS_ERP; c->surface.soft_cfm = SPARTS_CFM; c->surface.bounce = SPARTS_BOUNCE; } dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c); dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2)); } } }
void StaticEnvironment ( bool& do_colide, bool bo1, dContact& c, SGameMtl* material_1, SGameMtl* material_2 ) { dJointID contact_joint = dJointCreateContact(0, ContactGroup, &c); if(bo1) { ((CPHActivationShape*)(retrieveGeomUserData(c.geom.g1)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint); dJointAttach (contact_joint, dGeomGetBody(c.geom.g1), 0); } else { ((CPHActivationShape*)(retrieveGeomUserData(c.geom.g2)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint); dJointAttach (contact_joint, 0, dGeomGetBody(c.geom.g2)); } do_colide=false; }