added 3d picking capability to OdeWorld

git-svn-id: svn://anubis/misc/OdeWorld@235 bd8a9e45-a331-0410-811e-c64571078777

OdeWorld.cc

@@ -3,6 +3,7 @@
 #include <vector>
 #include <fstream>
 #include <iostream>
+#include <algorithm>                /* std::sort() */
 #include <GL/gl.h>
 using namespace std;
 
@@ -12,8 +13,8 @@ using namespace std;
 void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
 {
     const int maxNumContacts = 4;
-    OdeWorld * ow = (OdeWorld *) data;
     static dContact contact[maxNumContacts];
+    OdeWorld * ow = (OdeWorld *) data;
     dBodyID b1 = dGeomGetBody(o1);
     dBodyID b2 = dGeomGetBody(o2);
     if ((b1 == b2) || (b1 && b2 && dAreConnected(b1, b2)))
@@ -37,6 +38,19 @@ void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
     }
 }
 
+void OdeWorld_pick_collide_callback(void * data, dGeomID o1, dGeomID o2)
+{
+    dContactGeom contact_geom;
+    OdeWorld * ow = (OdeWorld *) data;
+    int num = dCollide(o1, o2, 1, &contact_geom, sizeof(contact_geom));
+    if (num > 0)
+    {
+        dGeomID other = o1 == ow->m_pick_ray ? o2 : o1;
+        ow->m_pick_points.push_back(
+                new OdeWorld::PickPoint(other, contact_geom.depth));
+    }
+}
+
 OdeWorld::OdeWorld()
 {
     m_world = dWorldCreate();
@@ -126,6 +140,33 @@ dJointID OdeWorld::createHinge(dBodyID b1, dBodyID b2,
     return j;
 }
 
+static bool RPPickPointComparator(const refptr<OdeWorld::PickPoint> & one,
+        const refptr<OdeWorld::PickPoint> & two)
+{
+    return one->dist < two->dist;
+}
+
+refptr< vector<OdeWorld::Object *> > OdeWorld::pickObjects(
+        float start_x, float start_y, float start_z,
+        float dir_x, float dir_y, float dir_z)
+{
+    m_pick_ray = dCreateRay(0, dInfinity);
+    dGeomRaySet(m_pick_ray, start_x, start_y, start_z, dir_x, dir_y, dir_z);
+    m_pick_points.clear();
+    dSpaceCollide2(m_pick_ray, (dGeomID) m_space,
+            this, OdeWorld_pick_collide_callback);
+    std::sort(m_pick_points.begin(), m_pick_points.end(),
+            RPPickPointComparator);
+    refptr< vector<Object *> > ret = new vector<Object *>();
+    for (vector< refptr<PickPoint> >::const_iterator it = m_pick_points.begin();
+         it != m_pick_points.end();
+         it++)
+    {
+        ret->push_back((Object *) dGeomGetData((*it)->geom));
+    }
+    return ret;
+}
+
 void OdeWorld::destroyBody(dBodyID body)
 {
     m_bodies.erase(body);
@@ -134,8 +175,7 @@ void OdeWorld::destroyBody(dBodyID body)
 }
 
 OdeWorld::Object::Object(bool is_static, OdeWorld * ode_world,
-        dWorldID world, dSpaceID space,
+        dWorldID world, dSpaceID space, float scale)
-        float scale)
 {
     m_is_static = is_static;
     m_world = world;
@@ -290,6 +330,7 @@ dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body)
             dGeomSetPosition(id, pos[0], pos[1], pos[2]);
             dGeomSetRotation(id, rot);
         }
+        dGeomSetData(id, dGeomGetData(geom));
     }
     return id;
 }
@@ -302,6 +343,7 @@ bool OdeWorld::Object::addBox(refptr< std::vector<float> > args)
             m_scale * (*args)[0],
             m_scale * (*args)[1],
             m_scale * (*args)[2]);
