From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001
From: sanine <sanine.not@pm.me>
Date: Sat, 1 Oct 2022 20:59:36 -0500
Subject: add ode

---
 libs/ode-0.16.1/ode/demo/demo_tracks.cpp | 498 +++++++++++++++++++++++++++++++
 1 file changed, 498 insertions(+)
 create mode 100644 libs/ode-0.16.1/ode/demo/demo_tracks.cpp

(limited to 'libs/ode-0.16.1/ode/demo/demo_tracks.cpp')

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;
+}
-- 
cgit v1.2.1