ode/demo/demo_plane2d.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 // Test my Plane2D constraint.
  24 // Uses ode-0.35 collision API.
  25
  26 # include       <stdio.h>
  27 # include       <stdlib.h>
  28 # include       <math.h>
  29 # include       <ode/ode.h>
  30 # include       <drawstuff/drawstuff.h>
  31 #include "texturepath.h"
  32
  33
  34 #   define drand48()  ((double) (((double) rand()) / ((double) RAND_MAX)))
  35
  36 # define        N_BODIES        40
  37 # define        STAGE_SIZE      8.0  // in m
  38
  39 # define        TIME_STEP       0.01
  40 # define        K_SPRING        10.0
  41 # define        K_DAMP          10.0
  42
  43 //using namespace ode;
  44
  45 struct GlobalVars
  46 {
  47     dWorld   dyn_world;
  48     dBody    dyn_bodies[N_BODIES];
  49     dReal    bodies_sides[N_BODIES][3];
  50
  51     dSpaceID coll_space_id;
  52     dJointID plane2d_joint_ids[N_BODIES];
  53     dJointGroup coll_contacts;
  54 };
  55
  56 static GlobalVars *g_globals_ptr = NULL;
  57
  58
  59
  60 static void     cb_start ()
  61 /*************************/
  62 {
  63     dAllocateODEDataForThread(dAllocateMaskAll);
  64
  65     static float    xyz[3] = { 0.5f*STAGE_SIZE, 0.5f*STAGE_SIZE, 0.85f*STAGE_SIZE};
  66     static float    hpr[3] = { 90.0f, -90.0f, 0 };
  67
  68     dsSetViewpoint (xyz, hpr);
  69 }
  70
  71
  72
  73 static void     cb_near_collision (void *, dGeomID o1, dGeomID o2)
  74 /********************************************************************/
  75 {
  76     dBodyID     b1 = dGeomGetBody (o1);
  77     dBodyID     b2 = dGeomGetBody (o2);
  78     dContact    contact;
  79
  80
  81     // exit without doing anything if the two bodies are static
  82     if (b1 == 0 && b2 == 0)
  83         return;
  84
  85     // exit without doing anything if the two bodies are connected by a joint
  86     if (b1 && b2 && dAreConnected (b1, b2))
  87     {
  88         /* MTRAP; */
  89         return;
  90     }
  91
  92     contact.surface.mode = 0;
  93     contact.surface.mu = 0; // frictionless
  94
  95     if (dCollide (o1, o2, 1, &contact.geom, sizeof (dContactGeom)))
  96     {
  97         dJointID c = dJointCreateContact (g_globals_ptr->dyn_world.id(),
  98                         g_globals_ptr->coll_contacts.id (), &contact);
  99         dJointAttach (c, b1, b2);
 100     }
 101 }
 102
 103
 104 static void     track_to_pos (dBody &body, dJointID joint_id,
 105                               dReal target_x, dReal target_y)
 106 /************************************************************************/
 107 {
 108     dReal  curr_x = body.getPosition()[0];
 109     dReal  curr_y = body.getPosition()[1];
 110
 111     dJointSetPlane2DXParam (joint_id, dParamVel, 1 * (target_x - curr_x));
 112     dJointSetPlane2DYParam (joint_id, dParamVel, 1 * (target_y - curr_y));
 113 }
 114
 115
 116
 117 static void     cb_sim_step (int pause)
 118 /*************************************/
 119 {
 120     if (! pause)
 121     {
 122         static dReal angle = 0;
 123
 124         angle += REAL( 0.01 );
 125
 126         track_to_pos (g_globals_ptr->dyn_bodies[0], g_globals_ptr->plane2d_joint_ids[0],
 127             dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * cos (angle) ),
 128             dReal( STAGE_SIZE/2 + STAGE_SIZE/2.0 * sin (angle) ));
 129
 130         /* double   f0 = 0.001; */
 131         /* for (int b = 0; b < N_BODIES; b ++) */
 132         /* { */
 133             /* double   p = 1 + b / (double) N_BODIES; */
 134             /* double   q = 2 - b / (double) N_BODIES; */
 135             /* g_globals_ptr->dyn_bodies[b].addForce (f0 * cos (p*angle), f0 * sin (q*angle), 0); */
 136         /* } */
 137         /* g_globals_ptr->dyn_bodies[0].addTorque (0, 0, 0.1); */
 138
 139         const int n = 10;
 140         for (int i = 0; i < n; i ++)
 141         {
 142             dSpaceCollide (g_globals_ptr->coll_space_id, 0, &cb_near_collision);
 143             g_globals_ptr->dyn_world.step (dReal(TIME_STEP/n));
 144             g_globals_ptr->coll_contacts.empty ();
 145         }
 146     }
 147
 148 # if 1  /* [ */
 149     {
 150         // @@@ hack Plane2D constraint error reduction here:
 151         for (int b = 0; b < N_BODIES; b ++)
 152         {
 153             const dReal     *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
 154             const dReal     *quat_ptr;
 155             dReal           quat[4],
 156                             quat_len;
 157
 158
 159             quat_ptr = dBodyGetQuaternion (g_globals_ptr->dyn_bodies[b].id());
 160             quat[0] = quat_ptr[0];
 161             quat[1] = 0;
 162             quat[2] = 0;
 163             quat[3] = quat_ptr[3];
 164             quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
 165             quat[0] /= quat_len;
 166             quat[3] /= quat_len;
 167             dBodySetQuaternion (g_globals_ptr->dyn_bodies[b].id(), quat);
 168             dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(), 0, 0, rot[2]);
 169         }
 170     }
 171 # endif  /* ] */
 172
 173
 174 # if 0  /* [ */
 175     {
 176         // @@@ friction
 177         for (int b = 0; b < N_BODIES; b ++)
 178         {
 179             const dReal *vel = dBodyGetLinearVel (g_globals_ptr->dyn_bodies[b].id()),
 180                         *rot = dBodyGetAngularVel (g_globals_ptr->dyn_bodies[b].id());
 181             dReal       s = 1.00;
 182             dReal       t = 0.99;
 183
 184             dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id(), s*vel[0],s*vel[1],s*vel[2]);
 185             dBodySetAngularVel (g_globals_ptr->dyn_bodies[b].id(),t*rot[0],t*rot[1],t*rot[2]);
 186         }
 187     }
 188 # endif  /* ] */
 189
 190
 191     {
 192         // ode  drawstuff
 193
 194         dsSetTexture (DS_WOOD);
 195         for (int b = 0; b < N_BODIES; b ++)
 196         {
 197             if (b == 0)
 198             dsSetColor (1.0, 0.5, 1.0);
 199             else
 200             dsSetColor (0, 0.5, 1.0);
 201 #ifdef dDOUBLE
 202             dsDrawBoxD (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
 203 #else
 204             dsDrawBox (g_globals_ptr->dyn_bodies[b].getPosition(), g_globals_ptr->dyn_bodies[b].getRotation(), g_globals_ptr->bodies_sides[b]);
 205 #endif
 206         }
 207     }
 208 }
 209
 210
 211
 212 extern int      main
 213 /******************/
 214 (
 215     int         argc,
 216     char        **argv
 217 )
 218 {
 219     int         b;
 220     dsFunctions drawstuff_functions;
 221
 222
 223     dInitODE2(0);
 224
 225     g_globals_ptr = new GlobalVars();
 226
 227     // dynamic world
 228
 229     dReal  cf_mixing;// = 1 / TIME_STEP * K_SPRING + K_DAMP;
 230     dReal  err_reduct;// = TIME_STEP * K_SPRING * cf_mixing;
 231     err_reduct = REAL( 0.5 );
 232     cf_mixing = REAL( 0.001 );
 233     dWorldSetERP (g_globals_ptr->dyn_world.id (), err_reduct);
 234     dWorldSetCFM (g_globals_ptr->dyn_world.id (), cf_mixing);
 235     g_globals_ptr->dyn_world.setGravity (0, 0.0, -1.0);
 236
 237     g_globals_ptr->coll_space_id = dSimpleSpaceCreate (0);
 238
 239     // dynamic bodies
 240     for (b = 0; b < N_BODIES; b ++)
 241     {
 242         int     l = (int) (1 + sqrt ((double) N_BODIES));
 243         dReal  x = dReal( (0.5 + (b / l)) / l * STAGE_SIZE );
 244         dReal  y = dReal( (0.5 + (b % l)) / l * STAGE_SIZE );
 245         dReal  z = REAL( 1.0 ) + REAL( 0.1 ) * (dReal)drand48();
 246
 247         g_globals_ptr->bodies_sides[b][0] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
 248         g_globals_ptr->bodies_sides[b][1] = dReal( 5 * (0.2 + 0.7*drand48()) / sqrt((double)N_BODIES) );
 249         g_globals_ptr->bodies_sides[b][2] = z;
 250
 251         g_globals_ptr->dyn_bodies[b].create (g_globals_ptr->dyn_world);
 252         g_globals_ptr->dyn_bodies[b].setPosition (x, y, z/2);
 253         g_globals_ptr->dyn_bodies[b].setData ((void*) (dsizeint)b);
 254         dBodySetLinearVel (g_globals_ptr->dyn_bodies[b].id (),
 255             dReal( 3 * (drand48 () - 0.5) ),
 256                         dReal( 3 * (drand48 () - 0.5) ), 0);
 257
 258         dMass m;
 259         m.setBox (1, g_globals_ptr->bodies_sides[b][0],g_globals_ptr->bodies_sides[b][1],g_globals_ptr->bodies_sides[b][2]);
 260         m.adjust (REAL(0.1) * g_globals_ptr->bodies_sides[b][0] * g_globals_ptr->bodies_sides[b][1]);
 261         g_globals_ptr->dyn_bodies[b].setMass (&m);
 262
 263         g_globals_ptr->plane2d_joint_ids[b] = dJointCreatePlane2D (g_globals_ptr->dyn_world.id (), 0);
 264         dJointAttach (g_globals_ptr->plane2d_joint_ids[b], g_globals_ptr->dyn_bodies[b].id (), 0);
 265     }
 266
 267     dJointSetPlane2DXParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
 268     dJointSetPlane2DYParam (g_globals_ptr->plane2d_joint_ids[0], dParamFMax, 10);
 269
 270
 271     // collision geoms and joints
 272     dCreatePlane (g_globals_ptr->coll_space_id,  1, 0, 0, 0);
 273     dCreatePlane (g_globals_ptr->coll_space_id, -1, 0, 0, -STAGE_SIZE);
 274     dCreatePlane (g_globals_ptr->coll_space_id,  0,  1, 0, 0);
 275     dCreatePlane (g_globals_ptr->coll_space_id,  0, -1, 0, -STAGE_SIZE);
 276
 277     for (b = 0; b < N_BODIES; b ++)
 278     {
 279         dGeomID coll_box_id;
 280         coll_box_id = dCreateBox (g_globals_ptr->coll_space_id,
 281             g_globals_ptr->bodies_sides[b][0], g_globals_ptr->bodies_sides[b][1], g_globals_ptr->bodies_sides[b][2]);
 282         dGeomSetBody (coll_box_id, g_globals_ptr->dyn_bodies[b].id ());
 283     }
 284
 285     g_globals_ptr->coll_contacts.create ();
 286
 287     {
 288         // simulation loop (by drawstuff lib)
 289         drawstuff_functions.version = DS_VERSION;
 290         drawstuff_functions.start = &cb_start;
 291         drawstuff_functions.step = &cb_sim_step;
 292         drawstuff_functions.command = 0;
 293         drawstuff_functions.stop = 0;
 294         drawstuff_functions.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
 295
 296         dsSimulationLoop (argc, argv, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &drawstuff_functions);
 297     }
 298
 299     delete g_globals_ptr;
 300     g_globals_ptr = NULL;
 301
 302         dCloseODE();
 303     return 0;
 304 }