void Wheel::reset() {
dSpaceID space = dGeomGetSpace(ph.geom);
dGeomDestroy(ph.geom);
dBodyDestroy(ph.body);
Utils::Xml x(cst.xmlFile, "wheel");
createPhysics(x, space);
disposePhysics(x);
}
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 PhysicsGeom::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & BodyFieldMask)
{
if(getBody() != NULL)
{
dGeomSetBody(_GeomID, getBody()->getBodyID());
}
else
{
dGeomSetBody(_GeomID, 0);
}
}
if(whichField & PositionFieldMask)
{
dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
}
if(whichField & RotationFieldMask)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetRotation(_GeomID, rotation);
}
if(whichField & QuaternionFieldMask)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dGeomSetQuaternion(_GeomID,q);
}
if(whichField & OffsetPositionFieldMask &&
getBody() != NULL)
{
dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
}
if(whichField & OffsetRotationFieldMask &&
getBody() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getOffsetRotation()[0];
Vec4f v2 = getOffsetRotation()[1];
Vec4f v3 = getOffsetRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetOffsetRotation(_GeomID, rotation);
}
if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
{
dQuaternion q;
q[0]=getOffsetQuaternion().w();
q[1]=getOffsetQuaternion().x();
q[2]=getOffsetQuaternion().y();
q[3]=getOffsetQuaternion().z();
dGeomSetOffsetQuaternion(_GeomID,q);
}
if(whichField & CategoryBitsFieldMask)
{
dGeomSetCategoryBits(_GeomID, getCategoryBits());
}
if(whichField & CollideBitsFieldMask)
{
dGeomSetCollideBits(_GeomID, getCollideBits());
}
if(whichField & SpaceFieldMask)
{
dSpaceID CurSpace(dGeomGetSpace(_GeomID));
if(CurSpace != 0 &&
(getSpace() == NULL ||
CurSpace != getSpace()->getSpaceID()))
{
dSpaceRemove(CurSpace,_GeomID);
}
if(getSpace() != NULL)
{
dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
}
}
if(whichField & EnableFieldMask)
{
if(getEnable())
{
dGeomEnable(_GeomID);
}
else
{
dGeomDisable(_GeomID);
}
}
}
