341 lines
8.6 KiB
C++
341 lines
8.6 KiB
C++
|
|
#include "OdeWorld.h"
|
|
#include <vector>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <GL/gl.h>
|
|
using namespace std;
|
|
|
|
#define WORLD_STEP 0.001
|
|
#define WHITESPACE " \t\r\n\f"
|
|
|
|
static string trim(const string & orig)
|
|
{
|
|
string result = orig;
|
|
size_t pos = result.find_first_not_of(WHITESPACE);
|
|
if (pos == string::npos)
|
|
{
|
|
result = "";
|
|
}
|
|
else
|
|
{
|
|
if (pos > 0)
|
|
result = result.substr(pos, result.length() - pos);
|
|
pos = result.find_last_not_of(WHITESPACE);
|
|
if (pos < result.length() - 1)
|
|
result = result.substr(0, pos + 1);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/* used by ODE to perform collision detection */
|
|
void OdeWorld_collide_callback(void * data, dGeomID o1, dGeomID o2)
|
|
{
|
|
const int maxNumContacts = 4;
|
|
OdeWorld * ow = (OdeWorld *) data;
|
|
static dContact contact[maxNumContacts];
|
|
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.8;
|
|
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);
|
|
}
|
|
}
|
|
|
|
OdeWorld::OdeWorld()
|
|
{
|
|
m_world = dWorldCreate();
|
|
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)
|
|
{
|
|
return new Object(is_static, m_world, m_space);
|
|
}
|
|
|
|
/* 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);
|
|
}
|
|
|
|
OdeWorld::Object::Object(bool is_static, dWorldID world, dSpaceID space)
|
|
{
|
|
m_is_static = is_static;
|
|
m_world = world;
|
|
m_space = space;
|
|
m_body = 0;
|
|
}
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
void OdeWorld::Object::getPosition(double * x, double * y, double * z)
|
|
{
|
|
const dReal * pos = NULL;
|
|
if (m_is_static)
|
|
{
|
|
if (m_geoms.size() > 0)
|
|
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];
|
|
}
|
|
}
|
|
|
|
void OdeWorld::Object::loadPhy(const std::string & path)
|
|
{
|
|
ifstream ifs(path.c_str());
|
|
if (ifs.is_open())
|
|
{
|
|
while (!ifs.eof())
|
|
{
|
|
string line;
|
|
getline(ifs, line);
|
|
line = trim(line);
|
|
if (line == "" || line[0] == '#')
|
|
continue;
|
|
size_t pos = line.find_first_of(WHITESPACE);
|
|
if (pos == string::npos)
|
|
continue;
|
|
string type = line.substr(0, pos);
|
|
pos = line.find("\"", pos);
|
|
if (pos == string::npos)
|
|
continue;
|
|
size_t pos2 = line.find("\"", pos + 1);
|
|
if (pos2 == string::npos)
|
|
continue;
|
|
string name = line.substr(pos + 1, pos2 - pos - 1);
|
|
pos = pos2 + 1;
|
|
vector<float> args;
|
|
for (;;)
|
|
{
|
|
pos = line.find_first_not_of(WHITESPACE, pos);
|
|
if (pos == string::npos)
|
|
break;
|
|
pos2 = line.find_first_of(WHITESPACE, pos);
|
|
string n = line.substr(pos, pos2 - pos);
|
|
float f = atof(n.c_str());
|
|
args.push_back(f);
|
|
if (pos2 == string::npos)
|
|
break;
|
|
pos = pos2 + 1;
|
|
}
|
|
if (type == "cube")
|
|
{
|
|
addCube(args);
|
|
}
|
|
else if (type == "sphere")
|
|
{
|
|
addSphere(args);
|
|
}
|
|
else if (type == "cylinder")
|
|
{
|
|
addCylinder(args);
|
|
}
|
|
else if (type == "plane")
|
|
{
|
|
addPlane(args);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void OdeWorld::Object::addCube(const vector<float> args)
|
|
{
|
|
if (args.size() != 9)
|
|
return;
|
|
dGeomID id = dCreateBox(0, args[0], args[1], args[2]);
|
|
dMass mass;
|
|
dMassSetBox(&mass, 1.0, args[0], args[1], args[2]);
|
|
setupGeom(id, &mass,
|
|
args[3], args[4], args[5],
|
|
args[6], args[7], args[8]);
|
|
}
|
|
|
|
void OdeWorld::Object::addSphere(const vector<float> args)
|
|
{
|
|
if (args.size() != 4)
|
|
return;
|
|
dGeomID id = dCreateSphere(0, args[0]);
|
|
dMass mass;
|
|
dMassSetSphere(&mass, 1.0, args[0]);
|
|
setupGeom(id, &mass,
|
|
args[1], args[2], args[3],
|
|
0.0, 0.0, 0.0);
|
|
}
|
|
|
|
void OdeWorld::Object::addCylinder(const vector<float> args)
|
|
{
|
|
if (args.size() != 8)
|
|
return;
|
|
dGeomID id = dCreateCylinder(0, args[0], args[1]);
|
|
dMass mass;
|
|
dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]);
|
|
setupGeom(id, &mass,
|
|
args[2], args[3], args[4],
|
|
args[5], args[6], args[7]);
|
|
}
|
|
|
|
void OdeWorld::Object::addCCylinder(const vector<float> args)
|
|
{
|
|
if (args.size() != 8)
|
|
return;
|
|
dGeomID id = dCreateCCylinder(0, args[0], args[1]);
|
|
dMass mass;
|
|
dMassSetCappedCylinder(&mass, 1.0, 3, args[0], args[1]);
|
|
setupGeom(id, &mass,
|
|
args[2], args[3], args[4],
|
|
args[5], args[6], args[7]);
|
|
}
|
|
|
|
void OdeWorld::Object::addPlane(const vector<float> args)
|
|
{
|
|
if (args.size() != 6)
|
|
return;
|
|
|
|
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);
|
|
|
|
float a = rotated_plane_direction[0];
|
|
float b = rotated_plane_direction[1];
|
|
float c = rotated_plane_direction[2];
|
|
float d = a * args[0] + b * args[1] + c * args[2];
|
|
|
|
dGeomID id = dCreatePlane(m_space, a, b, c, d);
|
|
m_geoms.push_back(id);
|
|
}
|
|
|
|
void OdeWorld::Object::setupGeom(dGeomID geom, dMass * mass,
|
|
float locx, float locy, float locz,
|
|
float rotx, float roty, float rotz)
|
|
{
|
|
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);
|
|
|
|
m_geoms.push_back(transform);
|
|
|
|
if (!m_is_static)
|
|
{
|
|
/* attach the geometry to the body */
|
|
if (m_body == 0)
|
|
m_body = dBodyCreate(m_world);
|
|
|
|
dGeomSetBody(transform, m_body);
|
|
|
|
dMassRotate(mass, rot);
|
|
dMassTranslate(mass, locx, locy, locz);
|
|
dMass origmass;
|
|
dBodyGetMass(m_body, &origmass);
|
|
dMassAdd(&origmass, mass);
|
|
dBodySetMass(m_body, &origmass);
|
|
}
|
|
}
|
|
|
|
const dReal * OdeWorld::Object::getPosition()
|
|
{
|
|
if (m_body != 0)
|
|
{
|
|
return dBodyGetPosition(m_body);
|
|
}
|
|
else if (m_geoms.size() > 0)
|
|
{
|
|
return dGeomGetPosition(m_geoms[0]);
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
const dReal * OdeWorld::Object::getRotation()
|
|
{
|
|
if (m_body != 0)
|
|
{
|
|
return dBodyGetRotation(m_body);
|
|
}
|
|
else if (m_geoms.size() > 0)
|
|
{
|
|
return dGeomGetRotation(m_geoms[0]);
|
|
}
|
|
return NULL;
|
|
}
|