summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/demo/demo_tracks.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/demo/demo_tracks.cpp')
-rw-r--r--libs/ode-0.16.1/ode/demo/demo_tracks.cpp498
1 files changed, 498 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/demo/demo_tracks.cpp b/libs/ode-0.16.1/ode/demo/demo_tracks.cpp
new file mode 100644
index 0000000..9b4f5dd
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_tracks.cpp
@@ -0,0 +1,498 @@
+/*************************************************************************
+ * *
+ * 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 <iostream>
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include "texturepath.h"
+
+#ifdef dDOUBLE
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawBox dsDrawBoxD
+#define dsDrawTriangle dsDrawTriangleD
+#define dsDrawLine dsDrawLineD
+#endif
+
+
+
+const dReal ball_radius = 0.4;
+const dReal balls_sep = 2; // separation between the balls
+
+/* Choose one test case
+ */
+#define TEST_CASE 0
+
+#if TEST_CASE == 0
+const dReal track_len = 10;
+const dReal track_height = 1;
+const dReal track_width = 0.1;
+const dReal track_gauge = 1;
+const dReal track_elevation = 2;
+const dReal track_angle = 80 * M_PI/180.;
+const dReal track_incl = 10 * M_PI/180.;
+#elif TEST_CASE == 1
+const dReal track_len = 10;
+const dReal track_height = 1;
+const dReal track_width = 0.1;
+const dReal track_gauge = 1.9*ball_radius;
+const dReal track_elevation = 2;
+const dReal track_angle = 0 * M_PI/180.;
+const dReal track_incl = 10 * M_PI/180.;
+#elif TEST_CASE == 2
+const dReal track_len = 10;
+const dReal track_height = 1;
+const dReal track_width = 0.1;
+const dReal track_gauge = 1.9*ball_radius;
+const dReal track_elevation = 2;
+const dReal track_angle = 15 * M_PI/180.;
+const dReal track_incl = 10 * M_PI/180.;
+#elif TEST_CASE == 3
+const dReal track_len = 10;
+const dReal track_height = .7;
+const dReal track_width = 0.1;
+const dReal track_gauge = track_height*1.1;
+const dReal track_elevation = 2;
+const dReal track_angle = 90 * M_PI/180.;
+const dReal track_incl = 10 * M_PI/180.;
+#else
+#error "TEST_CAST to a valid value!"
+#endif
+
+
+
+dWorldID world;
+dSpaceID space;
+dJointGroupID contact_group;
+dGeomID ground;
+dGeomID ball1_geom, ball2_geom;
+dTriMeshDataID mesh_data;
+dGeomID mesh_geom;
+
+dBodyID ball1_body, ball2_body;
+
+const unsigned n_box_verts = 8;
+dVector3 box_verts[n_box_verts] = {
+ {-track_len/2, -track_width/2, track_height/2}, // 0
+ { track_len/2, -track_width/2, track_height/2}, // 1
+ { track_len/2, track_width/2, track_height/2}, // 2
+ {-track_len/2, track_width/2, track_height/2}, // 3
+ { track_len/2, -track_width/2, -track_height/2}, // 4
+ {-track_len/2, -track_width/2, -track_height/2}, // 5
+ {-track_len/2, track_width/2, -track_height/2}, // 6
+ { track_len/2, track_width/2, -track_height/2} // 7
+};
+
+const unsigned n_box_faces = 12;
+dTriIndex box_faces[n_box_faces * 3] = {
+ 0, 1, 2,
+ 0, 2, 3,
+ 1, 4, 7,
+ 1, 7, 2,
+ 4, 5, 6,
+ 4, 6, 7,
+ 5, 0, 3,
+ 5, 3, 6,
+ 3, 2, 7,
+ 3, 7, 6,
+ 0, 5, 4,
+ 0, 4, 1
+};
+
+
+const unsigned n_track_verts = n_box_verts * 2;
+const unsigned n_track_faces = n_box_faces * 2;
+
+dVector3 track_verts[n_track_verts];
+dTriIndex track_faces[n_track_faces * 3];
+
+
+
+void resetBall(dBodyID b, unsigned idx)
+{
+ dBodySetPosition(b,
+ 0.5*track_len*cos(track_incl) // Z
+ - 0.5*track_height*sin(track_incl)
+ - ball_radius, // X
+ balls_sep*idx, // Y
+ track_elevation + ball_radius// Z
+ + 0.5*track_len*sin(track_incl)
+ + 0.5*track_height*cos(track_incl));
+ dMatrix3 r = {1, 0, 0, 0,
+ 0, 1, 0, 0,
+ 0, 0, 1, 0};
+ dBodySetRotation(b, r);
+ dBodySetLinearVel(b, 0, 0, 0);
+ dBodySetAngularVel(b, 0, 0, 0);
+
+}
+
+
+void resetSim()
+{
+ resetBall(ball1_body, 0);
+ resetBall(ball2_body, 1);
+}
+
+
+void start()
+{
+ dAllocateODEDataForThread(dAllocateMaskAll);
+
+ world = dWorldCreate();
+ dWorldSetGravity (world,0,0,-9.8);
+
+ contact_group = dJointGroupCreate(0);
+
+ space = dSimpleSpaceCreate (0);
+
+
+ // first, the ground plane
+ // it has to coincide with the plane we have in drawstuff
+ ground = dCreatePlane(space, 0, 0, 1, 0);
+
+
+ // now a ball
+ dMass m;
+ dMassSetSphere(&m, 0.1, ball_radius);
+
+ ball1_geom = dCreateSphere(space, ball_radius);
+ ball1_body = dBodyCreate(world);
+ dGeomSetBody(ball1_geom, ball1_body);
+ dBodySetMass(ball1_body, &m);
+
+ ball2_geom = dCreateSphere(space, ball_radius);
+ ball2_body = dBodyCreate(world);
+ dGeomSetBody(ball2_geom, ball2_body);
+ dBodySetMass(ball2_body, &m);
+
+
+
+
+ // tracks made out of boxes
+ dGeomID trk;
+ dMatrix3 r1, r2, r3;
+ dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
+ dMatrix3 s1, s2, s3;
+ dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};
+
+ dRFromAxisAndAngle(r1, 1, 0, 0, track_angle);
+ dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
+ dMultiply0_333(r3, r2, r1);
+
+ dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
+ dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
+ dMultiply0_333(s3, s2, s1);
+
+ trk = dCreateBox(space, track_len, track_width, track_height);
+ dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
+ dGeomSetRotation(trk, r3);
+
+ trk = dCreateBox(space, track_len, track_width, track_height);
+ dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
+ dGeomSetRotation(trk, s3);
+
+
+
+
+
+ // tracks made out of trimesh
+ for (unsigned i=0; i<n_box_verts; ++i) {
+ dVector3 p;
+ dMultiply0_331(p, s3, box_verts[i]);
+ dAddVectors3(p, p, so);
+ dCopyVector3(track_verts[i], p);
+ }
+ // trimesh tracks 2, transform all vertices by s3
+ for (unsigned i=0; i<n_box_verts; ++i) {
+ dVector3 p;
+ dMultiply0_331(p, r3, box_verts[i]);
+ dAddVectors3(p, p, ro);
+ dCopyVector3(track_verts[n_box_verts + i], p);
+ }
+
+ // copy face indices
+ for (unsigned i=0; i<n_box_faces; ++i)
+ for (unsigned j=0; j<3; ++j) // each face index
+ track_faces[3*i+j] = box_faces[3*i+j];
+ for (unsigned i=0; i<n_box_faces; ++i)
+ for (unsigned j=0; j<3; ++j) // each face index
+ track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;
+
+ mesh_data = dGeomTriMeshDataCreate();
+ dGeomTriMeshDataBuildSimple(mesh_data,
+ track_verts[0], n_track_verts,
+ track_faces, 3*n_track_faces);
+ mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);
+
+
+
+
+
+ resetSim();
+
+
+ // initial camera position
+ static float xyz[3] = {-5.9414,-0.4804,2.9800};
+ static float hpr[3] = {32.5000,-10.0000,0.0000};
+ dsSetViewpoint (xyz,hpr);
+
+ dsSetSphereQuality(3);
+}
+
+
+void nearCallback(void *, dGeomID a, dGeomID b)
+{
+ const unsigned max_contacts = 8;
+ dContact contacts[max_contacts];
+
+ if (!dGeomGetBody(a) && !dGeomGetBody(b))
+ return; // don't handle static geom collisions
+
+ int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
+ //clog << "got " << n << " contacts" << endl;
+
+ /* Simple contact merging:
+ * If we have contacts that are too close with the same normal, keep only
+ * the one with maximum depth.
+ * The epsilon that defines what "too close" means can be a heuristic.
+ */
+ int new_n = 0;
+ dReal epsilon = 1e-1; // default
+ /* If we know one of the geoms is a sphere, we can base the epsilon on the
+ * sphere's radius.
+ */
+ dGeomID s = 0;
+ if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
+ (dGeomGetClass(b) == dSphereClass && (s = b))) {
+ epsilon = dGeomSphereGetRadius(s) * 0.3;
+ }
+
+
+ for (int i=0; i<n; ++i) {
+
+ // this block draws the contact points before merging, in red
+ dMatrix3 r;
+ dRSetIdentity(r);
+ dsSetColor(1, 0, 0);
+ dsSetTexture(DS_NONE);
+ dsDrawSphere(contacts[i].geom.pos, r, 0.008);
+
+ // let's offset the line a bit to avoid drawing overlap issues
+ float xyzf[3], hprf[3];
+ dsGetViewpoint(xyzf, hprf);
+ dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
+ dVector3 v;
+ dSubtractVectors3(v, contacts[i].geom.pos, xyz);
+ dVector3 c;
+ dCalcVectorCross3(c, v, contacts[i].geom.pos);
+ dNormalize3(c);
+ dVector3 pos1;
+ dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
+ dVector3 pos2;
+ dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
+ dsDrawLine(pos1, pos2);
+ // end of contacts drawing code
+
+
+
+ int closest_point = i;
+ for (int j=0; j<new_n; ++j) {
+ dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
+ if (alignment > 0.99 // about 8 degrees of difference
+ &&
+ dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
+ // they are too close
+ closest_point = j;
+ //clog << "found close points: " << j << " and " << i << endl;
+ break;
+ }
+ }
+
+ if (closest_point != i) {
+ // we discard one of the points
+ if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
+ // the new point is deeper, copy it over closest_point
+ contacts[closest_point] = contacts[i];
+ } else
+ contacts[new_n++] = contacts[i]; // the point is preserved
+ }
+ //clog << "reduced from " << n << " to " << new_n << endl;
+ n = new_n;
+
+ for (int i=0; i<n; ++i) {
+ contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
+ contacts[i].surface.mu = 10;
+ contacts[i].surface.bounce = 0.2;
+ contacts[i].surface.bounce_vel = 0;
+ contacts[i].surface.soft_erp = 1e-3;
+ //clog << "depth: " << contacts[i].geom.depth << endl;
+
+
+ dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
+ dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
+
+ dMatrix3 r;
+ dRSetIdentity(r);
+ dsSetColor(0, 0, 1);
+ dsSetTexture(DS_NONE);
+ dsDrawSphere(contacts[i].geom.pos, r, 0.01);
+ dsSetColor(0, 1, 0);
+ dVector3 pos2;
+ dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
+ dsDrawLine(contacts[i].geom.pos, pos2);
+ }
+ //clog << "----" << endl;
+}
+
+
+
+
+void stop()
+{
+ dGeomDestroy(mesh_geom);
+ dGeomTriMeshDataDestroy(mesh_data);
+
+ dBodyDestroy(ball1_body);
+ dBodyDestroy(ball2_body);
+
+ dGeomDestroy(ground);
+
+ dJointGroupDestroy(contact_group);
+
+ dSpaceDestroy(space); // will destroy all geoms
+
+ dWorldDestroy(world);
+}
+
+
+static void command (int cmd)
+{
+ switch (cmd) {
+ case ' ':
+ resetSim();
+ break;
+ }
+}
+
+
+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, 0.5);
+ dsSetTexture (DS_CHECKERED);
+ dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
+ break;
+ case dBoxClass:
+ {
+ dVector3 lengths;
+ dsSetColorAlpha(1, 1, 0, 0.5);
+ dsSetTexture (DS_WOOD);
+ dGeomBoxGetLengths(g, lengths);
+ dsDrawBox(pos, rot, lengths);
+ break;
+ }
+ case dTriMeshClass:
+ {
+ int numi = dGeomTriMeshGetTriangleCount(g);
+
+ for (int i=0; i<numi; ++i) {
+ dVector3 v0, v1, v2;
+ dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);
+
+ dsSetTexture (DS_WOOD);
+
+ dsSetDrawMode(DS_WIREFRAME);
+ dsSetColorAlpha(0, 0, 0, 1.0);
+ dsDrawTriangle(pos, rot, v0, v1, v2, true);
+
+ dsSetDrawMode(DS_POLYFILL);
+ dsSetColorAlpha(1, 1, 0, 0.5);
+ dsDrawTriangle(pos, rot, v0, v1, v2, true);
+ }
+ break;
+ }
+
+ default:
+ {}
+ }
+}
+
+
+
+void simLoop (int pause)
+{
+ if (!pause) {
+
+ const dReal step = 0.02;
+ const unsigned nsteps = 1;
+
+ for (unsigned i=0; i<nsteps; ++i) {
+ dSpaceCollide(space, 0, nearCallback);
+ dWorldQuickStep(world, step);
+ dJointGroupEmpty(contact_group);
+ }
+ } else {
+ dSpaceCollide(space, 0, nearCallback);
+ dJointGroupEmpty(contact_group);
+ }
+
+ // now we draw everything
+ unsigned ngeoms = dSpaceGetNumGeoms(space);
+ for (unsigned i=0; i<ngeoms; ++i) {
+ dGeomID g = dSpaceGetGeom(space, i);
+
+ if (g == ground)
+ continue; // drawstuff is already drawing it for us
+
+ drawGeom(g);
+ }
+
+ if (dBodyGetPosition(ball1_body)[0] < -track_len)
+ resetSim();
+}
+
+
+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 = stop;
+ fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+ // create world
+ dInitODE();
+
+ // run demo
+ dsSimulationLoop (argc, argv, 800, 600, &fn);
+
+ dCloseODE();
+ return 0;
+}