Jeongseok Lee avatar Jeongseok Lee committed 9dbafcd

blank skeleton that compiles .

Comments (0)

Files changed (13)

gazebo/physics/rtql8/CMakeLists.txt

              RTQL8Hinge2Joint.cc
              RTQL8UniversalJoint.cc
              RTQL8BallJoint.cc
-#             ODETrimeshShape.cc
-#             ODERayShape.cc
-#             ODEMultiRayShape.cc
-#             ODEHeightmapShape.cc
+             RTQL8TrimeshShape.cc
+             RTQL8RayShape.cc
+             RTQL8MultiRayShape.cc
+             RTQL8HeightmapShape.cc
              RTQL8ScrewJoint.cc
              RTQL8Model.cc
 )
              RTQL8Hinge2Joint.hh
              RTQL8UniversalJoint.hh
              RTQL8BallJoint.hh
-#             ODETrimeshShape.hh
-#             ODERayShape.hh
-#             ODEMultiRayShape.hh
-#             ODEHeightmapShape.hh
-#             ODESphereShape.hh
-#             ODEBoxShape.hh
-#             ODEPlaneShape.hh
-#             ODECylinderShape.hh
+             RTQL8TrimeshShape.hh
+             RTQL8RayShape.hh
+             RTQL8MultiRayShape.hh
+             RTQL8HeightmapShape.hh
+             RTQL8SphereShape.hh
+             RTQL8BoxShape.hh
+             RTQL8PlaneShape.hh
+             RTQL8CylinderShape.hh
              RTQL8Types.hh
              RTQL8ScrewJoint.hh
              RTQL8Model.hh

gazebo/physics/rtql8/RTQL8BoxShape.hh

  * limitations under the License.
  *
 */
-#ifndef _ODEBOXSHAPE_HH_
-#define _ODEBOXSHAPE_HH_
+#ifndef _RTQL8BOXSHAPE_HH_
+#define _RTQL8BOXSHAPE_HH_
 
 #include "math/Vector3.hh"
 
-#include "physics/ode/ODEPhysics.hh"
-#include "physics/ode/ODETypes.hh"
-#include "physics/ode/ODECollision.hh"
+#include "physics/rtql8/RTQL8Physics.hh"
+#include "physics/rtql8/RTQL8Types.hh"
+#include "physics/rtql8/RTQL8Collision.hh"
 
 #include "physics/PhysicsTypes.hh"
 #include "physics/BoxShape.hh"
 {
   namespace physics
   {
-    /// \brief ODE Box shape
-    class ODEBoxShape : public BoxShape
+    /// \brief RTQL8 Box shape
+    class RTQL8BoxShape : public BoxShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Parent Collision.
-      public: explicit ODEBoxShape(ODECollisionPtr _parent)
+      public: explicit RTQL8BoxShape(RTQL8CollisionPtr _parent)
               : BoxShape(_parent) {}
 
       /// \brief Destructor.
-      public: virtual ~ODEBoxShape() {}
+      public: virtual ~RTQL8BoxShape() {}
 
       // Documentation inherited.
       public: virtual void SetSize(const math::Vector3 &_size)
       {
-        BoxShape::SetSize(_size);
-
-        ODECollisionPtr oParent;
-        oParent = boost::shared_dynamic_cast<ODECollision>(
-            this->collisionParent);
-
-        if (oParent->GetCollisionId() == NULL)
-          oParent->SetCollision(dCreateBox(0, _size.x, _size.y, _size.z), true);
-        else
-          dGeomBoxSetLengths(oParent->GetCollisionId(),
-                             _size.x, _size.y, _size.z);
+//         BoxShape::SetSize(_size);
+// 
+//         RTQL8CollisionPtr oParent;
+//         oParent = boost::shared_dynamic_cast<RTQL8Collision>(
+//             this->collisionParent);
+// 
+//         if (oParent->GetCollisionId() == NULL)
+//           oParent->SetCollision(dCreateBox(0, _size.x, _size.y, _size.z), true);
+//         else
+//           dGeomBoxSetLengths(oParent->GetCollisionId(),
+//                              _size.x, _size.y, _size.z);
       }
     };
   }

gazebo/physics/rtql8/RTQL8CylinderShape.hh

  * limitations under the License.
  *
 */
-#ifndef _ODECYLINDERSHAPE_HH_
-#define _ODECYLINDERSHAPE_HH_
+#ifndef _RTQL8CYLINDERSHAPE_HH_
+#define _RTQL8CYLINDERSHAPE_HH_
 
 #include "gazebo/physics/CylinderShape.hh"
