diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_dball.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/demo/demo_dball.cpp | 194 |
1 files changed, 194 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_dball.cpp b/libs/ode-0.16.1/ode/demo/demo_dball.cpp new file mode 100644 index 0000000..d264cd7 --- /dev/null +++ b/libs/ode-0.16.1/ode/demo/demo_dball.cpp @@ -0,0 +1,194 @@ +/************************************************************************* + * * + * 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 dDOUBLE +#define dsDrawSphere dsDrawSphereD +#define dsDrawBox dsDrawBoxD +#define dsDrawLine dsDrawLineD +#endif + + +dWorldID world; +dSpaceID space; +dBodyID body1; +dBodyID body2; +dJointID joint1, joint2; + +void start() +{ + world = dWorldCreate(); + dWorldSetGravity (world,0,0,-9.8); + + dWorldSetDamping(world, 1e-4, 1e-5); +// dWorldSetERP(world, 1); + + space = dSimpleSpaceCreate (0); + + body1 = dBodyCreate(world); + body2 = dBodyCreate(world); + + dBodySetPosition(body1, 0, 0, 3); + dBodySetPosition(body2, 0, 0, 1); + + + dGeomID g; + dMass mass; + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body1); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body1, &mass); + + g = dCreateBox(space, 0.2, 0.2, 1); + dGeomSetBody(g, body2); + dMassSetBox(&mass, 1, 0.2, 0.2, 1); + dBodySetMass(body2, &mass); + + joint1 = dJointCreateDBall(world, 0); + dJointAttach(joint1, body1, 0); + dJointSetDBallAnchor1(joint1, 0, 0, 3.5); + dJointSetDBallAnchor2(joint1, 0, 0, 4.5); + + joint2 = dJointCreateDBall(world, 0); + dJointAttach(joint2, body1, body2); + dJointSetDBallAnchor1(joint2, 0, 0, 2.5); + dJointSetDBallAnchor2(joint2, 0, 0, 1.5); + + + // initial camera position + static float xyz[3] = {3.8966, -2.0614, 4.0300}; + static float hpr[3] = {153.5, -16.5, 0}; + dsSetViewpoint (xyz,hpr); +} + +void stop() +{ + dSpaceDestroy(space); + + dWorldDestroy(world); +} + + +void drawGeom(dGeomID g) +{ + int gclass = dGeomGetClass(g); + const dReal *pos = dGeomGetPosition(g); + const dReal *rot = dGeomGetRotation(g); + + switch (gclass) { + case dSphereClass: + dsSetColorAlpha(0, 0.75, 0.5, 1); + dsSetTexture (DS_CHECKERED); + dsDrawSphere(pos, rot, dGeomSphereGetRadius(g)); + break; + case dBoxClass: + { + dVector3 lengths; + dsSetColorAlpha(1, 1, 0, 1); + dsSetTexture (DS_WOOD); + dGeomBoxGetLengths(g, lengths); + dsDrawBox(pos, rot, lengths); + break; + } + + default: + {} + } +} + +void simLoop(int pause) +{ + if (!pause) { + + static dReal t = 0; + + const dReal step = 0.005; + const unsigned nsteps = 4; + + for (unsigned i=0; i<nsteps; ++i) { + + dReal f = sin(t*1.2)*0.8; + dBodyAddForceAtRelPos(body1, + f, 0, 0, + 0, 0, -0.5); // at the lower end + + dReal g = sin(t*0.7)*0.8; + dBodyAddForceAtRelPos(body2, + 0, g, 0, + 0, 0, -0.5); // at the lower end + t += step; + + dWorldQuickStep(world, step); + } + } + + // now we draw everything + unsigned ngeoms = dSpaceGetNumGeoms(space); + for (unsigned i=0; i<ngeoms; ++i) { + dGeomID g = dSpaceGetGeom(space, i); + + drawGeom(g); + } + + dVector3 a11, a12; + dJointGetDBallAnchor1(joint1, a11); + dJointGetDBallAnchor2(joint1, a12); + dsSetColor(1, 0, 0); + dsDrawLine(a11, a12); + + //printf("Error 1: %f\n", fabs(dJointGetDBallDistance(joint1) - dCalcPointsDistance3(a11, a12))); + + dVector3 a21, a22; + dJointGetDBallAnchor1(joint2, a21); + dJointGetDBallAnchor2(joint2, a22); + dsSetColor(0, 1, 0); + dsDrawLine(a21, a22); + + //printf("Error 2: %f\n", fabs(dJointGetDBallDistance(joint2) - dCalcPointsDistance3(a21, a22))); +} + + + +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 = 0; + fn.stop = stop; + fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; + + // create world + dInitODE(); + + // run demo + dsSimulationLoop (argc, argv, 800, 600, &fn); + + dCloseODE(); + return 0; +} |