632 lines
17 KiB
C++
632 lines
17 KiB
C++
|
|
#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);
|
|
return new vector<OdeWorld::PickedObjectRef>(m_pick_points);
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|