/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}
// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
void PhysicsActor::postLoad(){
dMass m;
if (type==CUBESHAPE)
{
geom = dCreateBox(space,shape.x,shape.y,shape.z);
dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
}
if (type==CAPSULESHAPE)
{
geom = dCreateCapsule(space,shape.x,shape.y);
dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y); //the '3' means align on z-axis and density is shape.z
generateCapsuleList();
}
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
//initialise position
if (base){
Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix;
dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]);
}else{
dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]);
}
dBodySetDamping(body, linearDamp, angleDamp);
bInit=true;
}
void PhysicsBody::setCapsuleMass( Real32 density, Int32 direction,
Real32 radius, Real32 length )
{
dMass mass;
dMassSetCapsule(&mass, density, direction, radius, length);
setMassStruct(mass);
}
dMass ColCapsule::GetMass() const
{
dMass m;
float scale = PhysicsServer::s_pInstance->GetWorldScale();
float height = m_Height - m_Radius*2.0f;
dMassSetCapsule( &m, GetDensity(), 3, m_Radius*scale, height*scale );
return m;
}
void Cylinder::computeMassProperties(dMass& m)
{
if(capped)
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
else
dMassSetCylinder(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
}
void Capsule::setMass(double mass, bool density){
if(mass){
dMass m;
dMassSetCapsule(&m, mass, 3 , osgcapsule->getRadius(), osgcapsule->getHeight());
if(!density)
dMassAdjust (&m, mass);
dBodySetMass (body,&m); //assign the mass to the body
}
}
int main (int argc, char **argv)
{
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
dWorldSetGravity(world,0,0,-0.1);
dMass m; // a parameter for mass
dMassSetZero (&m); // initialize the parameter
//@a sphere
sphere.body = dBodyCreate (world); // create a rigid body
dReal radius = 0.5; // radius [m]
dMassSetSphere (&m,DENSITY,radius); // calculate a mass parameter for a sphere
dBodySetMass (sphere.body,&m); // set the mass parameter to the body
dBodySetPosition (sphere.body,0,1, 1); // set the position of the body
//@a box
box.body = dBodyCreate (world);
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
dBodySetMass (box.body,&m);
dBodySetPosition (box.body,0,2,1);
// a capsule
capsule.body = dBodyCreate (world);
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule.body,&m);
dBodySetPosition (capsule.body,0,4,1);
// a cylinder
cylinder.body = dBodyCreate (world);
dMassSetCylinder(&m,DENSITY,3,radius,length);
dBodySetMass (cylinder.body,&m);
dBodySetPosition (cylinder.body,0,3,1);
// do the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
dWorldDestroy (world); // destroy the world
dCloseODE(); // close ODE
return 0;
}
int main (int argc, char **argv)
{
static dMass m;
dReal mass = 1.0;
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
space = dSimpleSpaceCreate (0);
//@a box
capsule = dBodyCreate (world);
geom = dCreateCapsule (space,radius,length); //create geometry.
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule,&m);
dGeomSetBody(geom,capsule);
dBodySetPosition (capsule,0,4,1);
//Gravedad y cosas de simulacion
dWorldSetGravity(world,0,0,-9.81);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
contactgroup = dJointGroupCreate (0);
// the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
//-- Destroy the world!!!
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
void CappedCylinder::createBody()
{
if(body == 0)
{
body = dBodyCreate(*simulation->getPhysicalWorld());
dMass m;
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
dBodySetMass(body, &m);
dBodySetPosition(body, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
dMatrix3 rotationMatrix;
ODETools::convertMatrix(rotation, rotationMatrix);
dBodySetRotation(body, rotationMatrix);
}
}
/*
=================================================================================
createUniversalLeg
Use parameters to create leg body/geom and attach to body with universal joint
**Warning**
mass is not set
=================================================================================
*/
void createUniversalLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius, dReal length,
dReal maxAngle, dReal minAngle,
dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
//dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//universal joint
joint = dJointCreateUniversal(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetUniversalAxis1(joint, 0, 0, 1);
dJointSetUniversalAxis2(joint, 0, 1, 0);
//Max and min angles
dJointSetUniversalParam(joint, dParamHiStop, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop, minAngle);
dJointSetUniversalParam(joint, dParamHiStop2, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop2, minAngle);
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()*WORLD_SIZE-(WORLD_SIZE/2),dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == 'o') {
draw_geom ^= 1;
}
}
static void command (int cmd)
{
size_t i;
int j,k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dReal maxheight = 0;
for (k=0; k<num; k++)
{
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRSetIdentity (R);
//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
//<---- Convex Object
else if (cmd == 'v')
{
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
obj[i].geom[0] = dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
#else
obj[i].geom[0] = dCreateConvex (space,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x' && USE_GEOM_OFFSET) {
setBody = 1;
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule (space,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate (&m2,drot[k]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition (obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// Translation *after* rotation
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<GPB; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == 'p'&& selected >= 0)
{
const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
rot[0],rot[1],rot[2],rot[3],
rot[4],rot[5],rot[6],rot[7],
rot[8],rot[9],rot[10],rot[11]);
}
else if (cmd == 'f' && selected >= 0 && selected < num) {
if (dBodyIsEnabled(obj[selected].body))
doFeedback = 1;
}
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v'
/* || cmd == 'l' */) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
/*
// cylinder option not yet implemented
else if (cmd == 'l') {
sides[1] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
*/
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
// Backwards compatible API
void dMassSetCappedCylinder(dMass *a, dReal b, int c, dReal d, dReal e)
{
return dMassSetCapsule(a,b,c,d,e);
}
void dMassSetCapsuleTotal (dMass *m, dReal total_mass, int direction,
dReal a, dReal b)
{
dMassSetCapsule (m, 1.0, direction, a, b);
dMassAdjust (m, total_mass);
}
/*******************************************************************************
Function to initialize ODE.
*******************************************************************************/
void initODE()
{
///////////////// Initializing the ODE general features ////////////////////
dInitODE(); //Initialize library.
World = dWorldCreate(); //Crate a new dynamics, empty world.
Space = dSimpleSpaceCreate(0); //Create a new space for collision (independent).
ContactGroup = dJointGroupCreate(0); //Create a joints container, without specifying size.
dWorldSetGravity( World, 0.0, -9.81, 0 ); //Add gravity to this World.
//Define error conrrection constants.
dWorldSetERP( World, 0.2 );
dWorldSetCFM( World, 1e-5 );
//Set the velocity that interpenetrating objects will separate at.
dWorldSetContactMaxCorrectingVel( World, 0.9 );
//Set the safety area for contacts to be considered at rest.
dWorldSetContactSurfaceLayer( World, 0.001 );
//Automatically disable objects that have come to a rest.
dWorldSetAutoDisableFlag( World, false );
/////////////// Initializing the rigid bodies in the world /////////////////
//Create a collision plane and add it to space. The next parameters are the
//literal components of ax + by + cz = d.
dCreatePlane( Space, 0.0, 1.0, 0.0, 0.0 );
const dReal xPos = 0;
const dReal yPos = 5;
const dReal zPos = 0;
const dReal xRot = 0;
const dReal yRot = 0;
const dReal zRot = 0;
const dReal radius = .75;
const dReal length = 4;
const dReal sides[3] = { 2, 2, 2 };
//Create body
body.Body = dBodyCreate(World);
dBodySetPosition(body.Body, xPos, yPos, zPos);
dMatrix3 Orient3;
//dRFromAxisAndAngle(Orient3, 0, 0, 1, 3.14/4);
dRFromEulerAngles(Orient3, xRot, yRot, zRot);
//dRFromEulerAngles(Orient3, 0, 0, 0);
dBodySetRotation(body.Body, Orient3);
dBodySetLinearVel(body.Body, 0, 0, 0);
dBodySetData(body.Body, (void *)0);
dMass bodyMass;
dMassSetCapsule(&bodyMass, 1.0, 3, radius, length);
//dMassSetBox(&bodyMass, 10, sides[0], sides[1], sides[2]);
dBodySetMass(body.Body, &bodyMass);
body.Geom = dCreateCapsule(Space, radius, length);
//body.Geom = dCreateBox(Space, sides[0], sides[1], sides[2]);
dGeomSetBody(body.Geom, body.Body);
float head_pos[ 3 ] = { 0.0f, 5.0f, -1.0f };
createInvisibleHead( head_pos );
createFrontLegs();
createMiddleLegs();
createBackLegs();
rng_engine.seed( std::chrono::duration_cast< std::chrono::microseconds >( std::chrono::system_clock::now().time_since_epoch() ).count() );
const int foodPrize = 10;
dReal position[3] = { 10.0f, 0.0f, -20.0f };
addFoodParticle( position, foodPrize, &World, &Space );
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
bool setBody = false;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
} else if (cmd == 'v') {
dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
obj[i].geom[0] = dCreateConvex(space,
planes,
planecount,
points,
pointcount,
polygons);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL);
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
setBody = true;
// start accumulating masses for the composite geometries
dMass m2;
dMassSetZero (&m);
dReal dpos[GPB][3]; // delta-positions for composite geometries
dMatrix3 drot[GPB];
// set random delta positions
for (j=0; j<GPB; j++)
for (k=0; k<3; k++)
dpos[j][k] = dRandReal()*0.3-0.15;
for (k=0; k<GPB; k++) {
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
obj[i].geom[k] = dCreateSphere (space,radius);
dMassSetSphere (&m2,DENSITY,radius);
} else if (k==1) {
obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]);
dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]);
} else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
obj[i].geom[k] = dCreateCapsule(space,radius,length);
dMassSetCapsule(&m2,DENSITY,3,radius,length);
}
dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dMassRotate(&m2,drot[k]);
dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
// add to the total mass
dMassAdd(&m,&m2);
}
for (k=0; k<GPB; k++) {
dGeomSetBody(obj[i].geom[k],obj[i].body);
dGeomSetOffsetPosition(obj[i].geom[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
}
dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]);
dBodySetMass(obj[i].body,&m);
}
if (!setBody) { // avoid calling for composite geometries
for (k=0; k < GPB; k++)
if (obj[i].geom[k])
dGeomSetBody(obj[i].geom[k],obj[i].body);
dBodySetMass(obj[i].body,&m);
}
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
void testMassFunctions()
{
dMass m;
int i,j;
dReal q[NUMP][3]; // particle positions
dReal pm[NUMP]; // particle masses
dMass m1,m2;
dMatrix3 R;
HEADER;
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
printf (" FAILED (1)\n"), printf (" passed (1)\n"));
printf ("\t");
dMassSetZero (&m);
TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
printf ("passed (2)\n") , printf (" FAILED (2)\n"));
if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");
dMassSetZero (&m);
dMassSetSphere (&m,1.4, 0.86);
if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.10349124669826) &&
cmp(m._I(1,1),1.10349124669826) &&
cmp(m._I(2,2),1.10349124669826) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");
dMassSetZero (&m);
dMassSetCapsule (&m,1.3,1,0.76,1.53);
if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),1.59461986077384) &&
cmp(m._I(1,1),4.21878433864904) &&
cmp(m._I(2,2),4.21878433864904) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");
dMassSetZero (&m);
dMassSetBox (&m,0.27,3,4,5);
if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");
// test dMassAdjust?
// make random particles and compute the mass, COM and inertia, then
// translate and repeat.
for (i=0; i<NUMP; i++) {
pm[i] = dRandReal()+0.5;
for (j=0; j<3; j++) {
q[i][j] = 2.0*(dRandReal()-0.5);
}
}
computeMassParams (&m1,q,pm);
memcpy (&m2,&m1,sizeof(dMass));
dMassTranslate (&m2,1,2,-3);
for (i=0; i<NUMP; i++) {
q[i][0] += 1;
q[i][1] += 2;
q[i][2] -= 3;
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"7");
// rotate the masses
_R(0,0) = -0.87919618797635;
_R(0,1) = 0.15278881840384;
_R(0,2) = -0.45129772879842;
_R(1,0) = -0.47307856232664;
_R(1,1) = -0.39258064912909;
_R(1,2) = 0.78871864932708;
_R(2,0) = -0.05666336483842;
_R(2,1) = 0.90693771059546;
_R(2,2) = 0.41743652473765;
dMassRotate (&m2,R);
for (i=0; i<NUMP; i++) {
dReal a[3];
dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
q[i][0] = a[0];
q[i][1] = a[1];
q[i][2] = a[2];
}
computeMassParams (&m1,q,pm);
compareMassParams (&m1,&m2,"8");
}
int main (int argc, char **argv)
{
dInitODE2(0);
bool fixed = true;
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dVector3 offset;
dSetZero (offset, 4);
// Default test case
if (argc >= 2 )
{
for (int i=1; i < argc; ++i)
{
//static int tata = 0;
if (1)
{
if ( 0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
Help (argv);
if ( 0 == strcmp ("-s", argv[i]) || 0 == strcmp ("--slider", argv[i]) )
type = dJointTypeSlider;
if ( 0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) )
{
int j = i+1;
if ( j+1 > argc || // Check if we have enough arguments
argv[j] == '\0' || // We should have a path here
argv[j][0] == '-' ) // We should have a path not a command line
Help (argv);
else
fn.path_to_textures = argv[++i]; // Increase i since we use this argument
}
}
if ( 0 == strcmp ("-1", argv[i]) || 0 == strcmp ("--offset1", argv[i]) )
tc = 1;
if ( 0 == strcmp ("-2", argv[i]) || 0 == strcmp ("--offset2", argv[i]) )
tc = 2;
if ( 0 == strcmp ("-3", argv[i]) || 0 == strcmp ("--offset3", argv[i]) )
tc = 3;
if (0 == strcmp ("-n", argv[i]) || 0 == strcmp ("--notFixed", argv[i]) )
fixed = false;
}
}
world = dWorldCreate();
dWorldSetERP (world, 0.8);
space = dSimpleSpaceCreate (0);
contactgroup = dJointGroupCreate (0);
geom[GROUND] = dCreatePlane (space, 0,0,1,0);
dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
dGeomSetCollideBits (geom[GROUND], catBits[ALL]);
dMass m;
dMatrix3 R;
// Create the Obstacle
geom[OBS] = dCreateBox (space, OBS_SIDES[0], OBS_SIDES[1], OBS_SIDES[2]);
dGeomSetCategoryBits (geom[OBS], catBits[OBS]);
dGeomSetCollideBits (geom[OBS], catBits[ALL]);
//Rotation of 45deg around y
dRFromAxisAndAngle (R, 1,1,0, -0.25*PI);
dGeomSetRotation (geom[OBS], R);
dGeomSetPosition (geom[OBS], 1.95, -0.2, 0.5);
//Rotation of 90deg around y
// Will orient the Z axis along X
dRFromAxisAndAngle (R, 0,1,0, -0.5*PI);
// Create Body2 (Wiil be attached to the world)
body[BODY2] = dBodyCreate (world);
// Main axis of cylinder is along X=1
dMassSetBox (&m, 1, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dMassAdjust (&m, Mass1);
geom[BODY2] = dCreateBox (space, BODY2_SIDES[0], BODY2_SIDES[1], BODY2_SIDES[2]);
dGeomSetBody (geom[BODY2], body[BODY2]);
dGeomSetOffsetRotation (geom[BODY2], R);
dGeomSetCategoryBits (geom[BODY2], catBits[BODY2]);
dGeomSetCollideBits (geom[BODY2], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY2], &m);
// Create Body 1 (Slider on the prismatic axis)
body[BODY1] = dBodyCreate (world);
// Main axis of capsule is along X=1
dMassSetCapsule (&m, 1, 1, RADIUS, BODY1_LENGTH);
dMassAdjust (&m, Mass1);
geom[BODY1] = dCreateCapsule (space, RADIUS, BODY1_LENGTH);
dGeomSetBody (geom[BODY1], body[BODY1]);
dGeomSetOffsetRotation (geom[BODY1], R);
dGeomSetCategoryBits (geom[BODY1], catBits[BODY1]);
dGeomSetCollideBits (geom[BODY1], catBits[ALL] & ~catBits[BODY2] & ~catBits[RECT]);
dMass mRect;
dMassSetBox (&mRect, 1, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dMassAdd (&m, &mRect);
// TODO: translate m?
geom[RECT] = dCreateBox (space, RECT_SIDES[0], RECT_SIDES[1], RECT_SIDES[2]);
dGeomSetBody (geom[RECT], body[BODY1]);
dGeomSetOffsetPosition (geom[RECT],
(BODY1_LENGTH-RECT_SIDES[0]) /2.0,
0.0,
-RADIUS -RECT_SIDES[2]/2.0);
dGeomSetCategoryBits (geom[RECT], catBits[RECT]);
dGeomSetCollideBits (geom[RECT], catBits[ALL] & (~catBits[BODY1]) );
dBodySetMass (body[BODY1], &m);
setPositionBodies (tc);
if ( fixed )
{
// Attache external cylinder to the world
dJointID fixed = dJointCreateFixed (world,0);
dJointAttach (fixed , NULL, body[BODY2]);
dJointSetFixed (fixed );
dWorldSetGravity (world,0,0,-0.8);
}
else
{
dWorldSetGravity (world,0,0,0);
}
// The static is here only to help debugging
switch (type)
{
case dJointTypeSlider :
{
dSliderJoint *sj = new dSliderJoint (world, 0);
sj->attach (body[BODY1], body[BODY2]);
sj->setAxis (1, 0, 0);
joint = sj;
}
break;
case dJointTypePiston : // fall through default
default:
{
dPistonJoint *pj = new dPistonJoint (world, 0);
pj->attach (body[BODY1], body[BODY2]);
pj->setAxis (1, 0, 0);
dJointSetPistonAnchor(pj->id(), anchor[X], anchor[Y], anchor[Z]);
joint = pj;
}
break;
};
// run simulation
dsSimulationLoop (argc,argv,400,300,&fn);
delete joint;
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
dCloseODE();
return 0;
}
static void command (int cmd)
{
size_t i;
int k;
dReal sides[3];
dMass m;
int setBody;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'y')
{
setBody = 0;
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
if (obj[i].body) {
dBodyDestroy (obj[i].body);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomDestroy (obj[i].geom[k]);
}
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos)
{
dBodySetPosition (obj[i].body,
dRandReal()*2-1 + platpos[0],
dRandReal()*2-1 + platpos[1],
dRandReal()+2 + platpos[2]);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else
{
dBodySetPosition (obj[i].body,
platpos[0],
platpos[1],
platpos[2]+2);
dRSetIdentity (R);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
if (!setBody)
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) {
dGeomSetBody (obj[i].geom[k],obj[i].body);
}
}
dBodySetMass (obj[i].body,&m);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
else if (cmd == '1') {
write_world = 1;
}
else if (cmd == ' ') {
mov_time = 0;
}
else if (cmd == 'm') {
mov_type = mov_type==1 ? 2 : 1;
mov_time = 0;
}
}
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd = locase (cmd);
if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
if (num < NUM) {
i = num;
num++;
}
else {
i = nextobj;
nextobj++;
if (nextobj >= num) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body = dBodyCreate (world);
for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if (random_pos) {
dBodySetPosition (obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
}
else {
dReal maxheight = 0;
for (k=0; k<num; k++) {
const dReal *pos = dBodyGetPosition (obj[k].body);
if (pos[2] > maxheight) maxheight = pos[2];
}
dBodySetPosition (obj[i].body, 0,0,maxheight+1);
dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
}
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*)(size_t)i);
if (cmd == 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd == 'c') {
sides[0] *= 0.5;
dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
}
else if (cmd == 'y') {
sides[1] *= 0.5;
dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
}
else if (cmd == 's') {
sides[0] *= 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] = dCreateSphere (space,sides[0]);
}
else if (cmd == 'm') {
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount,
(dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));
obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);
// remember the mesh's dTriMeshDataID on its userdata for convenience.
dGeomSetData(obj[i].geom[0], new_tmdata);
dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
}
else if (cmd == 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=0; j<GPB; j++) {
for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
}
for (k=0; k<GPB; k++) {
obj[i].geom[k] = dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k==0) {
dReal radius = dRandReal()*0.25+0.05;
g2[k] = dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k==1) {
g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius = dRandReal()*0.1+0.05;
dReal length = dRandReal()*1.0+0.1;
g2[k] = dCreateCapsule (0,radius,length);
dMassSetCapsule (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is (0,0,0)
for (k=0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd == ' ') {
selected++;
if (selected >= num) selected = 0;
if (selected < 0) selected = 0;
}
else if (cmd == 'd' && selected >= 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd == 'e' && selected >= 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
else if (cmd == 'a') {
show_aabb ^= 1;
}
else if (cmd == 't') {
show_contacts ^= 1;
}
else if (cmd == 'r') {
random_pos ^= 1;
}
}
// called when a key pressed
static void command( int cmd )
{
int i,k;
dReal sides[3];
dMass m;
cmd = locase( cmd );
if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' )
{
if ( num < NUM )
{
i = num;
num++;
}
else
{
i = nextobj;
nextobj++;
if ( nextobj >= num ) nextobj = 0;
// destroy the body and geoms for slot i
dBodyDestroy( obj[i].body );
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] );
}
memset( &obj[i],0,sizeof( obj[i] ) );
}
obj[i].body = dBodyCreate( world );
for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1;
dMatrix3 R;
if ( random_pos )
{
dBodySetPosition( obj[i].body,
dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 );
dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 );
}
else
{
dReal maxheight = 0;
for ( k=0; k<num; k++ )
{
const dReal *pos = dBodyGetPosition( obj[k].body );
if ( pos[2] > maxheight ) maxheight = pos[2];
}
dBodySetPosition( obj[i].body, 0,0,maxheight+1 );
dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 );
}
dBodySetRotation( obj[i].body,R );
dBodySetData( obj[i].body,( void* )( size_t )i );
if ( cmd == 'b' )
{
dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] );
obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] );
}
else if ( cmd == 'c' )
{
sides[0] *= 0.5;
dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] );
obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] );
}
else if ( cmd == 's' )
{
sides[0] *= 0.5;
dMassSetSphere( &m,DENSITY,sides[0] );
obj[i].geom[0] = dCreateSphere( space,sides[0] );
}
else if ( cmd == 'v' )
{
obj[i].geom[0] = dCreateConvex( space,
convexBunnyPlanes,
convexBunnyPlaneCount,
convexBunnyPoints,
convexBunnyPointCount,
convexBunnyPolygons );
/// Use equivalent TriMesh to set mass
dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount,
( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) );
dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 );
dMassSetTrimesh( &m, DENSITY, triMesh );
dGeomDestroy( triMesh );
dGeomTriMeshDataDestroy( new_tmdata );
printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] );
dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] );
dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] );
}
for ( k=0; k < GPB; k++ )
{
if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body );
}
dBodySetMass( obj[i].body,&m );
}
if ( cmd == ' ' )
{
selected++;
if ( selected >= num ) selected = 0;
if ( selected < 0 ) selected = 0;
}
else if ( cmd == 'd' && selected >= 0 && selected < num )
{
dBodyDisable( obj[selected].body );
}
else if ( cmd == 'e' && selected >= 0 && selected < num )
{
dBodyEnable( obj[selected].body );
}
else if ( cmd == 'a' )
{
show_aabb ^= 1;
}
else if ( cmd == 't' )
{
show_contacts ^= 1;
}
else if ( cmd == 'r' )
{
random_pos ^= 1;
}
}