-#include "gazebo/physics/ode/ODEPhysics.hh"
+#include "gazebo/physics/rtql8/RTQL8Physics.hh"
 
 namespace gazebo
 {
   namespace physics
   {
-    /// \brief ODE cylinder shape
-    class ODECylinderShape : public CylinderShape
+    /// \brief RTQL8 cylinder shape
+    class RTQL8CylinderShape : public CylinderShape
     {
       /// \brief Constructor
       /// \param[in] _parent Collision parent.
-      public: explicit ODECylinderShape(CollisionPtr _parent)
+      public: explicit RTQL8CylinderShape(CollisionPtr _parent)
               : CylinderShape(_parent) {}
 
       /// \brief Destructor.
-      public: virtual ~ODECylinderShape() {}
+      public: virtual ~RTQL8CylinderShape() {}
 
       // Documentation inerited.
       public: void SetSize(double _radius, double _length)
       {
-        CylinderShape::SetSize(_radius, _length);
-        ODECollisionPtr oParent;
-        oParent =
-          boost::shared_dynamic_cast<ODECollision>(this->collisionParent);
-
-        if (oParent->GetCollisionId() == NULL)
-          oParent->SetCollision(dCreateCylinder(0, _radius, _length), true);
-        else
-          dGeomCylinderSetParams(oParent->GetCollisionId(), _radius, _length);
+//         CylinderShape::SetSize(_radius, _length);
+//         RTQL8CollisionPtr oParent;
+//         oParent =
+//           boost::shared_dynamic_cast<RTQL8Collision>(this->collisionParent);
+// 
+//         if (oParent->GetCollisionId() == NULL)
+//           oParent->SetCollision(dCreateCylinder(0, _radius, _length), true);
+//         else
+//           dGeomCylinderSetParams(oParent->GetCollisionId(), _radius, _length);
       }
     };
   }

gazebo/physics/rtql8/RTQL8HeightmapShape.cc

  */
 
 #include "common/Exception.hh"
-#include "physics/ode/ODECollision.hh"
-#include "physics/ode/ODEHeightmapShape.hh"
+#include "physics/rtql8/RTQL8Collision.hh"
+#include "physics/rtql8/RTQL8HeightmapShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 
 //////////////////////////////////////////////////
-ODEHeightmapShape::ODEHeightmapShape(CollisionPtr _parent)
+RTQL8HeightmapShape::RTQL8HeightmapShape(CollisionPtr _parent)
     : HeightmapShape(_parent)
 {
 }
 
 //////////////////////////////////////////////////
-ODEHeightmapShape::~ODEHeightmapShape()
+RTQL8HeightmapShape::~RTQL8HeightmapShape()
 {
 }
 
 //////////////////////////////////////////////////
-dReal ODEHeightmapShape::GetHeightCallback(void *_data, int _x, int _y)
-{
-  // Return the height at a specific vertex
-  return static_cast<ODEHeightmapShape*>(_data)->GetHeight(_x, _y);
-}
+// dReal RTQL8HeightmapShape::GetHeightCallback(void *_data, int _x, int _y)
+// {
+//   // Return the height at a specific vertex
+//   return static_cast<ODEHeightmapShape*>(_data)->GetHeight(_x, _y);
+// }
 
 //////////////////////////////////////////////////
-void ODEHeightmapShape::Init()
+void RTQL8HeightmapShape::Init()
 {
-  HeightmapShape::Init();
-
-  ODECollisionPtr oParent =
-    boost::shared_static_cast<ODECollision>(this->collisionParent);
-
-  // Step 2: Create the ODE heightfield collision
-  this->odeData = dGeomHeightfieldDataCreate();
-
-  // Step 3: Setup a callback method for ODE
-  dGeomHeightfieldDataBuildCallback(
-      this->odeData,
-      this,
-      ODEHeightmapShape::GetHeightCallback,
-      this->GetSize().x,  // in meters
-      this->GetSize().y,  // in meters
-      this->vertSize,  // width sampling size
-      this->vertSize,  // depth sampling size (along height of image)
-      1.0,  // vertical (z-axis) scaling
-      this->GetPos().z,  // vertical (z-axis) offset
-      1.0,  // vertical thickness for closing the height map mesh
-      0);  // wrap mode
-
-  // Step 4: Restrict the bounds of the AABB to improve efficiency
-  dGeomHeightfieldDataSetBounds(this->odeData, 0, this->GetSize().z);
-
-  oParent->SetCollision(dCreateHeightfield(0, this->odeData, 1), false);
-  oParent->SetStatic(true);
-
-  // Rotate so Z is up, not Y (which is the default orientation)
-  math::Quaternion quat;
-  math::Pose pose = oParent->GetWorldPose();
-
-  // TODO: FIXME:  double check this, if Y is up,
-  // rotating by roll of 90 deg will put Z-down.
-  quat.SetFromEuler(math::Vector3(GZ_DTOR(90), 0, 0));
-
-  pose.rot = pose.rot * quat;
-  // this->body->SetPose(pose);
-
-  dQuaternion q;
-  q[0] = pose.rot.w;
-  q[1] = pose.rot.x;
-  q[2] = pose.rot.y;
-  q[3] = pose.rot.z;
-
-  dGeomSetQuaternion(oParent->GetCollisionId(), q);
+//   HeightmapShape::Init();
+// 
+//   ODECollisionPtr oParent =
+//     boost::shared_static_cast<ODECollision>(this->collisionParent);
+// 
+//   // Step 2: Create the ODE heightfield collision
+//   this->odeData = dGeomHeightfieldDataCreate();
+// 
+//   // Step 3: Setup a callback method for ODE
+//   dGeomHeightfieldDataBuildCallback(
+//       this->odeData,
+//       this,
+//       ODEHeightmapShape::GetHeightCallback,
+//       this->GetSize().x,  // in meters
+//       this->GetSize().y,  // in meters
+//       this->vertSize,  // width sampling size
+//       this->vertSize,  // depth sampling size (along height of image)
+//       1.0,  // vertical (z-axis) scaling
+//       this->GetPos().z,  // vertical (z-axis) offset
+//       1.0,  // vertical thickness for closing the height map mesh
+//       0);  // wrap mode
+// 
+//   // Step 4: Restrict the bounds of the AABB to improve efficiency
+//   dGeomHeightfieldDataSetBounds(this->odeData, 0, this->GetSize().z);
+// 
+//   oParent->SetCollision(dCreateHeightfield(0, this->odeData, 1), false);
+//   oParent->SetStatic(true);
+// 
+//   // Rotate so Z is up, not Y (which is the default orientation)
+//   math::Quaternion quat;
+//   math::Pose pose = oParent->GetWorldPose();
+// 
+//   // TODO: FIXME:  double check this, if Y is up,
+//   // rotating by roll of 90 deg will put Z-down.
+//   quat.SetFromEuler(math::Vector3(GZ_DTOR(90), 0, 0));
+// 
+//   pose.rot = pose.rot * quat;
+//   // this->body->SetPose(pose);
+// 
+//   dQuaternion q;
+//   q[0] = pose.rot.w;
+//   q[1] = pose.rot.x;
+//   q[2] = pose.rot.y;
+//   q[3] = pose.rot.z;
+// 
+//   dGeomSetQuaternion(oParent->GetCollisionId(), q);
 }

gazebo/physics/rtql8/RTQL8HeightmapShape.hh

  * Date: 12 Nov 2009
  */
 
-#ifndef _ODEHEIGHTMAPSHAPE_HH_
-#define _ODEHEIGHTMAPSHAPE_HH_
+#ifndef _RTQL8HEIGHTMAPSHAPE_HH_
+#define _RTQL8HEIGHTMAPSHAPE_HH_
 
 #include <vector>
 
 #include "gazebo/physics/HeightmapShape.hh"
-#include "gazebo/physics/ode/ODEPhysics.hh"
+#include "gazebo/physics/rtql8/RTQL8Physics.hh"
 #include "gazebo/physics/Collision.hh"
 
 namespace gazebo
 {
   namespace physics
   {
-    /// \brief ODE Height map collision.
-    class ODEHeightmapShape : public HeightmapShape
+    /// \brief RTQL8 Height map collision.
+    class RTQL8HeightmapShape : public HeightmapShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Collision parent.
-      public: ODEHeightmapShape(CollisionPtr _parent);
+      public: RTQL8HeightmapShape(CollisionPtr _parent);
 
       /// \brief Destructor
-      public: virtual ~ODEHeightmapShape();
+      public: virtual ~RTQL8HeightmapShape();
 
       // Documentation inerited.
       public: virtual void Init();
       /// \param[in] _data Pointer to the heightmap data.
       /// \param[in] _x X location.
       /// \param[in] _y Y location.
-      private: static dReal GetHeightCallback(void *_data, int _x, int _y);
+      //private: static dReal GetHeightCallback(void *_data, int _x, int _y);
 
       /// \brief The heightmap data.
-      private: dHeightfieldDataID odeData;
+      //private: dHeightfieldDataID odeData;
     };
   }
 }

gazebo/physics/rtql8/RTQL8MultiRayShape.cc

 #include "common/Exception.hh"
 
 #include "physics/World.hh"
-#include "physics/ode/ODETypes.hh"
-#include "physics/ode/ODELink.hh"
-#include "physics/ode/ODECollision.hh"
-#include "physics/ode/ODEPhysics.hh"
-#include "physics/ode/ODERayShape.hh"
-#include "physics/ode/ODEMultiRayShape.hh"
+#include "physics/rtql8/RTQL8Types.hh"
+#include "physics/rtql8/RTQL8Link.hh"
+#include "physics/rtql8/RTQL8Collision.hh"
+#include "physics/rtql8/RTQL8Physics.hh"
+#include "physics/rtql8/RTQL8RayShape.hh"
+#include "physics/rtql8/RTQL8MultiRayShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 
 
 //////////////////////////////////////////////////
-ODEMultiRayShape::ODEMultiRayShape(CollisionPtr _parent)
+RTQL8MultiRayShape::RTQL8MultiRayShape(CollisionPtr _parent)
   : MultiRayShape(_parent)
 {
-  this->SetName("ODE Multiray Shape");
-
-  // Create a space to contain the ray space
-  this->superSpaceId = dSimpleSpaceCreate(0);
-
-  // Create a space to contain all the rays
-  this->raySpaceId = dSimpleSpaceCreate(this->superSpaceId);
-
-  // Set collision bits
-  dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE);
-  dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE);
-
-  // These three lines may be unessecary
-  ODELinkPtr pLink =
-    boost::shared_static_cast<ODELink>(this->collisionParent->GetLink());
-  pLink->SetSpaceId(this->raySpaceId);
-  boost::shared_static_cast<ODECollision>(_parent)->SetSpaceId(
-      this->raySpaceId);
+//   this->SetName("RTQL8 Multiray Shape");
+// 
+//   // Create a space to contain the ray space
+//   this->superSpaceId = dSimpleSpaceCreate(0);
+// 
+//   // Create a space to contain all the rays
+//   this->raySpaceId = dSimpleSpaceCreate(this->superSpaceId);
+// 
+//   // Set collision bits
+//   dGeomSetCategoryBits((dGeomID) this->raySpaceId, GZ_SENSOR_COLLIDE);
+//   dGeomSetCollideBits((dGeomID) this->raySpaceId, ~GZ_SENSOR_COLLIDE);
+// 
+//   // These three lines may be unessecary
+//   RTQL8LinkPtr pLink =
+//     boost::shared_static_cast<RTQL8Link>(this->collisionParent->GetLink());
+//   pLink->SetSpaceId(this->raySpaceId);
+//   boost::shared_static_cast<RTQL8Collision>(_parent)->SetSpaceId(
+//       this->raySpaceId);
 }
 
 //////////////////////////////////////////////////
