259 lines
6.7 KiB
C++
259 lines
6.7 KiB
C++
|
/*************************************************************************
|
||
|
* *
|
||
|
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||
|
* *
|
||
|
* This library is free software; you can redistribute it and/or *
|
||
|
* modify it under the terms of EITHER: *
|
||
|
* (1) The GNU Lesser General Public License as published by the Free *
|
||
|
* Software Foundation; either version 2.1 of the License, or (at *
|
||
|
* your option) any later version. The text of the GNU Lesser *
|
||
|
* General Public License is included with this library in the *
|
||
|
* file LICENSE.TXT. *
|
||
|
* (2) The BSD-style license that is included with this library in *
|
||
|
* the file LICENSE-BSD.TXT. *
|
||
|
* *
|
||
|
* This library is distributed in the hope that it will be useful, *
|
||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||
|
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||
|
* *
|
||
|
*************************************************************************/
|
||
|
|
||
|
#include <ode/ode.h>
|
||
|
#include <drawstuff/drawstuff.h>
|
||
|
#include "texturepath.h"
|
||
|
|
||
|
#ifdef _MSC_VER
|
||
|
#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
|
||
|
#endif
|
||
|
|
||
|
// select correct drawing functions
|
||
|
|
||
|
#ifdef dDOUBLE
|
||
|
#define dsDrawBox dsDrawBoxD
|
||
|
#define dsDrawSphere dsDrawSphereD
|
||
|
#define dsDrawCylinder dsDrawCylinderD
|
||
|
#define dsDrawCapsule dsDrawCapsuleD
|
||
|
#define dsDrawConvex dsDrawConvexD
|
||
|
#endif
|
||
|
|
||
|
bool write_world = false;
|
||
|
bool show_contacts = false;
|
||
|
dWorld * world;
|
||
|
dBody *top1, *top2;
|
||
|
dSpace *space;
|
||
|
dJointGroup contactgroup;
|
||
|
|
||
|
const dReal pinradius = 0.05f;
|
||
|
const dReal pinlength = 1.5f;
|
||
|
const dReal topradius = 1.0f;
|
||
|
const dReal toplength = 0.25f;
|
||
|
const dReal topmass = 1.0f;
|
||
|
|
||
|
#define MAX_CONTACTS 4
|
||
|
|
||
|
static void nearCallback (void *, dGeomID o1, dGeomID o2)
|
||
|
{
|
||
|
// for drawing the contact points
|
||
|
dMatrix3 RI;
|
||
|
dRSetIdentity (RI);
|
||
|
const dReal ss[3] = {0.02,0.02,0.02};
|
||
|
|
||
|
int i;
|
||
|
dBodyID b1 = dGeomGetBody(o1);
|
||
|
dBodyID b2 = dGeomGetBody(o2);
|
||
|
|
||
|
dContact contact[MAX_CONTACTS];
|
||
|
int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
|
||
|
sizeof(dContact));
|
||
|
|
||
|
for (i=0; i<numc; i++) {
|
||
|
contact[i].surface.mode = dContactApprox1;
|
||
|
contact[i].surface.mu = 2;
|
||
|
|
||
|
dJointID c = dJointCreateContact (*world,contactgroup,contact+i);
|
||
|
dJointAttach (c,b1,b2);
|
||
|
if (show_contacts)
|
||
|
dsDrawBox (contact[i].geom.pos, RI, ss);
|
||
|
|
||
|
}
|
||
|
}
|
||
|
|
||
|
|
||
|
// start simulation - set viewpoint
|
||
|
|
||
|
static void start()
|
||
|
{
|
||
|
static float xyz[3] = {4.777f, -2.084f, 2.18f};
|
||
|
static float hpr[3] = {153.0f, -14.5f, 0.0f};
|
||
|
dsSetViewpoint (xyz,hpr);
|
||
|
printf ("Orange top approximates conservation of angular momentum\n");
|
||
|
printf ("Green top uses conservation of angular velocity\n");
|
||
|
printf ("---\n");
|
||
|
printf ("SPACE to reset\n");
|
||
|
printf ("A to tilt the tops.\n");
|
||
|
printf ("T to toggle showing the contact points.\n");
|
||
|
printf ("1 to save the current state to 'state.dif'.\n");
|
||
|
}
|
||
|
|
||
|
|
||
|
char locase (char c)
|
||
|
{
|
||
|
if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
|
||
|
else return c;
|
||
|
}
|
||
|
|
||
|
|
||
|
// called when a key pressed
|
||
|
static void reset();
|
||
|
static void tilt();
|
||
|
|
||
|
static void command (int cmd)
|
||
|
{
|
||
|
cmd = locase (cmd);
|
||
|
if (cmd == ' ')
|
||
|
{
|
||
|
reset();
|
||
|
}
|
||
|
else if (cmd == 'a') {
|
||
|
tilt();
|
||
|
}
|
||
|
else if (cmd == 't') {
|
||
|
show_contacts = !show_contacts;
|
||
|
}
|
||
|
else if (cmd == '1') {
|
||
|
write_world = true;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// simulation loop
|
||
|
|
||
|
static void simLoop (int pause)
|
||
|
{
|
||
|
dsSetColor (0,0,2);
|
||
|
space->collide(0,&nearCallback);
|
||
|
if (!pause)
|
||
|
//world->quickStep(0.02);
|
||
|
world->step(0.02);
|
||
|
|
||
|
if (write_world) {
|
||
|
FILE *f = fopen ("state.dif","wt");
|
||
|
if (f) {
|
||
|
dWorldExportDIF (*world,f,"X");
|
||
|
fclose (f);
|
||
|
}
|
||
|
write_world = false;
|
||
|
}
|
||
|
|
||
|
// remove all contact joints
|
||
|
dJointGroupEmpty (contactgroup);
|
||
|
|
||
|
dsSetTexture (DS_WOOD);
|
||
|
|
||
|
dsSetColor (1,0.5f,0);
|
||
|
dsDrawCylinder(top1->getPosition(),
|
||
|
top1->getRotation(),
|
||
|
toplength, topradius);
|
||
|
dsDrawCapsule(top1->getPosition(),
|
||
|
top1->getRotation(),
|
||
|
pinlength, pinradius);
|
||
|
|
||
|
dsSetColor (0.5f,1,0);
|
||
|
dsDrawCylinder(top2->getPosition(),
|
||
|
top2->getRotation(),
|
||
|
toplength, topradius);
|
||
|
dsDrawCapsule(top2->getPosition(),
|
||
|
top2->getRotation(),
|
||
|
pinlength, pinradius);
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
static void reset()
|
||
|
{
|
||
|
dMatrix3 R;
|
||
|
dRSetIdentity(R);
|
||
|
|
||
|
top1->setRotation(R);
|
||
|
top2->setRotation(R);
|
||
|
|
||
|
top1->setPosition(0.8f, -2, 2);
|
||
|
top2->setPosition(0.8f, 2, 2);
|
||
|
|
||
|
top1->setAngularVel(1,0,7);
|
||
|
top2->setAngularVel(1,0,7);
|
||
|
|
||
|
top1->setLinearVel(0,0.2f,0);
|
||
|
top2->setLinearVel(0,0.2f,0);
|
||
|
}
|
||
|
|
||
|
static void tilt()
|
||
|
{
|
||
|
top1->addTorque(0, 10, 0);
|
||
|
top2->addTorque(0, 10, 0);
|
||
|
}
|
||
|
|
||
|
int main (int argc, char **argv)
|
||
|
{
|
||
|
// 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;
|
||
|
|
||
|
|
||
|
// create world
|
||
|
dInitODE();
|
||
|
world = new dWorld();
|
||
|
world->setGravity(0,0,-0.5f);
|
||
|
world->setCFM(1e-5f);
|
||
|
world->setLinearDamping(0.00001f);
|
||
|
world->setAngularDamping(0.0001f);
|
||
|
|
||
|
space = new dSimpleSpace(0);
|
||
|
|
||
|
dPlane *floor = new dPlane(*space, 0,0,1,0);
|
||
|
|
||
|
top1 = new dBody(*world);
|
||
|
top2 = new dBody(*world);
|
||
|
|
||
|
dMass m;
|
||
|
m.setCylinderTotal(1, 3, topradius, toplength);
|
||
|
top1->setMass(m);
|
||
|
top2->setMass(m);
|
||
|
|
||
|
dGeom *g1, *g2, *pin1, *pin2;
|
||
|
g1 = new dCylinder(*space, topradius, toplength);
|
||
|
g1->setBody(*top1);
|
||
|
g2 = new dCylinder(*space, topradius, toplength);
|
||
|
g2->setBody(*top2);
|
||
|
|
||
|
pin1 = new dCapsule(*space, pinradius, pinlength);
|
||
|
pin1->setBody(*top1);
|
||
|
pin2 = new dCapsule(*space, pinradius, pinlength);
|
||
|
pin2->setBody(*top2);
|
||
|
|
||
|
top2->setGyroscopicMode(false);
|
||
|
|
||
|
reset();
|
||
|
|
||
|
// run simulation
|
||
|
dsSimulationLoop (argc,argv,512,384,&fn);
|
||
|
|
||
|
delete g1;
|
||
|
delete g2;
|
||
|
delete pin1;
|
||
|
delete pin2;
|
||
|
delete floor;
|
||
|
contactgroup.empty();
|
||
|
delete top1;
|
||
|
delete top2;
|
||
|
delete space;
|
||
|
delete world;
|
||
|
dCloseODE();
|
||
|
}
|