diff --git a/OdeWorld.cc b/OdeWorld.cc index b812640..2d53712 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -157,9 +157,38 @@ refptr< vector< OdeWorld::PickedObjectRef > > OdeWorld::pickObjects( this, OdeWorld_pick_collide_callback); std::sort(m_pick_points.begin(), m_pick_points.end(), RPPickedObjectComparator); + dGeomDestroy(m_pick_ray); return new vector(m_pick_points); } +OdeWorld::PickedObjectRef OdeWorld::pickOne( + float start_x, float start_y, float start_z, + float dir_x, float dir_y, float dir_z, + OdeWorld::Object * o) +{ + dGeomID pick_ray = dCreateRay(0, dInfinity); + dGeomRaySet(pick_ray, start_x, start_y, start_z, dir_x, dir_y, dir_z); + + PickedObjectRef por; + bool found_one = false; + + dContactGeom contact_geom; + for (vector::iterator it = o->getGeoms().begin(); + it != o->getGeoms().end(); + it++) + { + dGeomID g = *it; + int num = dCollide(pick_ray, g, 1, &contact_geom, sizeof(contact_geom)); + if (num > 0 && (!found_one || contact_geom.depth < por->dist)) + { + por = new OdeWorld::PickedObject(g, contact_geom); + found_one = true; + } + } + dGeomDestroy(pick_ray); + return por; +} + void OdeWorld::destroyBody(dBodyID body) { m_bodies.erase(body); diff --git a/OdeWorld.h b/OdeWorld.h index dedf6c7..de75333 100644 --- a/OdeWorld.h +++ b/OdeWorld.h @@ -56,6 +56,7 @@ class OdeWorld m_user_data = user_data; } void * getUserData() { return m_user_data; } + std::vector & getGeoms() { return m_geoms; } protected: bool m_is_static; @@ -175,6 +176,10 @@ class OdeWorld refptr< std::vector > pickObjects( float start_x, float start_y, float start_z, float dir_x, float dir_y, float dir_z); + PickedObjectRef pickOne( + float start_x, float start_y, float start_z, + float dir_x, float dir_y, float dir_z, + Object * o); friend void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2);