-ODEMultiRayShape::~ODEMultiRayShape()
+RTQL8MultiRayShape::~RTQL8MultiRayShape()
 {
-  dSpaceSetCleanup(this->raySpaceId, 0);
-  dSpaceDestroy(this->raySpaceId);
-
-  dSpaceSetCleanup(this->superSpaceId, 0);
-  dSpaceDestroy(this->superSpaceId);
+//   dSpaceSetCleanup(this->raySpaceId, 0);
+//   dSpaceDestroy(this->raySpaceId);
+// 
+//   dSpaceSetCleanup(this->superSpaceId, 0);
+//   dSpaceDestroy(this->superSpaceId);
 }
 
 //////////////////////////////////////////////////
-void ODEMultiRayShape::UpdateRays()
+void RTQL8MultiRayShape::UpdateRays()
 {
-  ODEPhysicsPtr ode = boost::shared_dynamic_cast<ODEPhysics>(
-      this->GetWorld()->GetPhysicsEngine());
-
-  if (ode == NULL)
-    gzthrow("Invalid physics engine. Must use ODE.");
-
-  // FIXME: Do we need to lock the physics engine here? YES!
-  //        especially when spawning models with sensors
-
-  ode->GetPhysicsUpdateMutex()->lock();
-  // Do collision detection
-  dSpaceCollide2((dGeomID) (this->superSpaceId),
-      (dGeomID) (ode->GetSpaceId()),
-      this, &UpdateCallback);
-  ode->GetPhysicsUpdateMutex()->unlock();
+//   RTQL8PhysicsPtr ode = boost::shared_dynamic_cast<RTQL8Physics>(
+//       this->GetWorld()->GetPhysicsEngine());
+// 
+//   if (ode == NULL)
+//     gzthrow("Invalid physics engine. Must use RTQL8.");
+// 
+//   // FIXME: Do we need to lock the physics engine here? YES!
+//   //        especially when spawning models with sensors
+// 
+//   ode->GetPhysicsUpdateMutex()->lock();
+//   // Do collision detection
+//   dSpaceCollide2((dGeomID) (this->superSpaceId),
+//       (dGeomID) (ode->GetSpaceId()),
+//       this, &UpdateCallback);
+//   ode->GetPhysicsUpdateMutex()->unlock();
 }
 
-//////////////////////////////////////////////////
-void ODEMultiRayShape::UpdateCallback(void *_data, dGeomID _o1, dGeomID _o2)
-{
-  dContactGeom contact;
-  ODECollision *collision1, *collision2 = NULL;
-  ODECollision *rayCollision = NULL;
-  ODECollision *hitCollision = NULL;
-  ODEMultiRayShape *self = NULL;
-
-  self = static_cast<ODEMultiRayShape*>(_data);
-
-  // Check space
-  if (dGeomIsSpace(_o1) || dGeomIsSpace(_o2))
-  {
-    if (dGeomGetSpace(_o1) == self->superSpaceId ||
-        dGeomGetSpace(_o2) == self->superSpaceId)
-      dSpaceCollide2(_o1, _o2, self, &UpdateCallback);
-
-    if (dGeomGetSpace(_o1) == self->raySpaceId ||
-        dGeomGetSpace(_o2) == self->raySpaceId)
-      dSpaceCollide2(_o1, _o2, self, &UpdateCallback);
-  }
-  else
-  {
-    collision1 = NULL;
-    collision2 = NULL;
-
-    // Get pointers to the underlying collisions
-    if (dGeomGetClass(_o1) == dGeomTransformClass)
-    {
-      collision1 = static_cast<ODECollision*>(
-          dGeomGetData(dGeomTransformGetGeom(_o1)));
-    }
-    else
-      collision1 = static_cast<ODECollision*>(dGeomGetData(_o1));
-
-    if (dGeomGetClass(_o2) == dGeomTransformClass)
-    {
-      collision2 =
-        static_cast<ODECollision*>(dGeomGetData(dGeomTransformGetGeom(_o2)));
-    }
-    else
-    {
-      collision2 = static_cast<ODECollision*>(dGeomGetData(_o2));
-    }
-
-    assert(collision1 && collision2);
-
-    rayCollision = NULL;
-    hitCollision = NULL;
-
-    // Figure out which one is a ray; note that this assumes
-    // that the ODE dRayClass is used *soley* by the RayCollision.
-    if (dGeomGetClass(_o1) == dRayClass)
-    {
-      rayCollision = static_cast<ODECollision*>(collision1);
-      hitCollision = static_cast<ODECollision*>(collision2);
-      dGeomRaySetParams(_o1, 0, 0);
-      dGeomRaySetClosestHit(_o1, 1);
-    }
-    else if (dGeomGetClass(_o2) == dRayClass)
-    {
-      assert(rayCollision == NULL);
-      rayCollision = static_cast<ODECollision*>(collision2);
-      hitCollision = static_cast<ODECollision*>(collision1);
-      dGeomRaySetParams(_o2, 0, 0);
-      dGeomRaySetClosestHit(_o2, 1);
-    }
-
-    // Check for ray/collision intersections
-    if (rayCollision && hitCollision)
-    {
-      int n = dCollide(_o1, _o2, 1, &contact, sizeof(contact));
-
-      if (n > 0)
-      {
-        RayShapePtr shape = boost::shared_static_cast<RayShape>(
-            rayCollision->GetShape());
-        if (contact.depth < shape->GetLength())
-        {
-          // gzerr << "ODEMultiRayShape UpdateCallback dSpaceCollide2 "
-          //      << " depth[" << contact.depth << "]"
-          //      << " position[" << contact.pos[0]
-          //        << ", " << contact.pos[1]
-          //        << ", " << contact.pos[2]
-          //        << ", " << "]"
-          //      << " ray[" << rayCollision->GetName() << "]"
-          //      << " pose[" << rayCollision->GetWorldPose() << "]"
-          //      << " hit[" << hitCollision->GetName() << "]"
-          //      << " pose[" << hitCollision->GetWorldPose() << "]"
-          //      << "\n";
-          shape->SetLength(contact.depth);
-          shape->SetRetro(hitCollision->GetLaserRetro());
-        }
-      }
-    }
-  }
-}
-
-//////////////////////////////////////////////////
-void ODEMultiRayShape::AddRay(const math::Vector3 &_start,
-    const math::Vector3 &_end)
-{
-  MultiRayShape::AddRay(_start, _end);
-
-  ODECollisionPtr odeCollision(new ODECollision(
-        this->collisionParent->GetLink()));
-  odeCollision->SetName("ode_ray_collision");
-  odeCollision->SetSpaceId(this->raySpaceId);
-
-  ODERayShapePtr ray(new ODERayShape(odeCollision));
-  odeCollision->SetShape(ray);
-
-  ray->SetPoints(_start, _end);
-  this->rays.push_back(ray);
-}
+// //////////////////////////////////////////////////
+// void RTQL8MultiRayShape::UpdateCallback(void *_data, dGeomID _o1, dGeomID _o2)
+// {
+//   dContactGeom contact;
+//   RTQL8Collision *collision1, *collision2 = NULL;
+//   RTQL8Collision *rayCollision = NULL;
+//   RTQL8Collision *hitCollision = NULL;
+//   RTQL8MultiRayShape *self = NULL;
+// 
+//   self = static_cast<RTQL8MultiRayShape*>(_data);
+// 
+//   // Check space
+//   if (dGeomIsSpace(_o1) || dGeomIsSpace(_o2))
+//   {
+//     if (dGeomGetSpace(_o1) == self->superSpaceId ||
+//         dGeomGetSpace(_o2) == self->superSpaceId)
+//       dSpaceCollide2(_o1, _o2, self, &UpdateCallback);
+// 
+//     if (dGeomGetSpace(_o1) == self->raySpaceId ||
+//         dGeomGetSpace(_o2) == self->raySpaceId)
+//       dSpaceCollide2(_o1, _o2, self, &UpdateCallback);
+//   }
+//   else
+//   {
+//     collision1 = NULL;
+//     collision2 = NULL;
+// 
+//     // Get pointers to the underlying collisions
+//     if (dGeomGetClass(_o1) == dGeomTransformClass)
+//     {
+//       collision1 = static_cast<RTQL8Collision*>(
+//           dGeomGetData(dGeomTransformGetGeom(_o1)));
+//     }
+//     else
+//       collision1 = static_cast<RTQL8Collision*>(dGeomGetData(_o1));
+// 
+//     if (dGeomGetClass(_o2) == dGeomTransformClass)
+//     {
+//       collision2 =
+//         static_cast<RTQL8Collision*>(dGeomGetData(dGeomTransformGetGeom(_o2)));
+//     }
+//     else
+//     {
+//       collision2 = static_cast<RTQL8Collision*>(dGeomGetData(_o2));
+//     }
+// 
+//     assert(collision1 && collision2);
+// 
+//     rayCollision = NULL;
+//     hitCollision = NULL;
+// 
+//     // Figure out which one is a ray; note that this assumes
+//     // that the RTQL8 dRayClass is used *soley* by the RayCollision.
+//     if (dGeomGetClass(_o1) == dRayClass)
+//     {
+//       rayCollision = static_cast<RTQL8Collision*>(collision1);
+//       hitCollision = static_cast<RTQL8Collision*>(collision2);
+//       dGeomRaySetParams(_o1, 0, 0);
+//       dGeomRaySetClosestHit(_o1, 1);
+//     }
+//     else if (dGeomGetClass(_o2) == dRayClass)
+//     {
+//       assert(rayCollision == NULL);
+//       rayCollision = static_cast<RTQL8Collision*>(collision2);
+//       hitCollision = static_cast<RTQL8Collision*>(collision1);
+//       dGeomRaySetParams(_o2, 0, 0);
+//       dGeomRaySetClosestHit(_o2, 1);
+//     }
+// 
+//     // Check for ray/collision intersections
+//     if (rayCollision && hitCollision)
+//     {
+//       int n = dCollide(_o1, _o2, 1, &contact, sizeof(contact));
+// 
+//       if (n > 0)
+//       {
+//         RayShapePtr shape = boost::shared_static_cast<RayShape>(
+//             rayCollision->GetShape());
+//         if (contact.depth < shape->GetLength())
+//         {
+//           // gzerr << "RTQL8MultiRayShape UpdateCallback dSpaceCollide2 "
+//           //      << " depth[" << contact.depth << "]"
+//           //      << " position[" << contact.pos[0]
+//           //        << ", " << contact.pos[1]
+//           //        << ", " << contact.pos[2]
+//           //        << ", " << "]"
+//           //      << " ray[" << rayCollision->GetName() << "]"
+//           //      << " pose[" << rayCollision->GetWorldPose() << "]"
+//           //      << " hit[" << hitCollision->GetName() << "]"
+//           //      << " pose[" << hitCollision->GetWorldPose() << "]"
+//           //      << "\n";
+//           shape->SetLength(contact.depth);
+//           shape->SetRetro(hitCollision->GetLaserRetro());
+//         }
+//       }
+//     }
+//   }
+// }
+// 
+// //////////////////////////////////////////////////
+// void RTQL8MultiRayShape::AddRay(const math::Vector3 &_start,
+//     const math::Vector3 &_end)
+// {
+//   MultiRayShape::AddRay(_start, _end);
+// 
+//   RTQL8CollisionPtr odeCollision(new RTQL8Collision(
+//         this->collisionParent->GetLink()));
+//   odeCollision->SetName("ode_ray_collision");
+//   odeCollision->SetSpaceId(this->raySpaceId);
+// 
+//   RTQL8RayShapePtr ray(new RTQL8RayShape(odeCollision));
+//   odeCollision->SetShape(ray);
+// 
+//   ray->SetPoints(_start, _end);
+//   this->rays.push_back(ray);
+// }

