static void printGeom (PrintingContext &c, dxGeom *g)
{
unsigned long category = dGeomGetCategoryBits (g);
if (category != (unsigned long)(~0)) {
c.printIndent();
fprintf (c.file,"category_bits = %lu\n",category);
}
unsigned long collide = dGeomGetCollideBits (g);
if (collide != (unsigned long)(~0)) {
c.printIndent();
fprintf (c.file,"collide_bits = %lu\n",collide);
}
if (!dGeomIsEnabled (g)) {
c.print ("disabled",1);
}
switch (g->type) {
case dSphereClass: printSphere (c,g); break;
case dBoxClass: printBox (c,g); break;
case dCapsuleClass: printCapsule (c,g); break;
case dCylinderClass: printCylinder (c,g); break;
case dPlaneClass: printPlane (c,g); break;
case dRayClass: printRay (c,g); break;
case dConvexClass: printConvex (c,g); break;
case dTriMeshClass: printTriMesh (c,g); break;
case dHeightfieldClass: printHeightfieldClass (c,g); break;
}
}
/***************************************************************************\
* Instance methods *
\***************************************************************************/
void PhysicsPlaneGeom::onCreate(const PhysicsPlaneGeom *)
{
PhysicsPlaneGeomPtr tmpPtr(*this);
tmpPtr->id = dCreatePlane(0, 0, 0, 1, 0);
PhysicsPlaneGeomBase::setParams(tmpPtr->getParams());
PhysicsGeomBase::setCategoryBits(dGeomGetCategoryBits(id));
PhysicsGeomBase::setCollideBits(dGeomGetCollideBits(id));
}
int get_phys_geom_attr_i(dGeomID geom, int p)
{
switch (p)
{
case GEOM_ATTR_CATEGORY: return dGeomGetCategoryBits(geom);
case GEOM_ATTR_COLLIDER: return dGeomGetCollideBits (geom);
case GEOM_ATTR_RESPONSE: return get_data(geom)->response;
case GEOM_ATTR_CALLBACK: return get_data(geom)->callback;
}
return 0;
}
/*-------------------------------------------------------------------------*\
- public -
\*-------------------------------------------------------------------------*/
void PhysicsTriMeshGeom::onCreate(const PhysicsTriMeshGeom *)
{
PhysicsTriMeshGeomPtr tmpPtr(*this);
tmpPtr->id = dCreateTriMesh(0, 0, 0, 0, 0);
data = 0;
numVertices = 0;
numFaces = 0;
vertexData = 0;
faceData = 0;
normalData = 0;
geoNode=NullFC;
PhysicsGeomBase::setCategoryBits(dGeomGetCategoryBits(id));
PhysicsGeomBase::setCollideBits(dGeomGetCollideBits(id));
}
static void callback(void *data, dGeomID o1, dGeomID o2)
{
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))
return;
/* Two geoms associated with the same body do not collide. */
if (b1 == 0 || b2 == 0 || b1 != b2)
{
dContact contact[MAX_CONTACTS];
size_t sz = sizeof (dContact);
int i, n;
/* Find and enumerate all collision points. */
if ((n = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sz)))
{
struct geom_data *d1 = get_data(o1);
struct geom_data *d2 = get_data(o2);
/* Compute collision parameters from geom parameters. */
dReal bounce = MAX(d1->bounce, d2->bounce);
dReal friction = MIN(d1->friction, d2->friction);
dReal soft_erp = MIN(d1->soft_erp, d2->soft_erp);
dReal soft_cfm = MAX(d1->soft_cfm, d2->soft_cfm);
unsigned long category1 = dGeomGetCategoryBits(o1);
unsigned long category2 = dGeomGetCategoryBits(o2);
if (b1) dBodyEnable(b1);
if (b2) dBodyEnable(b2);
/* Create a contact joint for each collision, if requsted. */
if ((category1 & d2->response) ||
(category2 & d1->response))
for (i = 0; i < n; ++i)
if (dGeomGetClass(contact[0].geom.g1) != dRayClass &&
dGeomGetClass(contact[0].geom.g2) != dRayClass)
{
dJointID c;
contact[i].surface.mode = dContactBounce
| dContactSoftCFM
| dContactSoftERP;
contact[i].surface.mu = friction;
contact[i].surface.bounce = bounce;
contact[i].surface.soft_cfm = soft_cfm;
contact[i].surface.soft_erp = soft_erp;
contact[i].surface.bounce_vel = (dReal) 0.10;
c = dJointCreateContact(world, group, contact + i);
dJointAttach(c, b1, b2);
}
/* Report contacts to the Lua script, if requested. */
if ((category1 & d2->callback) ||
(category2 & d1->callback))
for (i = 0; i < n; ++i)
{
float p[3];
float v[3];
p[0] = (float) contact[i].geom.pos[0];
p[1] = (float) contact[i].geom.pos[1];
p[2] = (float) contact[i].geom.pos[2];
v[0] = (float) contact[i].geom.normal[0];
v[1] = (float) contact[i].geom.normal[1];
v[2] = (float) contact[i].geom.normal[2];
do_contact_script(d1->entity, d2->entity,
p, v, (float) contact[i].geom.depth);
}
}
}
}
unsigned long ODE_Particle::getCategoryBits()
{
return dGeomGetCategoryBits(geom);
}
void PhysicsCapsuleGeom::onCreate(const PhysicsCapsuleGeom *)
{
_GeomID = dCreateCapsule(0, getRadius(), getLength());
setCategoryBits(dGeomGetCategoryBits(_GeomID));
setCollideBits(dGeomGetCollideBits(_GeomID));
}
void PhysicsPlaneGeom::onCreate(const PhysicsPlaneGeom *)
{
_GeomID = dCreatePlane(0, getParameters().x(), getParameters().y(), getParameters().z(), getParameters().w());
setCategoryBits(dGeomGetCategoryBits(_GeomID));
setCollideBits(dGeomGetCollideBits(_GeomID));
}
unsigned long ODEGeom::getGeomCategoryBits()
{
return dGeomGetCategoryBits(geom);
}
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));
}
}
}
}
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
