add ode

1 files changed, 258 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp b/libs/ode-0.16.1/ode/demo/demo_rfriction.cpp
new file mode 100644
index 0000000..61d1115
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_rfriction.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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+Angular friction demo:
+
+A bunch of ramps of different pitch.
+A bunch of spheres with rolling friction.
+*/
+
+
+#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
+#endif
+
+
+// some constants
+#define GRAVITY 10	// the global gravity to use
+#define RAMP_COUNT 8
+
+static const dReal rampX = 6.0f;
+static const dReal rampY = 0.5f;
+static const dReal rampZ = 0.25f;
+static const dReal sphereRadius = 0.25f;
+static const dReal maxRamp = M_PI/4.0f; // Needs to be less than pi/2
+static const dReal rampInc = maxRamp/RAMP_COUNT;
+
+// dynamics and collision objects
+static dWorldID world = 0;
+static dSpaceID space = 0;
+static dJointGroupID contactgroup = 0;
+static dGeomID ground;
+
+static dReal mu = REAL(0.37); // the global mu to use
+static dReal rho = REAL(0.1); // the global rho to use
+static dReal omega = REAL(25.0);
+
+static dGeomID rampGeom[RAMP_COUNT];
+static dBodyID sphereBody[RAMP_COUNT];
+static dGeomID sphereGeom[RAMP_COUNT];
+
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback (void *, dGeomID o1, dGeomID o2)
+{
+    int i;
+
+    dBodyID b1 = dGeomGetBody(o1);
+    dBodyID b2 = dGeomGetBody(o2);
+  
+    if (b1==0 && b2==0) return;
+
+    dContact contact[3];		
+    for (int ii=0; ii<3; ii++) {
+        contact[ii].surface.mode = dContactApprox1 | dContactRolling;
+        contact[ii].surface.mu = mu;
+        contact[ii].surface.rho = rho;
+    }
+    if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
+        for (i=0; i<numc; i++) {
+            dJointID c = dJointCreateContact (world,contactgroup,contact+i);
+            dJointAttach (c,b1,b2);
+        }
+    }
+}
+
+
+// start simulation - set viewpoint
+
+static void start()
+{
+    dAllocateODEDataForThread(dAllocateMaskAll);
+
+    static float xyz[3] = {0,-3.0f,3.0f};
+    static float hpr[3] = {90.0000,-15.0000,0.0000};
+    dsSetViewpoint (xyz,hpr);
+    printf ("Press:\n"
+            "\t'[' or ']' to change initial angular velocity\n"
+            "\t'm' to increase sliding friction\n"
+            "\t'n' to decrease sliding friction\n"
+            "\t'j' to increase rolling friction\n"
+            "\t'h' to decrease rolling friction\n"
+            "\t'r' to reset simulation.\n");
+}
+
+/**
+  Delete the bodies, etc.
+*/
+static void clear()
+{
+    if (contactgroup) dJointGroupDestroy (contactgroup);
+    if (space) dSpaceDestroy (space);
+    if (world) dWorldDestroy (world);
+}
+
+
+
+/**
+  Cleanup if necessary and rebuild the
+  world.
+*/
+static void reset()
+{
+    clear();
+
+    // create world
+    world = dWorldCreate();
+    space = dHashSpaceCreate (0);
+    contactgroup = dJointGroupCreate (0);
+    dWorldSetGravity (world,0,0,-GRAVITY);
+    ground = dCreatePlane (space,0,0,1,0);
+
+    // Calculate mass for sphere a capsule with water density.
+    dMass sphereMass;
+    dMassSetSphere(&sphereMass,1000,sphereRadius);
+
+    for (int ii=0;ii<RAMP_COUNT;++ii) {
+        dQuaternion q;
+
+        dReal angle = (ii+1)*rampInc;
+        dReal cosA = dCos(angle);
+        dReal sinA = dSin(angle);
+        dReal rampW = rampX/cosA; // Box width that preserves ground distance
+        dReal zPos = REAL(0.5)*(sinA*rampW-cosA*rampZ); // Position that makes end meet ground
+        dReal yPos = ii*1.25*rampY;
+        dReal xPos = 0;
+    
+    
+        // Create the ramp
+        rampGeom[ii] = dCreateBox(space,rampW,rampY,rampZ);
+        dQFromAxisAndAngle(q,0,1,0,angle);
+        dGeomSetQuaternion(rampGeom[ii],q);
+        dGeomSetPosition(rampGeom[ii],xPos,yPos,zPos);
+
+        // Create the spheres
+        xPos = -REAL(0.5)*rampX + sphereRadius;
+        zPos = sinA*rampW + sphereRadius;
+        sphereBody[ii] = dBodyCreate(world);
+        dBodySetMass(sphereBody[ii],&sphereMass);
+        sphereGeom[ii] = dCreateSphere(space,sphereRadius);
+        dGeomSetBody(sphereGeom[ii],sphereBody[ii]);
+        dBodySetPosition(sphereBody[ii],xPos,yPos,zPos);
+        dBodySetAngularVel(sphereBody[ii],0,omega,0);
+    }
+}
+
+
+static void command (int cmd)
+{
+    switch (cmd) {
+        case 'h': case 'H':
+            rho-=0.02; 
+            if (rho<0) rho=0;
+            break;	
+        case 'j': case 'J':
+            rho+=0.02;
+            if (rho>1) rho=1;
+            break;
+        case 'n': case 'N':
+            mu-=0.02;
+            if (mu<0) mu=0;
+            break;
+        case 'm': case 'M':
+            mu+=0.02;
+            if (mu>1) mu=1;
+            break;
+        case 'r': case 'R':
+            reset();
+            break;
+        case ']':
+            omega+=1;
+            break;
+        case '[':
+            omega-=1;
+            break;
+    }
+}
+
+// simulation loop
+
+static void simLoop (int pause)
+{
+    if (!pause) {
+        dSpaceCollide (space,0,&nearCallback);
+        dWorldStep (world,0.017); // 60 fps
+        // remove all contact joints
+        dJointGroupEmpty (contactgroup);
+    }
+
+    // Render ramps and spheres
+    dsSetTexture (DS_WOOD);
+    for (int ii=0;ii<RAMP_COUNT;++ii) {
+        dVector3 sides;
+
+        dsSetColor (1,0.5,0);
+        dGeomBoxGetLengths(rampGeom[ii],sides);
+        dsDrawBox (dGeomGetPosition(rampGeom[ii]),dGeomGetRotation(rampGeom[ii]),sides);
+
+        dsSetColor(0,0,1);
+        dsDrawSphere (dGeomGetPosition(sphereGeom[ii]),dGeomGetRotation(sphereGeom[ii]), sphereRadius);
+    }
+}
+
+
+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;
+
+    dInitODE2(0);
+    reset();
+
+    // run simulation
+    dsSimulationLoop (argc,argv,352,288,&fn);
+
+    clear();
+    dCloseODE();
+    return 0;
+}