gazebo/physics/rtql8/RTQL8MultiRayShape.hh

  * limitations under the License.
  *
 */
-#ifndef _ODEMULTIRAYSHAPE_HH_
-#define _ODEMULTIRAYSHAPE_HH_
+#ifndef _RTQL8MULTIRAYSHAPE_HH_
+#define _RTQL8MULTIRAYSHAPE_HH_
 
 #include "gazebo/physics/MultiRayShape.hh"
 
 {
   namespace physics
   {
-    /// \brief ODE specific version of MultiRayShape
-    class ODEMultiRayShape : public MultiRayShape
+    /// \brief RTQL8 specific version of MultiRayShape
+    class RTQL8MultiRayShape : public MultiRayShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Parent Collision.
-      public: explicit ODEMultiRayShape(CollisionPtr _parent);
+      public: explicit RTQL8MultiRayShape(CollisionPtr _parent);
 
       /// \brief Destructor.
-      public: virtual ~ODEMultiRayShape();
+      public: virtual ~RTQL8MultiRayShape();
 
       // Documentation inherited.
       public: virtual void UpdateRays();
       /// \param[in] _data Pointer to user data.
       /// \param[in] _o1 First geom to check for collisions.
       /// \param[in] _o2 Second geom to check for collisions.
-      private: static void UpdateCallback(void *_data, dGeomID _o1,
-                                          dGeomID _o2);
+//       private: static void UpdateCallback(void *_data, dGeomID _o1,
+//                                           dGeomID _o2);
 
       /// \brief Add a ray to the collision.
       /// \param[in] _start Start of a ray.
       /// \param[in] _end End of a ray.
-      protected: void AddRay(const math::Vector3 &_start,
-                             const math::Vector3 &_end);
+//       protected: void AddRay(const math::Vector3 &_start,
+//                              const math::Vector3 &_end);
 
       /// \brief Space to contain the ray space, for efficiency.
-      private: dSpaceID superSpaceId;
+      //private: dSpaceID superSpaceId;
 
       /// \brief Ray space for collision detector.
-      private: dSpaceID raySpaceId;
+      //private: dSpaceID raySpaceId;
     };
   }
 }

gazebo/physics/rtql8/RTQL8PlaneShape.hh

  * limitations under the License.
  *
 */
-#ifndef _ODEPLANESHAPE_HH_
-#define _ODEPLANESHAPE_HH_
+#ifndef _RTQL8PLANESHAPE_HH_
+#define _RTQL8PLANESHAPE_HH_
 
 #include "gazebo/physics/PlaneShape.hh"
