josh/OdeWorld
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added 3d picking capability to OdeWorld

Browse Source 
git-svn-id: svn://anubis/misc/OdeWorld@235 bd8a9e45-a331-0410-811e-c64571078777
This commit is contained in:
josh 2010-02-21 17:10:01 +00:00
parent c9d3242314
commit ddf453379e
2 changed files with 73 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
OdeWorld.cc
View File

@ -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,
float scale)
dWorldID world, dSpaceID space, 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);

27
OdeWorld.h
View File

@ -17,15 +17,23 @@ class OdeWorld
public:
enum GeomType { BOX, SPHERE, PLANE, CYLINDER, CAPSULE };
OdeWorld();
~OdeWorld();
class PickPoint
{
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,
float scale = 1.0f);
dWorldID world, dSpaceID space, 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