Index: ode.c
===================================================================
--- ode.c	(revision 1111)
+++ ode.c	(revision 1112)
@@ -5509,116 +5509,144 @@
 }
 
 
-int raydium_ode_mouse_pick(dReal dist,dReal pos[3],dReal *depth)
+int raydium_ode_ray_launch(dReal *from, dReal *to, dReal max_dist,
+                            dReal *touched_dist, dReal *touched_pos,
+                            signed char (*filter)(int,int, dContact *))
 {
-    GLdouble dX, dY, dZ;
-    int id;
-    dReal min_dist;
-    dGeomID ray;
-    dContact pt;
-    float mx,my;
-    signed char (*f)(int,int, dContact *);
+int touched_element_id;
+dGeomID ray;
+dReal vect[3];
+dContact pt;
 
-    f=raydium_ode_PickCallback;
+vect[0]=to[0]-from[0];
+vect[1]=to[1]-from[1];
+vect[2]=to[2]-from[2];
 
-    if(!raydium_mouse_mode_delta)
-        {
-        mx=raydium_mouse_x;
-        my=raydium_mouse_y;
-        }
-    else
-        {
-        mx=raydium_window_tx/2;
-        my=raydium_window_ty/2;
-        }
+ray=dCreateRay(raydium_ode_object[raydium_ode_object_find("GLOBAL")].group,max_dist);
+dGeomRaySet(ray, from[0], from[1],from[2],vect[0], vect[1], vect[2]);
 
-    // Get mouse pointed coordinate
-    gluUnProject( (float)mx, (float)(raydium_window_ty - my), (float) -1.0, raydium_camera_gl_modelview, raydium_camera_gl_projection, raydium_camera_gl_viewport, &dX, &dY, &dZ);
+touched_element_id=-1;
+*touched_dist=max_dist;
 
-    //Create Ray
-    ray =  dCreateRay (raydium_ode_object[raydium_ode_object_find("GLOBAL")].group,dist);
-    // Set ray origin and dist
-    dGeomRaySet (ray, raydium_camera_x, raydium_camera_y,raydium_camera_z,dX-raydium_camera_x, dY-raydium_camera_y, dZ-raydium_camera_z);
-    //dGeomRaySetClosestHit(ray, true);
-
-    id=-1;
-    min_dist=dist;
-
     {
     // Private callback for ray picking only
     void dNearPickback (void *data, dGeomID o1, dGeomID o2)
         {
-            #define N 400
-            static  dContact contact[N];
-            int i,n;
+        #define N 400
+        static  dContact contact[N];
+        int i,n;
 
-            // Recurse into space
-            if(dGeomIsSpace (o1) || dGeomIsSpace (o2))
-            {
-                raydium_ode_Object *oo1, *oo2;
-                signed char (*g)(int,int);
-                oo1=dGeomGetData(o1);
-                oo2=dGeomGetData(o2);
-                g=raydium_ode_ObjectNearCollide;
-                if(g && !g(oo1->id,oo2->id)) return;
-                dSpaceCollide2 (o1,o2,data,&dNearPickback);
-                return;
-            }
+        if (o1==o2) // Quickly filter same object check.
+            return;
 
+        // Recurse into space
+        if(dGeomIsSpace (o1) || dGeomIsSpace (o2))
+        {
+            raydium_ode_Object *oo1, *oo2;
+            signed char (*g)(int,int);
+            oo1=dGeomGetData(o1);
+            oo2=dGeomGetData(o2);
+            g=raydium_ode_ObjectNearCollide;
+            if(g && !g(oo1->id,oo2->id)) return;
+            dSpaceCollide2 (o1,o2,data,&dNearPickback);
+            return;
+        }
 
-            if (o1==ray || o2==ray) {
-                raydium_ode_Element *e1;
+        if (o1==ray || o2==ray)
+            {
+            raydium_ode_Element *e1;
 
-                //Should never exist, but in case of swap o2 ray, o1 object
-                if (o2!=ray){
-                    o1=o2;
-                    o2=ray;
+            //Should never exist, but in case of swap o2 ray, o1 object
+            if (o2!=ray)
+                {
+                o1=o2;
+                o2=ray;
                 }
 
-                e1=dGeomGetData(o1);
+            e1=dGeomGetData(o1);
 
-                n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
+            // Filter same object detection (no ?)
+            /*if (ro1==ro2)
+                return;*/
 
-                for (i=0; i<n; i++) {
+            // Raydium normal ray callback is not used here !
+            n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
 
-                    if(f)
+            for (i=0; i<n; i++)
+                {
+                if(filter)
                     {
-                        int id1;
-                        id1=-1;
-                        if(e1) id1=e1->id;
-                        // Can't test between two object since ray not wrapped to a raydium_element
-                        if(!f(id1,id1,&contact[i])) continue;
+                    int id1;
+                    id1=-1;
+                    if(e1) id1=e1->id;
+                    // Can't test between two object since ray not wrapped to a raydium_element
+                    if(!filter(id1,id1,&contact[i])) continue;
                     }
 
-                    if (contact[i].geom.g1==ray)
-                        if (contact[i].geom.depth<min_dist){
-                            min_dist=contact[i].geom.depth;
-                                memcpy(&pt,&contact[i],sizeof(dContact));
-                            id = e1->id;
+                if (contact[i].geom.g1==ray)
+                    if (contact[i].geom.depth<*touched_dist)
+                        {
+                        *touched_dist=contact[i].geom.depth;
+                            memcpy(&pt,&contact[i],sizeof(dContact));
+                        touched_element_id = e1->id;
                         }
 
-                    if (contact[i].geom.g2==ray)
-                        if (contact[i].geom.depth<min_dist){
-                            min_dist=contact[i].geom.depth;
-                            memcpy(&pt,&contact[i],sizeof(dContact));
-                            id = e1->id;
+                if (contact[i].geom.g2==ray)
+                    if (contact[i].geom.depth<*touched_dist)
+                        {
+                        *touched_dist=contact[i].geom.depth;
+                        memcpy(&pt,&contact[i],sizeof(dContact));
+                        touched_element_id = e1->id;
                         }
                 }
             }
         }
+    dSpaceCollide2((dGeomID) raydium_ode_space,ray,(void *) NULL,&dNearPickback);
+    }
 
-        dSpaceCollide2((dGeomID) raydium_ode_space,ray,(void *) NULL,&dNearPickback);
+dGeomDestroy(ray);
+
+if(touched_element_id>=0 && touched_pos!=NULL)
+    {
+    touched_pos[0]=pt.geom.pos[0];
+    touched_pos[1]=pt.geom.pos[1];
+    touched_pos[2]=pt.geom.pos[2];
     }
 
-    dGeomDestroy(ray);
+return touched_element_id;
+}
 
-    pos[0]=pt.geom.pos[0];
-    pos[1]=pt.geom.pos[1];
-    pos[2]=pt.geom.pos[2];
-    *depth = pt.geom.depth;
+int raydium_ode_mouse_pick(dReal dist,dReal pos[3],dReal *depth)
+{
+    GLdouble dX, dY, dZ;
+    float mx,my;
+    dReal from[3];
+    dReal to[3];
 
-    return id;
+    if(!raydium_mouse_mode_delta)
+        {
+        mx=raydium_mouse_x;
+        my=raydium_mouse_y;
+        }
+    else
+        {
+        mx=raydium_window_tx/2;
+        my=raydium_window_ty/2;
+        }
 
+    // Get mouse pointed coordinate
+    gluUnProject( (float)mx, (float)(raydium_window_ty - my), (float) -1.0, raydium_camera_gl_modelview, raydium_camera_gl_projection, raydium_camera_gl_viewport, &dX, &dY, &dZ);
+
+
+    from[0]=raydium_camera_x;
+    from[1]=raydium_camera_y;
+    from[2]=raydium_camera_z;
+
+    to[0]=dX;
+    to[1]=dY;
+    to[2]=dZ;
+
+    return raydium_ode_ray_launch(from,to,dist,depth,pos,raydium_ode_PickCallback);
 }
 
 void raydium_ode_set_physics_freq(GLfloat freq)