-#include "gazebo/physics/ode/ODEPhysics.hh"
+#include "gazebo/physics/rtql8/RTQL8Physics.hh"
 
 namespace gazebo
 {
   namespace physics
   {
-    /// \brief An ODE Plane shape.
-    class ODEPlaneShape : public PlaneShape
+    /// \brief An RTQL8 Plane shape.
+    class RTQL8PlaneShape : public PlaneShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Parent Collision.
-      public: explicit ODEPlaneShape(CollisionPtr _parent)
+      public: explicit RTQL8PlaneShape(CollisionPtr _parent)
               : PlaneShape(_parent) {}
 
       /// \brief Destructor.
-      public: virtual ~ODEPlaneShape() {}
+      public: virtual ~RTQL8PlaneShape() {}
 
       // Documentation inherited
       public: virtual void CreatePlane()
       {
-        PlaneShape::CreatePlane();
-        ODECollisionPtr oParent;
-        oParent =
-          boost::shared_dynamic_cast<ODECollision>(this->collisionParent);
-
-        double altitude = 0;
-
-        math::Vector3 n = this->GetNormal();
-        if (oParent->GetCollisionId() == NULL)
-          oParent->SetCollision(dCreatePlane(oParent->GetSpaceId(),
-                n.x, n.y, n.z, altitude), false);
-        else
-          dGeomPlaneSetParams(oParent->GetCollisionId(),
-                              n.x, n.y, n.z, altitude);
+//         PlaneShape::CreatePlane();
+//         RTQL8CollisionPtr oParent;
+//         oParent =
+//           boost::shared_dynamic_cast<RTQL8Collision>(this->collisionParent);
+// 
+//         double altitude = 0;
+// 
+//         math::Vector3 n = this->GetNormal();
+//         if (oParent->GetCollisionId() == NULL)
+//           oParent->SetCollision(dCreatePlane(oParent->GetSpaceId(),
+//                 n.x, n.y, n.z, altitude), false);
+//         else
+//           dGeomPlaneSetParams(oParent->GetCollisionId(),
+//                               n.x, n.y, n.z, altitude);
       }
 
       // Documentation inherited
       public: virtual void SetAltitude(const math::Vector3 &_pos)
       {
-        PlaneShape::SetAltitude(_pos);
-        ODECollisionPtr odeParent;
-        odeParent =
-          boost::shared_dynamic_cast<ODECollision>(this->collisionParent);
-
-        dVector4 vec4;
-
-        dGeomPlaneGetParams(odeParent->GetCollisionId(), vec4);
-
-        // Compute "altitude": scalar product of position and normal
-        vec4[3] = vec4[0] * _pos.x + vec4[1] * _pos.y + vec4[2] * _pos.z;
-
-        dGeomPlaneSetParams(odeParent->GetCollisionId(), vec4[0], vec4[1],
-                            vec4[2], vec4[3]);
+//         PlaneShape::SetAltitude(_pos);
+//         RTQL8CollisionPtr odeParent;
+//         odeParent =
+//           boost::shared_dynamic_cast<RTQL8Collision>(this->collisionParent);
+// 
+//         dVector4 vec4;
+// 
+//         dGeomPlaneGetParams(odeParent->GetCollisionId(), vec4);
+// 
+//         // Compute "altitude": scalar product of position and normal
+//         vec4[3] = vec4[0] * _pos.x + vec4[1] * _pos.y + vec4[2] * _pos.z;
+// 
+//         dGeomPlaneSetParams(odeParent->GetCollisionId(), vec4[0], vec4[1],
+//                             vec4[2], vec4[3]);
       }
     };
   }

gazebo/physics/rtql8/RTQL8RayShape.cc

 
 #include "physics/World.hh"
 #include "physics/Link.hh"
-#include "physics/ode/ODEPhysics.hh"
-#include "physics/ode/ODETypes.hh"
-#include "physics/ode/ODECollision.hh"
-#include "physics/ode/ODERayShape.hh"
+#include "physics/rtql8/RTQL8Physics.hh"
+#include "physics/rtql8/RTQL8Types.hh"
+#include "physics/rtql8/RTQL8Collision.hh"
+#include "physics/rtql8/RTQL8RayShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 
 //////////////////////////////////////////////////
-ODERayShape::ODERayShape(PhysicsEnginePtr _physicsEngine)
+RTQL8RayShape::RTQL8RayShape(PhysicsEnginePtr _physicsEngine)
   : RayShape(_physicsEngine)
 {
-  this->SetName("ODE Ray Shape");
-
-  this->physicsEngine =
-    boost::shared_static_cast<ODEPhysics>(_physicsEngine);
-  this->geomId = dCreateRay(this->physicsEngine->GetSpaceId(), 2.0);
-  dGeomSetCategoryBits(this->geomId, GZ_SENSOR_COLLIDE);
-  dGeomSetCollideBits(this->geomId, ~GZ_SENSOR_COLLIDE);
-  this->collisionParent.reset();
+//   this->SetName("RTQL8 Ray Shape");
+// 
+//   this->physicsEngine =
+//     boost::shared_static_cast<RTQL8Physics>(_physicsEngine);
+//   this->geomId = dCreateRay(this->physicsEngine->GetSpaceId(), 2.0);
+//   dGeomSetCategoryBits(this->geomId, GZ_SENSOR_COLLIDE);
+//   dGeomSetCollideBits(this->geomId, ~GZ_SENSOR_COLLIDE);
+//   this->collisionParent.reset();
 }
 
 
 //////////////////////////////////////////////////
-ODERayShape::ODERayShape(CollisionPtr _parent)
+RTQL8RayShape::RTQL8RayShape(CollisionPtr _parent)
     : RayShape(_parent)
 {
-  this->SetName("ODE Ray Shape");
-
-  ODECollisionPtr collision =
-    boost::shared_static_cast<ODECollision>(this->collisionParent);
-
-  this->physicsEngine = boost::shared_static_cast<ODEPhysics>(
-      this->collisionParent->GetWorld()->GetPhysicsEngine());
-  this->geomId = dCreateRay(collision->GetSpaceId(), 1.0);
-
-  // Create default ray with unit length
-  collision->SetCollision(this->geomId, false);
-  collision->SetCategoryBits(GZ_SENSOR_COLLIDE);
-  collision->SetCollideBits(~GZ_SENSOR_COLLIDE);
+//   this->SetName("RTQL8 Ray Shape");
+// 
+//   RTQL8CollisionPtr collision =
+//     boost::shared_static_cast<RTQL8Collision>(this->collisionParent);
+// 
+//   this->physicsEngine = boost::shared_static_cast<RTQL8Physics>(
+//       this->collisionParent->GetWorld()->GetPhysicsEngine());
+//   this->geomId = dCreateRay(collision->GetSpaceId(), 1.0);
+// 
+//   // Create default ray with unit length
+//   collision->SetCollision(this->geomId, false);
+//   collision->SetCategoryBits(GZ_SENSOR_COLLIDE);
+//   collision->SetCollideBits(~GZ_SENSOR_COLLIDE);
 }
 
 //////////////////////////////////////////////////
-ODERayShape::~ODERayShape()
+RTQL8RayShape::~RTQL8RayShape()
 {
-  dGeomDestroy(this->geomId);
+//   dGeomDestroy(this->geomId);
 }
 
 //////////////////////////////////////////////////
-void ODERayShape::Update()
+void RTQL8RayShape::Update()
 {
-  math::Vector3 dir;
-
-  if (this->collisionParent)
-  {
-    ODECollisionPtr collision =
-      boost::shared_static_cast<ODECollision>(this->collisionParent);
-
-    this->globalStartPos =
-      this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
-          this->relativeStartPos);
-
-    this->globalEndPos =
-      this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
-          this->relativeEndPos);
-  }
-
-  dir = this->globalEndPos - this->globalStartPos;
-  dir.Normalize();
-
-  if (!math::equal(this->contactLen, 0.0))
-  {
-    dGeomRaySet(this->geomId,
-        this->globalStartPos.x, this->globalStartPos.y, this->globalStartPos.z,
-        dir.x, dir.y, dir.z);
-
-    dGeomRaySetLength(this->geomId,
-        this->globalStartPos.Distance(this->globalEndPos));
-  }
+//   math::Vector3 dir;
+// 
+//   if (this->collisionParent)
+//   {
+//     RTQL8CollisionPtr collision =
+//       boost::shared_static_cast<RTQL8Collision>(this->collisionParent);
+// 
+//     this->globalStartPos =
+//       this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
+//           this->relativeStartPos);
+// 
+//     this->globalEndPos =
+//       this->collisionParent->GetLink()->GetWorldPose().CoordPositionAdd(
+//           this->relativeEndPos);
+//   }
+// 
+//   dir = this->globalEndPos - this->globalStartPos;
+//   dir.Normalize();
+// 
+//   if (!math::equal(this->contactLen, 0.0))
+//   {
+//     dGeomRaySet(this->geomId,
+//         this->globalStartPos.x, this->globalStartPos.y, this->globalStartPos.z,
+//         dir.x, dir.y, dir.z);
+// 
+//     dGeomRaySetLength(this->geomId,
+//         this->globalStartPos.Distance(this->globalEndPos));
+//   }
 }
 
 //////////////////////////////////////////////////
