diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_kinematic.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/demo/demo_kinematic.cpp | 239 |
1 files changed, 239 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp b/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp new file mode 100644 index 0000000..5f2c613 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_kinematic.cpp @@ -0,0 +1,239 @@ +/************************************************************************* + * * + * 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 <iostream> +#include <set> +#include <algorithm> +#include <functional> +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> +#include "texturepath.h" + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawCylinder dsDrawCylinderD +#endif + + +using namespace std; + +dWorld *world; +dSpace *space; +dPlane *ground; +dBody *kbody; +dBox *kbox; +dJointGroup joints; +dCylinder *kpole; +dBody *matraca; +dBox *matraca_geom; +dHingeJoint *hinge; + +struct Box { + dBody body; + dBox geom; + Box() : + body(*world), + geom(*space, 0.2, 0.2, 0.2) + { + dMass mass; + mass.setBox(10, 0.2, 0.2, 0.2); + body.setMass(mass); + geom.setData(this); + geom.setBody(body); + } + void draw() const + { + dVector3 lengths; + geom.getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(0,1,0); + dsDrawBox(geom.getPosition(), geom.getRotation(), lengths); + } +}; + +set<Box*> boxes; +set<Box*> to_remove; + +void dropBox() +{ + Box *box = new Box(); + + dReal px = (rand() / float(RAND_MAX)) * 2 - 1; + dReal py = (rand() / float(RAND_MAX)) * 2 - 1; + dReal pz = 2.5; + box->body.setPosition(px, py, pz); + + boxes.insert(box); +} + +void queueRemoval(dGeomID g) +{ + Box *b = (Box*)dGeomGetData(g); + to_remove.insert(b); +} + +void removeQueued() +{ + while (!to_remove.empty()) { + Box *b = *to_remove.begin(); + to_remove.erase(b); + boxes.erase(b); + delete b; + } +} + + +void nearCallback(void *, dGeomID g1, dGeomID g2) +{ + if (g1 == ground->id()) { + queueRemoval(g2); + return; + } + if (g2 == ground->id()) { + queueRemoval(g1); + return; + } + + dBodyID b1 = dGeomGetBody(g1); + dBodyID b2 = dGeomGetBody(g2); + + if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) + return; + + const int MAX_CONTACTS = 10; + dContact contact[MAX_CONTACTS]; + int n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact)); + for (int i=0; i<n; ++i) { + contact[i].surface.mode = 0; + contact[i].surface.mu = 1; + dJointID j = dJointCreateContact (*world, joints.id(), contact+i); + dJointAttach(j, b1, b2); + } +} + + +void +simLoop(int pause) +{ + if (!pause) { + const dReal timestep = 0.04; + + // this does a hard-coded circular motion animation + static float t=0; + t += timestep/4; + if (t > 2*M_PI) + t = 0; + dVector3 next_pos = { dCos(t), dSin(t), REAL(0.5)}; + dVector3 vel; + // vel = (next_pos - cur_pos) / timestep + dSubtractVectors3(vel, next_pos, kbody->getPosition()); + dScaleVector3(vel, 1/timestep); + kbody->setLinearVel(vel); + // end of hard-coded animation + + space->collide(0, nearCallback); + removeQueued(); + + world->quickStep(timestep); + joints.clear(); + } + + dVector3 lengths; + + // the moving platform + kbox->getLengths(lengths); + dsSetTexture(DS_WOOD); + dsSetColor(.3, .3, 1); + dsDrawBox(kbox->getPosition(), kbox->getRotation(), lengths); + dReal length, radius; + kpole->getParams(&radius, &length); + dsSetTexture(DS_CHECKERED); + dsSetColor(1, 1, 0); + dsDrawCylinder(kpole->getPosition(), kpole->getRotation(), length, radius); + + // the matraca + matraca_geom->getLengths(lengths); + dsSetColor(1,0,0); + dsSetTexture(DS_WOOD); + dsDrawBox(matraca_geom->getPosition(), matraca_geom->getRotation(), lengths); + + // and the boxes + for_each(boxes.begin(), boxes.end(), mem_fun(&Box::draw)); +} + +void command(int c) +{ + switch (c) { + case ' ': + dropBox(); + break; + } +} + +int main(int argc, char **argv) +{ + dInitODE(); + + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = 0; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + cout << endl << "*** Press SPACE to drop boxes **" << endl; + + space = new dSimpleSpace(); + ground = new dPlane(*space, 0, 0, 1, 0); + + world = new dWorld; + world->setGravity(0, 0, -.5); + + kbody = new dBody(*world); + kbody->setKinematic(); + const dReal kx = 1, ky = 0, kz = .5; + kbody->setPosition(kx, ky, kz); + kbox = new dBox(*space, 3, 3, .5); + kbox->setBody(*kbody); + kpole = new dCylinder(*space, .125, 1.5); + kpole->setBody(*kbody); + dGeomSetOffsetPosition(kpole->id(), 0, 0, 0.8); + + matraca = new dBody(*world); + matraca->setPosition(kx+0, ky+1, kz+1); + matraca_geom = new dBox(*space, 0.5, 2, 0.75); + matraca_geom->setBody(*matraca); + dMass mass; + mass.setBox(1, 0.5, 2, 0.75); + matraca->setMass(mass); + + hinge = new dHingeJoint(*world); + hinge->attach(*kbody, *matraca); + hinge->setAnchor(kx, ky, kz+1); + hinge->setAxis(0, 0, 1); + + dsSimulationLoop (argc, argv, 640, 480, &fn); + + dCloseODE(); +} |