add ode

1 files changed, 304 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp
new file mode 100644
index 0000000..559f9ae
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_plane2d.cpp
@@ -0,0 +1,304 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// Test my Plane2D constraint.
+// Uses ode-0.35 collision API.
+
+# include       <stdio.h>
+# include       <stdlib.h>
+# include       <math.h>
+# include       <ode/ode.h>
+# include       <drawstuff/drawstuff.h>
+#include "texturepath.h"
+
+
+#   define drand48()  ((double) (((double) rand()) / ((double) RAND_MAX)))
+
+# define        N_BODIES        40
+# define        STAGE_SIZE      8.0  // in m
+
+# define        TIME_STEP       0.01
+# define        K_SPRING        10.0
+# define        K_DAMP          10.0
+
+//using namespace ode;
+
+struct GlobalVars
+{
+    dWorld   dyn_world;
+    dBody    dyn_bodies[N_BODIES];
+    dReal    bodies_sides[N_BODIES][3];
+
+    dSpaceID coll_space_id;
+    dJointID plane2d_joint_ids[N_BODIES];
+    dJointGroup coll_contacts;
+};
+
+static GlobalVars *g_globals_ptr = NULL;
+
+
+
+static void     cb_start ()
+/*************************/
+{
+    dAllocateODEDataForThread(dAllocateMaskAll);
+
+    static float    xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.65f*STAGE_SIZE};
+    static float    hpr[3] = { 90.0f, -90.0f, 0 };
+
+    dsSetViewpoint (xyz, hpr);
+}
+
+
+
+static void     cb_near_collision (void *, dGeomID o1, dGeomID o2)
+/********************************************************************/
+{
+    dBodyID     b1 = dGeomGetBody (o1);
+    dBodyID     b2 = dGeomGetBody (o2);
+    dContact    contact;
+
+
+    // exit without doing anything if the two bodies are static
+    if (b1 == 0 && b2 == 0)
+        return;
+
+    // exit without doing anything if the two bodies are connected by a joint
+    if (b1 && b2 && dAreConnected (b1, b2))
+    {
+        /* MTRAP; */
+        return;
+    }
+
+    contact.surface.mode = 0;
+    contact.surface.mu = 0; // frictionless
+
+    if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom)))
+    {
+        dJointID c = dJointCreateContact (g_globals_ptr->dyn_world.id(),
+                        g_globals_ptr->coll_contacts.id (), &contact);
+        dJointAttach (c, b1, b2);
+    }
+}
+
+
+static void     track_to_pos (dBody &body, dJointID joint_id,
+                              dReal target_x, dReal target_y)
+/************************************************************************/
+{
+    dReal  curr_x = body.getPosition()[0];
+    dReal  curr_y = body.getPosition()[1];
+
+    dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
+    dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
+}
+
+
+
+static void     cb_sim_step (int pause)
+/*************************************/
+{
+    if (! pause)
+    {
+        static dReal angle = 0;
+
+        angle += REAL( 0.01 );
+
+        track_to_pos (g_globals_ptr->dyn_bodies[0], g_globals_ptr->plane2d_joint_ids[0],
+            dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ),
+            dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) ));
+
+        /* double   f0 = 0.001; */
+        /* for (int b = 0; b < N_BODIES; b ++) */
+        /* { */
+            /* double   p = 1 + b / (double) N_BODIES; */
+            /* double   q = 2 - b / (double) N_BODIES; */
+            /* g_globals_ptr->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */
+        /* } */
+        /* g_globals_ptr->dyn_bodies[0].addTorque (0, 0, 0.1); */
+
+        const int n = 10;
+        for (int i = 0; i < n; i ++)
+        {
+            dSpaceCollide (g_globals_ptr->coll_space_id, 0, &cb_near_collision);
+            g_globals_ptr->dyn_world.step (dReal(TIME_STEP/n));
+            g_globals_ptr->coll_contacts.empty ();
+        }
+    }
+
+# if 1  /* [ */
+    {
+        // @@@ hack Plane2D constraint error reduction here:
+        for (int b = 0; b < N_BODIES; b ++)
+        {
+            const dReal     *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
+            const dReal     *quat_ptr;
+            dReal           quat[4],
+                            quat_len;
+
+
+            quat_ptr = dBodyGetQuaternion (g_globals_ptr->dyn_bodies[b].id());
+            quat[0] = quat_ptr[0];
+            quat[1] = 0;
+            quat[2] = 0;
+            quat[3] = quat_ptr[3];
+            quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
+            quat[0] /= quat_len;
+            quat[3] /= quat_len;
+            dBodySetQuaternion (g_globals_ptr->dyn_bodies[b].id(), quat);
+            dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(), 0, 0, rot[2]);
+        }
+    }
+# endif  /* ] */
+
+
+# if 0  /* [ */
+    {
+        // @@@ friction
+        for (int b = 0; b < N_BODIES; b ++)
+        {
+            const dReal *vel = dBodyGetLinearVel (g_globals_ptr->dyn_bodies[b].id()),
+                        *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
+            dReal       s = 1.00;
+            dReal       t = 0.99;
+
+            dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]);
+            dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]);
+        }
+    }
+# endif  /* ] */
+
+
+    {
+        // ode  drawstuff
+
+        dsSetTexture (DS_WOOD);
+        for (int b = 0; b < N_BODIES; b ++)
+        {
+            if (b == 0)
+            dsSetColor (1.0, 0.5, 1.0);
+            else
+            dsSetColor (0, 0.5, 1.0);
+#ifdef dDOUBLE
+            dsDrawBoxD (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
+#else
+            dsDrawBox (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
+#endif
+        }
+    }
+}
+
+
+
+extern int      main
+/******************/
+(
+    int         argc,
+    char        **argv
+)
+{
+    int         b;
+    dsFunctions drawstuff_functions;
+
+
+    dInitODE2(0);
+
+    g_globals_ptr = new GlobalVars();
+
+    // dynamic world
+
+    dReal  cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP;
+    dReal  err_reduct;// = TIME_STEP * K_SPRING * cf_mixing;
+    err_reduct = REAL( 0.5 );
+    cf_mixing = REAL( 0.001 );
+    dWorldSetERP (g_globals_ptr->dyn_world.id (), err_reduct);
+    dWorldSetCFM (g_globals_ptr->dyn_world.id (), cf_mixing);
+    g_globals_ptr->dyn_world.setGravity (0, 0.0, -1.0);
+
+    g_globals_ptr->coll_space_id = dSimpleSpaceCreate (0);
+
+    // dynamic bodies
+    for (b = 0; b < N_BODIES; b ++)
+    {
+        int     l = (int) (1 + sqrt ((double) N_BODIES));
+        dReal  x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE );
+        dReal  y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE );
+        dReal  z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48();
+
+        g_globals_ptr->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
+        g_globals_ptr->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
+        g_globals_ptr->bodies_sides[b][2] = z;
+
+        g_globals_ptr->dyn_bodies[b].create (g_globals_ptr->dyn_world);
+        g_globals_ptr->dyn_bodies[b].setPosition (x, y, z/2);
+        g_globals_ptr->dyn_bodies[b].setData ((void*) (dsizeint)b);
+        dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id (),
+            dReal( 3 * (drand48 () - 0.5) ), 
+			dReal( 3 * (drand48 () - 0.5) ), 0);
+
+        dMass m;
+        m.setBox (1, g_globals_ptr->bodies_sides[b][0],g_globals_ptr->bodies_sides[b][1],g_globals_ptr->bodies_sides[b][2]);
+        m.adjust (REAL(0.1) * g_globals_ptr->bodies_sides[b][0] * g_globals_ptr->bodies_sides[b][1]);
+        g_globals_ptr->dyn_bodies[b].setMass (&m);
+
+        g_globals_ptr->plane2d_joint_ids[b] = dJointCreatePlane2D (g_globals_ptr->dyn_world.id (), 0);
+        dJointAttach (g_globals_ptr->plane2d_joint_ids[b], g_globals_ptr->dyn_bodies[b].id (), 0);
+    }
+
+    dJointSetPlane2DXParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
+    dJointSetPlane2DYParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
+
+
+    // collision geoms and joints
+    dCreatePlane (g_globals_ptr->coll_space_id,  1, 0, 0, 0);
+    dCreatePlane (g_globals_ptr->coll_space_id, -1, 0, 0, -STAGE_SIZE);
+    dCreatePlane (g_globals_ptr->coll_space_id,  0,  1, 0, 0);
+    dCreatePlane (g_globals_ptr->coll_space_id,  0, -1, 0, -STAGE_SIZE);
+
+    for (b = 0; b < N_BODIES; b ++)
+    {
+        dGeomID coll_box_id;
+        coll_box_id = dCreateBox (g_globals_ptr->coll_space_id,
+            g_globals_ptr->bodies_sides[b][0], g_globals_ptr->bodies_sides[b][1], g_globals_ptr->bodies_sides[b][2]);
+        dGeomSetBody (coll_box_id, g_globals_ptr->dyn_bodies[b].id ());
+    }
+
+    g_globals_ptr->coll_contacts.create ();
+
+    {
+        // simulation loop (by drawstuff lib)
+        drawstuff_functions.version = DS_VERSION;
+        drawstuff_functions.start = &cb_start;
+        drawstuff_functions.step = &cb_sim_step;
+        drawstuff_functions.command = 0;
+        drawstuff_functions.stop = 0;
+        drawstuff_functions.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+        dsSimulationLoop (argc, argv, 352,288,&drawstuff_functions);
+    }
+
+    delete g_globals_ptr;
+    g_globals_ptr = NULL;
+
+	dCloseODE();
+    return 0;
+}