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]; // 第１摩擦方向の設定(x軸成分)
contact[i].fdir1[1] = tmp_fdir[1]; // 第１摩擦方向の設定(y軸成分)
contact[i].fdir1[2] = tmp_fdir[2]; // 第１摩擦方向の設定(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;
}
}