+    dGeomSetData(id, this);
     dMass mass;
     dMassSetBox(&mass, 1.0,
             m_scale * (*args)[0],
@@ -318,6 +360,7 @@ bool OdeWorld::Object::addSphere(refptr< std::vector<float> > args)
     if (args->size() != 4)
         return false;
     dGeomID id = dCreateSphere(0, m_scale * (*args)[0]);
+    dGeomSetData(id, this);
     dMass mass;
     dMassSetSphere(&mass, 1.0, m_scale * (*args)[0]);
     setupGeom(id, &mass,
@@ -331,6 +374,7 @@ bool OdeWorld::Object::addCylinder(refptr< std::vector<float> > args)
     if (args->size() != 8)
         return false;
     dGeomID id = dCreateCylinder(0, m_scale * (*args)[0], m_scale * (*args)[1]);
+    dGeomSetData(id, this);
     dMass mass;
     dMassSetCylinder(&mass, 1.0, 3, m_scale * (*args)[0], m_scale * (*args)[1]);
     setupGeom(id, &mass,
@@ -345,6 +389,7 @@ bool OdeWorld::Object::addCapsule(refptr< std::vector<float> > args)
         return false;
     dGeomID id = dCreateCapsule(0,
             m_scale * (*args)[0], m_scale * (*args)[1]);
+    dGeomSetData(id, this);
     dMass mass;
     dMassSetCapsule(&mass, 1.0, 3, m_scale * (*args)[0],
             m_scale * (*args)[1]);
@@ -383,6 +428,7 @@ bool OdeWorld::Object::addPlane(refptr< std::vector<float> > args)
     }
 
     dGeomID id = dCreatePlane(m_space, a, b, c, d);
+    dGeomSetData(id, this);
     m_geoms.push_back(id);
     return true;
 }
@@ -404,6 +450,7 @@ void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass,
     dGeomTransformSetCleanup(transform, 1);
     dGeomTransformSetInfo(transform, 1);
     dGeomTransformSetGeom(transform, geom);
+    dGeomSetData(transform, this);
 
     m_geoms.push_back(transform);
 

OdeWorld.h

@@ -17,15 +17,23 @@ class OdeWorld
     public:
         enum GeomType { BOX, SPHERE, PLANE, CYLINDER, CAPSULE };
 
-        OdeWorld();
+        class PickPoint
-        ~OdeWorld();
+        {
+            public:
+                dGeomID     geom;
+                float       dist;
+
+                PickPoint(dGeomID g, float d)
+                    : geom(g), dist(d)
+                {
+                }
+        };
 
         class Object
         {
             public:
                 Object(bool is_static, OdeWorld * ode_world,
-                        dWorldID world, dSpaceID space,
+                        dWorldID world, dSpaceID space, float scale = 1.0f);
-                        float scale = 1.0f);
                 Object(const Object & orig);
                 ~Object();
 
@@ -74,6 +82,9 @@ class OdeWorld
                 dGeomID cloneGeom(dGeomID geom, dBodyID body);
         };
 
+        OdeWorld();
+        ~OdeWorld();
+
         Object * createObject(bool is_static, float scale = 1.0f);
         void setGravity(float x, float y, float z)
         {
@@ -145,10 +156,16 @@ class OdeWorld
 #endif
             dJointSetAMotorParam(j, dParamBounce, val);
         }
+        refptr< std::vector<Object *> > pickObjects(
+                float start_x, float start_y, float start_z,
+                float dir_x, float dir_y, float dir_z);
 
         friend void OdeWorld_collide_callback(void * data,
                 dGeomID o1, dGeomID o2);
 
+        friend void OdeWorld_pick_collide_callback(void * data,
+                dGeomID o1, dGeomID o2);
+
         static void pushTransform(const dReal * pos, const dReal * R);
 
     protected:
@@ -159,6 +176,8 @@ class OdeWorld
         dSpaceID        m_space;
         dJointGroupID   m_contactJointGroup;
         std::map<dBodyID, int>  m_bodies;
+        dGeomID         m_pick_ray;
+        std::vector< refptr<PickPoint> > m_pick_points;
 };
 
 #endif