add ode

1 files changed, 258 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp
new file mode 100644
index 0000000..5b6a532
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_gyroscopic.cpp
@@ -0,0 +1,258 @@
+/*************************************************************************
+ *                                                                       *
+ * 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();
+}