void CProtoHapticDoc::nearCallback(dGeomID o1, dGeomID o2)
{
int index1= (int)dGeomGetData(o1);
int index2= (int)dGeomGetData(o2);
if(m_shapes[index1]->isCollisionDynamic() ||
m_shapes[index2]->isCollisionDynamic()) {
int n, i;
const int N = 50;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
OutputDebugString("Collision\n");
for (i=0; i<n; i++) {
contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
contact[i].surface.mu = 20;
else
contact[i].surface.mu = 0.5;
contact[i].surface.slip1 = 1.0 - (m_shapes[index1]->getSurfaceFriction());
contact[i].surface.slip2 = 1.0 - (m_shapes[index2]->getSurfaceFriction());
contact[i].surface.soft_erp = 0.8;
contact[i].surface.soft_cfm = 0.01;
dJointID c = dJointCreateContact (m_worldID,m_jointGroup,contact+i);
dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
}
}
}
}
void PWorld::initAllObjects()
{
objects_count = objects.count();
int c = objects_count;
bool flag = false;
if (sur_matrix!=NULL)
{
for (int i=0;i<c;i++)
{
if(sur_matrix[i]!=NULL)
delete sur_matrix[i];
}
delete sur_matrix;
flag = true;
}
sur_matrix = new int* [c];
for (int i=0;i<c;i++)
{
sur_matrix[i] = new int [c];
for (int j=0;j<c;j++)
sur_matrix[i][j] = -1;
}
if (flag)
{
for (int i=0;i<surfaces.count();i++)
sur_matrix[(*(int*)(dGeomGetData(surfaces[i]->id1)))][*((int*)(dGeomGetData(surfaces[i]->id2)))] =
sur_matrix[(*(int*)(dGeomGetData(surfaces[i]->id2)))][*((int*)(dGeomGetData(surfaces[i]->id1)))] = i;
}
}
int Geoms_Can_Interpenetrate(dGeomID o1, dGeomID o2) {
int object1IsPartOfRobot = false;
if ( o1!=ground ) {
OBJECT *obj1 = (OBJECT *)dGeomGetData(o1);
object1IsPartOfRobot = obj1->Is_Part_Of_Robot();
}
int object2IsPartOfRobot = false;
if ( o2!=ground ) {
OBJECT *obj2 = (OBJECT *)dGeomGetData(o2);
object2IsPartOfRobot = obj2->Is_Part_Of_Robot();
}
// Robot body parts can interpenetrate; don't compute
// contacts between them.
// Don't bother computing contacts between different
// parts of the environment, because they do not move.
// TBD: Last statement not necessarily true; could have hinged parts
// in environment.
// return true if both objects are part of a robot
// or both objects are part of the environment
return !(object1IsPartOfRobot ^ object2IsPartOfRobot);
}
int Geoms_Can_Interpenetrate(dGeomID o1, dGeomID o2) {
int object1IsPartOfRobot = false;
if ( o1!=ground ) {
OBJECT *obj1 = (OBJECT *)dGeomGetData(o1);
object1IsPartOfRobot = obj1->Is_Part_Of_Robot();
}
int object2IsPartOfRobot = false;
if ( o2!=ground ) {
OBJECT *obj2 = (OBJECT *)dGeomGetData(o2);
object2IsPartOfRobot = obj2->Is_Part_Of_Robot();
}
int bothArePartOfRobot = object1IsPartOfRobot && object2IsPartOfRobot;
// Robot body parts can interpenetrate
int bothArePartOfEnvironment = (!object1IsPartOfRobot) &&
(!object2IsPartOfRobot);
// Don't bother computing contacts between different
// parts of the environment, because they do not move.
return( bothArePartOfRobot || bothArePartOfEnvironment );
}
void PhysWorld::mCallback(void * data, dGeomID g1, dGeomID g2)
{
//Get our CollidableObjects and tell them that a collision
//has occured
CollidableObject* colObj1 = (CollidableObject*)dGeomGetData(g1);
CollidableObject* colObj2 = (CollidableObject*)dGeomGetData(g2);
colObj1->OnCollide(colObj2);
colObj2->OnCollide(colObj1);
}
void ODEEngine::near_callback(dGeomID o1, dGeomID o2) {
ODECollidable* c1 = (ODECollidable*) dGeomGetData(o1);
ODECollidable* c2 = (ODECollidable*) dGeomGetData(o2);
dContact contact[MAX_CONTACTS];
int collision_count = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
for(int32_t i = 0; i < collision_count; ++i) {
//Calculate combined surface values to build contact joints
contact[i].surface.mode = dContactBounce;
contact[i].surface.bounce = c1->bounciness() * c2->bounciness();
if(c1->has_infinite_friction() || c2->has_infinite_friction()) {
contact[i].surface.mu = dInfinity;
} else {
contact[i].surface.mu = c1->friction() * c2->friction();
}
dBodyID body1 = dGeomGetBody(contact[i].geom.g1);
dBodyID body2 = dGeomGetBody(contact[i].geom.g2);
if(body1 == body2) {
//Don't collide the same body against itself
continue;
}
if(i == 0) {
//Fire the collision signal on the first loop only
c1->signal_collided_(*c2);
c2->signal_collided_(*c1);
//Fire any combined signals
fire_collision_signals_for(*c1, *c2);
}
//Check to see if we should create response joints, or just ignore the collision
if(c1->is_ghost() || c2->is_ghost()) {
//Don't respond if we're not supposed to (e.g. we're a hit zone or something)
continue;
}
if(c1->should_respond_callback_ && !c1->should_respond_callback_(*c2)) {
continue;
}
if(c2->should_respond_callback_ && !c2->should_respond_callback_(*c1)) {
continue;
}
dJointID c = dJointCreateContact(world(), contact_group_, &contact[i]);
dJointAttach(c, body1, body2);
}
}
// main collision handling function
void PhysicsServer::checkCollision(void *data, dGeomID o1, dGeomID o2)
{
int i,n;
if( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) )
{
dSpaceCollide2( o1, o2, data, &collisionCallback );
}
else
{
int mode = 0;
double slip1 = 0;
double slip2 = 0;
// get bodies
dBodyID body1 = dGeomGetBody(o1);
dBodyID body2 = dGeomGetBody(o2);
// get geom data
PhysGeomData* pData1 = (PhysGeomData*)dGeomGetData(o1);
PhysGeomData* pData2 = (PhysGeomData*)dGeomGetData(o2);
// set contact params according to geom data
if (pData1 != NULL)
slip1 = pData1->slip;
if (pData2 != NULL)
slip2 = pData2->slip;
// set mode
if (slip1 != 0)
mode |= dContactSlip1;
if (slip2 != 0)
mode |= dContactSlip2;
static const int N = 8; // max number of contact points
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0)
{
for (i=0; i<n; i++)
{
contact[i].surface.mode = mode; /*dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM;*/
contact[i].surface.mu = dInfinity;
contact[i].surface.slip1 = slip1;
contact[i].surface.slip2 = slip2;
// contact[i].surface.soft_erp = 0.7;
// contact[i].surface.soft_cfm = 0.1;
dJointID c = dJointCreateContact (_world, _contactgroup, &contact[i]);
dJointAttach (c, body1, body2);
}
}
}
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
size_t i,j;
i = (size_t) dGeomGetData (o1);
j = (size_t) dGeomGetData (o2);
if (i==j)
printf ("collision (%d,%d) is between the same object\n",i,j);
if (!good_matrix[i][j] || !good_matrix[j][i])
printf ("collision (%d,%d) is incorrect\n",i,j);
if (test_matrix[i][j] || test_matrix[j][i])
printf ("collision (%d,%d) reported more than once\n",i,j);
test_matrix[i][j] = 1;
test_matrix[j][i] = 1;
}
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);
}
}
void TurbulentDispersionScene::Collide(dGeomID g1, dGeomID g2)
{
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
dContact contact[MAX_CONTACTS];
int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
if(n>0)
{
//Particle g1 collides with ground g2
if ((dGeomGetClass(g1) == dSphereClass)&&(g2==ground_wall))
{
unsigned int index=(size_t) dGeomGetData(g1);
//bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index));
Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index));
return;
}
//Particle g2 collides with ground g1
if((dGeomGetClass(g2) == dSphereClass)&&(g1==ground_wall))
{
unsigned int index=(size_t) dGeomGetData(g2);
//bodies_prts_indexes_to_remove.push_back(FindPrtsIndexByGlobalId(index));
Add_prts_body_index_to_remove(FindPrtsIndexByGlobalId(index));
return;
}
}
for (int i=0; i<n; ++i)
{
// contact[i].surface.mode = dContactBounce | dContactSoftCFM;
contact[i].surface.mode = dContactBounce;
//contact[i].surface.mu = dInfinity;
contact[i].surface.mu = 0.5;
//contact[i].surface.mu2 = 0.5;
contact[i].surface.bounce = 0.000999990;
//contact[i].surface.bounce_vel = 0.1;
//contact[i].surface.soft_cfm = 0.001;
//contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM;
contact[i].surface.soft_erp = 1.0; //1.0;
contact[i].surface.soft_cfm = 1e-10;
dJointID j = dJointCreateContact (domain->getWorld(), domain->getContactGroup(), contact+i);
dJointAttach(j, b1, b2);
}
}
void ApproxDistanceSensor::DistanceSensor::updateValue()
{
pose = physicalObject->pose;
pose.conc(offset);
invertedPose = pose.invert();
Vector3<> boxPos = pose * Vector3<>(max * 0.5f, 0.f, 0.f);
dGeomSetPosition(geom, boxPos.x, boxPos.y, boxPos.z);
dMatrix3 matrix3;
ODETools::convertMatrix(pose.rotation, matrix3);
dGeomSetRotation(geom, matrix3);
closestGeom = 0;
closestSqrDistance = maxSqrDist;
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
dSpaceCollide2(geom, (dGeomID)Simulation::simulation->staticSpace, this, (dNearCallback*)&staticCollisionCallback);
if(closestGeom)
{
const dReal* pos = dGeomGetPosition(closestGeom);
Geometry* geometry = (Geometry*)dGeomGetData(closestGeom);
data.floatValue = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - pose.translation).abs() - geometry->innerRadius;
if(data.floatValue < min)
data.floatValue = min;
}
else
data.floatValue = max;
}
void Set_Touch_Sensor(dGeomID o) {
OBJECT* obj = (OBJECT *)dGeomGetData(o);
if ( obj )
obj->Sensor_Touch_Set();
}
void ComponentBrainKamikaze::handleMessagePhysicsCollision(Message &message)
{
dGeomID o2 = message.getField<dGeomID>("o2");
Actor *a2 = reinterpret_cast<Actor*>(dGeomGetData(o2));
if(a2)
{
dGeomID o1 = message.getField<dGeomID>("o1");
Actor *a1 = reinterpret_cast<Actor*>(dGeomGetData(o1));
OBJECT_ID uid = getActorPtr()->getUID();
if(a1 && a1->getUID() == uid)
{
wanderTimer = 0.0f;
}
}
}
IoObject *IoODEGeom_geomFromId(void *state, dGeomID id)
{
if (id == 0)
{
return ((IoState*)state)->ioNil;
}
else
{
return (IoObject*)dGeomGetData(id);
}
}
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 PWorld::handleCollisions(dGeomID o1, dGeomID o2)
{
PSurface* sur;
int j=sur_matrix[*((int*)(dGeomGetData(o1)))][*((int*)(dGeomGetData(o2)))];
if (j!=-1)
{
const int N = 10;
dContact contact[N];
int n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
if (n > 0) {
sur = surfaces[j];
sur->contactPos [0] = contact[0].geom.pos[0];
sur->contactPos [1] = contact[0].geom.pos[1];
sur->contactPos [2] = contact[0].geom.pos[2];
sur->contactNormal[0] = contact[0].geom.normal[0];
sur->contactNormal[1] = contact[0].geom.normal[1];
sur->contactNormal[2] = contact[0].geom.normal[2];
bool flag=true;
if (sur->callback!=NULL) flag = sur->callback(o1,o2,sur);
if (flag)
for (int i=0; i<n; i++) {
contact[i].surface = sur->surface;
if (sur->usefdir1)
{
contact[i].fdir1[0] = sur->fdir1[0];
contact[i].fdir1[1] = sur->fdir1[1];
contact[i].fdir1[2] = sur->fdir1[2];
contact[i].fdir1[3] = sur->fdir1[3];
}
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c,
dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
}
void Update(dGeomID geom)
{
union {
void *data;
Geom *obj;
};
data = dGeomGetData(geom);
if (obj) obj->Update();
if (dGeomIsSpace(geom))
{
Update(dSpaceID(geom));
}
}
void setCurrentTransform(dGeomID geom)
{
const dTriMeshDataID TriMeshData = static_cast<dTriMeshDataID>(dGeomGetData(geom));
const dReal* Pos = dGeomGetPosition(geom);
const dReal* Rot = dGeomGetRotation(geom);
const double Transform[16] =
{
Rot[0], Rot[4], Rot[8], 0,
Rot[1], Rot[5], Rot[9], 0,
Rot[2], Rot[6], Rot[10], 0,
Pos[0], Pos[1], Pos[2], 1
};
dGeomTriMeshDataSet(TriMeshData, TRIMESH_LAST_TRANSFORMATION, (void *)Transform);
}
int dcTriListCollider::dSortedTriSphere(const dReal* /**v1/**/,const dReal* /**v2/**/,
const dReal* triAx,
CDB::TRI* T,
dReal dist,
dxGeom* Sphere,
dxGeom* Geometry,
int Flags,
dContactGeom* Contacts,
int skip
){
//const dReal* v1=(dReal*)T->verts[1];
//const dReal* v2=(dReal*)T->verts[2];
const dReal* SphereCenter=dGeomGetPosition(Sphere);
const float SphereRadius = dGeomSphereGetRadius(Sphere);
// dNormalize3(triAx);
const dReal *ContactNormal=triAx;//{triAx[0],triAx[1],triAx[2]};
dVector3 ContactPos={SphereCenter[0]-triAx[0]* SphereRadius,SphereCenter[1]-triAx[1]* SphereRadius,SphereCenter[2]-triAx[2]* SphereRadius};
float ContactDepth= -dist + SphereRadius;
if (ContactDepth >= 0){
Contacts->normal[0] =-ContactNormal[0];
Contacts->normal[1] =-ContactNormal[1];
Contacts->normal[2] =-ContactNormal[2];
Contacts->depth = ContactDepth;
////////////////////
Contacts->pos[0]=ContactPos[0];
Contacts->pos[1]=ContactPos[1];
Contacts->pos[2]=ContactPos[2];
Contacts->g1 = Geometry;
Contacts->g2 = Sphere;
((dxGeomUserData*)dGeomGetData(Sphere))->tri_material=T->material;
if(dGeomGetUserData(Sphere)->callback)dGeomGetUserData(Sphere)->callback(T,Contacts);
SURFACE(Contacts,0)->mode=T->material;
//////////////////////////////////
return 1;
}
return 0;
}
dGeomID set_phys_geom_type(dGeomID geom, dBodyID body,
int i, int t, const float *v)
{
/* Destroy the old geom and its data. */
if (geom)
{
free(dGeomGetData(geom));
dGeomDestroy(geom);
geom = 0;
}
/* Create a new geom of the required type. */
switch (t)
{
case dSphereClass:
geom = dCreateSphere(space, v[0]);
break;
case dCapsuleClass:
geom = dCreateCapsule(space, v[0], v[1]);
break;
case dBoxClass:
geom = dCreateBox(space, v[0], v[1], v[2]);
break;
case dPlaneClass:
geom = dCreatePlane(space, v[0], v[1], v[2], v[3]);
break;
case dRayClass:
geom = dCreateRay(space, (dReal) sqrt(v[3] * v[3] +
v[4] * v[4] +
v[5] * v[5]));
dGeomRaySet(geom, v[0], v[1], v[2], v[3], v[4], v[5]);
break;
}
/* Assign geom data and attach it to the body. */
if (geom)
{
dGeomSetData(geom, create_data(i));
dGeomSetBody(geom, body);
}
return geom;
}
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;
}
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 struct geom_data *get_data(dGeomID geom)
{
return (struct geom_data *) dGeomGetData(geom);
}
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);
}
}
}
int dcTriListCollider::dTriSphere(const dReal* v0,const dReal* v1,const dReal* v2,
Triangle* T,
dxGeom* Sphere,dxGeom* Geometry, int Flags,
dContactGeom* Contacts,int /**skip/**/)
{
const dVector3 &triSideAx0 =T->side0 ;
const dVector3 &triSideAx1 =T->side1 ;
const dVector3 &triAx =T->norm ;
//if(!TriPlaneContainPoint(triAx,v0,SphereCenter)) return 0;
const dReal radius=dGeomSphereGetRadius(Sphere);
float Depth=-T->dist+radius;
if(Depth<0.f) return 0;
const dReal* pos=dGeomGetPosition(Sphere);
dVector3 ContactNormal;
if(TriContainPoint(v0,v1,v2,triAx,triSideAx0,triSideAx1,pos))
{
ContactNormal[0]=triAx[0];ContactNormal[1]=triAx[1];ContactNormal[2]=triAx[2];
//dVector3 ContactPos={pos[0]-triAx[0]* radius,pos[1]-triAx[1]* radius,pos[2]-triAx[2]* radius};
}
else
{
CDB::TRI* T_array = Level().ObjectSpace.GetStaticTris();
flags8& gl_state=gl_cl_tries_state[I-B];
if(gl_state.test(fl_engaged_s0)||gl_state.test(fl_engaged_s1)||gl_state.test(fl_engaged_s2))
return 0;
if(FragmentonSphereTest(pos,radius,v0,v1,ContactNormal,Depth))
{
SideToGlClTriState(T->T->verts[0],T->T->verts[1],T_array);
}
else if(FragmentonSphereTest(pos,radius,v1,v2,ContactNormal,Depth))
{
SideToGlClTriState(T->T->verts[1],T->T->verts[2],T_array );
}
else if(FragmentonSphereTest(pos,radius,v2,v0,ContactNormal,Depth))
{
SideToGlClTriState(T->T->verts[2],T->T->verts[0],T_array );
}
else{
if(gl_state.test(fl_engaged_v0)||gl_state.test(fl_engaged_v1)||gl_state.test(fl_engaged_v2))
return 0;
if(PointSphereTest(pos,radius,v0,ContactNormal,Depth))
{
VxToGlClTriState(T->T->verts[0],T_array );
}
else if(PointSphereTest(pos,radius,v1,ContactNormal,Depth))
{
VxToGlClTriState(T->T->verts[1],T_array );
}
else if(PointSphereTest(pos,radius,v2,ContactNormal,Depth))
{
VxToGlClTriState(T->T->verts[2],T_array );
}
else return 0;
}
}
Contacts->normal[0] =-ContactNormal[0];
Contacts->normal[1] =-ContactNormal[1];
Contacts->normal[2] =-ContactNormal[2];
Contacts->depth = Depth;
////////////////////
Contacts->pos[0]=pos[0]-ContactNormal[0]*radius;
Contacts->pos[1]=pos[1]-ContactNormal[1]*radius;
Contacts->pos[2]=pos[2]-ContactNormal[2]*radius;
Contacts->g1 = Geometry;
Contacts->g2 = Sphere;
((dxGeomUserData*)dGeomGetData(Sphere))->tri_material=T->T->material;
if(dGeomGetUserData(Sphere)->callback)dGeomGetUserData(Sphere)->callback(T->T,Contacts);
SURFACE(Contacts,0)->mode=T->T->material;
//////////////////////////////////
// ++OutTriCount;
return 1;
}
void PhysicsServer::ProcessCollision( void *data, dGeomID o1, dGeomID o2 )
{
// skip connected bodies
dBodyID bodyID1 = dGeomGetBody( o1 );
dBodyID bodyID2 = dGeomGetBody( o2 );
if (bodyID1 && bodyID2 && dAreConnected( bodyID1, bodyID2 )) return;
if (!bodyID1 && !bodyID2) return;
// do collision
int nContacts = dCollide( o1, o2, c_MaxContacts, &m_Contacts[0].geom, sizeof( dContact ) );
if (nContacts == 0) return;
float scale = GetWorldScale();
nContacts = tmin( nContacts, c_MaxContacts - m_NContacts );
for (int i = 0; i < nContacts; i++)
{
PhysMaterial* pSurf1 = GetGeomSurface( o1 );
PhysMaterial* pSurf2 = GetGeomSurface( o2 );
dContact& contact = m_Contacts[i];
dSurfaceParameters& surface = contact.surface;
// TODO: proper flags setup
m_Contacts[i].surface.mode =
//dContactApprox1 |
//dContactFDir1 |
dContactSoftERP |
dContactSoftCFM |
//dContactSlip1 |
//dContactSlip2 |
dContactBounce;
if (!pSurf1 && !pSurf2)
{
surface.mu = 8.0f;
surface.mu2 = 8.0f;
surface.bounce = 1.0f;
surface.bounce_vel = 0.0f;
surface.soft_erp = 0.1f;
surface.soft_cfm = 1e-3f;
surface.motion1 = 0.0f;
surface.motion2 = 0.0f;
surface.slip1 = 0.1f;
surface.slip2 = 0.1f;
}
else if (pSurf2 == pSurf1)
{
surface.mu = pSurf1->m_Mu;
surface.mu2 = pSurf1->m_Mu2;
surface.bounce = pSurf1->m_Bounce;
surface.bounce_vel = pSurf1->m_BounceVel;
surface.soft_erp = pSurf1->m_SoftERP;
surface.soft_cfm = pSurf1->m_SoftCFM;
surface.motion1 = pSurf1->m_Motion1;
surface.motion2 = pSurf1->m_Motion2;
surface.slip1 = pSurf1->m_Slip1;
surface.slip2 = pSurf1->m_Slip2;
}
else
{
if (!pSurf1) pSurf1 = pSurf2;
if (!pSurf2) pSurf2 = pSurf1;
surface.mu = sqrtf( pSurf1->m_Mu * pSurf2->m_Mu );
surface.mu2 = sqrtf( pSurf1->m_Mu2 * pSurf2->m_Mu2 );
surface.bounce = 0.5f*( pSurf1->m_Bounce + pSurf2->m_Bounce );
surface.bounce_vel = tmin( pSurf1->m_BounceVel, pSurf2->m_BounceVel );
surface.soft_erp = sqrtf( pSurf1->m_SoftERP * pSurf2->m_SoftERP );
surface.soft_cfm = 0.5f*( pSurf1->m_SoftCFM + pSurf2->m_SoftCFM );
surface.motion1 = sqrtf( pSurf1->m_Motion1 * pSurf2->m_Motion1 );
surface.motion2 = sqrtf( pSurf1->m_Motion2 * pSurf2->m_Motion2 );
surface.slip1 = sqrtf( pSurf1->m_Slip1 * pSurf2->m_Slip1 );
surface.slip2 = sqrtf( pSurf1->m_Slip2 * pSurf2->m_Slip2 );
}
PhysObject* pObj1 = (PhysObject*)dGeomGetData( o1 );
PhysObject* pObj2 = (PhysObject*)dGeomGetData( o2 );
int obj1 = pObj1 ? pObj1->GetID() : -1;
int obj2 = pObj2 ? pObj2->GetID() : -1;
dContactGeom& geom = contact.geom;
m_NContacts++;
if (IsDrawBounds())
{
Vec3 pos = Vec3( geom.pos[0], geom.pos[1], geom.pos[2] );
pos /= scale;
Vec3 norm = Vec3( geom.normal[0], geom.normal[1], geom.normal[2] );
const float c_HandleSize = 0.3f;
g_pDrawServer->SetWorldTM( Mat4::identity );
g_pDrawServer->DrawBox( AABox( pos, c_HandleSize*0.5f ), 0x5500FF00, 0x22FFFF00 );
g_pDrawServer->DrawLine( pos, pos + norm*c_HandleSize, 0x5500FF00, 0x5500FF00 );
}
dJointID jID = dJointCreateContact( m_WorldID, m_ContactGroupID, &contact );
dJointAttach( jID, bodyID1, bodyID2 );
}
} // PhysicsServer::ProcessCollision
/**
* \brief This function handles the calculation of a step in the world.
*
* pre:
* - world_init = true
* - step_size > 0
*
* post:
* - handled the collisions
* - step the world for step_size seconds
* - the contactgroup should be empty
*/
void WorldPhysics::stepTheWorld(void) {
MutexLocker locker(&iMutex);
std::vector<dJointFeedback*>::iterator iter;
geom_data* data;
int i;
// if world_init = false or step_size <= 0 debug something
if(world_init && step_size > 0) {
if(old_gravity != world_gravity) {
old_gravity = world_gravity;
dWorldSetGravity(world, world_gravity.x(),
world_gravity.y(), world_gravity.z());
}
if(old_cfm != world_cfm) {
old_cfm = world_cfm;
dWorldSetCFM(world, (dReal)world_cfm);
}
if(old_erp != world_erp) {
old_erp = world_erp;
dWorldSetERP(world, (dReal)world_erp);
}
// printf("now WorldPhysics.cpp..stepTheWorld(void)....1 : dSpaceGetNumGeoms: %d\n",dSpaceGetNumGeoms(space));
/// first clear the collision counters of all geoms
for(i=0; i<dSpaceGetNumGeoms(space); i++) {
data = (geom_data*)dGeomGetData(dSpaceGetGeom(space, i));
data->num_ground_collisions = 0;
data->contact_ids.clear();
data->contact_points.clear();
data->ground_feedbacks.clear();
}
for(iter = contact_feedback_list.begin();
iter != contact_feedback_list.end(); iter++) {
free((*iter));
}
contact_feedback_list.clear();
draw_intern.clear();
/// then we have to clear the contacts
dJointGroupEmpty(contactgroup);
/// first check for collisions
num_contacts = log_contacts = 0;
create_contacts = 1;
dSpaceCollide(space,this, &WorldPhysics::callbackForward);
drawLock.lock();
draw_extern.swap(draw_intern);
drawLock.unlock();
// then calculate the next state for a time of step_size seconds
try {
if(fast_step) dWorldQuickStep(world, step_size);
else dWorldStep(world, step_size);
} catch (...) {
control->sim->handleError(PHYSICS_UNKNOWN);
}
if(WorldPhysics::error) {
control->sim->handleError(WorldPhysics::error);
WorldPhysics::error = PHYSICS_NO_ERROR;
}
}
}
/**
* \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;
}
}
}
}
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));
}
}
}
}
void Simulator::DrawGeometry(dGeomID geom)
{
if(dGeomGetData(geom) == ((void *) &TYPE_OBSTACLE))
glColor3d(0.8, 0.2, 0.2);
else if(dGeomGetData(geom) == ((void *) &TYPE_TERRAIN))
glColor3d(0.4, 0.5, 0.7);
else //type robot
glColor3d(0.0, 0, 1.0);
const int type = dGeomGetClass(geom);
const dReal *pos = dGeomGetPosition(geom);
const dReal *rot = dGeomGetRotation(geom);
double m[16];
glPushMatrix();
//transform position and orientation into an OpenGL matrix
m[0] = rot[0];
m[1] = rot[4];
m[2] = rot[8];
m[4] = rot[1];
m[5] = rot[5];
m[6] = rot[9];
m[8] = rot[2];
m[9] = rot[6];
m[10] = rot[10];
m[3] = m[7] = m[11] = 0;
m[15] = 1;
m[12] = pos[0];
m[13] = pos[1];
m[14] = pos[2];
glMultMatrixd(m);
if(type == dBoxClass)
{
dVector3 lengths;
dGeomBoxGetLengths(geom, lengths);
glPushMatrix();
glScaled(lengths[0], lengths[1], lengths[2]);
glutSolidCube(1.0);
glPopMatrix();
}
else if(type == dSphereClass)
glutSolidSphere(dGeomSphereGetRadius(geom), 20, 20);
else if(type == dCylinderClass)
{
dReal r, length;
dGeomCylinderGetParams(geom, &r, &length);
glTranslated(0, 0, -0.5 * length);
static GLUquadric *gluQuadric = NULL;
if(gluQuadric == NULL)
gluQuadric = gluNewQuadric();
gluCylinder(gluQuadric, r, r, length, 20, 20);
}
glPopMatrix();
}
