Commit 464700ff authored by Hina Shah's avatar Hina Shah
Browse files

ENH Adding an example for using ODE through RigidObject

parent ef92b714
......@@ -25,43 +25,105 @@
namespace imstk
{
static dWorldID odeWorld;
static dJointGroupID odeContactgroup;
// dynamics and collision objects
static dSpaceID odeSpace;
static dBodyID odeBody;
static dGeomID odeGeom;
static dMass odeMass;
void RigidObject::setup()
void
RigidObject::odeNearCallback(void *data, dGeomID o1, dGeomID o2)
{
// dynamics and collision objects
dWorldID world;
dSpaceID space;
dBodyID body;
dGeomID geom;
dMass m;
dJointGroupID contactgroup;
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
dContact contact;
contact.surface.mode = dContactBounce | dContactSoftCFM;
// friction parameter
contact.surface.mu = dInfinity;
// bounce is the amount of "bouncyness".
contact.surface.bounce = 0.9;
// bounce_vel is the minimum incoming velocity to cause a bounce
contact.surface.bounce_vel = 0.1;
// constraint force mixing parameter
contact.surface.soft_cfm = 0.001;
if (int numc = dCollide(o1, o2, 1, &contact.geom, sizeof(dContact)))
{
dJointID c = dJointCreateContact(odeWorld, odeContactgroup, &contact);
dJointAttach(c, b1, b2);
}
}
void
RigidObject::simulationStep()
{
// find collisions and add contact joints
dSpaceCollide(odeSpace, 0, &odeNearCallback);
// step the simulation
dWorldQuickStep(odeWorld, 0.01);
// remove all contact joints
dJointGroupEmpty(odeContactgroup);
}
void
RigidObject::getGeometryConfig(imstk::Vec3d &p, imstk::Mat3d &orientation)
{
const dReal *pos;
const dReal *R;
pos = dGeomGetPosition(odeGeom);
R = dGeomGetRotation(odeGeom);
p[0] = (double)pos[0];
p[1] = (double)pos[2];
p[2] = (double)pos[1];
size_t rotPos = 0;
for (size_t r = 0; r < 3; r++)
{
for (size_t c = 0; c < 3; c++, rotPos++)
{
orientation(r, c) = (double)R[rotPos];
}
}
}
void
RigidObject::initOde()
{
dInitODE();
// create world
world = dWorldCreate();
space = dHashSpaceCreate(0);
dWorldSetGravity(world, 0, 0, -0.2);
dWorldSetCFM(world, 1e-5);
dCreatePlane(space, 0, 0, 1, 0);
contactgroup = dJointGroupCreate(0);
// create object
body = dBodyCreate(world);
geom = dCreateSphere(space, 0.5);
dMassSetSphere(&m, 1, 0.5);
dBodySetMass(body, &m);
dGeomSetBody(geom, body);
// set initial position
dBodySetPosition(body, 0, 0, 3);
// run simulation
char** filename;
filename = new char*;
}
void
RigidObject::closeOde()
{
// clean up
dJointGroupDestroy(contactgroup);
dSpaceDestroy(space);
dWorldDestroy(world);
dJointGroupDestroy(odeContactgroup);
dSpaceDestroy(odeSpace);
dWorldDestroy(odeWorld);
dCloseODE();
}
void
RigidObject::setup()
{
// create world
odeWorld = dWorldCreate();
odeSpace = dHashSpaceCreate(0);
dWorldSetGravity(odeWorld, 0, 0, -0.2);
dWorldSetCFM(odeWorld, 1e-5);
dCreatePlane(odeSpace, 0, 0, 1, 0);
odeContactgroup = dJointGroupCreate(0);
// create object
odeBody = dBodyCreate(odeWorld);
odeGeom = dCreateSphere(odeSpace, 0.5);
dMassSetSphere(&odeMass, 1, 0.5);
dBodySetMass(odeBody, &odeMass);
dGeomSetBody(odeGeom, odeBody);
// set initial position
dBodySetPosition(odeBody, 0, 0, 3);
}
} // imstk
#endif
......@@ -22,8 +22,10 @@
#ifndef imstkRigidObject_h
#define imstkRigidObject_h
#ifdef iMSTK_USE_ODE
// imstk
#include "imstkDynamicObject.h"
#include "ode/ode.h"
namespace imstk
{
......@@ -47,8 +49,27 @@ public:
m_type = Type::Rigid;
}
//ode related functions
//
// callback called at every loop
//
static void odeNearCallback(void *data, dGeomID o1, dGeomID o2);
// initialize ode
static void initOde();
// close ode
static void closeOde();
//
static void simulationStep();
//
static void getGeometryConfig(imstk::Vec3d &p, imstk::Mat3d &orientation);
// This is just a simple function to test ODE
void setup();
static void setup();
///
/// \brief Destructor
......@@ -58,5 +79,5 @@ public:
protected:
};
} // imstk
#endif //imstk_USE_ODE
#endif // ifndef imstkRigidObject_h
......@@ -3231,23 +3231,59 @@ void testRigidBody()
{
// SDK and Scene
auto sdk = std::make_shared<imstk::SimulationManager>();
auto scene = sdk->createNewScene("RigidObjectPhysocs");
auto scene = sdk->createNewScene("RigidObjectPhysics");
// Create a plane in the scene (visual)
auto planeGeom = std::make_shared<imstk::Plane>();
planeGeom->setWidth(10);
planeGeom->setPosition(0.0, -50, 0.0);
planeGeom->setPosition(0.0, 2.5, 0.0);
auto planeObj = std::make_shared<imstk::VisualObject>("Plane");
planeObj->setVisualGeometry(planeGeom);
scene->addSceneObject(planeObj);
#ifdef iMSTK_USE_ODE
auto rgidibody = imstk::RigidObject("Rigid Object Yo!");
rgidibody.setup();
int counter = 1000;
#ifdef iMSTK_USE_ODE
//Initialize rigid body
//Initialize imstk side of things.
auto sphereObj = apiutils::createVisualAnalyticalSceneObject(
imstk::Geometry::Type::Sphere, scene, "VisualSphere", 1.0, Vec3d(0., 3.0, 0.));
auto sceneManager = sdk->getSceneManager("RigidObjectPhysics");
sceneManager->setPreInitCallback([&](Module* module)
{
imstk::RigidObject::initOde();
imstk::RigidObject::setup();
});
sceneManager->setPreUpdateCallback([&](Module* module)
{
if (counter == 0)
{
imstk::RigidObject::simulationStep();
counter = 1000;
}
else
{
counter--;
}
});
sceneManager->setPostUpdateCallback([&](Module* module)
{
imstk::Vec3d pos;
imstk::Mat3d matrix;
imstk::RigidObject::getGeometryConfig(pos, matrix);
sphereObj->getVisualGeometry()->setTranslation(pos);
});
sceneManager->setPostCleanUpCallback([&](Module* module)
{
imstk::RigidObject::closeOde();
});
#endif
// Move Camera
auto cam = scene->getCamera();
cam->setPosition(imstk::Vec3d(200, 200, 200));
cam->setPosition(imstk::Vec3d(10, 10, 10));
cam->setFocalPoint(imstk::Vec3d(0, 0, 0));
//Run
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment