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) { // 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()); } } } }
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)); } }
//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) ); } } }
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); } } }
/*** コールバック関数 ***/ 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); } } } }
static void nearCallback (void *, 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 odeCollisionCallback(void *data, dGeomID o1, dGeomID o2) { RobogenConfig *config = static_cast<RobogenConfig*>(data); // 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.slip1 = 0.01; contact[i].surface.slip2 = 0.01; contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2; contact[i].surface.mu = config->getTerrainConfig()->getFriction(); contact[i].surface.soft_erp = 0.96; contact[i].surface.soft_cfm = 0.01; } int collisionCounts = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); if (collisionCounts != 0) { for (int i = 0; i < collisionCounts; i++) { dJointID c = dJointCreateContact(odeWorld, odeContactGroup, contact + i); dJointAttach(c, b1, b2); } } }
void PhysicsSim::ode_nearCallback (void *data, dGeomID o1, dGeomID o2) { PhysicsSim *me = static_cast<PhysicsSim*>(data); int i; // 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))) { OscObject *p1 = static_cast<OscObject*>(dGeomGetData(o1)); OscObject *p2 = static_cast<OscObject*>(dGeomGetData(o2)); if (p1 && p2) { bool co1 = p1->collidedWith(p2, me->m_counter); bool co2 = p2->collidedWith(p1, me->m_counter); if ( (co1 || co2) && me->m_collide.m_value ) { lo_send(address_send, "/world/collide", "ssf", p1->c_name(), p2->c_name(), (double)(p1->m_velocity - p2->m_velocity).length()); } // TODO: this strategy will NOT work for multiple collisions between same objects!! } for (i=0; i<numc; i++) { dJointID c = dJointCreateContact (me->m_odeWorld, me->m_odeContactGroup, contact+i); dJointAttach (c,b1,b2); } } }
void nearCallback(void *data, dGeomID o1, dGeomID o2) { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return; CallbackParam *cp = reinterpret_cast<CallbackParam*>(data); const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2); if (maxContacts <= 0) return; dContact *contact = new dContact[maxContacts]; for (unsigned int i = 0; i < maxContacts; ++i) cp->env->setupContact(o1, o2, contact[i]); if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact))) { for (int i = 0; i < numc; ++i) { dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i); dJointAttach(c, b1, b2); bool valid = cp->env->isValidCollision(o1, o2, contact[i]); if (!valid) cp->collision = true; if (cp->env->verboseContacts_) { OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"), cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str()); } } } delete[] contact; }
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return; // body of the whole vehicle dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody; unsigned long geom1Categories = dGeomGetCategoryBits(o1); // speeds of the belts const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity(); const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity(); dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now if (geom1Categories & Category::LEFT) { beltSpeed = leftBeltSpeed; } else { beltSpeed = rightBeltSpeed; } // the desired linear and angular speeds (set by desired track velocities) const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2; const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance; // radius of the turn the robot is doing const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ? dInfinity : // is driving straight ((fabs(linearSpeed) < 0.1) ? 0 : // is rotating about a single point linearSpeed / angularSpeed // general movement ); dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case { // compute the center of rotation dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal); dCopyVector3(centerOfRotation, yAxisGlobal); // make the unit vector as long as we need (and change orientation if needed; the radius is a signed number) dScaleVector3(centerOfRotation, desiredRotationRadiusSigned); const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody); dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos); } int maxContacts = 20; dContact contact[maxContacts]; int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)); for(size_t i = 0; i < numContacts; i++) { dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos); dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal); if (beltSpeed > 0) { dNegateVector3(beltDirection); } if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation); // the friction force should be perpendicular to COR2Contact dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact); const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1; // contactInVehiclePos[0] > 0 means the contact is in the front part of the track if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) != sgn(contactInVehiclePos[0]) * linearSpeedSignum) { dNegateVector3(contact[i].fdir1); } } else { // straight drive dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal); if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) { dNegateVector3(contact[i].fdir1); } } // use friction direction and motion1 to simulate the track movement contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2; contact[i].surface.mu = 0.5; contact[i].surface.mu2 = 10; // the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors) contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07; // friction force visualization dMatrix3 forceRotation; dVector3 vec; dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec); dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]); posr data; dCopyVector3(data.pos, contact[i].geom.pos); dCopyMatrix4x3(data.R, forceRotation); forces.push_back(data); dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]); dJointAttach(c, b1, b2); if(!isValidCollision(o1, o2, contact[i])) this->badCollision = true; if(config.contact_grouser_terrain.debug) this->contacts.push_back(contact[i].geom); } }
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); } } } }
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); // } // } //} } } } }
/** * \brief In this function the collision handling from ode is performed. * * pre: * - world_init = true * - o1 and o2 are regular geoms * * post: * - if o1 or o2 was a Space, called SpaceCollide and exit * - otherwise tested if the geoms collide and created a contact * joint if so. * * A lot of the code is uncommented in this function. This * code maybe used later to handle sensors or other special cases * in the simulation. */ void WorldPhysics::nearCallback (dGeomID o1, dGeomID o2) { int i; int numc; //up to MAX_CONTACTS contact per Box-box //dContact contact[MAX_CONTACTS]; dVector3 v1, v; //dMatrix3 R; dReal dot; if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) { /// test if a space is colliding with something dSpaceCollide2(o1,o2,this,& WorldPhysics::callbackForward); return; } /// exit without doing anything if the two bodies are connected by a joint dBodyID b1=dGeomGetBody(o1); dBodyID b2=dGeomGetBody(o2); geom_data* geom_data1 = (geom_data*)dGeomGetData(o1); geom_data* geom_data2 = (geom_data*)dGeomGetData(o2); // test if we have a ray sensor: if(geom_data1->ray_sensor) { dContact contact; if(geom_data1->parent_geom == o2) { return; } if(geom_data1->parent_body == dGeomGetBody(o2)) { return; } numc = dCollide(o2, o1, 1|CONTACTS_UNIMPORTANT, &(contact.geom), sizeof(dContact)); if(numc) { if(contact.geom.depth < geom_data1->value) geom_data1->value = contact.geom.depth; ray_collision = 1; } return; } else if(geom_data2->ray_sensor) { dContact contact; if(geom_data2->parent_geom == o1) { return; } if(geom_data2->parent_body == dGeomGetBody(o1)) { return; } numc = dCollide(o2, o1, 1|CONTACTS_UNIMPORTANT, &(contact.geom), sizeof(dContact)); if(numc) { if(contact.geom.depth < geom_data2->value) geom_data2->value = contact.geom.depth; ray_collision = 1; } return; } if(b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return; if(!b1 && !b2 && !geom_data1->ray_sensor && !geom_data2->ray_sensor) return; int maxNumContacts = 0; if(geom_data1->c_params.max_num_contacts < geom_data2->c_params.max_num_contacts) { maxNumContacts = geom_data1->c_params.max_num_contacts; } else { maxNumContacts = geom_data2->c_params.max_num_contacts; } dContact *contact = new dContact[maxNumContacts]; //for granular test //if( (plane != o2) && (plane !=o1)) return ; /* /// we use the geomData to handle some special cases void* geom_data1 = dGeomGetData(o1); void* geom_data2 = dGeomGetData(o2); /// one case is, that we don't wont to handle a collision between some special /// geoms beweet each other and the ground if((geom_data1 && ((robot_geom*)geom_data1)->type & 16)) { if(plane == o2) return; if((geom_data2 && ((robot_geom*)geom_data2)->type & 16)) return; } else if((geom_data2 && ((robot_geom*)geom_data2)->type & 16) && (plane == o1)) return; /// an other case is a ray geom that we use simulate ray sensors /// this geom has to be handled in a different way if((geom_data1 && ((robot_geom*)geom_data1)->type & 8) || (geom_data2 && ((robot_geom*)geom_data2)->type & 8)) { int n; const int N = MAX_CONTACTS; dContactGeom contact[N]; n = dCollide (o2,o1,N,contact,sizeof(dContactGeom)); if (n > 0) { //const dReal ss[3] = {1,0.01,0.01}; for (i=0; i<n; i++) { contact[i].pos[2] += Z_OFFSET; if(contact[i].depth > 0.01){ if(geom_data1 && ((robot_geom*)geom_data1)->type & 8) ((robot_geom*)geom_data1)->i_length = contact[0].depth; if(geom_data2 && ((robot_geom*)geom_data2)->type & 8) ((robot_geom*)geom_data2)->i_length = contact[0].depth; } } } return; } */ // frist we set the softness values: contact[0].surface.mode = dContactSoftERP | dContactSoftCFM; contact[0].surface.soft_cfm = (geom_data1->c_params.cfm + geom_data2->c_params.cfm)/2; contact[0].surface.soft_erp = (geom_data1->c_params.erp + geom_data2->c_params.erp)/2; // then check if one of the geoms want to use the pyramid approximation if(geom_data1->c_params.approx_pyramid || geom_data2->c_params.approx_pyramid) contact[0].surface.mode |= dContactApprox1; // Then check the friction for both directions contact[0].surface.mu = (geom_data1->c_params.friction1 + geom_data2->c_params.friction1)/2; contact[0].surface.mu2 = (geom_data1->c_params.friction2 + geom_data2->c_params.friction2)/2; if(contact[0].surface.mu != contact[0].surface.mu2) contact[0].surface.mode |= dContactMu2; // check if we have to calculate friction direction1 if(geom_data1->c_params.friction_direction1 || geom_data2->c_params.friction_direction1) { // here the calculation becomes more complicated // maybe we should make some restrictions // so -> we only use friction motion in friction direction 1 // the friction motion is only set if a local vector for friction // direction 1 is given // the steps for the calculation: // 1. rotate the local vectors to global coordinates // 2. scale the vectors to the length of the motion if given // 3. vector 3 = vector 1 - vector 2 // 4. get the length of vector 3 // 5. set vector 3 as friction direction 1 // 6. set motion 1 to the length contact[0].surface.mode |= dContactFDir1; if(!geom_data2->c_params.friction_direction1) { // get the orientation of the geom //dGeomGetQuaternion(o1, v); //dRfromQ(R, v); // copy the friction direction v1[0] = geom_data1->c_params.friction_direction1->x(); v1[1] = geom_data1->c_params.friction_direction1->y(); v1[2] = geom_data1->c_params.friction_direction1->z(); // translate the friction direction to global coordinates // and set friction direction for contact //dMULTIPLY0_331(contact[0].fdir1, R, v1); contact[0].fdir1[0] = v1[0]; contact[0].fdir1[1] = v1[1]; contact[0].fdir1[2] = v1[2]; if(geom_data1->c_params.motion1) { contact[0].surface.mode |= dContactMotion1; contact[0].surface.motion1 = geom_data1->c_params.motion1; } } else if(!geom_data1->c_params.friction_direction1) { // get the orientation of the geom //dGeomGetQuaternion(o2, v); //dRfromQ(R, v); // copy the friction direction v1[0] = geom_data2->c_params.friction_direction1->x(); v1[1] = geom_data2->c_params.friction_direction1->y(); v1[2] = geom_data2->c_params.friction_direction1->z(); // translate the friction direction to global coordinates // and set friction direction for contact //dMULTIPLY0_331(contact[0].fdir1, R, v1); contact[0].fdir1[0] = v1[0]; contact[0].fdir1[1] = v1[1]; contact[0].fdir1[2] = v1[2]; if(geom_data2->c_params.motion1) { contact[0].surface.mode |= dContactMotion1; contact[0].surface.motion1 = geom_data2->c_params.motion1; } } else { // the calculation steps as mentioned above fprintf(stderr, "the calculation for friction directen set for both nodes is not done yet.\n"); } } // then check for fds if(geom_data1->c_params.fds1 || geom_data2->c_params.fds1) { contact[0].surface.mode |= dContactSlip1; contact[0].surface.slip1 = (geom_data1->c_params.fds1 + geom_data2->c_params.fds1); } if(geom_data1->c_params.fds2 || geom_data2->c_params.fds2) { contact[0].surface.mode |= dContactSlip2; contact[0].surface.slip2 = (geom_data1->c_params.fds2 + geom_data2->c_params.fds2); } if(geom_data1->c_params.bounce || geom_data2->c_params.bounce) { contact[0].surface.mode |= dContactBounce; contact[0].surface.bounce = (geom_data1->c_params.bounce + geom_data2->c_params.bounce); if(geom_data1->c_params.bounce_vel > geom_data2->c_params.bounce_vel) contact[0].surface.bounce_vel = geom_data1->c_params.bounce_vel; else contact[0].surface.bounce_vel = geom_data2->c_params.bounce_vel; } for (i=1;i<maxNumContacts;i++){ contact[i] = contact[0]; } numc=dCollide(o1,o2, maxNumContacts, &contact[0].geom,sizeof(dContact)); if(numc){ dJointFeedback *fb; draw_item item; Vector contact_point; num_contacts++; if(create_contacts) { fb = 0; item.id = 0; item.type = DRAW_LINE; item.draw_state = DRAW_STATE_CREATE; item.point_size = 10; item.myColor.r = 1; item.myColor.g = 0; item.myColor.b = 0; item.myColor.a = 1; item.label = ""; item.t_width = item.t_height = 0; item.texture = ""; item.get_light = 0; for(i=0;i<numc;i++){ item.start.x() = contact[i].geom.pos[0]; item.start.y() = contact[i].geom.pos[1]; item.start.z() = contact[i].geom.pos[2]; item.end.x() = contact[i].geom.pos[0] + contact[i].geom.normal[0]; item.end.y() = contact[i].geom.pos[1] + contact[i].geom.normal[1]; item.end.z() = contact[i].geom.pos[2] + contact[i].geom.normal[2]; draw_intern.push_back(item); if(geom_data1->c_params.friction_direction1 || geom_data2->c_params.friction_direction1) { v[0] = contact[i].geom.normal[0]; v[1] = contact[i].geom.normal[1]; v[2] = contact[i].geom.normal[2]; dot = dDOT(v, contact[i].fdir1); dOPEC(v, *=, dot); contact[i].fdir1[0] -= v[0]; contact[i].fdir1[1] -= v[1]; contact[i].fdir1[2] -= v[2]; dNormalize3(contact[0].fdir1); } contact[0].geom.depth += (geom_data1->c_params.depth_correction + geom_data2->c_params.depth_correction); if(contact[0].geom.depth < 0.0) contact[0].geom.depth = 0.0; dJointID c=dJointCreateContact(world,contactgroup,contact+i); dJointAttach(c,b1,b2); geom_data1->num_ground_collisions += numc; geom_data2->num_ground_collisions += numc; contact_point.x() = contact[i].geom.pos[0]; contact_point.y() = contact[i].geom.pos[1]; contact_point.z() = contact[i].geom.pos[2]; geom_data1->contact_ids.push_back(geom_data2->id); geom_data2->contact_ids.push_back(geom_data1->id); geom_data1->contact_points.push_back(contact_point); geom_data2->contact_points.push_back(contact_point); //if(dGeomGetClass(o1) == dPlaneClass) { fb = 0; if(geom_data2->sense_contact_force) { fb = (dJointFeedback*)malloc(sizeof(dJointFeedback)); dJointSetFeedback(c, fb); contact_feedback_list.push_back(fb); geom_data2->ground_feedbacks.push_back(fb); geom_data2->node1 = false; } //else if(dGeomGetClass(o2) == dPlaneClass) { if(geom_data1->sense_contact_force) { if(!fb) { fb = (dJointFeedback*)malloc(sizeof(dJointFeedback)); dJointSetFeedback(c, fb); contact_feedback_list.push_back(fb); } geom_data1->ground_feedbacks.push_back(fb); geom_data1->node1 = true; } } } }
// The "Near Callback". ODE calls this during collision detection to // decide whether 2 geoms collide, and if yes, to generate Contact // joints between the 2 involved rigid bodies. void CLevel::ODENearCallback(void* data, dGeomID o1, dGeomID o2) { CLevel* Level = (CLevel*)data; Level->statsNumNearCallbackCalled++; // handle sub-spaces if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) { // collide a space with something Level->statsNumSpaceCollideCalled++; dSpaceCollide2(o1, o2, data, &ODENearCallback); return; } // handle shape/shape collisions dBodyID Body1 = dGeomGetBody(o1); dBodyID Body2 = dGeomGetBody(o2); n_assert(Body1 != Body2); // do nothing if 2 bodies are connected by a joint if (Body1 && Body2 && dAreConnectedExcluding(Body1, Body2, dJointTypeContact)) { // FIXME: bodies are connected, check if jointed-collision is enabled // for both bodies (whether 2 bodies connected by a joint should // collide or not, for this, both bodies must have set the // CollideConnected() flag set. CRigidBody* PhysicsBody0 = (CRigidBody*)dBodyGetData(Body1); n_assert(PhysicsBody0 && PhysicsBody0->IsInstanceOf(CRigidBody::RTTI)); if (!PhysicsBody0->CollideConnected) return; CRigidBody* PhysicsBody1 = (CRigidBody*) dBodyGetData(Body2); n_assert(PhysicsBody1 && PhysicsBody1->IsInstanceOf(CRigidBody::RTTI)); if (!PhysicsBody1->CollideConnected) return; } CShape* Shape1 = CShape::GetShapeFromGeom(o1); CShape* Shape2 = CShape::GetShapeFromGeom(o2); n_assert(Shape1 && Shape2); n_assert(!((Shape1->GetType() == CShape::Mesh) && (Shape2->GetType() == CShape::Mesh))); Level->statsNumCollideCalled++; // initialize Contact array bool MaterialsValid = (Shape1->GetMaterialType() != InvalidMaterial && Shape2->GetMaterialType() != InvalidMaterial); float Friction; float Bounce; if (MaterialsValid) { Friction = Physics::CMaterialTable::GetFriction(Shape1->GetMaterialType(), Shape2->GetMaterialType()); Bounce = Physics::CMaterialTable::GetBounce(Shape1->GetMaterialType(), Shape2->GetMaterialType()); } else { Friction = 0.f; Bounce = 0.f; } static dContact Contact[MaxContacts]; for (int i = 0; i < MaxContacts; i++) { Contact[i].surface.mode = dContactBounce | dContactSoftCFM; Contact[i].surface.mu = Friction; Contact[i].surface.mu2 = 0.0f; Contact[i].surface.bounce = Bounce; Contact[i].surface.bounce_vel = 1.0f; Contact[i].surface.soft_cfm = 0.0001f; Contact[i].surface.soft_erp = 0.2f; } // do collision detection int CollisionCount = dCollide(o1, o2, MaxContacts, &(Contact[0].geom), sizeof(dContact)); //???!!!set in OnCollision?! Shape1->SetNumCollisions(Shape1->GetNumCollisions() + CollisionCount); Shape2->SetNumCollisions(Shape2->GetNumCollisions() + CollisionCount); if (CollisionCount > 0) { Level->statsNumCollided++; if (!Shape1->OnCollide(Shape2) || !Shape2->OnCollide(Shape1)) return; // create a Contact for each collision for (int i = 0; i < CollisionCount; i++) dJointAttach( dJointCreateContact(Level->ODEWorldID, Level->ContactJointGroup, Contact + i), Body1, Body2); } //???sounds here or in sound system on event? // FIXME: not really ready for prime Time // TODO: implement roll / slide sounds (sounds that stop as soon as the Contact is gone) // roll / slide sounds also need to consider relative velocity nTime Now = GameSrv->GetTime(); if (CollisionCount != 0) { CShape* Key[2]; // build an unique Key for every colliding shape combination if (Shape1 < Shape2) { Key[0] = Shape1; Key[1] = Shape2; } else { Key[0] = Shape2; Key[1] = Shape1; } if ((Now - Level->CollisionSounds.At(Key, sizeof(Key))) > 0.25f) { CRigidBody* Rigid1 = Shape1->GetRigidBody(); CRigidBody* Rigid2 = Shape2->GetRigidBody(); if ((!Rigid1 || !Rigid1->IsEnabled()) && (!Rigid2 || !Rigid2->IsEnabled())) return; nString Sound; if (MaterialsValid) Physics::CMaterialTable::GetCollisionSound(Shape1->GetMaterialType(), Shape2->GetMaterialType()); if (Sound.IsValid()) { vector3 Normal; CPhysicsServer::OdeToVector3(Contact[0].geom.normal, Normal); vector3 Velocity = Rigid1 ? Rigid1->GetLinearVelocity() : vector3(0.0f, 0.0f, 0.0f); if (Rigid2) Velocity -= Rigid2->GetLinearVelocity(); float Volume = n_saturate((-Velocity.dot(Normal) - 0.3f) / 4.0f); if (Volume > 0.0f) { Ptr<Event::PlaySound> Evt = Event::PlaySound::Create(); Evt->Name = Sound; Evt->Position.set(Contact[0].geom.pos[0], Contact[0].geom.pos[1], Contact[0].geom.pos[2]); Evt->Volume = Volume; EventMgr->FireEvent(*Evt); Level->CollisionSounds.At(Key, sizeof(Key)) = Now; } } } } }
static void nearCallback(void *data, dGeomID o1, dGeomID o2) { dBodyID b1 = dGeomGetBody(o1), b2 = dGeomGetBody(o2); if (b1 && b2 && dAreConnectedExcluding(b1,b2,dJointTypeContact)) return; bool neitherCollidingObjectsAreTheGround = false; if ((o1 != ground) && (o2 != ground)) neitherCollidingObjectsAreTheGround=true; //if(neitherCollidingObjectsAreTheGround) return; //only do collisions between something and the ground bool itsAFootTouching = false; if(o1 ==ground){ printf("this code get run?"); exit(30); } if(o2 ==ground){ for(int segment = 0; segment < BODY_SEGMENTS; ++segment) { for(int z=0;z<num_legs;z++){ if (o1==leg[segment][z][2].geom){ itsAFootTouching = true; thisLegIsTouching[segment][z]=1; } } } } if(!neitherCollidingObjectsAreTheGround and !itsAFootTouching ) someNonFootIsTouchingTheGround = true; static const int N = 20; dContact contact[N]; int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact)); //if(neitherCollidingObjectsAreTheGround) return; //only do collisions between something and the ground /* for(int segment = 0; segment < BODY_SEGMENTS; ++segment) { for(int legNUM = 0; legNUM < num_legs; ++legNUM) { for(int jointNUM = 0; jointNUM < num_joints; ++jointNUM) { if(o1==leg[segment][legNUM][jointNUM].geom) cerr <<"LEG o1: "<<segment << " " << legNUM << " "<<jointNUM <<endl; if(o2==leg[segment][legNUM][jointNUM].geom) cerr <<"LEG o2: "<<segment << " " << legNUM << " "<<jointNUM <<endl; } } if(o1 == torso[segment].geom) cerr <<"TORSO o1: "<<segment <<endl; if(o2 == torso[segment].geom) cerr <<"TORSO o2: "<<segment <<endl; } */ if (n > 0) { for (int i=0; i<n; i++) { // cerr<<" i = " << i << " n = " << n<<endl; contact[i].surface.mode = dContactSoftERP | dContactSoftCFM; contact[i].surface.mu = dInfinity; //2.0; contact[i].surface.soft_erp = 0.8; contact[i].surface.soft_cfm = 1e-5; // cerr<<"0\n"; dJointID c = dJointCreateContact(world,contactgroup,&contact[i]); // cerr<<"1\n"; dJointAttach(c,b1,b2); // cerr<<"2\n"; } // cerr<<"3\n"; } // cerr<<"4\n"; // if(neitherCollidingObjectsAreTheGround) return; //only do collisions between something and the ground }
void SimWorld::collideGeoms(void* data,dGeomID object_1_ID,dGeomID object_2_ID) { dContact contact[MAX_CONTACTS]; SimWorld* world_object = static_cast<SimWorld*>(data); dBodyID body_1 = dGeomGetBody(object_1_ID); dBodyID body_2 = dGeomGetBody(object_2_ID); if (body_1 == NULL) { body_1 = body_2; body_2 = NULL; dGeomID object_3_ID = object_1_ID; object_1_ID = object_2_ID; object_2_ID = object_3_ID; } // Don't use body-body collisions for now. if (!world_object->self_collide && (body_2!=NULL)) return; // exit without doing anything if the two bodies are connected by a joint if (body_1 && body_2 && dAreConnectedExcluding (body_1,body_2,dJointTypeContact)) return; // If all bodies are kinematic, we don't need to do anything here since // adding constraints has no effect. if ( ((body_1==NULL)||dBodyIsKinematic(body_1)) && ((body_2==NULL)||dBodyIsKinematic(body_2))) return; // Exclude collisions between the upper arm and torso. if (((object_1_ID==world_object->body->geometries[CapBody::UP_TORSO_BODY])|| (object_2_ID==world_object->body->geometries[CapBody::UP_TORSO_BODY])) && ((object_1_ID==world_object->body->geometries[CapBody::RUP_ARM_BODY])|| (object_2_ID==world_object->body->geometries[CapBody::RUP_ARM_BODY])|| (object_1_ID==world_object->body->geometries[CapBody::LUP_ARM_BODY])|| (object_2_ID==world_object->body->geometries[CapBody::LUP_ARM_BODY]))) return; contact[0].surface.mode = dContactSoftCFM | ((body_1&&body_2)?0:dContactApprox1); //Surfaces are just a little squishy contact[0].surface.soft_cfm = (body_1&&body_2)?0.001:world_object->ground_squish; // contact[0].surface.bounce_vel = 0.01; contact[0].surface.bounce = 0.25; contact[0].surface.mu = (body_1&&body_2)?1:world_object->ground_friction; // Strong friction with ground for (int ii=1;ii<MAX_CONTACTS;++ii) { contact[ii].surface.mode = contact[0].surface.mode; contact[ii].surface.soft_cfm = contact[0].surface.soft_cfm; contact[ii].surface.bounce_vel = contact[0].surface.bounce_vel; contact[ii].surface.bounce = contact[0].surface.bounce; contact[ii].surface.mu = contact[0].surface.mu; } int numC = dCollide (object_1_ID,object_2_ID,MAX_CONTACTS,&contact[0].geom,sizeof(dContact) ); for (int ii=0;ii<numC;++ii) { dJointID cc = dJointCreateContact (world_object->world,world_object->contact_group,&contact[ii]); dJointAttach(cc,body_1,body_2); if ((body_2==NULL)&&(world_object->number_active_ground_contacts<MAX_GROUND_FEEDBACK)) { dJointSetFeedback(cc,&(world_object->ground_feedback[world_object->number_active_ground_contacts])); dOPE(world_object->contact_point[world_object->number_active_ground_contacts],=,contact[ii].geom.pos); if ((body_1==world_object->body->body_segments[CapBody::L_HEEL_BODY]) ||(body_1==world_object->body->body_segments[CapBody::L_TARSAL_BODY])) //||(body_1==world_object->body->bodies[CapBody::L_TOE_BODY])) { world_object->left_foot_feedback.push_back(world_object->number_active_ground_contacts); } else if ((body_1==world_object->body->body_segments[CapBody::R_HEEL_BODY]) ||(body_1==world_object->body->body_segments[CapBody::R_TARSAL_BODY])) //||(body_1==world_object->body->bodies[CapBody::R_TOE_BODY])) { world_object->right_foot_feedback.push_back(world_object->number_active_ground_contacts); } world_object->number_active_ground_contacts+=1; } }