import svn:externals
This commit is contained in:
Josh Holtrop committed
1 parent
d3da66da4b
commit
7b057bcf86
17 files changed
+2275
No files matched your search
@@ -0,0 +1,12 @@
|
||||
|
||||
TARGET := OdeWorld.o
|
||||
SOURCES := $(wildcard *.cc)
|
||||
HEADERS := $(wildcard *.h)
|
||||
|
||||
all: $(TARGET)
|
||||
|
||||
$(TARGET): $(SOURCES) $(HEADERS)
|
||||
$(CXX) -c -o $@ $(CXXFLAGS) $(SOURCES)
|
||||
|
||||
clean:
|
||||
-rm -f *~ *.o
|
||||
@@ -0,0 +1,660 @@
|
||||
|
||||
#include "OdeWorld.h"
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <algorithm> /* std::sort() */
|
||||
#include <GL/gl.h>
|
||||
using namespace std;
|
||||
|
||||
#define WORLD_STEP 0.001
|
||||
|
||||
/* used by ODE to perform collision detection */
|
||||
void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
|
||||
{
|
||||
const int maxNumContacts = 4;
|
||||
static dContact contact[maxNumContacts];
|
||||
OdeWorld * ow = (OdeWorld *) data;
|
||||
dBodyID b1 = dGeomGetBody(o1);
|
||||
dBodyID b2 = dGeomGetBody(o2);
|
||||
if ((b1 == b2) || (b1 && b2 && dAreConnected(b1, b2)))
|
||||
return;
|
||||
int num = dCollide(o1, o2, maxNumContacts,
|
||||
&contact[0].geom, sizeof(contact[0]));
|
||||
for (int i = 0; i < num; i++)
|
||||
{
|
||||
contact[i].surface.mode =
|
||||
dContactSlip1 | dContactSlip2 | dContactBounce |
|
||||
dContactSoftERP | dContactSoftCFM | dContactApprox1;
|
||||
contact[i].surface.mu = 0.5;
|
||||
contact[i].surface.slip1 = 0.0;
|
||||
contact[i].surface.slip2 = 0.0;
|
||||
contact[i].surface.soft_erp = 0.1;
|
||||
contact[i].surface.soft_cfm = 0.01;
|
||||
contact[i].surface.bounce = 0.0;
|
||||
dJointID joint = dJointCreateContact(ow->m_world,
|
||||
ow->m_contactJointGroup, contact + i);
|
||||
dJointAttach(joint, b1, b2);
|
||||
}
|
||||
}
|
||||
|
||||
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::PickedObject(other, contact_geom));
|
||||
}
|
||||
}
|
||||
|
||||
OdeWorld::OdeWorld()
|
||||
{
|
||||
m_world = dWorldCreate();
|
||||
dWorldSetAutoDisableFlag(m_world, 1);
|
||||
m_space = dHashSpaceCreate(0);
|
||||
m_contactJointGroup = dJointGroupCreate(0);
|
||||
setGravity(0, 0, -9.81);
|
||||
}
|
||||
|
||||
OdeWorld::~OdeWorld()
|
||||
{
|
||||
dJointGroupDestroy(m_contactJointGroup);
|
||||
dSpaceDestroy(m_space);
|
||||
dWorldDestroy(m_world);
|
||||
}
|
||||
|
||||
/* invokes ODE to do physics on our world */
|
||||
void OdeWorld::step()
|
||||
{
|
||||
dSpaceCollide(m_space, this, OdeWorld_collide_callback);
|
||||
dWorldQuickStep(m_world, WORLD_STEP);
|
||||
dJointGroupEmpty(m_contactJointGroup);
|
||||
}
|
||||
|
||||
OdeWorld::Object * OdeWorld::createObject(bool is_static, float scale)
|
||||
{
|
||||
return new Object(is_static, this, m_world, m_space, scale);
|
||||
}
|
||||
|
||||
/* push an OpenGL matrix onto the matrix stack for a given
|
||||
* ODE body position and rotation */
|
||||
void OdeWorld::pushTransform(const dReal * pos, const dReal * R)
|
||||
{
|
||||
GLfloat matrix[16];
|
||||
matrix[0] = R[0];
|
||||
matrix[1] = R[4];
|
||||
matrix[2] = R[8];
|
||||
matrix[3] = 0;
|
||||
matrix[4] = R[1];
|
||||
matrix[5] = R[5];
|
||||
matrix[6] = R[9];
|
||||
matrix[7] = 0;
|
||||
matrix[8] = R[2];
|
||||
matrix[9] = R[6];
|
||||
matrix[10] = R[10];
|
||||
matrix[11] = 0;
|
||||
matrix[12] = pos[0];
|
||||
matrix[13] = pos[1];
|
||||
matrix[14] = pos[2];
|
||||
matrix[15] = 1;
|
||||
glPushMatrix();
|
||||
glMultMatrixf(matrix);
|
||||
}
|
||||
|
||||
dBodyID OdeWorld::createBody()
|
||||
{
|
||||
dBodyID id = dBodyCreate(m_world);
|
||||
m_bodies[id] = 1;
|
||||
return id;
|
||||
}
|
||||
|
||||
void OdeWorld::enableAllBodies()
|
||||
{
|
||||
for (std::map<dBodyID, int>::iterator it = m_bodies.begin();
|
||||
it != m_bodies.end();
|
||||
it++)
|
||||
{
|
||||
dBodyEnable(it->first);
|
||||
}
|
||||
}
|
||||
|
||||
dJointID OdeWorld::createAMotor(dBodyID b1, dBodyID b2)
|
||||
{
|
||||
dJointID j = dJointCreateAMotor(m_world, 0);
|
||||
dJointAttach(j, b1, b2);
|
||||
return j;
|
||||
}
|
||||
|
||||
dJointID OdeWorld::createHinge(dBodyID b1, dBodyID b2,
|
||||
dReal anchor_x, dReal anchor_y, dReal anchor_z,
|
||||
dReal axis_x, dReal axis_y, dReal axis_z)
|
||||
{
|
||||
dJointID j = dJointCreateHinge(m_world, 0);
|
||||
dJointAttach(j, b1, b2);
|
||||
dJointSetHingeAnchor(j, anchor_x, anchor_y, anchor_z);
|
||||
dJointSetHingeAxis(j, axis_x, axis_y, axis_z);
|
||||
return j;
|
||||
}
|
||||
|
||||
static bool RPPickedObjectComparator(const refptr<OdeWorld::PickedObject> & one,
|
||||
const refptr<OdeWorld::PickedObject> & two)
|
||||
{
|
||||
return one->dist < two->dist;
|
||||
}
|
||||
|
||||
refptr< vector< OdeWorld::PickedObjectRef > > 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(),
|
||||
RPPickedObjectComparator);
|
||||
dGeomDestroy(m_pick_ray);
|
||||
return new vector<OdeWorld::PickedObjectRef>(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<dGeomID>::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);
|
||||
dBodyDestroy(body);
|
||||
enableAllBodies();
|
||||
}
|
||||
|
||||
OdeWorld::Object::Object(bool is_static, OdeWorld * ode_world,
|
||||
dWorldID world, dSpaceID space, float scale)
|
||||
{
|
||||
m_is_static = is_static;
|
||||
m_world = world;
|
||||
m_space = space;
|
||||
m_body = 0;
|
||||
m_scale = scale;
|
||||
m_ode_world = ode_world;
|
||||
dMassSetZero(&m_mass);
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_position[i] = 0.0;
|
||||
for (int j = 0; j < 3; j++)
|
||||
for (int i = 0; i < 4; i++)
|
||||
m_rotation[j * 4 + i] = (j == i) ? 1.0 : 0.0;
|
||||
}
|
||||
|
||||
OdeWorld::Object::Object(const OdeWorld::Object & orig)
|
||||
{
|
||||
m_is_static = orig.m_is_static;
|
||||
m_world = orig.m_world;
|
||||
m_space = orig.m_space;
|
||||
m_scale = orig.m_scale;
|
||||
m_ode_world = orig.m_ode_world;
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_position[i] = orig.m_position[i];
|
||||
for (int i = 0; i < 12; i++)
|
||||
m_rotation[i] = orig.m_rotation[i];
|
||||
/* make a copy of the ODE body */
|
||||
if (orig.m_body != 0)
|
||||
{
|
||||
m_body = m_ode_world->createBody();
|
||||
m_mass = orig.m_mass;
|
||||
dBodySetMass(m_body, &m_mass);
|
||||
const dReal * pos = dBodyGetPosition(orig.m_body);
|
||||
dBodySetPosition(m_body, pos[0], pos[1], pos[2]);
|
||||
const dReal * rot = dBodyGetRotation(orig.m_body);
|
||||
dBodySetRotation(m_body, rot);
|
||||
dBodySetAutoDisableFlag(m_body,
|
||||
dBodyGetAutoDisableFlag(orig.m_body));
|
||||
dBodySetAutoDisableLinearThreshold(m_body,
|
||||
dBodyGetAutoDisableLinearThreshold(orig.m_body));
|
||||
dBodySetAutoDisableAngularThreshold(m_body,
|
||||
dBodyGetAutoDisableAngularThreshold(orig.m_body));
|
||||
dBodySetAutoDisableSteps(m_body,
|
||||
dBodyGetAutoDisableSteps(orig.m_body));
|
||||
dBodySetAutoDisableTime(m_body,
|
||||
dBodyGetAutoDisableTime(orig.m_body));
|
||||
int finite_rotation_mode = dBodyGetFiniteRotationMode(orig.m_body);
|
||||
dBodySetFiniteRotationMode(m_body, finite_rotation_mode);
|
||||
if (finite_rotation_mode)
|
||||
{
|
||||
dVector3 finite_rotation_axis;
|
||||
dBodyGetFiniteRotationAxis(orig.m_body, finite_rotation_axis);
|
||||
dBodySetFiniteRotationAxis(m_body,
|
||||
finite_rotation_axis[0],
|
||||
finite_rotation_axis[1],
|
||||
finite_rotation_axis[2]);
|
||||
}
|
||||
dBodySetGravityMode(m_body, dBodyGetGravityMode(orig.m_body));
|
||||
}
|
||||
else
|
||||
{
|
||||
m_body = 0;
|
||||
}
|
||||
/* make a copy of the ODE geoms */
|
||||
for (int i = 0, sz = orig.m_geoms.size(); i < sz; i++)
|
||||
{
|
||||
m_geoms.push_back(cloneGeom(orig.m_geoms[i], m_body));
|
||||
}
|
||||
}
|
||||
|
||||
OdeWorld::Object::~Object()
|
||||
{
|
||||
for (int i = 0, sz = m_geoms.size(); i < sz; i++)
|
||||
dGeomDestroy(m_geoms[i]);
|
||||
if (m_body != 0)
|
||||
m_ode_world->destroyBody(m_body);
|
||||
}
|
||||
|
||||
dGeomID OdeWorld::Object::cloneGeom(dGeomID geom, dBodyID body)
|
||||
{
|
||||
dGeomID id = 0;
|
||||
dSpaceID space = dGeomGetSpace(geom);
|
||||
switch (dGeomGetClass(geom))
|
||||
{
|
||||
case dSphereClass:
|
||||
id = dCreateSphere(space, dGeomSphereGetRadius(geom));
|
||||
break;
|
||||
case dBoxClass:
|
||||
{
|
||||
dVector3 size;
|
||||
dGeomBoxGetLengths(geom, size);
|
||||
id = dCreateBox(space, size[0], size[1], size[2]);
|
||||
}
|
||||
break;
|
||||
case dCylinderClass:
|
||||
{
|
||||
dReal radius, length;
|
||||
dGeomCylinderGetParams(geom, &radius, &length);
|
||||
id = dCreateCylinder(space, radius, length);
|
||||
}
|
||||
break;
|
||||
case dCapsuleClass:
|
||||
{
|
||||
dReal radius, length;
|
||||
dGeomCapsuleGetParams(geom, &radius, &length);
|
||||
id = dCreateCapsule(space, radius, length);
|
||||
}
|
||||
break;
|
||||
case dPlaneClass:
|
||||
{
|
||||
dVector4 params;
|
||||
dGeomPlaneGetParams(geom, params);
|
||||
id = dCreatePlane(space,
|
||||
params[0], params[1], params[2], params[3]);
|
||||
}
|
||||
break;
|
||||
case dGeomTransformClass:
|
||||
{
|
||||
dGeomID old_inner_geom = dGeomTransformGetGeom(geom);
|
||||
dGeomID new_inner_geom = cloneGeom(old_inner_geom, body);
|
||||
if (new_inner_geom != 0)
|
||||
{
|
||||
id = dCreateGeomTransform(space);
|
||||
dGeomTransformSetGeom(id, new_inner_geom);
|
||||
dGeomTransformSetCleanup(id,
|
||||
dGeomTransformGetCleanup(geom));
|
||||
dGeomTransformSetInfo(id, dGeomTransformGetInfo(geom));
|
||||
}
|
||||
}
|
||||
break;
|
||||
case dRayClass:
|
||||
case dTriMeshClass:
|
||||
case dSimpleSpaceClass:
|
||||
case dHashSpaceClass:
|
||||
/* unsupported for cloning */
|
||||
break;
|
||||
}
|
||||
if (id != 0)
|
||||
{
|
||||
if (dGeomGetBody(geom) != 0)
|
||||
{
|
||||
/* if the original geom was attached to a body
|
||||
* (i.e., a non-static geom), then attach it to the new body */
|
||||
dGeomSetBody(id, body);
|
||||
}
|
||||
else if (dGeomGetClass(id) != dPlaneClass)
|
||||
{
|
||||
/* if the original geom was static, then copy the
|
||||
* position and rotation to the new geom */
|
||||
const dReal * pos = dGeomGetPosition(geom);
|
||||
const dReal * rot = dGeomGetRotation(geom);
|
||||
dGeomSetPosition(id, pos[0], pos[1], pos[2]);
|
||||
dGeomSetRotation(id, rot);
|
||||
}
|
||||
dGeomSetData(id, dGeomGetData(geom));
|
||||
}
|
||||
return id;
|
||||
}
|
||||
|
||||
bool OdeWorld::Object::addBox(refptr< std::vector<float> > args)
|
||||
{
|
||||
if (args->size() != 9)
|
||||
return false;
|
||||
dGeomID id = dCreateBox(0,
|
||||
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],
|
||||
m_scale * (*args)[1],
|
||||
m_scale * (*args)[2]);
|
||||
setupGeom(id, &mass,
|
||||
(*args)[3], (*args)[4], (*args)[5],
|
||||
(*args)[6], (*args)[7], (*args)[8]);
|
||||
return true;
|
||||
}
|
||||
|
||||
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,
|
||||
(*args)[1], (*args)[2], (*args)[3],
|
||||
0.0, 0.0, 0.0);
|
||||
return true;
|
||||
}
|
||||
|
||||
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,
|
||||
(*args)[2], (*args)[3], (*args)[4],
|
||||
(*args)[5], (*args)[6], (*args)[7]);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OdeWorld::Object::addCapsule(refptr< std::vector<float> > args)
|
||||
{
|
||||
if (args->size() != 8)
|
||||
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]);
|
||||
setupGeom(id, &mass,
|
||||
(*args)[2], (*args)[3], (*args)[4],
|
||||
(*args)[5], (*args)[6], (*args)[7]);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OdeWorld::Object::addPlane(refptr< std::vector<float> > args)
|
||||
{
|
||||
float a, b, c, d;
|
||||
if (args->size() == 6)
|
||||
{
|
||||
dMatrix3 r;
|
||||
dRFromEulerAngles(r, (*args)[3], (*args)[4], (*args)[5]);
|
||||
dVector3 default_plane_direction = {0, 0, 1, 0};
|
||||
dVector3 rotated_plane_direction;
|
||||
dMultiply0(rotated_plane_direction, default_plane_direction, r, 1, 3, 3);
|
||||
|
||||
a = rotated_plane_direction[0];
|
||||
b = rotated_plane_direction[1];
|
||||
c = rotated_plane_direction[2];
|
||||
d = m_scale * (a * (*args)[0] + b * (*args)[1] + c * (*args)[2]);
|
||||
}
|
||||
else if (args->size() == 4)
|
||||
{
|
||||
a = (*args)[0];
|
||||
b = (*args)[1];
|
||||
c = (*args)[2];
|
||||
d = (*args)[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
dGeomID id = dCreatePlane(m_space, a, b, c, d);
|
||||
dGeomSetData(id, this);
|
||||
m_geoms.push_back(id);
|
||||
return true;
|
||||
}
|
||||
|
||||
void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass,
|
||||
float locx, float locy, float locz,
|
||||
float rotx, float roty, float rotz)
|
||||
{
|
||||
locx = m_scale * locx;
|
||||
locy = m_scale * locy;
|
||||
locz = m_scale * locz;
|
||||
|
||||
dMatrix3 rot;
|
||||
dRFromEulerAngles(rot, rotx, roty, rotz);
|
||||
dGeomSetRotation(geom, rot);
|
||||
dGeomSetPosition(geom, locx, locy, locz);
|
||||
|
||||
dGeomID transform = dCreateGeomTransform(m_space);
|
||||
dGeomTransformSetCleanup(transform, 1);
|
||||
dGeomTransformSetInfo(transform, 1);
|
||||
dGeomTransformSetGeom(transform, geom);
|
||||
dGeomSetData(transform, this);
|
||||
|
||||
m_geoms.push_back(transform);
|
||||
|
||||
if (!m_is_static)
|
||||
{
|
||||
/* attach the geometry to the body */
|
||||
if (m_body == 0)
|
||||
m_body = m_ode_world->createBody();
|
||||
|
||||
dGeomSetBody(transform, m_body);
|
||||
|
||||
dMassRotate(mass, rot);
|
||||
dMassTranslate(mass, locx, locy, locz);
|
||||
dMassAdd(&m_mass, mass);
|
||||
}
|
||||
}
|
||||
|
||||
const dReal * OdeWorld::Object::getPosition()
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
return dBodyGetPosition(m_body);
|
||||
}
|
||||
else if (m_geoms.size() > 0)
|
||||
{
|
||||
if (dGeomGetClass(m_geoms[0]) != dPlaneClass)
|
||||
return dGeomGetPosition(m_geoms[0]);
|
||||
}
|
||||
return m_position;
|
||||
}
|
||||
|
||||
const dReal * OdeWorld::Object::getRotation()
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
return dBodyGetRotation(m_body);
|
||||
}
|
||||
else if (m_geoms.size() > 0)
|
||||
{
|
||||
if (dGeomGetClass(m_geoms[0]) != dPlaneClass)
|
||||
return dGeomGetRotation(m_geoms[0]);
|
||||
}
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
void OdeWorld::Object::setPosition(double x, double y, double z)
|
||||
{
|
||||
if (m_is_static)
|
||||
{
|
||||
for (int i = 0, sz = m_geoms.size(); i < sz; i++)
|
||||
dGeomSetPosition(m_geoms[i], x, y, z);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodySetPosition(m_body, x, y, z);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
m_position[0] = x;
|
||||
m_position[1] = y;
|
||||
m_position[2] = z;
|
||||
}
|
||||
|
||||
void OdeWorld::Object::getPosition(double * x, double * y, double * z)
|
||||
{
|
||||
const dReal * pos = NULL;
|
||||
if (m_is_static)
|
||||
{
|
||||
if (m_geoms.size() > 0)
|
||||
{
|
||||
if (dGeomGetClass(m_geoms[0]) == dPlaneClass)
|
||||
pos = &m_position[0];
|
||||
else
|
||||
pos = dGeomGetPosition(m_geoms[0]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_body != 0)
|
||||
pos = dBodyGetPosition(m_body);
|
||||
}
|
||||
|
||||
if (pos != NULL)
|
||||
{
|
||||
*x = pos[0];
|
||||
*y = pos[1];
|
||||
*z = pos[2];
|
||||
}
|
||||
else
|
||||
{
|
||||
*x = m_position[0];
|
||||
*y = m_position[1];
|
||||
*z = m_position[2];
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::setRotation(dReal x, dReal y, dReal z)
|
||||
{
|
||||
dMatrix3 r;
|
||||
dRFromEulerAngles(r, x, y, z);
|
||||
if (m_is_static)
|
||||
{
|
||||
for (int i = 0, sz = m_geoms.size(); i < sz; i++)
|
||||
dGeomSetRotation(m_geoms[i], r);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodySetRotation(m_body, r);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::addForce(dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodyAddForce(m_body, fx, fy, fz);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::addForceRel(dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodyAddRelForce(m_body, fx, fy, fz);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::addTorque(dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodyAddTorque(m_body, fx, fy, fz);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::addTorqueRel(dReal fx, dReal fy, dReal fz)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodyAddRelTorque(m_body, fx, fy, fz);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::finalize()
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
#if 0
|
||||
cerr << "Warning: translating mass by ("
|
||||
<< (-m_mass.c[0]) << ", "
|
||||
<< (-m_mass.c[1]) << ", "
|
||||
<< (-m_mass.c[2]) << ")" << endl;
|
||||
#endif
|
||||
dMassTranslate(&m_mass, -m_mass.c[0], -m_mass.c[1], -m_mass.c[2]);
|
||||
dBodySetMass(m_body, &m_mass);
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::setMass(dReal newmass)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dMassAdjust(&m_mass, newmass);
|
||||
dBodySetMass(m_body, &m_mass);
|
||||
enableBody();
|
||||
}
|
||||
}
|
||||
|
||||
void OdeWorld::Object::enableBody()
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodyEnable(m_body);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,204 @@
|
||||
|
||||
#ifndef ODEWORLD_H
|
||||
#define ODEWORLD_H
|
||||
|
||||
#include <ode/ode.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include "refptr/refptr.h"
|
||||
#ifdef DEBUG_AMOTOR
|
||||
#include <iostream>
|
||||
using namespace std;
|
||||
#endif
|
||||
|
||||
class OdeWorld
|
||||
{
|
||||
public:
|
||||
enum GeomType { BOX, SPHERE, PLANE, CYLINDER, CAPSULE };
|
||||
|
||||
class Object
|
||||
{
|
||||
public:
|
||||
Object(bool is_static, OdeWorld * ode_world,
|
||||
dWorldID world, dSpaceID space, float scale = 1.0f);
|
||||
Object(const Object & orig);
|
||||
~Object();
|
||||
|
||||
void setPosition(double x, double y, double z);
|
||||
void getPosition(double * x, double * y, double * z);
|
||||
void setRotation(dReal x, dReal y, dReal z);
|
||||
const dReal * getPosition();
|
||||
const dReal * getRotation();
|
||||
bool addBox(refptr< std::vector<float> > args);
|
||||
bool addSphere(refptr< std::vector<float> > args);
|
||||
bool addCylinder(refptr< std::vector<float> > args);
|
||||
bool addCapsule(refptr< std::vector<float> > args);
|
||||
bool addPlane(refptr< std::vector<float> > args);
|
||||
void addForce(dReal fx, dReal fy, dReal fz);
|
||||
void addForceRel(dReal fx, dReal fy, dReal fz);
|
||||
void addTorque(dReal fx, dReal fy, dReal fz);
|
||||
void addTorqueRel(dReal fx, dReal fy, dReal fz);
|
||||
void finalize();
|
||||
dReal getMass() { return m_mass.mass; }
|
||||
void setMass(dReal newmass);
|
||||
void enableBody();
|
||||
dBodyID getBody() { return m_body; }
|
||||
void setGravityMode(bool enabled)
|
||||
{
|
||||
if (m_body != 0)
|
||||
{
|
||||
dBodySetGravityMode(m_body, enabled);
|
||||
}
|
||||
}
|
||||
void setUserData(void * user_data)
|
||||
{
|
||||
m_user_data = user_data;
|
||||
}
|
||||
void * getUserData() { return m_user_data; }
|
||||
std::vector<dGeomID> & getGeoms() { return m_geoms; }
|
||||
|
||||
protected:
|
||||
bool m_is_static;
|
||||
dBodyID m_body;
|
||||
dMass m_mass;
|
||||
dWorldID m_world;
|
||||
dSpaceID m_space;
|
||||
std::vector<dGeomID> m_geoms;
|
||||
float m_scale;
|
||||
dReal m_position[3];
|
||||
dReal m_rotation[12];
|
||||
OdeWorld * m_ode_world;
|
||||
void * m_user_data;
|
||||
|
||||
void setupGeom(dGeomID geom, dMass * mass,
|
||||
float locx, float locy, float locz,
|
||||
float rotx, float roty, float rotz);
|
||||
dGeomID cloneGeom(dGeomID geom, dBodyID body);
|
||||
};
|
||||
|
||||
class PickedObject
|
||||
{
|
||||
public:
|
||||
Object * obj;
|
||||
float dist;
|
||||
float pos[3];
|
||||
float normal[3];
|
||||
|
||||
PickedObject(dGeomID geom, dContactGeom & contact)
|
||||
{
|
||||
obj = (Object *) dGeomGetData(geom);
|
||||
dist = contact.depth;
|
||||
pos[0] = contact.pos[0];
|
||||
pos[1] = contact.pos[1];
|
||||
pos[2] = contact.pos[2];
|
||||
normal[0] = contact.normal[0];
|
||||
normal[1] = contact.normal[1];
|
||||
normal[2] = contact.normal[2];
|
||||
}
|
||||
};
|
||||
typedef refptr<PickedObject> PickedObjectRef;
|
||||
|
||||
OdeWorld();
|
||||
~OdeWorld();
|
||||
|
||||
Object * createObject(bool is_static, float scale = 1.0f);
|
||||
void setGravity(float x, float y, float z)
|
||||
{
|
||||
dWorldSetGravity(m_world, x, y, z);
|
||||
enableAllBodies();
|
||||
}
|
||||
void step();
|
||||
void enableAllBodies();
|
||||
dWorldID getWorldID() { return m_world; }
|
||||
dJointID createAMotor(dBodyID b1, dBodyID b2);
|
||||
dJointID createHinge(dBodyID b1, dBodyID b2,
|
||||
dReal anchor_x, dReal anchor_y, dReal anchor_z,
|
||||
dReal axis_x, dReal axis_y, dReal axis_z);
|
||||
void setAMotorNumAxes(dJointID j, int num_axes)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "num axes: " << num_axes << endl;
|
||||
#endif
|
||||
dJointSetAMotorNumAxes(j, num_axes);
|
||||
}
|
||||
void setAMotorAngle(dJointID j, int anum, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "angle: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorAngle(j, anum, val);
|
||||
}
|
||||
void setAMotorAxis(dJointID j, int anum, int rel,
|
||||
dReal x, dReal y, dReal z)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "axis: " << anum << ", " << rel
|
||||
<< ", (" << x << ", " << y << ", " << z << ")" << endl;
|
||||
#endif
|
||||
dJointSetAMotorAxis(j, anum, rel, x, y, z);
|
||||
}
|
||||
void setAMotorLoStop(dJointID j, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "lo stop: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorParam(j, dParamLoStop, val);
|
||||
}
|
||||
void setAMotorHiStop(dJointID j, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "hi stop: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorParam(j, dParamHiStop, val);
|
||||
}
|
||||
void setAMotorVel(dJointID j, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "vel: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorParam(j, dParamVel, val);
|
||||
}
|
||||
void setAMotorFMax(dJointID j, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "fmax: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorParam(j, dParamFMax, val);
|
||||
}
|
||||
void setAMotorBounce(dJointID j, dReal val)
|
||||
{
|
||||
#ifdef DEBUG_AMOTOR
|
||||
cout << "bounce: " << val << endl;
|
||||
#endif
|
||||
dJointSetAMotorParam(j, dParamBounce, val);
|
||||
}
|
||||
refptr< std::vector<PickedObjectRef> > 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);
|
||||
|
||||
friend void OdeWorld_pick_collide_callback(void * data,
|
||||
dGeomID o1, dGeomID o2);
|
||||
|
||||
static void pushTransform(const dReal * pos, const dReal * R);
|
||||
|
||||
protected:
|
||||
dBodyID createBody();
|
||||
void destroyBody(dBodyID body);
|
||||
|
||||
dWorldID m_world;
|
||||
dSpaceID m_space;
|
||||
dJointGroupID m_contactJointGroup;
|
||||
std::map<dBodyID, int> m_bodies;
|
||||
dGeomID m_pick_ray;
|
||||
std::vector< refptr<PickedObject> > m_pick_points;
|
||||
};
|
||||
|
||||
#endif
|
||||
Reference in new issue
Block a user