OdeWorld/OdeWorld.cc

#include "OdeWorld.h"
#include <vector>
#include <fstream>
#include <iostream>
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, -9.81, 0);
}

OdeWorld::~OdeWorld()
{
    dJointGroupDestroy(m_contactJointGroup);
    dSpaceDestroy(m_space);
    dWorldDestroy(m_world);
}

/* invokes ODE to do physics on our world */
void OdeWorld::worldStep()
{
    dSpaceCollide(m_space, this, OdeWorld_collide_callback);
    dWorldQuickStep(m_world, WORLD_STEP);
    dJointGroupEmpty(m_contactJointGroup);
}

vector<dGeomID> OdeWorld::loadPhy(const std::string & path,
                bool static_data)
{
    vector<dGeomID> ret;
    dBodyID body = 0;

    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")
            {
                ret.push_back(addCube(name, static_data, &body, args));
            }
            else if (type == "sphere")
            {
                ret.push_back(addSphere(name, static_data, &body, args));
            }
            else if (type == "cylinder")
            {
                ret.push_back(addCylinder(name, static_data, &body, args));
            }
            else if (type == "plane")
            {
                ret.push_back(addPlane(name, static_data, &body, args));
            }
        }
    }

    return ret;
}

dGeomID OdeWorld::addCube(const string & name, bool static_data,
        dBodyID * body, const vector<float> args)
{
    if (args.size() != 9)
        return 0;
    dGeomID id = dCreateBox(m_space, args[0], args[1], args[2]);
    dMass mass;
    dMassSetBox(&mass, 1.0, args[0], args[1], args[2]);
    setupGeom(name, static_data, body, id, &mass,
            args[3], args[4], args[5],
            args[6], args[7], args[8]);
    return id;
}

dGeomID OdeWorld::addSphere(const string & name, bool static_data,
        dBodyID * body, const vector<float> args)
{
    if (args.size() != 4)
        return 0;
    dGeomID id = dCreateSphere(m_space, args[0]);
    dMass mass;
    dMassSetSphere(&mass, 1.0, args[0]);
    setupGeom(name, static_data, body, id, &mass,
            args[1], args[2], args[3],
            0.0, 0.0, 0.0);
    return id;
}

dGeomID OdeWorld::addCylinder(const string & name, bool static_data,
        dBodyID * body, const vector<float> args)
{
    if (args.size() != 8)
        return 0;
    dGeomID id = dCreateCylinder(m_space, args[0], args[1]);
    dMass mass;
    dMassSetCylinder(&mass, 1.0, 3, args[0], args[1]);
    setupGeom(name, static_data, body, id, &mass,
            args[2], args[3], args[4],
            args[5], args[6], args[7]);
    return id;
}

dGeomID OdeWorld::addCCylinder(const string & name, bool static_data,
        dBodyID * body, const vector<float> args)
{
    if (args.size() != 8)
        return 0;
    dGeomID id = dCreateCCylinder(m_space, args[0], args[1]);
    dMass mass;
    dMassSetCappedCylinder(&mass, 1.0, 3, args[0], args[1]);
    setupGeom(name, static_data, body, id, &mass,
            args[2], args[3], args[4],
            args[5], args[6], args[7]);
    return id;
}

dGeomID OdeWorld::addPlane(const string & name, bool static_data,
        dBodyID * body, const vector<float> args)
{
    if (args.size() != 6)
        return 0;
    float a, b, c, d;
    /* TODO: find a, b, c, d from args */
    dGeomID id = dCreatePlane(m_space, a, b, c, d);
    return id;
}

void OdeWorld::setupGeom(const std::string & name, bool static_data,
        dBodyID * body, dGeomID geom, dMass * mass,
        float locx, float locy, float locz,
        float rotx, float roty, float rotz)
{
    if (!static_data)
    {
        if (*body == 0)
            *body = dBodyCreate(m_world);

        dGeomSetBody(geom, *body);

        dMatrix3 rot;
        dRFromEulerAngles(rot, rotx, roty, rotz);
        dGeomSetRotation(geom, rot);

        dMassRotate(mass, rot);
        dMassTranslate(mass, locx, locy, locz);
        dMass origmass;
        dBodyGetMass(*body, &origmass);
        dMassAdd(&origmass, mass);
        dBodySetMass(*body, &origmass);
    }
}