static void collideCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
// const int N = 32;
const int N = 10; // TODO: Magic number should be removed
dContact contacts[N];
int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0]));
if (n > 0) {
ODEWorld *world = ODEWorld::get();
for (int i=0; i<n; i++) {
dContact *c = &contacts[i];
c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//
// Reflection of material parameters of the collided object
// Fliction force should be regarded as average of contiguous material (???)
// TODO: Calclation of fliction force sould be considered
//
if (odeobj1 && odeobj2) {
c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0;
c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0;
c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0;
c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0;
c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0;
c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0;
c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0;
}
else {
c->surface.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP;
c->surface.soft_cfm = SPARTS_CFM;
c->surface.bounce = SPARTS_BOUNCE;
}
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2));
}
}
}
static void nearCB(void *data, dGeomID o1, dGeomID o2)
{
ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1);
ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2);
/*
SSimRobotEntity *ent1 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o1);
SSimRobotEntity *ent2 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o2);
if(ent1 != NULL && ent2 != NULL && ent1->name() == ent2->name()){
//LOG_MSG(("name (%s, %s)",ent1->name().c_str(), ent2->name().c_str()));
return;
}
*/
SParts *p1 = NULL;
SParts *p2 = NULL;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 == b2) {
return;
}
ODEWorld *world = ODEWorld::get();
dGeomID ground = world->getGround();
if (b1 && b2) {
if (dAreConnected(b1, b2)) {
return;
}
}
if (b1) {
void *d = dBodyGetData(b1);
if (d) {
p1 = (SParts*)d;
if (p1->isBlind()) { return; }
}
}
if (b2) {
void *d = dBodyGetData(b2);
if (d) {
p2 = (SParts*)d;
if (p2->isBlind()) { return; }
}
}
if (p1 && p2 && p1->sameParent(*p2)) { return; }
if (dGeomIsSpace(o1) && dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &collideCB);
return;
}
#define F_SCHOLAR(V) sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2])
static dJointFeedback fb;
#define MAX_COLLISION 32
const int N = MAX_COLLISION;
dContact contacts[N];
int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0]));
if (n > 0) {
ODEWorld *world = ODEWorld::get();
for (int i=0; i<n; i++) {
dContact *c = &contacts[i];
dContactGeom &g = c->geom;
if (p1 && p2) {
#if 0
LOG_SYS(("Collision #%d/%d %s(geomID=%d) <-> %s(geomID=%d)", i, n,
p1->getParent()->name(), o1,
p2->getParent()->name(), o2));
LOG_SYS(("\tpos = (%f, %f, %f)", g.pos[0], g.pos[1], g.pos[2]));
LOG_SYS(("\tnormal = (%f, %f, %f)", g.normal[0], g.normal[1], g.normal[2]));
LOG_SYS(("\tfdir = (%f, %f, %f)", c->fdir1[0], c->fdir1[1], c->fdir1[2]));
LOG_SYS(("\tdepth = %f", g.depth));
#endif
const char *name1 = p1->getParent()->name();
const char *name2 = p2->getParent()->name();
std::string key = chash_key(name1, name2);
if (key.length() <= 0) { continue; }
if (chash_find(key)) { continue; }
s_chash[key] = true;
s_collisions.push_back(ODEWorld::Collision(name1, name2, p1->name(), p2->name()));
// Set the collision flag to be ON
p1->setOnCollision(true);
p2->setOnCollision(true);
}
//c->surface.mode = dContactBounce;
c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//c->surface.mode = dContactSlip1 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce;
//
// Reflection of material parameters of the collided object
// Fliction force should be regarded as average of contiguous material (???)
// TODO: Calclation of fliction force sould be considered
//
#if 0
if (odeobj1 && odeobj2) {
c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0;
c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0;
c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0;
c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0;
c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0;
c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0;
c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0;
}
else {
c->surface.bounce_vel = world->getCollisionParam("bounce_vel");
c->surface.bounce = world->getCollisionParam("bounce");
c->surface.mu = world->getCollisionParam("mu");
c->surface.mu2 = world->getCollisionParam("mu2");
c->surface.slip1 = world->getCollisionParam("slip1");
c->surface.slip2 = world->getCollisionParam("slip2");
c->surface.soft_erp = world->getCollisionParam("soft_erp");
c->surface.soft_cfm = world->getCollisionParam("soft_cfm");
}
#else
c->surface.mu = SPARTS_MU1;
c->surface.mu2 = SPARTS_MU2;
c->surface.slip1 = SPARTS_SLIP1;
c->surface.slip2 = SPARTS_SLIP2;
c->surface.soft_erp = SPARTS_ERP; // parameter for modify the error of Joint position (0..1); if it is 1, modification will be perfect
c->surface.soft_cfm = SPARTS_CFM; // If this value=0, joint velocity is strange
c->surface.bounce = SPARTS_BOUNCE; // is a little smaller than ball
c->surface.bounce_vel = 0.0;
#endif
dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c);
//dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2)); //by MSI
dJointAttach(cj, dGeomGetBody(c->geom.g1), dGeomGetBody(c->geom.g2)); // by Demura.net
#if 0
if (p1 && p2) {
dJointSetFeedback(cj, &fb);
dJointFeedback *pfb = dJointGetFeedback(cj);
if (F_SCHOLAR(pfb->f1) > 1.0) {
LOG_SYS(("\tF1 = (%f, %f, %f)", pfb->f1[0], pfb->f1[1], pfb->f1[2]));
LOG_SYS(("\tF2 = (%f, %f, %f)\n", pfb->f2[0], pfb->f2[1], pfb->f2[2]));
}
}
#endif
}
}
}