-void ODERayShape::GetIntersection(double &_dist, std::string &_entity)
+void RTQL8RayShape::GetIntersection(double &_dist, std::string &_entity)
 {
-  if (this->physicsEngine)
-  {
-    Intersection intersection;
-    intersection.depth = 1000;
-
-    this->physicsEngine->GetPhysicsUpdateMutex()->lock();
-
-    // Do collision detection
-    dSpaceCollide2(this->geomId,
-        (dGeomID)(this->physicsEngine->GetSpaceId()),
-        &intersection, &UpdateCallback);
-    this->physicsEngine->GetPhysicsUpdateMutex()->unlock();
-
-    _dist = intersection.depth;
-    _entity = intersection.name;
-  }
+//   if (this->physicsEngine)
+//   {
+//     Intersection intersection;
+//     intersection.depth = 1000;
+// 
+//     this->physicsEngine->GetPhysicsUpdateMutex()->lock();
+// 
+//     // Do collision detection
+//     dSpaceCollide2(this->geomId,
+//         (dGeomID)(this->physicsEngine->GetSpaceId()),
+//         &intersection, &UpdateCallback);
+//     this->physicsEngine->GetPhysicsUpdateMutex()->unlock();
+// 
+//     _dist = intersection.depth;
+//     _entity = intersection.name;
+//   }
 }
 
 //////////////////////////////////////////////////
-void ODERayShape::SetPoints(const math::Vector3 &_posStart,
+void RTQL8RayShape::SetPoints(const math::Vector3 &_posStart,
                             const math::Vector3 &_posEnd)
 {
-  math::Vector3 dir;
-  RayShape::SetPoints(_posStart, _posEnd);
-
-  dir = this->globalEndPos - this->globalStartPos;
-  dir.Normalize();
-
-  dGeomRaySet(this->geomId, this->globalStartPos.x,
-              this->globalStartPos.y, this->globalStartPos.z,
-              dir.x, dir.y, dir.z);
-
-  dGeomRaySetLength(this->geomId,
-                     this->globalStartPos.Distance(this->globalEndPos));
+//   math::Vector3 dir;
+//   RayShape::SetPoints(_posStart, _posEnd);
+// 
+//   dir = this->globalEndPos - this->globalStartPos;
+//   dir.Normalize();
+// 
+//   dGeomRaySet(this->geomId, this->globalStartPos.x,
+//               this->globalStartPos.y, this->globalStartPos.z,
+//               dir.x, dir.y, dir.z);
+// 
+//   dGeomRaySetLength(this->geomId,
+//                      this->globalStartPos.Distance(this->globalEndPos));
 }
 
 //////////////////////////////////////////////////
-void ODERayShape::UpdateCallback(void *_data, dGeomID _o1, dGeomID _o2)
-{
-  dContactGeom contact;
-  ODERayShape::Intersection *inter = NULL;
-
-  inter = static_cast<Intersection*>(_data);
-
-  // Check space
-  if (dGeomIsSpace(_o1) || dGeomIsSpace(_o2))
-  {
-    dSpaceCollide2(_o1, _o2, inter, &UpdateCallback);
-  }
-  else
-  {
-    ODECollision *collision1, *collision2;
-    ODECollision *hitCollision = NULL;
-
-    // Get pointers to the underlying collisions
-    if (dGeomGetClass(_o1) == dGeomTransformClass)
-      collision1 =
-        static_cast<ODECollision*>(dGeomGetData(dGeomTransformGetGeom(_o1)));
-    else
-      collision1 = static_cast<ODECollision*>(dGeomGetData(_o1));
-
-    if (dGeomGetClass(_o2) == dGeomTransformClass)
-      collision2 =
-        static_cast<ODECollision*>(dGeomGetData(dGeomTransformGetGeom(_o2)));
-    else
-      collision2 = static_cast<ODECollision*>(dGeomGetData(_o2));
-
-    // Figure out which one is a ray; note that this assumes
-    // that the ODE dRayClass is used *soley* by the RayCollision.
-    if (dGeomGetClass(_o1) == dRayClass)
-    {
-      hitCollision = collision2;
-      dGeomRaySetParams(_o1, 0, 0);
-      dGeomRaySetClosestHit(_o1, 1);
-    }
-    else if (dGeomGetClass(_o2) == dRayClass)
-    {
-      hitCollision = collision1;
-      dGeomRaySetParams(_o2, 0, 0);
-      dGeomRaySetClosestHit(_o2, 1);
-    }
-
-    if (hitCollision)
-    {
-      // Check for ray/collision intersections
-      int n = dCollide(_o1, _o2, 1, &contact, sizeof(contact));
-
-      if (n > 0)
-      {
-        if (contact.depth < inter->depth)
-        {
-          inter->depth = contact.depth;
-          inter->name = hitCollision->GetName();
-        }
-      }
-    }
-  }
-}
+// void RTQL8RayShape::UpdateCallback(void *_data, dGeomID _o1, dGeomID _o2)
+// {
+//   dContactGeom contact;
+//   RTQL8RayShape::Intersection *inter = NULL;
+// 
+//   inter = static_cast<Intersection*>(_data);
+// 
+//   // Check space
+//   if (dGeomIsSpace(_o1) || dGeomIsSpace(_o2))
+//   {
+//     dSpaceCollide2(_o1, _o2, inter, &UpdateCallback);
+//   }
+//   else
+//   {
+//     RTQL8Collision *collision1, *collision2;
+//     RTQL8Collision *hitCollision = NULL;
+// 
+//     // Get pointers to the underlying collisions
+//     if (dGeomGetClass(_o1) == dGeomTransformClass)
+//       collision1 =
+//         static_cast<RTQL8Collision*>(dGeomGetData(dGeomTransformGetGeom(_o1)));
+//     else
+//       collision1 = static_cast<RTQL8Collision*>(dGeomGetData(_o1));
+// 
+//     if (dGeomGetClass(_o2) == dGeomTransformClass)
+//       collision2 =
+//         static_cast<RTQL8Collision*>(dGeomGetData(dGeomTransformGetGeom(_o2)));
+//     else
+//       collision2 = static_cast<RTQL8Collision*>(dGeomGetData(_o2));
+// 
+//     // Figure out which one is a ray; note that this assumes
+//     // that the RTQL8 dRayClass is used *soley* by the RayCollision.
+//     if (dGeomGetClass(_o1) == dRayClass)
+//     {
+//       hitCollision = collision2;
+//       dGeomRaySetParams(_o1, 0, 0);
+//       dGeomRaySetClosestHit(_o1, 1);
+//     }
+//     else if (dGeomGetClass(_o2) == dRayClass)
+//     {
+//       hitCollision = collision1;
+//       dGeomRaySetParams(_o2, 0, 0);
+//       dGeomRaySetClosestHit(_o2, 1);
+//     }
+// 
+//     if (hitCollision)
+//     {
+//       // Check for ray/collision intersections
+//       int n = dCollide(_o1, _o2, 1, &contact, sizeof(contact));
+// 
+//       if (n > 0)
+//       {
+//         if (contact.depth < inter->depth)
+//         {
+//           inter->depth = contact.depth;
+//           inter->name = hitCollision->GetName();
+//         }
+//       }
+//     }
+//   }
+// }

gazebo/physics/rtql8/RTQL8RayShape.hh

  * Date: 14 Oct 2009
  */
 
-#ifndef _ODERAYSHAPE_HH_
-#define _ODERAYSHAPE_HH_
+#ifndef _RTQL8RAYSHAPE_HH_
+#define _RTQL8RAYSHAPE_HH_
 
 #include <string>
 
   namespace physics
   {
     /// \brief Ray collision
-    class ODERayShape : public RayShape
+    class RTQL8RayShape : public RayShape
     {
       /// \brief Constructor for a global ray.
       /// \param[in] _physicsEngine Pointer to the physics engine.
-      public: explicit ODERayShape(PhysicsEnginePtr _physicsEngine);
+      public: explicit RTQL8RayShape(PhysicsEnginePtr _physicsEngine);
 
       /// \brief Constructor.
       /// \param[in] _collision Collision object this ray is attached to.
-      public: explicit ODERayShape(CollisionPtr _collision);
+      public: explicit RTQL8RayShape(CollisionPtr _collision);
 
       /// \brief Destructor.
-      public: virtual ~ODERayShape();
+      public: virtual ~RTQL8RayShape();
 
       public: virtual void Update();
 
       /// \param[in] _data Pointer to user data.
       /// \param[in] _o1 First geom to check for collisions.
       /// \param[in] _o2 Second geom to check for collisions.
-      private: static void UpdateCallback(void *_data, dGeomID _o1,
-                                          dGeomID _o2);
+//       private: static void UpdateCallback(void *_data, dGeomID _o1,
+//                                           dGeomID _o2);
 
-      /// \brief ODE geom id.
-      private: dGeomID geomId;
+      /// \brief RTQL8 geom id.
+      //private: dGeomID geomId;
 
-      /// \brief Pointer to the ODE physics engine
-      private: ODEPhysicsPtr physicsEngine;
+      /// \brief Pointer to the RTQL8 physics engine
+      private: RTQL8PhysicsPtr physicsEngine;
 
       /// \brief An intersection class keeping track of name and depth of
       ///        intersections.
-      private: class Intersection
-               {
-                 /// \brief Depth of the ray intersection.
-                 public: double depth;
-
-                 /// \brief Name of the collision object that was hit.
-                 public: std::string name;
-               };
+//       private: class Intersection
+//                {
+//                  /// \brief Depth of the ray intersection.
+//                  public: double depth;
+// 
+//                  /// \brief Name of the collision object that was hit.
+//                  public: std::string name;
+//                };
     };
   }
 }

