void collidable_object::set_orientation(double x, double y, double z, double w)
{
if(body_id)
{
object::set_orientation(x, y, z, w);
return;
}
if(!geom_id) return;
dQuaternion quat = { w, x, y, z};
dGeomSetQuaternion(geom_id,quat);
}
void CollidableObject::Update(Ogre::Real timeSinceLastFrame)
{
//In the base case, the geom's orientation and position are synced with
//the referenced scene node
if(isInWorld)
{
Ogre::Quaternion curOrient = mSceneNode->getOrientation();
Ogre::Vector3 curPos = mSceneNode->getPosition() + (curOrient * geomOffset);
float dCurOrient[] = {curOrient.w, curOrient.x, curOrient.y, curOrient.z};
dGeomSetPosition(mGeom, curPos.x, curPos.y, curPos.z);
dGeomSetQuaternion(mGeom, dCurOrient);
}
}
//you should create your own world
void CreateWorld1(Simulator * sim)
{
const double zdim = 2.0;
const double xdim = 20;
const double ydim = 20;
AddBoundaries(sim, xdim, ydim, zdim, 0.2);
dGeomID geom;
dGeomID geom_gara1;
dGeomID geom_gara2;
dQuaternion q; //for rotations
// create a parking garage - 1 st wall
geom_gara1 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara1, -13 + 0.5*(30-xdim), -5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara1, q);
sim->AddGeometryAsObstacle(geom_gara1);
// creat a pakring garage - 2nd wall
geom_gara2 = dCreateBox(sim->GetSpaceID(), 4.0, 0.2, zdim);
dGeomSetPosition(geom_gara2, -13+0.5*(30-xdim), -7.5, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, M_PI * 1);
dGeomSetQuaternion(geom_gara2, q);
sim->AddGeometryAsObstacle(geom_gara2);
geom = dCreateBox(sim->GetSpaceID(), 9.0, 0.2, zdim);
dGeomSetPosition(geom, 6, -4, 0.5 * zdim);
dQFromAxisAndAngle(q, 0, 0, 1, -M_PI * 0.25);
dGeomSetQuaternion(geom, q);
sim->AddGeometryAsObstacle(geom);
geom = dCreateSphere(sim->GetSpaceID(), 0.5 * zdim);
dGeomSetPosition(geom, 0, -4, 0.5 * zdim);
sim->AddGeometryAsObstacle(geom);
}
CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
CBoxEntity& c_box) :
CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
m_cBoxEntity(c_box),
m_sGeomData(GEOM_NORMAL) {
/* Check whether the box is movable or not */
if(c_box.GetEmbodiedEntity().IsMovable()) {
/* Movable box */
/* Set the body to its initial position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Create the geometry and the mass */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Set Geom gripping properties. */
m_sGeomData.Type = GEOM_GRIPPABLE;
dGeomSetData(m_tGeom, &m_sGeomData);
/* Create its mass */
dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Associate the body to the geom */
dGeomSetBody(m_tGeom, m_tBody);
/* Set the parent body total mass */
dBodySetMass(m_tBody, &m_tMass);
}
else {
/* Unmovable box, get rid of the body and add only the geometry */
dBodyDestroy(m_tBody);
/* Create the geometry */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
dGeomSetData(m_tGeom, &m_sGeomData);
/* Set the geom to its position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dGeomSetQuaternion(m_tGeom, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Associate the geom to null body (this makes it static) */
dGeomSetBody(m_tGeom, 0);
}
}
void EOSOdeCollisionObject::applyTransform(Ogre::Vector3& position, Ogre::Quaternion& rotation, Ogre::Vector3& scale)
{
dQuaternion odeQuaternion;
odeQuaternion[0] = (dReal)rotation.w; odeQuaternion[1] = (dReal)rotation.x; odeQuaternion[2] = (dReal)rotation.y; odeQuaternion[3] = (dReal)rotation.z;
//
dGeomSetPosition(this->collisionGeometry, position.x, position.y, position.z);
dGeomSetQuaternion(this->collisionGeometry, odeQuaternion);
//
/*switch(this->collisionGeomType)
{
case EOSOdeCollisionObject::BoxGeomType:
case EOSOdeCollisionObject::SphereGeomType:
dGeomSetPosition(this->collisionGeometry, position.x, position.y, position.z);
dGeomSetQuaternion(this->collisionGeometry, odeQuaternion);
break;
case EOSOdeCollisionObject::TrimeshGeomType:
// makes a backup of the initial status of the vertices
if(this->verticesBackup == NULL)
{
this->verticesBackup = new dVector3[this->vertexCount];
memcpy(this->verticesBackup, this->vertices, sizeof(unsigned int) * this->vertexCount);
}
//
for(unsigned int i = 0; i < this->vertexCount; i++)
{
Ogre::Vector3 pt;
pt.x = this->verticesBackup[i][0];
pt.y = this->verticesBackup[i][1];
pt.z = this->verticesBackup[i][2];
// applies rotation, traslation and scale
pt = (rotation * (pt * scale)) + position;
// stores back the vertex
this->vertices[i][0] = (dReal)pt.x;
this->vertices[i][1] = (dReal)pt.y;
this->vertices[i][2] = (dReal)pt.z;
}
dGeomTriMeshDataBuildSimple(this->triMeshData, (const dReal*)this->vertices, (int)this->vertexCount, (int*)this->indices, (int)this->indexCount);
if(this->collisionGeometry)
dGeomDestroy(this->collisionGeometry);
this->collisionGeometry = dCreateTriMesh(0, this->triMeshData, 0, 0, 0);
break;
}*/
}
BoxObstacle::BoxObstacle(dWorldID odeWorld, dSpaceID odeSpace,
const osg::Vec3& pos, const osg::Vec3& size, float density,
const osg::Vec3& rotationAxis, float rotationAngle) :
size_(size) {
if (density >= RobogenUtils::OSG_EPSILON_2){
// if not fixed, create body
box_ = dBodyCreate(odeWorld);
dMass massOde;
dMassSetBox(&massOde, density, size.x(), size.y(), size.z());
dBodySetMass(box_, &massOde);
} else{
// otherwise make body 0
box_ = 0;
}
boxGeom_ = dCreateBox(odeSpace, size.x(), size.y(), size.z());
dGeomSetBody(boxGeom_, box_);
dGeomSetPosition(boxGeom_, pos.x(), pos.y(), pos.z());
// for some reason body/geom position do not get tied together as they
// should, so we set the body position as well, and use it when getting
// the position on non-stationary bodies
if (box_ != 0)
dBodySetPosition(box_, pos.x(), pos.y(), pos.z());
if (rotationAngle >= RobogenUtils::OSG_EPSILON_2){
osg::Quat rotation;
rotation.makeRotate(rotationAngle,rotationAxis);
dQuaternion quatOde;
quatOde[0] = rotation.w();
quatOde[1] = rotation.x();
quatOde[2] = rotation.y();
quatOde[3] = rotation.z();
dGeomSetQuaternion(boxGeom_, quatOde);
}
}
void Mesh::setPose(const Pose& pose){
if(body){
osg::Vec3 pos = pose.getTrans();
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dBodySetQuaternion(body, quat);
}else if(geom){ // okay there is just a geom no body
osg::Vec3 pos = pose.getTrans();
dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dGeomSetQuaternion(geom, quat);
} else
poseWithoutBodyAndGeom = Pose(pose);
update(); // update the scenegraph stuff
}
void Primitive::setPose(const Pose& pose){
if(body){
osg::Vec3 pos = pose.getTrans();
dBodySetPosition(body, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dBodySetQuaternion(body, quat);
}else if(geom){ // okay there is just a geom no body
osg::Vec3 pos = pose.getTrans();
dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
osg::Quat q;
pose.get(q);
// this should be
// dReal quat[4] = {q.x(), q.y(), q.z(), q.w()};
dReal quat[4] = {q.w(), q.x(), q.y(), q.z()};
dGeomSetQuaternion(geom, quat);
} else {
assert(0 && "Call setPose only after initialization");
}
update(); // update the scenegraph stuff
}
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);
}
}
}
