void CDynamics3DEngine::ManageCloseGeomsAddingContactJoints(SGeomCheckData* ps_data, dGeomID t_geom1, dGeomID t_geom2) {
/*
* The passed geometries can be spaces or shapes.
* Depending on the type of each passed geometry, we do something different
*/
if(dGeomIsSpace(t_geom1) || dGeomIsSpace(t_geom2)) {
/*
* At least one of the two geometries is a space.
* We need to open up the spaces to get to the basic elements.
*/
dSpaceCollide2(t_geom1, t_geom2, ps_data, &ManageCloseGeomsAddingContactJointsCallback);
}
else {
/* Both geoms are shapes. Let's check for collisions among them */
size_t unContacts = dCollide(t_geom1, t_geom2, m_unMaxContacts, &m_ptContacts->geom, sizeof(dContact));
/* If no collision is detected, we're done */
if(unContacts == 0) return;
/* Otherwise, let's add contact joints in each contact point detected */
/* Get the body ids */
dBodyID tBody1 = dGeomGetBody(t_geom1);
dBodyID tBody2 = dGeomGetBody(t_geom2);
/* Create a buffer for the contact joints */
dJointID tContactJoint;
/* Go through the contact points and create the joints */
for(UInt32 i = 0; i < unContacts; ++i) {
tContactJoint = dJointCreateContact(m_tWorldID, m_tContactJointGroupID, m_ptContacts+i);
dJointAttach(tContactJoint, tBody1, tBody2);
}
}
}
void nearCallback(void *data, dGeomID o1, dGeomID o2) {
State* state = (State*)data;
if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &nearCallback);
if(dGeomIsSpace(o1))
dSpaceCollide((dSpaceID)o1, data, &nearCallback);
if(dGeomIsSpace(o2))
dSpaceCollide((dSpaceID)o2, data, &nearCallback);
} else {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
const int MAX_CONTACTS = 18;
dContact contact[MAX_CONTACTS];
for(int i = 0; i < MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = 2000;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.15;
}
if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) {
for(int i = 0; i < numc; i++) {
dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]);
dJointAttach(c, b1, b2);
}
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", o1,o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = 100.0;
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.02;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// main collision handling function
void PhysicsServer::checkCollision(void *data, dGeomID o1, dGeomID o2)
{
int i,n;
if( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) )
{
dSpaceCollide2( o1, o2, data, &collisionCallback );
}
else
{
int mode = 0;
double slip1 = 0;
double slip2 = 0;
// get bodies
dBodyID body1 = dGeomGetBody(o1);
dBodyID body2 = dGeomGetBody(o2);
// get geom data
PhysGeomData* pData1 = (PhysGeomData*)dGeomGetData(o1);
PhysGeomData* pData2 = (PhysGeomData*)dGeomGetData(o2);
// set contact params according to geom data
if (pData1 != NULL)
slip1 = pData1->slip;
if (pData2 != NULL)
slip2 = pData2->slip;
// set mode
if (slip1 != 0)
mode |= dContactSlip1;
if (slip2 != 0)
mode |= dContactSlip2;
static const int N = 8; // max number of contact points
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
for (i=0; i<n; i++)
{
contact[i].surface.mode = mode; /*dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM;*/
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = slip1;
contact[i].surface.slip2 = slip2;
// contact[i].surface.soft_erp = 0.7;
// contact[i].surface.soft_cfm = 0.1;
dJointID c = dJointCreateContact (_world, _contactgroup, &contact[i]);
dJointAttach (c, body1, body2);
}
}
}
}
void selfCollisionCallback(void *data, dGeomID o1, dGeomID o2)
{
ODERobot* robot = reinterpret_cast<ODERobot*>(data);
Assert(!dGeomIsSpace(o1) && !dGeomIsSpace(o2));
intptr_t link1 = (intptr_t)dGeomGetData(o1);
intptr_t link2 = (intptr_t)dGeomGetData(o2);
Assert(link1 >= 0 && (int)link1 < (int)robot->robot.links.size());
Assert(link2 >= 0 && (int)link2 < (int)robot->robot.links.size());
if(robot->robot.selfCollisions(link1,link2)==NULL) {
return;
}
int num = dCollide (o1,o2,max_contacts,gContactTemp,sizeof(dContactGeom));
vector<dContactGeom> vcontact(num);
int numOk = 0;
for(int i=0;i<num;i++) {
if(gContactTemp[i].g1 == o2 && gContactTemp[i].g2 == o1) {
printf("Swapping contact\n");
std::swap(gContactTemp[i].g1,gContactTemp[i].g2);
for(int k=0;k<3;k++) gContactTemp[i].normal[k]*=-1.0;
std::swap(gContactTemp[i].side1,gContactTemp[i].side2);
}
Assert(gContactTemp[i].g1 == o1);
Assert(gContactTemp[i].g2 == o2);
vcontact[numOk] = gContactTemp[i];
const dReal* n=vcontact[numOk].normal;
if(Sqr(n[0])+Sqr(n[1])+Sqr(n[2]) < 0.9 || Sqr(n[0])+Sqr(n[1])+Sqr(n[2]) > 1.2) {
printf("Warning, degenerate contact with normal %f %f %f\n",vcontact[numOk].normal[0],vcontact[numOk].normal[1],vcontact[numOk].normal[2]);
//continue;
}
numOk++;
}
if(numOk > 0) printf("%d self collision contacts between links %d and %d\n",numOk,(int)link1,(int)link2);
vcontact.resize(numOk);
if(kMergeContacts && numOk > 0) {
MergeContacts(vcontact,kContactPosMergeTolerance,kContactOriMergeTolerance);
}
if(vcontact.size() > 0) {
if(numOk != (int)vcontact.size())
//// The int type is not guaranteed to be big enough, use intptr_t
//cout<<numOk<<" contacts between env "<<(int)dGeomGetData(o2)<<" and body "<<(int)dGeomGetData(o1)<<" (clustered to "<<vcontact.size()<<")"<<endl;
cout<<numOk<<" contacts between link "<<(intptr_t)dGeomGetData(o2)<<" and link "<<(intptr_t)dGeomGetData(o1)<<" (clustered to "<<vcontact.size()<<")"<<endl;
gContacts.push_back(ODEContactResult());
gContacts.back().o1 = o1;
gContacts.back().o2 = o2;
swap(gContacts.back().contacts,vcontact);
}
}
//Auxiliary function to print info on a space
void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name)
{
int num_geoms = 0;
dGeomID geom_temp;
int geom_class_temp = 0;
dReal aabb[6];
dBodyID body_temp;
num_geoms = dSpaceGetNumGeoms(my_space);
printf("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms);
for (int i=0;i<=(num_geoms-1);i++){
geom_temp = dSpaceGetGeom (my_space, i);
geom_class_temp = dGeomGetClass(geom_temp);
printGeomClassAndNr(geom_class_temp,i);
if (!dGeomIsSpace(geom_temp)){
if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom
printf(" ID: %p, Coordinates:",geom_temp);
ICubSim::printPositionOfGeom(geom_temp);
dGeomGetAABB(geom_temp,aabb);
printf(" Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
if (geom_class_temp==8){ // trimesh
body_temp = dGeomGetBody(geom_temp);
printf(" Coordinates of associated body are:");
ICubSim::printPositionOfBody(body_temp);
}
} else {
dGeomGetAABB(geom_temp,aabb);
printf(" ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
}
}
}
}
/**
* Returns a dBody id from its DEF name.
* @param def DEF name of teh desired dBody
* @return a pointer to the dBody if found NULL otherwise
*/
dBodyID getBodyFromDEF(const char *def) {
//std::cout<<"Try to get body "<<def<<std::endl;
dGeomID geom = dWebotsGetGeomFromDEF(def);
if (! geom) {
printf("Fatal error: did not find dGeom for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
dBodyID body;
if (dGeomIsSpace(geom)){
body = dGeomGetBody(dSpaceGetGeom((dSpaceID)geom, 0));
std::cerr<<"I don't know why I have to do this"<<std::endl;
s_data->disablePhysics();
}else
body = dGeomGetBody(geom);
if (! body) {
printf("Fatal error: did not find dBody for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
return body;
}
void CDynamics3DEngine::ManageCloseGeomsCheckContactsOnly(SGeomCheckData* ps_data, dGeomID t_geom1, dGeomID t_geom2) {
/*
* The passed geometries can be spaces or shapes.
* Depending on the type of each passed geometry, we do something different
*/
if(dGeomIsSpace(t_geom1) || dGeomIsSpace(t_geom2)) {
/*
* At least one of the two geometries is a space.
* We need to open up the spaces to get to the basic elements.
*/
dSpaceCollide2(t_geom1, t_geom2, ps_data, &ManageCloseGeomsCheckContactsOnlyCallback);
}
else {
/* Both geoms are shapes. Let's check for collisions among them */
size_t unContacts = dCollide(t_geom1, t_geom2, m_unMaxContacts, &m_ptContacts->geom, sizeof(dContact));
/* Update the contact presence flag */
ps_data->AreContactsPresent = (unContacts == 0);
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = 50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
if (show_contacts)
{
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.12,0.12,0.12};
dsSetColorAlpha (0,0,1,0.5);
dsDrawBox (contact[i].geom.pos,RI,ss);
dReal *pos = contact[i].geom.pos;
dReal depth = contact[i].geom.depth;
dReal *norm = contact[i].geom.normal;
dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
dsSetColorAlpha (1,1,1,1);
dsDrawLine (contact[i].geom.pos, endp);
}
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", o1,o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
// fprintf(stderr,"testing geoms %p %p\n", o1, o2);
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
// Paranoia <-- not working for some people, temporarily removed for 0.6
//dIASSERT(dVALIDVEC3(contact[i].geom.pos));
//dIASSERT(dVALIDVEC3(contact[i].geom.normal));
//dIASSERT(!dIsNan(contact[i].geom.depth));
contact[i].surface.slip1 = 0.7;
contact[i].surface.slip2 = 0.7;
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
contact[i].surface.mu = 50.0; // was: dInfinity
contact[i].surface.soft_erp = 0.96;
contact[i].surface.soft_cfm = 0.04;
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
// The ODENearCallback for CShape::Collide.
//
// 31-May-05 floh invert contact normal if necessary, the contact normal
// in ODE always points into pShape1, however, we want
// the contact normal always to point away from the
// "other" object
void CShape::ODENearCallback(void* data, dGeomID o1, dGeomID o2)
{
CShape* pThis = (CShape*)data;
// collide geom with sub-space
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
dSpaceCollide2(o1, o2, data, &ODENearCallback);
return;
}
CShape* pShape1 = CShape::GetShapeFromGeom(o1);
CShape* pShape2 = CShape::GetShapeFromGeom(o2);
CShape* pOtherShape;
if (pShape1 == pThis) pOtherShape = pShape2;
else if (pShape2 == pThis) pOtherShape = pShape1;
else n_error("No self in collision");
if (CShape::CollideFilterSet->CheckShape(pOtherShape)) return;
// do collision detection, only check for one contact
dContactGeom ODEContact;
int CurrCollCount;
if (pShape1 == pThis) CurrCollCount = dCollide(o1, o2, 1, &ODEContact, sizeof(dContactGeom));
else CurrCollCount = dCollide(o2, o1, 1, &ODEContact, sizeof(dContactGeom));
CContactPoint ContactPt;
if (CurrCollCount > 0)
{
ContactPt.Position.set(ODEContact.pos[0], ODEContact.pos[1], ODEContact.pos[2]);
ContactPt.UpVector.set(ODEContact.normal[0], ODEContact.normal[1], ODEContact.normal[2]);
ContactPt.Material = pOtherShape->GetMaterialType();
ContactPt.Depth = ODEContact.depth;
CEntity* pEntity = pOtherShape->GetEntity();
if (pEntity) ContactPt.EntityID = pEntity->GetUniqueID();
CRigidBody* pRigidBody = pOtherShape->GetRigidBody();
if (pRigidBody) ContactPt.RigidBodyID = pRigidBody->GetUniqueID();
CShape::CollideContacts->Append(ContactPt);
}
}
void Simulator::CollisionChecking(dGeomID geom1, dGeomID geom2)
{
const bool isRobot1 = dGeomGetData(geom1) == NULL || dGeomGetData(geom1) == ((void *) &TYPE_ROBOT);
const bool isObstacle1 = dGeomGetData(geom1) == ((void *) &TYPE_OBSTACLE);
const bool isTerrain1 = dGeomGetData(geom1) == ((void *) &TYPE_TERRAIN);
const bool isRobot2 = dGeomGetData(geom2) == NULL || dGeomGetData(geom2) == ((void *) &TYPE_ROBOT);
const bool isObstacle2 = dGeomGetData(geom2) == ((void *) &TYPE_OBSTACLE);
const bool isTerrain2 = dGeomGetData(geom2) == ((void *) &TYPE_TERRAIN);
if((isObstacle1 && isObstacle2) || (isTerrain1 && isTerrain2))
return;
if(dGeomIsSpace(geom1) || dGeomIsSpace(geom2))
{
dSpaceCollide2(geom1, geom2, this, &CollisionCheckingCallbackFn);
return;
}
const int NR_CONTACTS = 3;
dContact contact[NR_CONTACTS];
if(int numc = dCollide(geom1, geom2, NR_CONTACTS, &contact[0].geom, sizeof(dContact)))
{
if((isRobot1 && isObstacle2) || (isRobot2 && isObstacle1))
m_collision = true;
for(int i = 0; i < numc; ++i)
{
contact[i].surface.mode = dContactSoftCFM | dContactApprox1;
contact[i].surface.mu = 0.6;
contact[i].surface.soft_cfm = 0.2;
dJointAttach(dJointCreateContact(m_world, m_contacts, &contact[i]),
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
static void Near(void *unused, dGeomID g1, dGeomID g2)
{
bool s1 = dGeomIsSpace(g1);
bool s2 = dGeomIsSpace(g2);
if (s1 || s2)
{
dSpaceCollide2(g1, g2, unused, &Near);
if (s1)
dSpaceCollide(dSpaceID(g1), unused, &Near);
if (s2)
dSpaceCollide(dSpaceID(g2), unused, &Near);
}
else
{
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
const int N = 42;
dContact contact[N];
for (int i = 0; i < N; ++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;
}
int n = dCollide(g1, g2, N, &contact->geom, sizeof(dContact));
for (int i = 0; i < n; ++i)
{
dJointID joint = dJointCreateContact(World, Group, contact+i);
dJointAttach(joint, b1, b2);
}
}
}
void Update(dGeomID geom)
{
union {
void *data;
Geom *obj;
};
data = dGeomGetData(geom);
if (obj) obj->Update();
if (dGeomIsSpace(geom))
{
Update(dSpaceID(geom));
}
}
static void staticCollisionCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
{
ASSERT(!dGeomIsSpace(geom1));
ASSERT(!dGeomIsSpace(geom2));
ASSERT(dGeomGetBody(geom1) || dGeomGetBody(geom2));
dContact contact[1];
if(dCollide(geom1, geom2, 1, &contact[0].geom, sizeof(dContact)) < 1)
return;
dGeomID geom = geom2;
dBodyID bodyId = dGeomGetBody(geom2);
if(!bodyId)
{
bodyId = dGeomGetBody(geom1);
geom = geom1;
}
const dReal* pos = dGeomGetPosition(geom);
float sqrDistance = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - callback->cameraPos).squareAbs();
if(!callback->closestBody || sqrDistance < callback->closestSqrDistance)
{
callback->closestBody = (Body*)dBodyGetData(bodyId);
callback->closestSqrDistance = sqrDistance;
}
}
void insertGeomInMonitored(dGeomID geom, amarsi::Limbs limb){
if(!dGeomIsSpace(geom)){
s_data->addMonitoredBody(geom,limb);
//std::cerr<<" Directly found geom of type "<<dGeomGetClass(toe)<<std::endl;
return;
}
//std::cerr<<"IS a space, explore childrens"<<std::endl;
dSpaceID geomSpace=(dSpaceID)(geom);
for (int ii=0;ii<dSpaceGetNumGeoms(geomSpace);++ii){
dGeomID g=dSpaceGetGeom(geomSpace,ii);
if(g){
//std::cerr<<"store a geom of type "<<dGeomGetClassData(g)<<std::endl;
s_data->addMonitoredBody(geom,limb);
}
}
}
void SingleDistanceSensor::DistanceSensor::staticCollisionCallback(SingleDistanceSensor::DistanceSensor* sensor, dGeomID geom1, dGeomID geom2)
{
ASSERT(geom1 == sensor->geom);
ASSERT(!dGeomIsSpace(geom2));
dContactGeom contactGeoms[4];
int contacts = dCollide(geom1, geom2, 4, contactGeoms, sizeof(dContactGeom));
for(int i = 0; i < contacts; ++i)
{
dContactGeom& contactGeom = contactGeoms[i];
const float sqrDistance = (Vector3<>((float) contactGeom.pos[0], (float) contactGeom.pos[1], (float) contactGeom.pos[2]) - sensor->pose.translation).squareAbs();
if(sqrDistance < sensor->closestSqrDistance)
{
sensor->closestSqrDistance = sqrDistance;
sensor->closestGeom = geom2;
}
}
}
void ApproxDistanceSensor::DistanceSensor::staticCollisionCallback(ApproxDistanceSensor::DistanceSensor* sensor, dGeomID geom1, dGeomID geom2)
{
ASSERT(geom1 == sensor->geom);
ASSERT(!dGeomIsSpace(geom2));
Geometry* geometry = (Geometry*)dGeomGetData(geom2);
if((::PhysicalObject*)geometry->parentBody == sensor->physicalObject)
return; // avoid detecting the body on which the sensor is monted
const dReal* pos = dGeomGetPosition(geom2);
const Vector3<> geomPos((float) pos[0], (float) pos[1], (float) pos[2]);
const float approxSqrDist = (geomPos - sensor->pose.translation).squareAbs() - geometry->innerRadiusSqr;
if(approxSqrDist >= sensor->closestSqrDistance)
return; // we already found another geometrie that was closer
Vector3<> relPos = sensor->invertedPose * geomPos;
if(relPos.x <= 0.f)
return; // center of the geometry should be in front of the distance sensor
float halfMaxY = sensor->tanHalfAngleX * relPos.x;
float halfMaxZ = sensor->tanHalfAngleY * relPos.x;
if(std::max(std::abs(relPos.y) - geometry->outerRadius, 0.f) >= halfMaxY || std::max(std::abs(relPos.z) - geometry->outerRadius, 0.f) >= halfMaxZ)
return; // the sphere that covers the geometrie does not collide with the pyramid of the distance sensor
if(std::max(std::abs(relPos.y) - geometry->innerRadius, 0.f) < halfMaxY && std::max(std::abs(relPos.z) - geometry->innerRadius, 0.f) < halfMaxZ)
goto hit; // the sphere enclosed by the geometrie collides with the pyramid of the distance sensor
// geom2 might collide with the pyramid of the distance sensor. let us perform a hit scan along one of the pyramid's sides to find out..
{
Vector3<> scanDir = sensor->pose.rotation * Vector3<>(relPos.x, std::max(std::min(relPos.y, halfMaxY), -halfMaxY), std::max(std::min(relPos.z, halfMaxZ), -halfMaxZ));
const Vector3<>& sensorPos = sensor->pose.translation;
dGeomRaySet(sensor->scanRayGeom, sensorPos.x, sensorPos.y, sensorPos.z, scanDir.x, scanDir.y, scanDir.z);
dContactGeom contactGeom;
if(dCollide(sensor->scanRayGeom, geom2, CONTACTS_UNIMPORTANT | 1, &contactGeom, sizeof(dContactGeom)) <= 0)
return;
}
hit:
sensor->closestSqrDistance = approxSqrDist;
sensor->closestGeom = geom2;
}
static void staticCollisionWithSpaceCallback(Callback* callback, dGeomID geom1, dGeomID geom2)
{
ASSERT(!dGeomIsSpace(geom1));
ASSERT(dGeomIsSpace(geom2));
dSpaceCollide2(geom1, geom2, callback, (dNearCallback*)&staticCollisionCallback);
}
void ApproxDistanceSensor::DistanceSensor::staticCollisionWithSpaceCallback(ApproxDistanceSensor::DistanceSensor* sensor, dGeomID geom1, dGeomID geom2)
{
ASSERT(geom1 == sensor->geom);
ASSERT(dGeomIsSpace(geom2));
dSpaceCollide2(geom1, geom2, sensor, (dNearCallback*)&staticCollisionCallback);
}
/**
* \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;
}
}
}
}
void internal_raycastCollisionCallback(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_raycastCollisionCallback);
}
else
{
// Colliding two geoms.
// Sometimes we get a case where the ray geom is passed in
// as both objects, which is stupid.
if (o0 == o1)
{
return ;
}
// Get a pointer to the ODESimulator.
ODESimulator* sim = (ODESimulator*) data;
// Get pointers to the two geoms' GeomData structure. One
// of these (the one NOT belonging to the ray geom)
// will always be non-NULL.
GeomData* geomData0 = ((GeomData*) dGeomGetData(o0));
GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));
// Find the contact group of the collided Solid.
unsigned int geomContactGroup = defaults::shape::contactGroup;
if (geomData0)
{
geomContactGroup = geomData0->shape->contactGroup;
}
else
{
geomContactGroup = geomData1->shape->contactGroup;
}
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim->groupsMakeContacts(
geomContactGroup, sim->internal_getRayContactGroup());
if (!makeContacts)
{
return ;
}
// Now actually test for collision between the two geoms.
// This is a fairly expensive operation.
dContactGeom contactArray[defaults::ode::maxRaycastContacts];
int numContacts = dCollide(o0, o1,
defaults::ode::maxRaycastContacts, contactArray,
sizeof(dContactGeom));
if (0 == numContacts)
{
return ;
}
else
{
// These two geoms must be intersecting. We will store
// only the closest RaycastResult.
int closest = 0;
for (int i = 0; i < numContacts; ++i)
{
if (contactArray[i].depth < contactArray[closest].depth)
{
closest = i;
}
}
// Only one of the geoms will be part of a Solid we
// want to store; the other is the ray.
Solid* solid = NULL;
if (geomData0)
{
solid = geomData0->solid;
}
else
{
solid = geomData1->solid;
}
Point3r intersection((real) contactArray[closest].pos[0],
(real) contactArray[closest].pos[1],
(real) contactArray[closest].pos[2]);
Vec3r normal((real) contactArray[closest].normal[0],
(real) contactArray[closest].normal[1],
(real) contactArray[closest].normal[2]);
sim->internal_addRaycastResult(solid, intersection,
normal, (real) contactArray[closest].depth);
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
//make sure both of the geom's are enabled (otherwise this is
//a waste of our time
if( !dGeomIsEnabled( o1 ) || !dGeomIsEnabled (o2 ))
{
return;
}
//collide spaces if neccesary
if (dGeomIsSpace (o1) || dGeomIsSpace (o2))
{
// collide the space(s?)
dSpaceCollide2 (o1,o2,data,&nearCallback);
// collide all geoms/spaces within the spaces
//if (dGeomIsSpace (o1)) dSpaceCollide ((dSpaceID)o1,data,&nearCallback);
//if (dGeomIsSpace (o2)) dSpaceCollide ((dSpaceID)o2,data,&nearCallback);
}
//otherwise no spaces, just collide geoms
else
{
//make sure one of the geoms has a body
dBodyID body1 = dGeomGetBody( o1 );
dBodyID body2 = dGeomGetBody( o2 );
//if( body1 == 0 && body2 == 0)
//return;
//make sure that the bodies are enabled
//declarations
DynamicsObject* dynobj=NULL, *dynobj2=NULL;
int n;
DynamicsSolver* solver = (DynamicsSolver*)data;
//Get 64 contacts
const int N = 64;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
//Sort all the contacts
dContact* sortedContacts[N];
for( int i=0; i<n; i++ ) sortedContacts[i] = &contact[i];
qsort( sortedContacts, n,sizeof( dContact*), compareContacts );
//determine how many contacts to actually care about
//if( n > 8 )
// n = 8;
if (n > 0)
{
//figure out surface desc stuff
DynamicsSurfaceDesc* sd1, *sd2;
DynamicsSurfaceDesc finaldesc;
sd1 = (DynamicsSurfaceDesc*)dGeomGetData(contact[0].geom.g1);
sd2 = (DynamicsSurfaceDesc*)dGeomGetData(contact[0].geom.g2);
finaldesc.Combine( sd1, sd2);
//Get the bodies involved in the collision
dBodyID b1, b2;
b1 = dGeomGetBody(contact[0].geom.g1);
b2 = dGeomGetBody(contact[0].geom.g2);
//Inform objects of the collision that occured.
if(b1) dynobj = (DynamicsObject*)dBodyGetData(b1);
if(b2) dynobj2 = (DynamicsObject*)dBodyGetData(b2);
if( dynobj ) dynobj->OnCollide ( dynobj2 );
if( dynobj2) dynobj2->OnCollide( dynobj);
//Generate contact joints
for (int i=0; i<n; i++)
{
contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;// | dContactBounce;
//contact[i].surface.mu = 0.5f;
contact[i].surface.mu = finaldesc.mu;
if(contact[i].geom.normal[1] < .8f )
{
contact[i].surface.mu = 0.1f;
}
contact[i].surface.slip1 = finaldesc.ContactSlip1 ;
contact[i].surface.slip2 = finaldesc.ContactSlip2 ;
contact[i].surface.soft_erp = finaldesc.SoftERP ;
contact[i].surface.soft_cfm = finaldesc.SoftCFM;
contact[i].surface.bounce = finaldesc.Bounce;
contact[i].surface.bounce_vel = finaldesc.BounceVelocity ;
dJointID c = dJointCreateContact (solver->WorldID,solver->ContactGroup,&contact[i]);
//Insert friction anisotropy code here
fixFrictionVector( b1, b2, contact[i] );
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
}
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
}
}
}
void ompl::control::OpenDEStateSpace::setDefaultBounds()
{
// limit all velocities to 1 m/s, 1 rad/s, respectively
base::RealVectorBounds bounds1(3);
bounds1.setLow(-1);
bounds1.setHigh(1);
setLinearVelocityBounds(bounds1);
setAngularVelocityBounds(bounds1);
// find the bounding box that contains all geoms included in the collision spaces
double mX, mY, mZ, MX, MY, MZ;
mX = mY = mZ = std::numeric_limits<double>::infinity();
MX = MY = MZ = -std::numeric_limits<double>::infinity();
bool found = false;
std::queue<dSpaceID> spaces;
for (auto &collisionSpace : env_->collisionSpaces_)
spaces.push(collisionSpace);
while (!spaces.empty())
{
dSpaceID space = spaces.front();
spaces.pop();
int n = dSpaceGetNumGeoms(space);
for (int j = 0; j < n; ++j)
{
dGeomID geom = dSpaceGetGeom(space, j);
if (dGeomIsSpace(geom) != 0)
spaces.push((dSpaceID)geom);
else
{
bool valid = true;
dReal aabb[6];
dGeomGetAABB(geom, aabb);
// things like planes are infinite; we want to ignore those
for (double k : aabb)
if (fabs(k) >= std::numeric_limits<dReal>::max())
{
valid = false;
break;
}
if (valid)
{
found = true;
if (aabb[0] < mX)
mX = aabb[0];
if (aabb[1] > MX)
MX = aabb[1];
if (aabb[2] < mY)
mY = aabb[2];
if (aabb[3] > MY)
MY = aabb[3];
if (aabb[4] < mZ)
mZ = aabb[4];
if (aabb[5] > MZ)
MZ = aabb[5];
}
}
}
}
if (found)
{
double dx = MX - mX;
double dy = MY - mY;
double dz = MZ - mZ;
double dM = std::max(dx, std::max(dy, dz));
// add 10% in each dimension + 1% of the max dimension
dx = dx / 10.0 + dM / 100.0;
dy = dy / 10.0 + dM / 100.0;
dz = dz / 10.0 + dM / 100.0;
bounds1.low[0] = mX - dx;
bounds1.high[0] = MX + dx;
bounds1.low[1] = mY - dy;
bounds1.high[1] = MY + dy;
bounds1.low[2] = mZ - dz;
bounds1.high[2] = MZ + dz;
setVolumeBounds(bounds1);
}
}
/**
* nearCallback method that is called for every geom-pair that collides during the current simulation step. Here the collisions are resolved by creating contact joints with appropriate properties are added to the simulation and all contacts are stored to be reported to the CollisionManager.
* @param data
* @param o1 first ode geom that is involved in the contact.
* @param o2 second ode geom that is involved in the contact.
*/
void ODE_CollisionHandler::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
//TODO verify if this is correct (seems to be copied)
// checks, whether one of the dGeomID-objects is actually a space instead of a geom.
if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
// colliding a space with something :
dSpaceCollide2(o1, o2, data, &ODE_SimulationAlgorithm::nearCallback);
// collide all geoms internal to the space(s)
if(dGeomIsSpace(o1)) {
dSpaceCollide((dSpaceID) o1, data, &ODE_SimulationAlgorithm::nearCallback);
}
if(dGeomIsSpace(o2)) {
dSpaceCollide((dSpaceID) o2, data, &ODE_SimulationAlgorithm::nearCallback);
}
}
else {
// find the CollisionObjects which belong to the ode-objects.
CollisionObject *first = mLookUpTable.value(o1);
CollisionObject *second = mLookUpTable.value(o2);
if(first == 0 || second == 0) {
Core::log("ODE_CollisionHandler: CollisionObject could not be defined.");
return;
}
//check for disabled Pairs
bool collisionAllowed = false;
if(first->areCollisionsDisabled() || second->areCollisionsDisabled()) {
collisionAllowed = true;
}
// else if(mAllowedCollisionPairs.contains(first)) {
// QList<CollisionObject*> partners = mAllowedCollisionPairs.value(first);
// if(partners.indexOf(second) != -1) {
// collisionAllowed = true;
// }
// }
// if(mAllowedCollisionPairs.contains(second)) {
// QList<CollisionObject*> partners = mAllowedCollisionPairs.value(second);
// if(partners.indexOf(first) != -1) {
// collisionAllowed = true;
// }
// }
else {
QList<CollisionObject*> partners = mAllowedCollisionPairs.value(first);
if(partners.contains(second)) {
collisionAllowed = true;
}
else {
QList<CollisionObject*> partners = mAllowedCollisionPairs.value(second);
if(partners.contains(first)) {
collisionAllowed = true;
}
}
}
// colliding two non-space geoms, so generate contact
// points between o1 and o2
// there are reported max: mMaxContactPointsValue->get() contact points.
dContact contact[mMaxContactPoints];
int num_contact = dCollide(o1, o2, mMaxContactPoints,
&contact[0].geom, sizeof(dContact));
// TODO: maybe introduce getElasticity, getStaticFriction Methods to SimBody
double restitution = 0.0;
double staticFriction = 0.0;
double dynamicFriction = 0.0;
int material1 = -1;
int material2 = -1;
SimBody *host1 = 0;
SimBody *host2 = 0;
ODE_Body *odeHost1 = 0;
ODE_Body *odeHost2 = 0;
// determine material properties of the two colliding objects.
if(first != 0) {
material1 = first->getMaterialType();
host1 = first->getHostBody();
odeHost1 = dynamic_cast<ODE_Body*>(host1);
}
if(second != 0) {
material2 = second->getMaterialType();
host2 = second->getHostBody();
odeHost2 = dynamic_cast<ODE_Body*>(host2);
}
//TODO check if parameters should be accessed directly instead of via strings (performance)
if(material1 < 0 || material2 < 0) {
// Determine the friction properties of the colliding SimBody objects,
// in case one of the bodies has a unknown material property value.
if(first != 0 && second != 0) {
restitution = host1->getElasticityValue()->get()
+ host2->getElasticityValue()->get();
staticFriction = host1->getStaticFrictionValue()->get()
+ host2->getStaticFrictionValue()->get();
dynamicFriction = host1->getDynamicFrictionValue()->get()
+ host2->getDynamicFrictionValue()->get();
// DoubleValue *restitutionValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("Elasticity"));
// DoubleValue *restitutionValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("Elasticity"));
// if(restitutionValueOne != 0 && restitutionValueTwo != 0) {
// restitution = restitutionValueOne->get()
// * restitutionValueTwo->get();
// }
// DoubleValue *staticFValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("StaticFriction"));
// DoubleValue *staticFValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("StaticFriction"));
// if(staticFValueOne != 0 && staticFValueTwo != 0) {
// staticFriction = staticFValueOne->get() + staticFValueTwo->get();
// }
// DoubleValue *dynamicFValueOne =
// dynamic_cast<DoubleValue*>(host1->getParameter("DynamicFriction"));
// DoubleValue *dynamicFValueTwo =
// dynamic_cast<DoubleValue*>(host2->getParameter("DynamicFriction"));
// if(dynamicFValueOne != 0 && dynamicFValueTwo != 0) {
// dynamicFriction = dynamicFValueOne->get() + dynamicFValueTwo->get();
// }
}
}
else {
//Determine the friction properties in case both SimBody objects have valid material types.
if(mGlobalMaterialProperties != 0) {
restitution = mGlobalMaterialProperties->getRestitution(material1, material2);
dynamicFriction = mGlobalMaterialProperties->getDynamicFriction(material1, material2);
staticFriction = mGlobalMaterialProperties->getStaticFriction(material1, material2);
}
}
dynamicFriction = dynamicFriction * mFrictionScalingFactor;
//every notified collision is saved as a contact. Enables the notification of collision rules in every case
Contact newContact(first, second);
QList<Vector3D> contactPoints;
//generate contact joints for all collision points of the two objects which collide.
for(int i = 0; i < num_contact; i++) {
// enable required ode-specific contact-definition properties.
contact[i].surface.mode = dContactBounce
| dContactSoftCFM
| dContactSoftERP
| dContactSlip1
| dContactSlip2
| dContactApprox1
| dContactMu2;
dContactGeom contactGeom = contact[i].geom;
// add contact point to list of contact points.
contactPoints.push_back(Vector3D(contactGeom.pos[0], contactGeom.pos[1], contactGeom.pos[2]));
// If the two collision objects are allowed to collide, no contact joint is created.
if(collisionAllowed) {
break;
}
// set the friction coefficient depending on the velocity in the friction directions
// (TODO: needs to be confirmed)
if(Math::compareDoubles(contact[i].surface.motion1, 0.0, mStaticFrictionAccuracy) == true) {
contact[i].surface.mu = staticFriction;
}
else {
contact[i].surface.mu = dynamicFriction;
}
if(Math::compareDoubles(contact[i].surface.motion2, 0.0, mStaticFrictionAccuracy) == true) {
contact[i].surface.mu2 = staticFriction;
}
else {
contact[i].surface.mu2 = dynamicFriction;
}
contact[i].surface.bounce = restitution;
contact[i].surface.slip1 = mContactSlip;
contact[i].surface.slip2 = mContactSlip;
contact[i].surface.bounce_vel = mBouncingVelocity;
contact[i].surface.soft_cfm = mSoftCFM;
contact[i].surface.soft_erp = mSoftERP;
if(fabs(contact[i].geom.depth) < 0.000001 ) {
continue;
}
dJointID contactJoint = dJointCreateContact(mAlgorithm->getODEWorldID(),
mAlgorithm->getContactJointGroupID(), &contact[i]);
dJointAttach(contactJoint, dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
//add feedback structure if needed.
dJointFeedback *feedbackStructure = 0;
if(odeHost1 != 0 && odeHost1->isFeedbackEnabled()) {
feedbackStructure = new dJointFeedback();
dJointSetFeedback(contactJoint, feedbackStructure);
odeHost1->getOwnedJointFeedbackList().append(feedbackStructure);
odeHost1->getJointFeedbackList().insertMulti(contactGeom.g1, feedbackStructure);
odeHost1->getJointFeedbackList().insertMulti(contactGeom.g2, feedbackStructure);
}
if(odeHost2 != 0 && odeHost2->isFeedbackEnabled()) {
feedbackStructure = new dJointFeedback();
dJointSetFeedback(contactJoint, feedbackStructure);
odeHost2->getOwnedJointFeedbackList().append(feedbackStructure);
odeHost2->getJointFeedbackList().insertMulti(contactGeom.g1, feedbackStructure);
odeHost2->getJointFeedbackList().insertMulti(contactGeom.g2, feedbackStructure);
}
}
// add the contact points to the contact and add the contact to the list of occured contacts,
// if for both CollisionObjects reporting collisions is enabled (default).
if(num_contact != 0) {
if(first->areCollisionsReported() || second->areCollisionsReported()) {
newContact.setContactPoints(contactPoints);
mCurrentContacts.push_back(newContact);
}
}
}
}
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);
// }
// }
//}
}
}
}
}
void PhysicsSpace::collisionCallback (dGeomID o1, dGeomID o2)
{
StatRealElem *NCollisionTestsStatElem = StatCollector::getGlobalElem(PhysicsHandler::statNCollisionTests);
if(NCollisionTestsStatElem) { NCollisionTestsStatElem->add(1.0f); }
if (dGeomIsSpace (o1) || dGeomIsSpace (o2))
{
// colliding a space with something
dSpaceCollide2 (o1,o2,reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
// collide all geoms internal to the space(s)
if (dGeomIsSpace (o1)) dSpaceCollide (dGeomGetSpace(o1),reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
if (dGeomIsSpace (o2)) dSpaceCollide (dGeomGetSpace(o2),reinterpret_cast<void *>(this),&PhysicsSpace::collisionCallback);
}
else
{
_DiscardCollision = false;
// colliding two non-space geoms, so generate contact
// points between o1 and o2
Int32 numContacts = dCollide(o1, o2, _ContactJoints.size(),
&(_ContactJoints[0].geom), sizeof(dContact));
StatRealElem *NCollisionsStatElem = StatCollector::getGlobalElem(PhysicsHandler::statNCollisions);
if(NCollisionsStatElem) { NCollisionsStatElem->add(static_cast<Real32>(numContacts)); }
if(numContacts>0)
{
Vec3f v1,v2,normal;
Pnt3f position;
dVector3 odeVec;
Real32 projectedNormalSpeed;
for (Int32 i=0; i < numContacts; i++)
{
normal += Vec3f(&_ContactJoints[i].geom.normal[0]);
position += Vec3f(&_ContactJoints[i].geom.pos[0]);
if(dGeomGetBody(o1))
{
dBodyGetPointVel(dGeomGetBody(o1), _ContactJoints[i].geom.pos[0], _ContactJoints[i].geom.pos[1], _ContactJoints[i].geom.pos[2],odeVec);
v1 += Vec3f(&odeVec[0]);
}
if(dGeomGetBody(o2))
{
dBodyGetPointVel(dGeomGetBody(o2), _ContactJoints[i].geom.pos[0], _ContactJoints[i].geom.pos[1], _ContactJoints[i].geom.pos[2],odeVec);
v2 += Vec3f(&odeVec[0]);
}
}
normal = normal * (1.0f/static_cast<Real32>(numContacts));
position = position * (1.0f/static_cast<Real32>(numContacts));
v1 = v1 * (1.0f/static_cast<Real32>(numContacts));
v2 = v2 * (1.0f/static_cast<Real32>(numContacts));
projectedNormalSpeed = (v1+v2).projectTo(normal);
//TODO: Add a way to get the PhysicsGeomUnrecPtr from the GeomIDs so that the PhysicsGeomUnrecPtr can be
//sent to the collision event
produceCollision(position,
normal,
NULL,
NULL,
dGeomGetCategoryBits(o1),
dGeomGetCollideBits (o1),
dGeomGetCategoryBits(o2),
dGeomGetCollideBits (o2),
v1,
v2,
osgAbs(projectedNormalSpeed));
UInt32 Index(0);
for(; Index<_CollisionListenParamsVec.size() ; ++Index)
{
if((dGeomGetCategoryBits(o1) & _CollisionListenParamsVec[Index]._Category) || (dGeomGetCategoryBits(o2) & _CollisionListenParamsVec[Index]._Category))
{
break;
}
}
if(Index < _CollisionListenParamsVec.size())
{
for(UInt32 i(0); i<_CollisionListenParamsVec.size() ; ++i)
{
if( ((dGeomGetCategoryBits(o1) & _CollisionListenParamsVec[i]._Category) || (dGeomGetCategoryBits(o2) & _CollisionListenParamsVec[i]._Category)) &&
(osgAbs(projectedNormalSpeed) >= _CollisionListenParamsVec[i]._SpeedThreshold)
)
{
//TODO: Add a way to get the PhysicsGeomUnrecPtr from the GeomIDs so that the PhysicsGeomUnrecPtr can be
//sent to the collision event
produceCollision(_CollisionListenParamsVec[i]._Listener,
position,
normal,
NULL,
NULL,
dGeomGetCategoryBits(o1),
dGeomGetCollideBits (o1),
dGeomGetCategoryBits(o2),
dGeomGetCollideBits (o2),
v1,
v2,
osgAbs(projectedNormalSpeed));
}
}
}
}
if(!_DiscardCollision)
{
// add these contact points to the simulation
for (Int32 i=0; i < numContacts; i++)
{
getCollisionContact(dGeomGetCategoryBits(o1), dGeomGetCategoryBits(o2))->updateODEContactJoint(_ContactJoints[i]);
dJointID jointId = dJointCreateContact(_CollideWorldID,
_ColJointGroupId,
&_ContactJoints[i]);
dJointAttach(jointId, dGeomGetBody(o1), dGeomGetBody(o2));
}
}
}
}
// 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;
}
}
}
}
}
void internal_volumeCollisionCallback(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_volumeCollisionCallback);
}
else
{
// Colliding two geoms.
//dBodyID o0BodyID = dGeomGetBody(o0);
//dBodyID o1BodyID = dGeomGetBody(o1);
//bool bothHaveBodies = true;
//if (0 == o0BodyID || 0 == o1BodyID)
//{
// bothHaveBodies = false;
//}
// don't do collision detection for the following case:
// two shapes that are part of the same solid (they share a
// body)
// update-> this is already handled by ODE
//if (bothHaveBodies && o0BodyID == o1BodyID)
//{
// //don't do collision detection
// return;
//}
// Get a pointer to the ODESimulator.
ODESimulator* sim = (ODESimulator*) data;
// Get pointers to the two geoms' GeomData structure. Both
// of these should always be non-NULL.
const GeomData* geomData0 = ((GeomData*) dGeomGetData(o0));
const GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));
assert(geomData0);
assert(geomData1);
// Get pointers to the geoms' ShapeData structures.
const ShapeData* shape0 = geomData0->shape;
const ShapeData* shape1 = geomData1->shape;
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim->groupsMakeContacts(
shape0->contactGroup, shape1->contactGroup);
if (!makeContacts)
{
return ;
}
// Now actually test for collision between the two geoms.
// This is a fairly expensive operation.
dContactGeom contactArray[1];
int numContacts = dCollide(o0, o1, 1, contactArray,
sizeof(dContactGeom));
if (0 == numContacts)
{
return ;
}
else
{
// These two geoms must be intersecting.
// Get pointers to the geoms' Solids.
Solid* solid0 = geomData0->solid;
Solid* solid1 = geomData1->solid;
// Not sure at this point if we can know that o1 is the
// volume object, so we'll just call this twice. It
// will automatically keep from adding the same Solid
// multiple times by using its collision count. Later,
// the volume Solid will be removed from this list.
sim->internal_addCollidedSolid(solid0);
sim->internal_addCollidedSolid(solid1);
}
}
}