gazebo/physics/rtql8/RTQL8SphereShape.hh

  * limitations under the License.
  *
 */
-#ifndef _ODESPHERESHAPE_HH_
-#define _ODESPHERESHAPE_HH_
+#ifndef _RTQL8SPHERESHAPE_HH_
+#define _RTQL8SPHERESHAPE_HH_
 
-#include "gazebo/physics/ode/ODEPhysics.hh"
-#include "gazebo/physics/ode/ODECollision.hh"
+#include "gazebo/physics/rtql8/RTQL8Physics.hh"
+#include "gazebo/physics/rtql8/RTQL8Collision.hh"
 #include "gazebo/physics/PhysicsTypes.hh"
 #include "gazebo/physics/SphereShape.hh"
 
 {
   namespace physics
   {
-    /// \brief A ODE sphere shape
-    class ODESphereShape : public SphereShape
+    /// \brief A RTQL8 sphere shape
+    class RTQL8SphereShape : public SphereShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Parent Collision.
-      public: explicit ODESphereShape(ODECollisionPtr _parent)
+      public: explicit RTQL8SphereShape(RTQL8CollisionPtr _parent)
               : SphereShape(_parent) {}
 
       /// \brief Destructor.
-      public: virtual ~ODESphereShape() {}
+      public: virtual ~RTQL8SphereShape() {}
 
       // Documentation inherited.
       public: virtual void SetRadius(double _radius)
       {
-        SphereShape::SetRadius(_radius);
-        ODECollisionPtr oParent;
-        oParent =
-          boost::shared_dynamic_cast<ODECollision>(this->collisionParent);
-
-        // Create the sphere geometry
-        if (oParent->GetCollisionId() == NULL)
-          oParent->SetCollision(dCreateSphere(0, _radius), true);
-        else
-          dGeomSphereSetRadius(oParent->GetCollisionId(), _radius);
+//         SphereShape::SetRadius(_radius);
+//         RTQL8CollisionPtr oParent;
+//         oParent =
+//           boost::shared_dynamic_cast<RTQL8Collision>(this->collisionParent);
+// 
+//         // Create the sphere geometry
+//         if (oParent->GetCollisionId() == NULL)
+//           oParent->SetCollision(dCreateSphere(0, _radius), true);
+//         else
+//           dGeomSphereSetRadius(oParent->GetCollisionId(), _radius);
       }
     };
   }

gazebo/physics/rtql8/RTQL8TrimeshShape.cc

  * limitations under the License.
  *
 */
-/* Desc: ODE Trimesh shape
+/* Desc: RTQL8 Trimesh shape
  * Author: Nate Koenig
  * Date: 16 Oct 2009
  */
 #include "common/Exception.hh"
 #include "common/Console.hh"
 
-#include "physics/ode/ODECollision.hh"
-#include "physics/ode/ODEPhysics.hh"
-#include "physics/ode/ODETrimeshShape.hh"
+#include "physics/rtql8/RTQL8Collision.hh"
+#include "physics/rtql8/RTQL8Physics.hh"
+#include "physics/rtql8/RTQL8TrimeshShape.hh"
 
 using namespace gazebo;
 using namespace physics;
 
 
 //////////////////////////////////////////////////
-ODETrimeshShape::ODETrimeshShape(CollisionPtr _parent) : TrimeshShape(_parent)
+RTQL8TrimeshShape::RTQL8TrimeshShape(CollisionPtr _parent) : TrimeshShape(_parent)
 {
-  this->odeData = NULL;
-  this->vertices = NULL;
-  this->indices = NULL;
+//   this->odeData = NULL;
+//   this->vertices = NULL;
+//   this->indices = NULL;
 }
 
 //////////////////////////////////////////////////
-ODETrimeshShape::~ODETrimeshShape()
+RTQL8TrimeshShape::~RTQL8TrimeshShape()
 {
-  delete [] this->vertices;
-  delete [] this->indices;
-  dGeomTriMeshDataDestroy(this->odeData);
+//   delete [] this->vertices;
+//   delete [] this->indices;
+//   dGeomTriMeshDataDestroy(this->odeData);
 }
 
 //////////////////////////////////////////////////
-void ODETrimeshShape::Update()
+void RTQL8TrimeshShape::Update()
 {
-  ODECollisionPtr ocollision =
-    boost::shared_dynamic_cast<ODECollision>(this->collisionParent);
-
-  /// FIXME: use below to update trimesh geometry for collision without
-  // using above Ogre codes
-  // tell the tri-tri collider the current transform of the trimesh --
-  // this is fairly important for good results.
-
-  // Fill in the (4x4) matrix.
-  dReal *matrix = this->transform + (this->transformIndex * 16);
-  const dReal *Pos = dGeomGetPosition(ocollision->GetCollisionId());
-  const dReal *Rot = dGeomGetRotation(ocollision->GetCollisionId());
-
-  matrix[ 0 ] = Rot[ 0 ];
-  matrix[ 1 ] = Rot[ 1 ];
-  matrix[ 2 ] = Rot[ 2 ];
-  matrix[ 3 ] = 0;
-  matrix[ 4 ] = Rot[ 4 ];
-  matrix[ 5 ] = Rot[ 5 ];
-  matrix[ 6 ] = Rot[ 6 ];
-  matrix[ 7 ] = 0;
-  matrix[ 8 ] = Rot[ 8 ];
-  matrix[ 9 ] = Rot[ 9 ];
-  matrix[10 ] = Rot[10 ];
-  matrix[11 ] = 0;
-  matrix[12 ] = Pos[ 0 ];
-  matrix[13 ] = Pos[ 1 ];
-  matrix[14 ] = Pos[ 2 ];
-  matrix[15 ] = 1;
-
-  // Flip to other matrix.
-  this->transformIndex = !this->transformIndex;
-
-  dGeomTriMeshSetLastTransform(ocollision->GetCollisionId(),
-      *reinterpret_cast<dMatrix4*>(this->transform +
-                                   this->transformIndex * 16));
+//   RTQL8CollisionPtr ocollision =
+//     boost::shared_dynamic_cast<RTQL8Collision>(this->collisionParent);
+// 
+//   /// FIXME: use below to update trimesh geometry for collision without
+//   // using above Ogre codes
+//   // tell the tri-tri collider the current transform of the trimesh --
+//   // this is fairly important for good results.
+// 
+//   // Fill in the (4x4) matrix.
+//   dReal *matrix = this->transform + (this->transformIndex * 16);
+//   const dReal *Pos = dGeomGetPosition(ocollision->GetCollisionId());
+//   const dReal *Rot = dGeomGetRotation(ocollision->GetCollisionId());
+// 
+//   matrix[ 0 ] = Rot[ 0 ];
+//   matrix[ 1 ] = Rot[ 1 ];
+//   matrix[ 2 ] = Rot[ 2 ];
+//   matrix[ 3 ] = 0;
+//   matrix[ 4 ] = Rot[ 4 ];
+//   matrix[ 5 ] = Rot[ 5 ];
+//   matrix[ 6 ] = Rot[ 6 ];
+//   matrix[ 7 ] = 0;
+//   matrix[ 8 ] = Rot[ 8 ];
+//   matrix[ 9 ] = Rot[ 9 ];
+//   matrix[10 ] = Rot[10 ];
+//   matrix[11 ] = 0;
+//   matrix[12 ] = Pos[ 0 ];
+//   matrix[13 ] = Pos[ 1 ];
+//   matrix[14 ] = Pos[ 2 ];
+//   matrix[15 ] = 1;
+// 
+//   // Flip to other matrix.
+//   this->transformIndex = !this->transformIndex;
+// 
+//   dGeomTriMeshSetLastTransform(ocollision->GetCollisionId(),
+//       *reinterpret_cast<dMatrix4*>(this->transform +
+//                                    this->transformIndex * 16));
 }
 
 //////////////////////////////////////////////////
-void ODETrimeshShape::Load(sdf::ElementPtr _sdf)
+void RTQL8TrimeshShape::Load(sdf::ElementPtr _sdf)
 {
-  TrimeshShape::Load(_sdf);
+//   TrimeshShape::Load(_sdf);
 }
 
 //////////////////////////////////////////////////
-void ODETrimeshShape::Init()
+void RTQL8TrimeshShape::Init()
 {
-  TrimeshShape::Init();
-  if (!this->mesh)
-    return;
-
-  ODECollisionPtr pcollision =
-    boost::shared_static_cast<ODECollision>(this->collisionParent);
-
-  /// This will hold the vertex data of the triangle mesh
-  if (this->odeData == NULL)
-    this->odeData = dGeomTriMeshDataCreate();
-
-  unsigned int numVertices = this->mesh->GetVertexCount();
-  unsigned int numIndices = this->mesh->GetIndexCount();
-  this->vertices = NULL;
-  this->indices = NULL;
-
-  // Get all the vertex and index data
-  this->mesh->FillArrays(&this->vertices, &this->indices);
-
-  // Scale the vertex data
-  for (unsigned int j = 0;  j < numVertices; j++)
-  {
-    this->vertices[j*3+0] = this->vertices[j*3+0] *
-      this->sdf->GetValueVector3("scale").x;
-    this->vertices[j*3+1] = this->vertices[j*3+1] *
-      this->sdf->GetValueVector3("scale").y;
-    this->vertices[j*3+2] = this->vertices[j*3+2] *
-      this->sdf->GetValueVector3("scale").z;
-  }
-
-  // Build the ODE triangle mesh
-  dGeomTriMeshDataBuildSingle(this->odeData,
-      this->vertices, 3*sizeof(this->vertices[0]), numVertices,
-      this->indices, numIndices, 3*sizeof(this->indices[0]));
-
-  if (pcollision->GetCollisionId() == NULL)
-  {
-    pcollision->SetSpaceId(dSimpleSpaceCreate(pcollision->GetSpaceId()));
-    pcollision->SetCollision(dCreateTriMesh(pcollision->GetSpaceId(),
-          this->odeData, 0, 0, 0), true);
-  }
-  else
-  {
-    dGeomTriMeshSetData(pcollision->GetCollisionId(), this->odeData);
-  }
-
-  memset(this->transform, 0, 32*sizeof(dReal));
-  this->transformIndex = 0;
+//   TrimeshShape::Init();
+//   if (!this->mesh)
+//     return;
+// 
+//   RTQL8CollisionPtr pcollision =
+//     boost::shared_static_cast<RTQL8Collision>(this->collisionParent);
+// 
+//   /// This will hold the vertex data of the triangle mesh
+//   if (this->odeData == NULL)
+//     this->odeData = dGeomTriMeshDataCreate();
+// 
+//   unsigned int numVertices = this->mesh->GetVertexCount();
+//   unsigned int numIndices = this->mesh->GetIndexCount();
+//   this->vertices = NULL;
+//   this->indices = NULL;
+// 
+//   // Get all the vertex and index data
+//   this->mesh->FillArrays(&this->vertices, &this->indices);
+// 
+//   // Scale the vertex data
+//   for (unsigned int j = 0;  j < numVertices; j++)
+//   {
+//     this->vertices[j*3+0] = this->vertices[j*3+0] *
+//       this->sdf->GetValueVector3("scale").x;
+//     this->vertices[j*3+1] = this->vertices[j*3+1] *
+//       this->sdf->GetValueVector3("scale").y;
+//     this->vertices[j*3+2] = this->vertices[j*3+2] *
+//       this->sdf->GetValueVector3("scale").z;
+//   }
+// 
+//   // Build the RTQL8 triangle mesh
+//   dGeomTriMeshDataBuildSingle(this->odeData,
+//       this->vertices, 3*sizeof(this->vertices[0]), numVertices,
+//       this->indices, numIndices, 3*sizeof(this->indices[0]));
+// 
+//   if (pcollision->GetCollisionId() == NULL)
+//   {
+//     pcollision->SetSpaceId(dSimpleSpaceCreate(pcollision->GetSpaceId()));
+//     pcollision->SetCollision(dCreateTriMesh(pcollision->GetSpaceId(),
+//           this->odeData, 0, 0, 0), true);
+//   }
+//   else
+//   {
+//     dGeomTriMeshSetData(pcollision->GetCollisionId(), this->odeData);
+//   }
+// 
+//   memset(this->transform, 0, 32*sizeof(dReal));
+//   this->transformIndex = 0;
 }

gazebo/physics/rtql8/RTQL8TrimeshShape.hh

  * Date: 16 Oct 2009
  */
 
-#ifndef _ODETRIMESHSHAPE_HH_
-#define _ODETRIMESHSHAPE_HH_
+#ifndef _RTQL8TRIMESHSHAPE_HH_
+#define _RTQL8TRIMESHSHAPE_HH_
 
 #include "gazebo/physics/TrimeshShape.hh"
 
   namespace physics
   {
     /// \brief Triangle mesh collision.
-    class ODETrimeshShape : public TrimeshShape
+    class RTQL8TrimeshShape : public TrimeshShape
     {
       /// \brief Constructor.
       /// \param[in] _parent Parent collision object.
-      public: explicit ODETrimeshShape(CollisionPtr _parent);
+      public: explicit RTQL8TrimeshShape(CollisionPtr _parent);
 
       /// \brief Destructor.
-      public: virtual ~ODETrimeshShape();
+      public: virtual ~RTQL8TrimeshShape();
 
       // Documentation inherited
       public: virtual void Load(sdf::ElementPtr _sdf);
       public: virtual void Update();
 
       /// \brief Transform matrix.
-      private: dReal transform[16*2];
+      //private: dReal transform[16*2];
 
       /// \brief Transform matrix index.
       private: int transformIndex;
       /// \brief Array of index values.
       private: int *indices;
 
-      /// \brief ODE trimesh data.
-      private: dTriMeshDataID odeData;
+      /// \brief RTQL8 trimesh data.
+      //private: dTriMeshDataID odeData;
     };
   }
 }
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.