first commit

thirdparty/ode-0.16.5/ode/src/Makefile.am

@@ -0,0 +1,201 @@
+SUBDIRS = joints
+
+AM_CPPFLAGS = -I$(top_srcdir)/include \
+        -I$(top_builddir)/include \
+        -D__ODE__
+
+
+
+lib_LTLIBRARIES = libode.la
+
+libode_la_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@ @ODE_VERSION_INFO@
+libode_la_LIBADD = joints/libjoints.la
+
+
+# please, let's keep the filenames sorted
+libode_la_SOURCES =     nextafterf.c \
+                        array.cpp array.h \
+                        box.cpp \
+                        capsule.cpp \
+                        collision_cylinder_box.cpp \
+                        collision_cylinder_plane.cpp \
+                        collision_cylinder_sphere.cpp \
+                        collision_kernel.cpp collision_kernel.h \
+                        collision_quadtreespace.cpp \
+                        collision_sapspace.cpp \
+                        collision_space.cpp \
+                        collision_space_internal.h \
+                        collision_std.h \
+                        collision_transform.cpp collision_transform.h \
+                        collision_trimesh_colliders.h \
+                        collision_trimesh_disabled.cpp \
+                        collision_trimesh_internal.h \
+                        collision_trimesh_opcode.h \
+                        collision_trimesh_gimpact.h \
+                        collision_util.cpp collision_util.h \
+                        common.h \
+                        convex.cpp \
+                        coop_matrix_types.h \
+                        cylinder.cpp \
+                        default_threading.cpp default_threading.h \
+                        error.cpp error.h \
+                        export-dif.cpp \
+                        fastdot.cpp fastdot_impl.h \
+                        fastldltfactor.cpp fastldltfactor_impl.h \
+                        fastldltsolve.cpp fastldltsolve_impl.h \
+                        fastlsolve.cpp fastlsolve_impl.h \
+                        fastltsolve.cpp fastltsolve_impl.h \
+                        fastvecscale.cpp fastvecscale_impl.h \
+                        heightfield.cpp heightfield.h \
+                        lcp.cpp lcp.h \
+                        mass.cpp \
+                        mat.cpp mat.h \
+                        matrix.cpp matrix.h \
+                        memory.cpp \
+                        misc.cpp \
+                        objects.cpp objects.h \
+                        obstack.cpp obstack.h \
+                        ode.cpp \
+                        odeinit.cpp \
+                        odemath.cpp odemath.h \
+                        odeou.h \
+                        odetls.h \
+                        plane.cpp \
+                        quickstep.cpp quickstep.h \
+                        ray.cpp \
+                        resource_control.cpp resource_control.h \
+                        rotation.cpp \
+                        simple_cooperative.cpp simple_cooperative.h \
+                        sphere.cpp \
+                        step.cpp step.h \
+                        timer.cpp \
+                        threaded_solver_ldlt.h \
+                        threading_atomics_provs.h \
+                        threading_base.cpp threading_base.h \
+                        threading_fake_sync.h \
+                        threading_impl.cpp threading_impl.h \
+                        threading_impl_posix.h \
+                        threading_impl_templates.h \
+                        threading_impl_win.h \
+                        threading_pool_posix.cpp \
+                        threading_pool_win.cpp \
+                        threadingutils.h \
+                        typedefs.h \
+                        util.cpp util.h
+
+
+###################################
+#       O U    S T U F F
+###################################
+
+
+if ENABLE_OU
+
+AM_CPPFLAGS += -I$(top_srcdir)/ou/include
+libode_la_LIBADD += $(top_builddir)/ou/src/ou/libou.la
+libode_la_SOURCES +=    odetls.cpp odetls.h \
+                        odeou.cpp odeou.h
+
+endif
+
+
+###################################
+#   G I M P A C T    S T U F F
+###################################
+
+
+if GIMPACT
+AM_CPPFLAGS += -DdTRIMESH_ENABLED -DdTRIMESH_GIMPACT -I$(top_srcdir)/GIMPACT/include
+
+libode_la_LIBADD += $(top_builddir)/GIMPACT/src/libGIMPACT.la
+libode_la_SOURCES +=    collision_trimesh_gimpact.cpp \
+                        collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \
+                        gimpact_contact_export_helper.cpp gimpact_contact_export_helper.h \
+                        gimpact_gim_contact_accessor.h \
+                        gimpact_plane_contact_accessor.h \
+                        collision_trimesh_trimesh.cpp \
+                        collision_trimesh_sphere.cpp \
+                        collision_trimesh_ray.cpp \
+                        collision_trimesh_box.cpp \
+                        collision_trimesh_ccylinder.cpp \
+                        collision_trimesh_internal.h \
+                        collision_cylinder_trimesh.cpp \
+                        collision_trimesh_plane.cpp \
+                        collision_convex_trimesh.cpp
+endif
+
+
+
+#################################
+#   O P C O D E    S T U F F
+#################################
+
+
+if OPCODE
+AM_CPPFLAGS += -I$(top_srcdir)/OPCODE -I$(top_srcdir)/OPCODE/Ice -DdTRIMESH_ENABLED -DdTRIMESH_OPCODE
+libode_la_LIBADD += $(top_builddir)/OPCODE/libOPCODE.la \
+                    $(top_builddir)/OPCODE/Ice/libIce.la
+
+libode_la_SOURCES+=     collision_trimesh_opcode.cpp \
+                        collision_trimesh_internal.cpp collision_trimesh_internal_impl.h \
+                        collision_trimesh_trimesh.cpp \
+                        collision_trimesh_trimesh_old.cpp \
+                        collision_trimesh_sphere.cpp \
+                        collision_trimesh_ray.cpp \
+                        collision_trimesh_box.cpp \
+                        collision_trimesh_ccylinder.cpp \
+                        collision_trimesh_internal.h \
+                        collision_cylinder_trimesh.cpp \
+                        collision_trimesh_plane.cpp \
+                        collision_convex_trimesh.cpp
+endif
+
+
+if LIBCCD
+
+AM_CPPFLAGS += -DdLIBCCD_ENABLED
+AM_CPPFLAGS += -I$(top_srcdir)/libccd/src/custom
+
+if LIBCCD_INTERNAL
+AM_CPPFLAGS += -I$(top_srcdir)/libccd/src -I$(top_builddir)/libccd/src 
+libode_la_LIBADD += $(top_builddir)/libccd/src/libccd.la
+AM_CPPFLAGS += -DdLIBCCD_INTERNAL
+else
+AM_CPPFLAGS += $(CCD_CFLAGS)
+libode_la_LIBADD += $(CCD_LIBS)
+AM_CPPFLAGS += -DdLIBCCD_SYSTEM
+endif
+
+
+libode_la_SOURCES += collision_libccd.cpp collision_libccd.h
+
+if LIBCCD_BOX_CYL
+AM_CPPFLAGS += -DdLIBCCD_BOX_CYL
+endif
+
+if LIBCCD_CYL_CYL
+AM_CPPFLAGS += -DdLIBCCD_CYL_CYL
+endif
+
+if LIBCCD_CAP_CYL
+AM_CPPFLAGS += -DdLIBCCD_CAP_CYL
+endif
+
+if LIBCCD_CONVEX_BOX
+AM_CPPFLAGS += -DdLIBCCD_CONVEX_BOX
+endif
+if LIBCCD_CONVEX_CAP
+AM_CPPFLAGS += -DdLIBCCD_CONVEX_CAP
+endif
+if LIBCCD_CONVEX_CYL
+AM_CPPFLAGS += -DdLIBCCD_CONVEX_CYL
+endif
+if LIBCCD_CONVEX_SPHERE
+AM_CPPFLAGS += -DdLIBCCD_CONVEX_SPHERE
+endif
+if LIBCCD_CONVEX_CONVEX
+AM_CPPFLAGS += -DdLIBCCD_CONVEX_CONVEX
+endif
+
+
+endif

thirdparty/ode-0.16.5/ode/src/array.cpp

@@ -0,0 +1,81 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include <ode/memory.h>
+#include <ode/error.h>
+#include "config.h"
+#include "array.h"
+
+
+static inline int roundUpToPowerOfTwo (int x)
+{
+    int i = 1;
+    while (i < x) i <<= 1;
+    return i;
+}
+
+
+void dArrayBase::_freeAll (int sizeofT)
+{
+    if (_data) {
+        if (_data == this+1) return;	// if constructLocalArray() was called
+        dFree (_data,_anum * sizeofT);
+    }
+}
+
+
+void dArrayBase::_setSize (int newsize, int sizeofT)
+{
+    if (newsize < 0) return;
+    if (newsize > _anum) {
+        if (_data == this+1) {
+            // this is a no-no, because constructLocalArray() was called
+            dDebug (0,"setSize() out of space in LOCAL array");
+        }
+        int newanum = roundUpToPowerOfTwo (newsize);
+        if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
+        else _data = dAlloc (newanum*sizeofT);
+        _anum = newanum;
+    }
+    _size = newsize;
+}
+
+
+void * dArrayBase::operator new (size_t size)
+{
+    return dAlloc (size);
+}
+
+
+void dArrayBase::operator delete (void *ptr, size_t size)
+{
+    dFree (ptr,size);
+}
+
+
+void dArrayBase::constructLocalArray (int __anum)
+{
+    _size = 0;
+    _anum = __anum;
+    _data = this+1;
+}

thirdparty/ode-0.16.5/ode/src/array.h

@@ -0,0 +1,135 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this comes from the `reuse' library. copy any changes back to the source.
+ *
+ * Variable sized array template. The array is always stored in a contiguous
+ * chunk. The array can be resized. A size increase will cause more memory
+ * to be allocated, and may result in relocation of the array memory.
+ * A size decrease has no effect on the memory allocation.
+ *
+ * Array elements with constructors or destructors are not supported!
+ * But if you must have such elements, here's what to know/do:
+ *   - Bitwise copy is used when copying whole arrays.
+ *   - When copying individual items (via push(), insert() etc) the `='
+ *     (equals) operator is used. Thus you should define this operator to do
+ *     a bitwise copy. You should probably also define the copy constructor.
+ */
+
+
+#ifndef _ODE_ARRAY_H_
+#define _ODE_ARRAY_H_
+
+#include <ode/odeconfig.h>
+
+
+// this base class has no constructors or destructor, for your convenience.
+
+class dArrayBase {
+protected:
+    int _size;		// number of elements in `data'
+    int _anum;		// allocated number of elements in `data'
+    void *_data;		// array data
+
+    void _freeAll (int sizeofT);
+    void _setSize (int newsize, int sizeofT);
+    // set the array size to `newsize', allocating more memory if necessary.
+    // if newsize>_anum and is a power of two then this is guaranteed to
+    // set _size and _anum to newsize.
+
+public:
+    // not: dArrayBase () { _size=0; _anum=0; _data=0; }
+
+    int size() const { return _size; }
+    int allocatedSize() const { return _anum; }
+    void * operator new (size_t size);
+    void operator delete (void *ptr, size_t size);
+
+    void constructor() { _size=0; _anum=0; _data=0; }
+    // if this structure is allocated with malloc() instead of new, you can
+    // call this to set it up.
+
+    void constructLocalArray (int __anum);
+    // this helper function allows non-reallocating arrays to be constructed
+    // on the stack (or in the heap if necessary). this is something of a
+    // kludge and should be used with extreme care. this function acts like
+    // a constructor - it is called on uninitialized memory that will hold the
+    // Array structure and the data. __anum is the number of elements that
+    // are allocated. the memory MUST be allocated with size:
+    //   sizeof(ArrayBase) + __anum*sizeof(T)
+    // arrays allocated this way will never try to reallocate or free the
+    // memory - that's your job.
+};
+
+
+template <class T> class dArray : public dArrayBase {
+public:
+    void equals (const dArray<T> &x) {
+        setSize (x.size());
+        memcpy (_data,x._data,x._size * sizeof(T));
+    }
+
+    dArray () { constructor(); }
+    dArray (const dArray<T> &x) { constructor(); equals (x); }
+    ~dArray () { _freeAll(sizeof(T)); }
+    void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
+    T *data() const { return (T*) _data; }
+    T & operator[] (int i) const { return ((T*)_data)[i]; }
+    void operator = (const dArray<T> &x) { equals (x); }
+
+    void push (const T item) {
+        if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
+        memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T));
+    }
+
+    void swap (dArray<T> &x) {
+        int tmp1;
+        void *tmp2;
+        tmp1=_size; _size=x._size; x._size=tmp1;
+        tmp1=_anum; _anum=x._anum; x._anum=tmp1;
+        tmp2=_data; _data=x._data; x._data=tmp2;
+    }
+
+    // insert the item at the position `i'. if i<0 then add the item to the
+    // start, if i >= size then add the item to the end of the array.
+    void insert (int i, const T item) {
+        if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
+        if (i >= (_size-1)) i = _size-1;	// add to end
+        else {
+            if (i < 0) i=0;			// add to start
+            int n = _size-1-i;
+            if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
+        }
+        ((T*)_data)[i] = item;
+    }
+
+    void remove (int i) {
+        if (i >= 0 && i < _size) {	// passing this test guarantees size>0
+            int n = _size-1-i;
+            if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
+            _size--;
+        }
+    }
+};
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/box.cpp

@@ -0,0 +1,878 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+  standard ODE geometry primitives: public API and pairwise collision functions.
+
+  the rule is that only the low level primitive collision functions should set
+  dContactGeom::g1 and dContactGeom::g2.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// box public API
+
+dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)
+{
+    dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
+    type = dBoxClass;
+    side[0] = lx;
+    side[1] = ly;
+    side[2] = lz;
+    updateZeroSizedFlag(!lx || !ly || !lz);
+}
+
+
+void dxBox::computeAABB()
+{
+    const dMatrix3& R = final_posr->R;
+    const dVector3& pos = final_posr->pos;
+
+    dReal xrange = REAL(0.5) * (dFabs (R[0] * side[0]) +
+        dFabs (R[1] * side[1]) + dFabs (R[2] * side[2]));
+    dReal yrange = REAL(0.5) * (dFabs (R[4] * side[0]) +
+        dFabs (R[5] * side[1]) + dFabs (R[6] * side[2]));
+    dReal zrange = REAL(0.5) * (dFabs (R[8] * side[0]) +
+        dFabs (R[9] * side[1]) + dFabs (R[10] * side[2]));
+    aabb[0] = pos[0] - xrange;
+    aabb[1] = pos[0] + xrange;
+    aabb[2] = pos[1] - yrange;
+    aabb[3] = pos[1] + yrange;
+    aabb[4] = pos[2] - zrange;
+    aabb[5] = pos[2] + zrange;
+}
+
+
+dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
+{
+    return new dxBox (space,lx,ly,lz);
+}
+
+
+void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)
+{
+    dUASSERT (g && g->type == dBoxClass,"argument not a box");
+    dAASSERT (lx >= 0 && ly >= 0 && lz >= 0);
+    dxBox *b = (dxBox*) g;
+    b->side[0] = lx;
+    b->side[1] = ly;
+    b->side[2] = lz;
+    b->updateZeroSizedFlag(!lx || !ly || !lz);
+    dGeomMoved (g);
+}
+
+
+void dGeomBoxGetLengths (dGeomID g, dVector3 result)
+{
+    dUASSERT (g && g->type == dBoxClass,"argument not a box");
+    dxBox *b = (dxBox*) g;
+    result[0] = b->side[0];
+    result[1] = b->side[1];
+    result[2] = b->side[2];
+}
+
+
+dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
+{
+    dUASSERT (g && g->type == dBoxClass,"argument not a box");
+    g->recomputePosr();
+    dxBox *b = (dxBox*) g;
+
+    // Set p = (x,y,z) relative to box center
+    //
+    // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)
+
+    dVector3 p,q;
+
+    p[0] = x - b->final_posr->pos[0];
+    p[1] = y - b->final_posr->pos[1];
+    p[2] = z - b->final_posr->pos[2];
+
+    // Rotate p into box's coordinate frame, so we can
+    // treat the OBB as an AABB
+
+    dMultiply1_331 (q,b->final_posr->R,p);
+
+    // Record distance from point to each successive box side, and see
+    // if the point is inside all six sides
+
+    dReal dist[6];
+    int   i;
+
+    bool inside = true;
+
+    for (i=0; i < 3; i++) {
+        dReal side = b->side[i] * REAL(0.5);
+
+        dist[i  ] = side - q[i];
+        dist[i+3] = side + q[i];
+
+        if ((dist[i] < 0) || (dist[i+3] < 0)) {
+            inside = false;
+        }
+    }
+
+    // If point is inside the box, the depth is the smallest positive distance
+    // to any side
+
+    if (inside) {
+        dReal smallest_dist = (dReal) (unsigned) -1;
+
+        for (i=0; i < 6; i++) {
+            if (dist[i] < smallest_dist) smallest_dist = dist[i];
+        }
+
+        return smallest_dist;
+    }
+
+    // Otherwise, if point is outside the box, the depth is the largest
+    // distance to any side.  This is an approximation to the 'proper'
+    // solution (the proper solution may be larger in some cases).
+
+    dReal largest_dist = 0;
+
+    for (i=0; i < 6; i++) {
+        if (dist[i] > largest_dist) largest_dist = dist[i];
+    }
+
+    return -largest_dist;
+}
+
+//****************************************************************************
+// box-box collision utility
+
+
+// find all the intersection points between the 2D rectangle with vertices
+// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
+// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
+//
+// the intersection points are returned as x,y pairs in the 'ret' array.
+// the number of intersection points is returned by the function (this will
+// be in the range 0 to 8).
+
+static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16])
+{
+    // q (and r) contain nq (and nr) coordinate points for the current (and
+    // chopped) polygons
+    int nq=4,nr;
+    dReal buffer[16];
+    dReal *q = p;
+    dReal *r = ret;
+    for (int dir=0; dir <= 1; dir++) {
+        // direction notation: xy[0] = x axis, xy[1] = y axis
+        for (int sign=-1; sign <= 1; sign += 2) {
+            // chop q along the line xy[dir] = sign*h[dir]
+            dReal *pq = q;
+            dReal *pr = r;
+            nr = 0;
+            for (int i=nq; i > 0; i--) {
+                // go through all points in q and all lines between adjacent points
+                if (sign*pq[dir] < h[dir]) {
+                    // this point is inside the chopping line
+                    pr[0] = pq[0];
+                    pr[1] = pq[1];
+                    pr += 2;
+                    nr++;
+                    if (nr & 8) {
+                        q = r;
+                        goto done;
+                    }
+                }
+                dReal *nextq = (i > 1) ? pq+2 : q;
+                if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {
+                    // this line crosses the chopping line
+                    pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+                        (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+                    pr[dir] = sign*h[dir];
+                    pr += 2;
+                    nr++;
+                    if (nr & 8) {
+                        q = r;
+                        goto done;
+                    }
+                }
+                pq += 2;
+            }
+            q = r;
+            r = (q==ret) ? buffer : ret;
+            nq = nr;
+        }
+    }
+done:
+    if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal));
+    return nr;
+}
+
+
+// given n points in the plane (array p, of size 2*n), generate m points that
+// best represent the whole set. the definition of 'best' here is not
+// predetermined - the idea is to select points that give good box-box
+// collision detection behavior. the chosen point indexes are returned in the
+// array iret (of size m). 'i0' is always the first entry in the array.
+// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
+// in the range [0..n-1].
+
+void cullPoints (int n, dReal p[], int m, int i0, int iret[])
+{
+    // compute the centroid of the polygon in cx,cy
+    int i,j;
+    dReal a,cx,cy,q;
+    if (n==1) {
+        cx = p[0];
+        cy = p[1];
+    }
+    else if (n==2) {
+        cx = REAL(0.5)*(p[0] + p[2]);
+        cy = REAL(0.5)*(p[1] + p[3]);
+    }
+    else {
+        a = 0;
+        cx = 0;
+        cy = 0;
+        for (i=0; i<(n-1); i++) {
+            q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
+            a += q;
+            cx += q*(p[i*2]+p[i*2+2]);
+            cy += q*(p[i*2+1]+p[i*2+3]);
+        }
+        q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
+        a = dRecip(REAL(3.0)*(a+q));
+        cx = a*(cx + q*(p[n*2-2]+p[0]));
+        cy = a*(cy + q*(p[n*2-1]+p[1]));
+    }
+
+    // compute the angle of each point w.r.t. the centroid
+    dReal A[8];
+    for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
+
+    // search for points that have angles closest to A[i0] + i*(2*pi/m).
+    int avail[8];
+    for (i=0; i<n; i++) avail[i] = 1;
+    avail[i0] = 0;
+    iret[0] = i0;
+    iret++;
+    for (j=1; j<m; j++) {
+        a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]);
+        if (a > M_PI) a -= (dReal)(2*M_PI);
+        dReal maxdiff=1e9,diff;
+#ifndef dNODEBUG
+        *iret = i0;			// iret is not allowed to keep this value
+#endif
+        for (i=0; i<n; i++) {
+            if (avail[i]) {
+                diff = dFabs (A[i]-a);
+                if (diff > M_PI) diff = (dReal) (2*M_PI - diff);
+                if (diff < maxdiff) {
+                    maxdiff = diff;
+                    *iret = i;
+                }
+            }
+        }
+#ifndef dNODEBUG
+        dIASSERT (*iret != i0);	// ensure iret got set
+#endif
+        avail[*iret] = 0;
+        iret++;
+    }
+}
+
+
+// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
+// generate contact points. this returns 0 if there is no contact otherwise
+// it returns the number of contacts generated.
+// `normal' returns the contact normal.
+// `depth' returns the maximum penetration depth along that normal.
+// `return_code' returns a number indicating the type of contact that was
+// detected:
+//        1,2,3 = box 2 intersects with a face of box 1
+//        4,5,6 = box 1 intersects with a face of box 2
+//        7..15 = edge-edge contact
+// `maxc' is the maximum number of contacts allowed to be generated, i.e.
+// the size of the `contact' array.
+// `contact' and `skip' are the contact array information provided to the
+// collision functions. this function only fills in the position and depth
+// fields.
+
+
+int dBoxBox (const dVector3 p1, const dMatrix3 R1,
+             const dVector3 side1, const dVector3 p2,
+             const dMatrix3 R2, const dVector3 side2,
+             dVector3 normal, dReal *depth, int *return_code,
+             int flags, dContactGeom *contact, int skip)
+{
+    const dReal fudge_factor = REAL(1.05);
+    dVector3 p,pp,normalC={0,0,0};
+    const dReal *normalR = 0;
+    dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
+        Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
+    int i,j,invert_normal,code;
+
+    // get vector from centers of box 1 to box 2, relative to box 1
+    p[0] = p2[0] - p1[0];
+    p[1] = p2[1] - p1[1];
+    p[2] = p2[2] - p1[2];
+    dMultiply1_331 (pp,R1,p);		// get pp = p relative to body 1
+
+    // get side lengths / 2
+    A[0] = side1[0]*REAL(0.5);
+    A[1] = side1[1]*REAL(0.5);
+    A[2] = side1[2]*REAL(0.5);
+    B[0] = side2[0]*REAL(0.5);
+    B[1] = side2[1]*REAL(0.5);
+    B[2] = side2[2]*REAL(0.5);
+
+    // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+    R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
+    R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
+    R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);
+
+    Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
+    Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
+    Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
+
+    // for all 15 possible separating axes:
+    //   * see if the axis separates the boxes. if so, return 0.
+    //   * find the depth of the penetration along the separating axis (s2)
+    //   * if this is the largest depth so far, record it.
+    // the normal vector will be set to the separating axis with the smallest
+    // depth. note: normalR is set to point to a column of R1 or R2 if that is
+    // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+    // set to a vector relative to body 1. invert_normal is 1 if the sign of
+    // the normal should be flipped.
+
+    do {
+#define TST(expr1,expr2,norm,cc) \
+    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
+    s2 = dFabs(expr1_val) - (expr2); \
+    if (s2 > 0) return 0; \
+    if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1_val) < 0); \
+    code = (cc); \
+    if (flags & CONTACTS_UNIMPORTANT) break; \
+    }
+
+        s = -dInfinity;
+        invert_normal = 0;
+        code = 0;
+
+        // separating axis = u1,u2,u3
+        TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
+        TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
+        TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
+
+        // separating axis = v1,v2,v3
+        TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
+        TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
+        TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
+
+        // note: cross product axes need to be scaled when s is computed.
+        // normal (n1,n2,n3) is relative to box 1.
+#undef TST
+#define TST(expr1,expr2,n1,n2,n3,cc) \
+    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
+    s2 = dFabs(expr1_val) - (expr2); \
+    if (s2 > 0) return 0; \
+    l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+    if (l > 0) { \
+    s2 /= l; \
+    if (s2*fudge_factor > s) { \
+    s = s2; \
+    normalR = 0; \
+    normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
+    invert_normal = ((expr1_val) < 0); \
+    code = (cc); \
+    if (flags & CONTACTS_UNIMPORTANT) break; \
+    } \
+    }
+
+        // We only need to check 3 edges per box 
+        // since parallel edges are equivalent.
+
+        // separating axis = u1 x (v1,v2,v3)
+        TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
+        TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
+        TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
+
+        // separating axis = u2 x (v1,v2,v3)
+        TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
+        TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
+        TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
+
+        // separating axis = u3 x (v1,v2,v3)
+        TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
+        TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
+        TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
+#undef TST
+    } while (0);
+
+    if (!code) return 0;
+
+    // if we get to this point, the boxes interpenetrate. compute the normal
+    // in global coordinates.
+    if (normalR) {
+        normal[0] = normalR[0];
+        normal[1] = normalR[4];
+        normal[2] = normalR[8];
+    }
+    else {
+        dMultiply0_331 (normal,R1,normalC);
+    }
+    if (invert_normal) {
+        normal[0] = -normal[0];
+        normal[1] = -normal[1];
+        normal[2] = -normal[2];
+    }
+    *depth = -s;
+
+    // compute contact point(s)
+
+    if (code > 6) {
+        // An edge from box 1 touches an edge from box 2.
+        // find a point pa on the intersecting edge of box 1
+        dVector3 pa;
+        dReal sign;
+        // Copy p1 into pa
+        for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
+        // Get world position of p2 into pa
+        for (j=0; j<3; j++) {
+            sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
+            for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
+        }
+
+        // find a point pb on the intersecting edge of box 2
+        dVector3 pb;
+        // Copy p2 into pb
+        for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
+        // Get world position of p2 into pb
+        for (j=0; j<3; j++) {
+            sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
+            for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
+        }
+
+        dReal alpha,beta;
+        dVector3 ua,ub;
+        // Get direction of first edge
+        for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
+        // Get direction of second edge
+        for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
+        // Get closest points between edges (one at each)
+        dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);    
+        for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+        for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+        // Set the contact point as halfway between the 2 closest points
+        for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
+        contact[0].depth = *depth;
+        *return_code = code;
+        return 1;
+    }
+
+    // okay, we have a face-something intersection (because the separating
+    // axis is perpendicular to a face). define face 'a' to be the reference
+    // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+    // the incident face (the closest face of the other box).
+    // Note: Unmodified parameter values are being used here
+    const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
+    if (code <= 3) { // One of the faces of box 1 is the reference face
+        Ra = R1; // Rotation of 'a'
+        Rb = R2; // Rotation of 'b'
+        pa = p1; // Center (location) of 'a'
+        pb = p2; // Center (location) of 'b'
+        Sa = A;  // Side Lenght of 'a'
+        Sb = B;  // Side Lenght of 'b'
+    }
+    else { // One of the faces of box 2 is the reference face
+        Ra = R2; // Rotation of 'a'
+        Rb = R1; // Rotation of 'b'
+        pa = p2; // Center (location) of 'a'
+        pb = p1; // Center (location) of 'b'
+        Sa = B;  // Side Lenght of 'a'
+        Sb = A;  // Side Lenght of 'b'
+    }
+
+    // nr = normal vector of reference face dotted with axes of incident box.
+    // anr = absolute values of nr.
+    /*
+    The normal is flipped if necessary so it always points outward from box 'a',
+    box 'b' is thus always the incident box
+    */
+    dVector3 normal2,nr,anr;
+    if (code <= 3) {
+        normal2[0] = normal[0];
+        normal2[1] = normal[1];
+        normal2[2] = normal[2];
+    }
+    else {
+        normal2[0] = -normal[0];
+        normal2[1] = -normal[1];
+        normal2[2] = -normal[2];
+    }
+    // Rotate normal2 in incident box opposite direction
+    dMultiply1_331 (nr,Rb,normal2);
+    anr[0] = dFabs (nr[0]);
+    anr[1] = dFabs (nr[1]);
+    anr[2] = dFabs (nr[2]);
+
+    // find the largest compontent of anr: this corresponds to the normal
+    // for the incident face. the other axis numbers of the incident face
+    // are stored in a1,a2.
+    int lanr,a1,a2;
+    if (anr[1] > anr[0]) {
+        if (anr[1] > anr[2]) {
+            a1 = 0;
+            lanr = 1;
+            a2 = 2;
+        }
+        else {
+            a1 = 0;
+            a2 = 1;
+            lanr = 2;
+        }
+    }
+    else {
+        if (anr[0] > anr[2]) {
+            lanr = 0;
+            a1 = 1;
+            a2 = 2;
+        }
+        else {
+            a1 = 0;
+            a2 = 1;
+            lanr = 2;
+        }
+    }
+
+    // compute center point of incident face, in reference-face coordinates
+    dVector3 center;
+    if (nr[lanr] < 0) {
+        for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
+    }
+    else {
+        for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
+    }
+
+    // find the normal and non-normal axis numbers of the reference box
+    int codeN,code1,code2;
+    if (code <= 3) codeN = code-1; else codeN = code-4;
+    if (codeN==0) {
+        code1 = 1;
+        code2 = 2;
+    }
+    else if (codeN==1) {
+        code1 = 0;
+        code2 = 2;
+    }
+    else {
+        code1 = 0;
+        code2 = 1;
+    }
+
+    // find the four corners of the incident face, in reference-face coordinates
+    dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
+    dReal c1,c2,m11,m12,m21,m22;
+    c1 = dCalcVectorDot3_14 (center,Ra+code1);
+    c2 = dCalcVectorDot3_14 (center,Ra+code2);
+    // optimize this? - we have already computed this data above, but it is not
+    // stored in an easy-to-index format. for now it's quicker just to recompute
+    // the four dot products.
+    m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
+    m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
+    m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
+    m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2);
+    {
+        dReal k1 = m11*Sb[a1];
+        dReal k2 = m21*Sb[a1];
+        dReal k3 = m12*Sb[a2];
+        dReal k4 = m22*Sb[a2];
+        quad[0] = c1 - k1 - k3;
+        quad[1] = c2 - k2 - k4;
+        quad[2] = c1 - k1 + k3;
+        quad[3] = c2 - k2 + k4;
+        quad[4] = c1 + k1 + k3;
+        quad[5] = c2 + k2 + k4;
+        quad[6] = c1 + k1 - k3;
+        quad[7] = c2 + k2 - k4;
+    }
+
+    // find the size of the reference face
+    dReal rect[2];
+    rect[0] = Sa[code1];
+    rect[1] = Sa[code2];
+
+    // intersect the incident and reference faces
+    dReal ret[16];
+    int n = intersectRectQuad (rect,quad,ret);
+    if (n < 1) return 0;		// this should never happen
+
+    // convert the intersection points into reference-face coordinates,
+    // and compute the contact position and depth for each point. only keep
+    // those points that have a positive (penetrating) depth. delete points in
+    // the 'ret' array as necessary so that 'point' and 'ret' correspond.
+    dReal point[3*8];		// penetrating contact points
+    dReal dep[8];			// depths for those points
+    dReal det1 = dRecip(m11*m22 - m12*m21);
+    m11 *= det1;
+    m12 *= det1;
+    m21 *= det1;
+    m22 *= det1;
+    int cnum = 0;			// number of penetrating contact points found
+    for (j=0; j < n; j++) {
+        dReal k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+        dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+        for (i=0; i<3; i++) point[cnum*3+i] =
+            center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
+        dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3);
+        if (dep[cnum] >= 0) {
+            ret[cnum*2] = ret[j*2];
+            ret[cnum*2+1] = ret[j*2+1];
+            cnum++;
+            if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
+                break;
+            }
+        }
+    }
+    if (cnum < 1) { 
+        return 0;	// this should not happen, yet does at times (demo_plane2d single precision).
+    }
+
+    // we can't generate more contacts than we actually have
+    int maxc = flags & NUMC_MASK;
+    if (maxc > cnum) maxc = cnum;
+    if (maxc < 1) maxc = 1;	// Even though max count must not be zero this check is kept for backward compatibility as this is a public function
+
+    if (cnum <= maxc) {
+        // we have less contacts than we need, so we use them all
+        for (j=0; j < cnum; j++) {
+            dContactGeom *con = CONTACT(contact,skip*j);
+            for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
+            con->depth = dep[j];
+        }
+    }
+    else {
+        dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
+        // we have more contacts than are wanted, some of them must be culled.
+        // find the deepest point, it is always the first contact.
+        int i1 = 0;
+        dReal maxdepth = dep[0];
+        for (i=1; i<cnum; i++) {
+            if (dep[i] > maxdepth) {
+                maxdepth = dep[i];
+                i1 = i;
+            }
+        }
+
+        int iret[8];
+        cullPoints (cnum,ret,maxc,i1,iret);
+
+        for (j=0; j < maxc; j++) {
+            dContactGeom *con = CONTACT(contact,skip*j);
+            for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
+            con->depth = dep[iret[j]];
+        }
+        cnum = maxc;
+    }
+
+    *return_code = code;
+    return cnum;
+}
+
+
+
+int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
+                    dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dBoxClass);
+    dIASSERT (o2->type == dBoxClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    dVector3 normal;
+    dReal depth;
+    int code;
+    dxBox *b1 = (dxBox*) o1;
+    dxBox *b2 = (dxBox*) o2;
+    int num = dBoxBox (o1->final_posr->pos,o1->final_posr->R,b1->side, o2->final_posr->pos,o2->final_posr->R,b2->side,
+        normal,&depth,&code,flags,contact,skip);
+    for (int i=0; i<num; i++) {
+        dContactGeom *currContact = CONTACT(contact,i*skip);
+        currContact->normal[0] = -normal[0];
+        currContact->normal[1] = -normal[1];
+        currContact->normal[2] = -normal[2];
+        currContact->g1 = o1;
+        currContact->g2 = o2;
+        currContact->side1 = -1;
+        currContact->side2 = -1;
+    }
+    return num;
+}
+
+
+int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
+                      int flags, dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dBoxClass);
+    dIASSERT (o2->type == dPlaneClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    dxBox *box = (dxBox*) o1;
+    dxPlane *plane = (dxPlane*) o2;
+
+    contact->g1 = o1;
+    contact->g2 = o2;
+    contact->side1 = -1;
+    contact->side2 = -1;
+
+    int ret = 0;
+
+    //@@@ problem: using 4-vector (plane->p) as 3-vector (normal).
+    const dReal *R = o1->final_posr->R;		// rotation of box
+    const dReal *n = plane->p;		// normal vector
+
+    // project sides lengths along normal vector, get absolute values
+    dReal Q1 = dCalcVectorDot3_14(n,R+0);
+    dReal Q2 = dCalcVectorDot3_14(n,R+1);
+    dReal Q3 = dCalcVectorDot3_14(n,R+2);
+    dReal A1 = box->side[0] * Q1;
+    dReal A2 = box->side[1] * Q2;
+    dReal A3 = box->side[2] * Q3;
+    dReal B1 = dFabs(A1);
+    dReal B2 = dFabs(A2);
+    dReal B3 = dFabs(A3);
+
+    // early exit test
+    dReal depth = plane->p[3] + REAL(0.5)*(B1+B2+B3) - dCalcVectorDot3(n,o1->final_posr->pos);
+    if (depth < 0) return 0;
+
+    // find number of contacts requested
+    int maxc = flags & NUMC_MASK;
+    // if (maxc < 1) maxc = 1; // an assertion is made on entry
+    if (maxc > 4) maxc = 4;	// not more than 4 contacts per box allowed
+
+    // find deepest point
+    dVector3 p;
+    p[0] = o1->final_posr->pos[0];
+    p[1] = o1->final_posr->pos[1];
+    p[2] = o1->final_posr->pos[2];
+#define FOO(i,op) \
+    p[0] op REAL(0.5)*box->side[i] * R[0+i]; \
+    p[1] op REAL(0.5)*box->side[i] * R[4+i]; \
+    p[2] op REAL(0.5)*box->side[i] * R[8+i];
+#define BAR(i,iinc) if (A ## iinc > 0) { FOO(i,-=) } else { FOO(i,+=) }
+    BAR(0,1);
+    BAR(1,2);
+    BAR(2,3);
+#undef FOO
+#undef BAR
+
+    // the deepest point is the first contact point
+    contact->pos[0] = p[0];
+    contact->pos[1] = p[1];
+    contact->pos[2] = p[2];
+    contact->depth = depth;
+    ret = 1;		// ret is number of contact points found so far
+    if (maxc == 1) goto done;
+
+    // get the second and third contact points by starting from `p' and going
+    // along the two sides with the smallest projected length.
+
+#define FOO(i,j,op) \
+    CONTACT(contact,i*skip)->pos[0] = p[0] op box->side[j] * R[0+j]; \
+    CONTACT(contact,i*skip)->pos[1] = p[1] op box->side[j] * R[4+j]; \
+    CONTACT(contact,i*skip)->pos[2] = p[2] op box->side[j] * R[8+j];
+#define BAR(ctact,side,sideinc) \
+    if (depth - B ## sideinc < 0) goto done; \
+    if (A ## sideinc > 0) { FOO(ctact,side,+); } else { FOO(ctact,side,-); } \
+    CONTACT(contact,ctact*skip)->depth = depth - B ## sideinc; \
+    ret++;
+
+    if (B1 < B2) {
+        if (B3 < B1) goto use_side_3; else {
+            BAR(1,0,1);	// use side 1
+            if (maxc == 2) goto done;
+            if (B2 < B3) goto contact2_2; else goto contact2_3;
+        }
+    }
+    else {
+        if (B3 < B2) {
+use_side_3:	// use side 3
+            BAR(1,2,3);
+            if (maxc == 2) goto done;
+            if (B1 < B2) goto contact2_1; else goto contact2_2;
+        }
+        else {
+            BAR(1,1,2);	// use side 2
+            if (maxc == 2) goto done;
+            if (B1 < B3) goto contact2_1; else goto contact2_3;
+        }
+    }
+
+contact2_1: BAR(2,0,1); goto done;
+contact2_2: BAR(2,1,2); goto done;
+contact2_3: BAR(2,2,3); goto done;
+#undef FOO
+#undef BAR
+
+done:
+
+    if (maxc == 4 && ret == 3) { // If user requested 4 contacts, and the first 3 were created...
+        // Combine contacts 2 and 3 (vectorial sum) and get the fourth one
+        // Result: if a box face is completely inside a plane, contacts are created for all the 4 vertices
+        dReal d4 = CONTACT(contact,1*skip)->depth + CONTACT(contact,2*skip)->depth - depth;  // depth is the depth for first contact
+        if (d4 > 0) {
+            CONTACT(contact,3*skip)->pos[0] = CONTACT(contact,1*skip)->pos[0] + CONTACT(contact,2*skip)->pos[0] - p[0]; // p is the position of first contact
+            CONTACT(contact,3*skip)->pos[1] = CONTACT(contact,1*skip)->pos[1] + CONTACT(contact,2*skip)->pos[1] - p[1];
+            CONTACT(contact,3*skip)->pos[2] = CONTACT(contact,1*skip)->pos[2] + CONTACT(contact,2*skip)->pos[2] - p[2];
+            CONTACT(contact,3*skip)->depth  = d4;
+            ret++;
+        }
+    }
+
+    for (int i=0; i<ret; i++) {
+        dContactGeom *currContact = CONTACT(contact,i*skip);
+        currContact->g1 = o1;
+        currContact->g2 = o2;
+        currContact->side1 = -1;
+        currContact->side2 = -1;
+
+        currContact->normal[0] = n[0];
+        currContact->normal[1] = n[1];
+        currContact->normal[2] = n[2];
+    }
+    return ret;
+}

thirdparty/ode-0.16.5/ode/src/capsule.cpp

@@ -0,0 +1,416 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+standard ODE geometry primitives: public API and pairwise collision functions.
+
+the rule is that only the low level primitive collision functions should set
+dContactGeom::g1 and dContactGeom::g2.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// capped cylinder public API
+
+dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :
+dxGeom (space,1)
+{
+    dAASSERT (_radius >= 0 && _length >= 0);
+    type = dCapsuleClass;
+    radius = _radius;
+    lz = _length;
+    updateZeroSizedFlag(!_radius/* || !_length -- zero length capsule is not a zero sized capsule*/);
+}
+
+
+void dxCapsule::computeAABB()
+{
+    const dMatrix3& R = final_posr->R;
+    const dVector3& pos = final_posr->pos;
+
+    dReal xrange = dFabs(R[2]  * lz) * REAL(0.5) + radius;
+    dReal yrange = dFabs(R[6]  * lz) * REAL(0.5) + radius;
+    dReal zrange = dFabs(R[10] * lz) * REAL(0.5) + radius;
+    aabb[0] = pos[0] - xrange;
+    aabb[1] = pos[0] + xrange;
+    aabb[2] = pos[1] - yrange;
+    aabb[3] = pos[1] + yrange;
+    aabb[4] = pos[2] - zrange;
+    aabb[5] = pos[2] + zrange;
+}
+
+
+dGeomID dCreateCapsule (dSpaceID space, dReal radius, dReal length)
+{
+    return new dxCapsule (space,radius,length);
+}
+
+
+void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)
+{
+    dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
+    dAASSERT (radius >= 0 && length >= 0);
+    dxCapsule *c = (dxCapsule*) g;
+    c->radius = radius;
+    c->lz = length;
+    c->updateZeroSizedFlag(!radius/* || !length -- zero length capsule is not a zero sized capsule*/);
+    dGeomMoved (g);
+}
+
+
+void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length)
+{
+    dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
+    dxCapsule *c = (dxCapsule*) g;
+    *radius = c->radius;
+    *length = c->lz;
+}
+
+
+dReal dGeomCapsulePointDepth (dGeomID g, dReal x, dReal y, dReal z)
+{
+    dUASSERT (g && g->type == dCapsuleClass,"argument not a ccylinder");
+    g->recomputePosr();
+    dxCapsule *c = (dxCapsule*) g;
+
+    const dReal* R = g->final_posr->R;
+    const dReal* pos = g->final_posr->pos;
+
+    dVector3 a;
+    a[0] = x - pos[0];
+    a[1] = y - pos[1];
+    a[2] = z - pos[2];
+    dReal beta = dCalcVectorDot3_14(a,R+2);
+    dReal lz2 = c->lz*REAL(0.5);
+    if (beta < -lz2) beta = -lz2;
+    else if (beta > lz2) beta = lz2;
+    a[0] = c->final_posr->pos[0] + beta*R[0*4+2];
+    a[1] = c->final_posr->pos[1] + beta*R[1*4+2];
+    a[2] = c->final_posr->pos[2] + beta*R[2*4+2];
+    return c->radius -
+        dSqrt ((x-a[0])*(x-a[0]) + (y-a[1])*(y-a[1]) + (z-a[2])*(z-a[2]));
+}
+
+
+
+int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
+                           dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dCapsuleClass);
+    dIASSERT (o2->type == dSphereClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    dxCapsule *ccyl = (dxCapsule*) o1;
+    dxSphere *sphere = (dxSphere*) o2;
+
+    contact->g1 = o1;
+    contact->g2 = o2;
+    contact->side1 = -1;
+    contact->side2 = -1;
+
+    // find the point on the cylinder axis that is closest to the sphere
+    dReal alpha = 
+        o1->final_posr->R[2]  * (o2->final_posr->pos[0] - o1->final_posr->pos[0]) +
+        o1->final_posr->R[6]  * (o2->final_posr->pos[1] - o1->final_posr->pos[1]) +
+        o1->final_posr->R[10] * (o2->final_posr->pos[2] - o1->final_posr->pos[2]);
+    dReal lz2 = ccyl->lz * REAL(0.5);
+    if (alpha > lz2) alpha = lz2;
+    if (alpha < -lz2) alpha = -lz2;
+
+    // collide the spheres
+    dVector3 p;
+    p[0] = o1->final_posr->pos[0] + alpha * o1->final_posr->R[2];
+    p[1] = o1->final_posr->pos[1] + alpha * o1->final_posr->R[6];
+    p[2] = o1->final_posr->pos[2] + alpha * o1->final_posr->R[10];
+    return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact);
+}
+
+
+int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
+                        dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dCapsuleClass);
+    dIASSERT (o2->type == dBoxClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    dxCapsule *cyl = (dxCapsule*) o1;
+    dxBox *box = (dxBox*) o2;
+
+    contact->g1 = o1;
+    contact->g2 = o2;
+    contact->side1 = -1;
+    contact->side2 = -1;
+
+    // get p1,p2 = cylinder axis endpoints, get radius
+    dVector3 p1,p2;
+    dReal clen = cyl->lz * REAL(0.5);
+    p1[0] = o1->final_posr->pos[0] + clen * o1->final_posr->R[2];
+    p1[1] = o1->final_posr->pos[1] + clen * o1->final_posr->R[6];
+    p1[2] = o1->final_posr->pos[2] + clen * o1->final_posr->R[10];
+    p2[0] = o1->final_posr->pos[0] - clen * o1->final_posr->R[2];
+    p2[1] = o1->final_posr->pos[1] - clen * o1->final_posr->R[6];
+    p2[2] = o1->final_posr->pos[2] - clen * o1->final_posr->R[10];
+    dReal radius = cyl->radius;
+
+    // copy out box center, rotation matrix, and side array
+    dReal *c = o2->final_posr->pos;
+    dReal *R = o2->final_posr->R;
+    const dReal *side = box->side;
+
+    // get the closest point between the cylinder axis and the box
+    dVector3 pl,pb;
+    dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);
+
+    // if the capsule is penetrated further than radius 
+    //  then pl and pb are equal (up to mindist) -> unknown normal
+    // use normal vector of closest box surface
+#ifdef dSINGLE
+    dReal mindist = REAL(1e-6);
+#else
+    dReal mindist = REAL(1e-15);
+#endif
+    if (dCalcPointsDistance3(pl, pb)<mindist) {
+        // consider capsule as box
+        dVector3 normal;
+        dReal depth;
+        int code;
+        // WARNING! rad2 is declared as #define in Microsoft headers (as well as psh2, chx2, grp2, frm2, rct2, ico2, stc2, lst2, cmb2, edt2, scr2). Avoid abbreviations!
+        /* dReal rad2 = radius*REAL(2.0); */ dReal radiusMul2 = radius * REAL(2.0);
+        const dVector3 capboxside = {radiusMul2, radiusMul2, cyl->lz + radiusMul2};
+        int num = dBoxBox (c, R, side, 
+            o1->final_posr->pos, o1->final_posr->R, capboxside,
+            normal, &depth, &code, flags, contact, skip);
+
+        for (int i=0; i<num; i++) {
+            dContactGeom *currContact = CONTACT(contact,i*skip);
+            currContact->normal[0] = normal[0];
+            currContact->normal[1] = normal[1];
+            currContact->normal[2] = normal[2];
+            currContact->g1 = o1;
+            currContact->g2 = o2;
+            currContact->side1 = -1;
+            currContact->side2 = -1;
+        }
+        return num;
+    } else {
+        // generate contact point
+        return dCollideSpheres (pl,radius,pb,0,contact);
+    }
+}
+
+
+int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
+                            int flags, dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dCapsuleClass);
+    dIASSERT (o2->type == dCapsuleClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    int i;
+    const dReal tolerance = REAL(1e-5);
+
+    dxCapsule *cyl1 = (dxCapsule*) o1;
+    dxCapsule *cyl2 = (dxCapsule*) o2;
+
+    contact->g1 = o1;
+    contact->g2 = o2;
+    contact->side1 = -1;
+    contact->side2 = -1;
+
+    // copy out some variables, for convenience
+    dReal lz1 = cyl1->lz * REAL(0.5);
+    dReal lz2 = cyl2->lz * REAL(0.5);
+    dReal *pos1 = o1->final_posr->pos;
+    dReal *pos2 = o2->final_posr->pos;
+    dReal axis1[3],axis2[3];
+    axis1[0] = o1->final_posr->R[2];
+    axis1[1] = o1->final_posr->R[6];
+    axis1[2] = o1->final_posr->R[10];
+    axis2[0] = o2->final_posr->R[2];
+    axis2[1] = o2->final_posr->R[6];
+    axis2[2] = o2->final_posr->R[10];
+
+    // if the cylinder axes are close to parallel, we'll try to detect up to
+    // two contact points along the body of the cylinder. if we can't find any
+    // points then we'll fall back to the closest-points algorithm. note that
+    // we are not treating this special case for reasons of degeneracy, but
+    // because we want two contact points in some situations. the closet-points
+    // algorithm is robust in all casts, but it can return only one contact.
+
+    dVector3 sphere1,sphere2;
+    dReal a1a2 = dCalcVectorDot3 (axis1,axis2);
+    dReal det = REAL(1.0)-a1a2*a1a2;
+    if (det < tolerance) {
+        // the cylinder axes (almost) parallel, so we will generate up to two
+        // contacts. alpha1 and alpha2 (line position parameters) are related by:
+        //       alpha2 =   alpha1 + (pos1-pos2)'*axis1   (if axis1==axis2)
+        //    or alpha2 = -(alpha1 + (pos1-pos2)'*axis1)  (if axis1==-axis2)
+        // first compute where the two cylinders overlap in alpha1 space:
+        if (a1a2 < 0) {
+            axis2[0] = -axis2[0];
+            axis2[1] = -axis2[1];
+            axis2[2] = -axis2[2];
+        }
+        dReal q[3];
+        for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
+        dReal k = dCalcVectorDot3 (axis1,q);
+        dReal a1lo = -lz1;
+        dReal a1hi = lz1;
+        dReal a2lo = -lz2 - k;
+        dReal a2hi = lz2 - k;
+        dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
+        dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
+        if (lo <= hi) {
+            int num_contacts = flags & NUMC_MASK;
+            if (num_contacts >= 2 && lo < hi) {
+                // generate up to two contacts. if one of those contacts is
+                // not made, fall back on the one-contact strategy.
+                for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
+                for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
+                int n1 = dCollideSpheres (sphere1,cyl1->radius,
+                    sphere2,cyl2->radius,contact);
+                if (n1) {
+                    for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
+                    for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
+                    dContactGeom *c2 = CONTACT(contact,skip);
+                    int n2 = dCollideSpheres (sphere1,cyl1->radius,
+                        sphere2,cyl2->radius, c2);
+                    if (n2) {
+                        c2->g1 = o1;
+                        c2->g2 = o2;
+                        c2->side1 = -1;
+                        c2->side2 = -1;
+                        return 2;
+                    }
+                }
+            }
+
+            // just one contact to generate, so put it in the middle of
+            // the range
+            dReal alpha1 = (lo + hi) * REAL(0.5);
+            dReal alpha2 = alpha1 + k;
+            for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+            for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+            return dCollideSpheres (sphere1,cyl1->radius,
+                sphere2,cyl2->radius,contact);
+        }
+    }
+
+    // use the closest point algorithm
+    dVector3 a1,a2,b1,b2;
+    a1[0] = o1->final_posr->pos[0] + axis1[0]*lz1;
+    a1[1] = o1->final_posr->pos[1] + axis1[1]*lz1;
+    a1[2] = o1->final_posr->pos[2] + axis1[2]*lz1;
+    a2[0] = o1->final_posr->pos[0] - axis1[0]*lz1;
+    a2[1] = o1->final_posr->pos[1] - axis1[1]*lz1;
+    a2[2] = o1->final_posr->pos[2] - axis1[2]*lz1;
+    b1[0] = o2->final_posr->pos[0] + axis2[0]*lz2;
+    b1[1] = o2->final_posr->pos[1] + axis2[1]*lz2;
+    b1[2] = o2->final_posr->pos[2] + axis2[2]*lz2;
+    b2[0] = o2->final_posr->pos[0] - axis2[0]*lz2;
+    b2[1] = o2->final_posr->pos[1] - axis2[1]*lz2;
+    b2[2] = o2->final_posr->pos[2] - axis2[2]*lz2;
+
+    dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);
+    return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
+}
+
+
+int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dCapsuleClass);
+    dIASSERT (o2->type == dPlaneClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    dxCapsule *ccyl = (dxCapsule*) o1;
+    dxPlane *plane = (dxPlane*) o2;
+
+    // collide the deepest capping sphere with the plane
+    dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
+    dVector3 p;
+    p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2]  * ccyl->lz * REAL(0.5) * sign;
+    p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6]  * ccyl->lz * REAL(0.5) * sign;
+    p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
+
+    dReal k = dCalcVectorDot3 (p,plane->p);
+    dReal depth = plane->p[3] - k + ccyl->radius;
+    if (depth < 0) return 0;
+    contact->normal[0] = plane->p[0];
+    contact->normal[1] = plane->p[1];
+    contact->normal[2] = plane->p[2];
+    contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
+    contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
+    contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
+    contact->depth = depth;
+
+    int ncontacts = 1;
+    if ((flags & NUMC_MASK) >= 2) {
+        // collide the other capping sphere with the plane
+        p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2]  * ccyl->lz * REAL(0.5) * sign;
+        p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6]  * ccyl->lz * REAL(0.5) * sign;
+        p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
+
+        k = dCalcVectorDot3 (p,plane->p);
+        depth = plane->p[3] - k + ccyl->radius;
+        if (depth >= 0) {
+            dContactGeom *c2 = CONTACT(contact,skip);
+            c2->normal[0] = plane->p[0];
+            c2->normal[1] = plane->p[1];
+            c2->normal[2] = plane->p[2];
+            c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
+            c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
+            c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
+            c2->depth = depth;
+            ncontacts = 2;
+        }
+    }
+
+    for (int i=0; i < ncontacts; i++) {
+        dContactGeom *currContact = CONTACT(contact,i*skip);
+        currContact->g1 = o1;
+        currContact->g2 = o2;
+        currContact->side1 = -1;
+        currContact->side2 = -1;
+    }
+    return ncontacts;
+}
+

thirdparty/ode-0.16.5/ode/src/collision_convex_trimesh.cpp

@@ -0,0 +1,120 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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 <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+typedef struct _sLocalContactData
+{
+    dVector3	vPos;
+    dVector3	vNormal;
+    dReal		fDepth;
+    int			triIndex;
+    int			nFlags; // 0 = filtered out, 1 = OK
+}sLocalContactData;
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_trimesh_internal.h"
+#if dLIBCCD_ENABLED
+#include "collision_libccd.h"
+#endif
+
+int dCollideConvexTrimesh( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
+{
+    int contactcount = 0;
+    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
+    dIASSERT( o1->type == dConvexClass );
+    dIASSERT( o2->type == dTriMeshClass );
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+#if dLIBCCD_ENABLED
+
+#if dTRIMESH_OPCODE
+    const dVector3 &meshPosition = *(const dVector3 *)dGeomGetPosition(o2);
+    // Find convex OBB in trimesh coordinates
+    Point convexAABBMin(o1->aabb[0] - meshPosition[0], o1->aabb[2] - meshPosition[1], o1->aabb[4] - meshPosition[2]);
+    Point convexAABBMax(o1->aabb[1] - meshPosition[0], o1->aabb[3] - meshPosition[1], o1->aabb[5] - meshPosition[2]);
+    
+    const Point convexCenter = 0.5f * (convexAABBMax + convexAABBMin);
+    const Point convexExtents = 0.5f * (convexAABBMax - convexAABBMin);
+    const Matrix3x3 convexRotation(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
+    OBB convexOOB(convexCenter, convexExtents, convexRotation);
+
+    Matrix4x4 meshTransformation;
+    const dMatrix3 &meshRotation = *(const dMatrix3 *)dGeomGetRotation(o2);
+    const dVector3 zeroVector = { REAL(0.0), };
+    MakeMatrix(zeroVector, meshRotation, meshTransformation);
+    
+    OBBCollider collider;
+    collider.SetFirstContact(false);
+    collider.SetTemporalCoherence(false);
+    collider.SetPrimitiveTests(false);
+    
+    OBBCache cache;
+    dxTriMesh *trimesh = (dxTriMesh *)o2;
+    if (collider.Collide(cache, convexOOB, trimesh->retrieveMeshBVTreeRef(), null, &meshTransformation)) {
+        int triCount = collider.GetNbTouchedPrimitives();
+        if (triCount > 0) {
+            int* triangles = (int*)collider.GetTouchedPrimitives();
+            contactcount = dCollideConvexTrimeshTrianglesCCD(o1, o2, triangles, triCount, flags, contacts, skip);
+        }
+    }
+
+#elif dTRIMESH_GIMPACT
+    dxTriMesh *trimesh = (dxTriMesh *)o2;
+
+    aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]);
+
+    GDYNAMIC_ARRAY collision_result;
+    GIM_CREATE_BOXQUERY_LIST(collision_result);
+
+    gim_aabbset_box_collision(&test_aabb, &trimesh->m_collision_trimesh.m_aabbset, &collision_result);
+
+    if (collision_result.m_size != 0)
+    {
+        GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result);
+        GIM_TRIMESH * ptrimesh = &trimesh->m_collision_trimesh;
+        gim_trimesh_locks_work_data(ptrimesh);
+
+        contactcount = dCollideConvexTrimeshTrianglesCCD(o1, o2, (int *)boxesresult, collision_result.m_size, flags, contacts, skip);
+
+        gim_trimesh_unlocks_work_data(ptrimesh);
+    }
+
+    GIM_DYNARRAY_DESTROY(collision_result);
+#endif // dTRIMESH_GIMPACT
+
+#endif // dLIBCCD_ENABLED
+
+    return contactcount;
+}
+
+#endif // dTRIMESH_ENABLED
+

thirdparty/ode-0.16.5/ode/src/collision_cylinder_plane.cpp

@@ -0,0 +1,266 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+/*
+ * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de )
+ *
+ * This testing basically comes down to testing the intersection
+ * of the cylinder caps (discs) with the plane.
+ * 
+ */
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"	// for dxGeom
+#include "collision_util.h"
+
+
+int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (Cylinder->type == dCylinderClass);
+    dIASSERT (Plane->type == dPlaneClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    int GeomCount = 0; // count of used contactgeoms
+
+#ifdef dSINGLE
+    const dReal toleranz = REAL(0.0001);
+#endif
+#ifdef dDOUBLE
+    const dReal toleranz = REAL(0.0000001);
+#endif
+
+    // Get the properties of the cylinder (length+radius)
+    dReal radius, length;
+    dGeomCylinderGetParams(Cylinder, &radius, &length);
+    dVector3 &cylpos = Cylinder->final_posr->pos;
+    // and the plane
+    dVector4 planevec;
+    dGeomPlaneGetParams(Plane, planevec);
+    dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
+    //dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
+
+    dVector3 G1Pos1, G1Pos2, vDir1;
+    vDir1[0] = Cylinder->final_posr->R[2];
+    vDir1[1] = Cylinder->final_posr->R[6];
+    vDir1[2] = Cylinder->final_posr->R[10];
+
+    dReal s;
+    s = length * REAL(0.5);
+    G1Pos2[0] = vDir1[0] * s + cylpos[0];
+    G1Pos2[1] = vDir1[1] * s + cylpos[1];
+    G1Pos2[2] = vDir1[2] * s + cylpos[2];
+
+    G1Pos1[0] = vDir1[0] * -s + cylpos[0];
+    G1Pos1[1] = vDir1[1] * -s + cylpos[1];
+    G1Pos1[2] = vDir1[2] * -s + cylpos[2];
+
+    dVector3 C;
+
+    // parallel-check
+    s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
+    if(s < 0)
+        s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
+    else
+        s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
+    if(s < toleranz && s > (-toleranz))
+    {
+        // discs are parallel to the plane
+
+        // 1.compute if, and where contacts are
+        dVector3 P;
+        s = planevec[3] - dVector3Dot(planevec, G1Pos1);
+        dReal t;
+        t = planevec[3] - dVector3Dot(planevec, G1Pos2);
+        if(s >= t) // s == t does never happen, 
+        {
+            if(s >= 0)
+            {
+                // 1. Disc
+                dVector3Copy(G1Pos1, P);
+            }
+            else
+                return GeomCount; // no contacts
+        }
+        else
+        {
+            if(t >= 0)
+            {
+                // 2. Disc
+                dVector3Copy(G1Pos2, P);
+            }
+            else
+                return GeomCount; // no contacts
+        }
+
+        // 2. generate a coordinate-system on the disc
+        dVector3 V1, V2;
+        if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
+        {
+            // not x-axis
+            V1[0] = vDir1[0] + REAL(1.0); // random value
+            V1[1] = vDir1[1];
+            V1[2] = vDir1[2];
+        }
+        else
+        {
+            // maybe x-axis
+            V1[0] = vDir1[0];
+            V1[1] = vDir1[1] + REAL(1.0); // random value
+            V1[2] = vDir1[2];
+        }
+        // V1 is now another direction than vDir1
+        // Cross-product
+        dVector3Cross(V1, vDir1, V2);
+        // make unit V2
+        t = dVector3Length(V2);
+        t = radius / t;
+        dVector3Scale(V2, t);
+        // cross again
+        dVector3Cross(V2, vDir1, V1);
+        // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
+        // V1 = first axis
+        // V2 = second axis
+
+        // 3. generate contactpoints
+
+        // Potential contact 1
+        dVector3Add(P, V1, contact->pos);
+        contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
+        if(contact->depth > 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+
+        // Potential contact 2
+        dVector3Subtract(P, V1, contact->pos);
+        contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
+        if(contact->depth > 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+
+        // Potential contact 3
+        dVector3Add(P, V2, contact->pos);
+        contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
+        if(contact->depth > 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+
+        // Potential contact 4
+        dVector3Subtract(P, V2, contact->pos);
+        contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
+        if(contact->depth > 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+    }
+    else
+    {
+        dReal t = dVector3Dot(PlaneNormal, vDir1);
+        C[0] = vDir1[0] * t - PlaneNormal[0];
+        C[1] = vDir1[1] * t - PlaneNormal[1];
+        C[2] = vDir1[2] * t - PlaneNormal[2];
+        s = dVector3Length(C);
+        // move C onto the circle
+        s = radius / s;
+        dVector3Scale(C, s);
+
+        // deepest point of disc 1
+        dVector3Add(C, G1Pos1, contact->pos);
+
+        // depth of the deepest point
+        contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos);
+        if(contact->depth >= 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+
+        // C is still computed
+
+        // deepest point of disc 2
+        dVector3Add(C, G1Pos2, contact->pos);
+
+        // depth of the deepest point
+        contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
+        if(contact->depth >= 0)
+        {
+            dVector3Copy(PlaneNormal, contact->normal);
+            contact->g1 = Cylinder;
+            contact->g2 = Plane;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            if( GeomCount >= (flags & NUMC_MASK))
+                return GeomCount; // enough contactgeoms
+            contact = (dContactGeom *)((char *)contact + skip);
+        }
+    }
+    return GeomCount;
+}

thirdparty/ode-0.16.5/ode/src/collision_cylinder_sphere.cpp

@@ -0,0 +1,277 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+/*******************************************************************
+ *                                                                 *
+ * cylinder-sphere collider by Christoph Beyer (boernerb@web.de)   *
+ *                                                                 *
+ * In Cylinder/Sphere-collisions, there are three possibilies:     *
+ * 1. collision with the cylinder's nappe                          *
+ * 2. collision with one of the cylinder's disc                    *
+ * 3. collision with one of the disc's border                      *
+ *                                                                 *
+ * This collider computes two distances (s, t) and based on them,  *
+ * it decides, which collision we have.                            *
+ * This collider always generates 1 (or 0, if we have no collison) *
+ * contacts.                                                       *
+ * It is able to "separate" cylinder and sphere in all             *
+ * configurations, but it never pays attention to velocity.        *
+ * So, in extrem situations, "tunneling-effect" is possible.       *
+ *                                                                 *
+ *******************************************************************/
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"	// for dxGeom
+#include "collision_util.h"
+
+int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, 
+                           int flags, dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (Cylinder->type == dCylinderClass);
+    dIASSERT (Sphere->type == dSphereClass);
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    //unsigned char* pContactData = (unsigned char*)contact;
+    int GeomCount = 0; // count of used contacts
+
+#ifdef dSINGLE
+    const dReal toleranz = REAL(0.0001);
+#endif
+#ifdef dDOUBLE
+    const dReal toleranz = REAL(0.0000001);
+#endif
+
+    // get the data from the geoms
+    dReal radius, length;
+    dGeomCylinderGetParams(Cylinder, &radius, &length);
+    dVector3 &cylpos = Cylinder->final_posr->pos;
+    //const dReal* pfRot1 = dGeomGetRotation(Cylinder);
+
+    dReal radius2;
+    radius2 = dGeomSphereGetRadius(Sphere);
+    const dReal* SpherePos = dGeomGetPosition(Sphere);
+
+    // G1Pos1 is the middle of the first disc
+    // G1Pos2 is the middle of the second disc
+    // vDir1 is the unit direction of the cylinderaxis
+    dVector3 G1Pos1, G1Pos2, vDir1;
+    vDir1[0] = Cylinder->final_posr->R[2];
+    vDir1[1] = Cylinder->final_posr->R[6];
+    vDir1[2] = Cylinder->final_posr->R[10];
+
+    dReal s;
+    s = length * REAL(0.5); // just a precomputed factor
+    G1Pos2[0] = vDir1[0] * s + cylpos[0];
+    G1Pos2[1] = vDir1[1] * s + cylpos[1];
+    G1Pos2[2] = vDir1[2] * s + cylpos[2];
+
+    G1Pos1[0] = vDir1[0] * -s + cylpos[0];
+    G1Pos1[1] = vDir1[1] * -s + cylpos[1];
+    G1Pos1[2] = vDir1[2] * -s + cylpos[2];
+
+    dVector3 C;
+    dReal t;
+    // Step 1: compute the two distances 's' and 't'
+    // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
+    s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
+    if(s < (-radius2) || s > (length + radius2) )
+    {
+        // Sphere is too far away from the discs
+        // no collision
+        return 0;
+    }
+
+    // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
+    C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
+    C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
+    C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
+    // t is the distance from the Sphere-middle to the cylinder-axis!
+    t = dVector3Length(C);
+    if(t > (radius + radius2) )
+    {
+        // Sphere is too far away from the cylinder axis!
+        // no collision
+        return 0;
+    }
+
+    // decide which kind of collision we have:
+    if(t > radius && (s < 0 || s > length) )
+    {
+        // 3. collision
+        if(s <= 0)
+        {
+            contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
+            if(contact->depth < 0)
+            {
+                // no collision!
+                return 0;
+            }
+            contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
+            contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
+            contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
+            contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
+            contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
+            contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
+            contact->g1 = Cylinder;
+            contact->g2 = Sphere;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            return GeomCount;
+        }
+        else
+        {
+            // now s is bigger than length here!
+            contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
+            if(contact->depth < 0)
+            {
+                // no collision!
+                return 0;
+            }
+            contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
+            contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
+            contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
+            contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
+            contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
+            contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
+            contact->g1 = Cylinder;
+            contact->g2 = Sphere;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            return GeomCount;
+        }
+    }
+    else if( (radius - t) <= s && (radius - t) <= (length - s) )
+    {
+        // 1. collision
+        if(t > (radius2 + toleranz))
+        {
+            // cylinder-axis is outside the sphere
+            contact->depth = (radius2 + radius) - t;
+            if(contact->depth < 0)
+            {
+                // should never happen, but just for safeness
+                return 0;
+            }
+            else
+            {
+                C[0] /= t;
+                C[1] /= t;
+                C[2] /= t;
+                contact->pos[0] = C[0] * radius2 + SpherePos[0];
+                contact->pos[1] = C[1] * radius2 + SpherePos[1];
+                contact->pos[2] = C[2] * radius2 + SpherePos[2];
+                contact->normal[0] = C[0];
+                contact->normal[1] = C[1];
+                contact->normal[2] = C[2];
+                contact->g1 = Cylinder;
+                contact->g2 = Sphere;
+                contact->side1 = -1;
+                contact->side2 = -1;
+                GeomCount++;
+                return GeomCount;
+            }
+        }
+        else
+        {
+            // cylinder-axis is outside of the sphere
+            contact->depth = (radius2 + radius) - t;
+            if(contact->depth < 0)
+            {
+                // should never happen, but just for safeness
+                return 0;
+            }
+            else
+            {
+                contact->pos[0] = C[0] + SpherePos[0];
+                contact->pos[1] = C[1] + SpherePos[1];
+                contact->pos[2] = C[2] + SpherePos[2];
+                contact->normal[0] = C[0] / t;
+                contact->normal[1] = C[1] / t;
+                contact->normal[2] = C[2] / t;
+                contact->g1 = Cylinder;
+                contact->g2 = Sphere;
+                contact->side1 = -1;
+                contact->side2 = -1;
+                GeomCount++;
+                return GeomCount;
+            }
+        }
+    }
+    else
+    {
+        // 2. collision
+        if(s <= (length * REAL(0.5)) )
+        {
+            // collision with the first disc
+            contact->depth = s + radius2;
+            if(contact->depth < 0)
+            {
+                // should never happen, but just for safeness
+                return 0;
+            }
+            contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
+            contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
+            contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
+            contact->normal[0] = vDir1[0];
+            contact->normal[1] = vDir1[1];
+            contact->normal[2] = vDir1[2];
+            contact->g1 = Cylinder;
+            contact->g2 = Sphere;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            return GeomCount;
+        }
+        else
+        {
+            // collision with the second disc
+            contact->depth = (radius2 + length - s);
+            if(contact->depth < 0)
+            {
+                // should never happen, but just for safeness
+                return 0;
+            }
+            contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
+            contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
+            contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
+            contact->normal[0] = -vDir1[0];
+            contact->normal[1] = -vDir1[1];
+            contact->normal[2] = -vDir1[2];
+            contact->g1 = Cylinder;
+            contact->g2 = Sphere;
+            contact->side1 = -1;
+            contact->side2 = -1;
+            GeomCount++;
+            return GeomCount;
+        }
+    }
+    return GeomCount;
+}

thirdparty/ode-0.16.5/ode/src/collision_kernel.h

@@ -0,0 +1,293 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+internal data structures and functions for collision detection.
+
+*/
+
+#ifndef _ODE_COLLISION_KERNEL_H_
+#define _ODE_COLLISION_KERNEL_H_
+
+#include <ode/common.h>
+#include <ode/contact.h>
+#include <ode/collision.h>
+#include "objects.h"
+#include "odetls.h"
+#include "common.h"
+
+
+//****************************************************************************
+// constants and macros
+
+// mask for the number-of-contacts field in the dCollide() flags parameter
+#define NUMC_MASK (0xffff)
+
+#define IS_SPACE(geom) \
+    dIN_RANGE((geom)->type, dFirstSpaceClass, dLastSpaceClass + 1)
+
+#define CHECK_NOT_LOCKED(space) \
+    dUASSERT ((space) == NULL || (space)->lock_count == 0, \
+        "Invalid operation for locked space")
+
+
+//****************************************************************************
+// geometry object base class
+
+
+// geom flags.
+//
+// GEOM_DIRTY means that the space data structures for this geom are
+// potentially not up to date. NOTE THAT all space parents of a dirty geom
+// are themselves dirty. this is an invariant that must be enforced.
+//
+// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date.
+// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might
+// recalculate its own AABB but does not know how to update the space data
+// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY.
+// the valid combinations are: 
+//		0
+//		GEOM_DIRTY
+//		GEOM_DIRTY|GEOM_AABB_BAD
+//		GEOM_DIRTY|GEOM_AABB_BAD|GEOM_POSR_BAD
+
+enum {
+    GEOM_DIRTY	= 1,    // geom is 'dirty', i.e. position unknown
+    GEOM_POSR_BAD = 2,    // geom's final posr is not valid
+    GEOM_AABB_BAD	= 4,    // geom's AABB is not valid
+    GEOM_PLACEABLE = 8,   // geom is placeable
+    GEOM_ENABLED = 16,    // geom is enabled
+    GEOM_ZERO_SIZED = 32, // geom is zero sized
+
+    GEOM_ENABLE_TEST_MASK = GEOM_ENABLED | GEOM_ZERO_SIZED,
+    GEOM_ENABLE_TEST_VALUE = GEOM_ENABLED,
+
+    // Ray specific
+    RAY_FIRSTCONTACT = 0x10000,
+    RAY_BACKFACECULL = 0x20000,
+    RAY_CLOSEST_HIT  = 0x40000
+};
+
+enum dxContactMergeOptions {
+    DONT_MERGE_CONTACTS,
+    MERGE_CONTACT_NORMALS,
+    MERGE_CONTACTS_FULLY
+};
+
+
+// geometry object base class. pos and R will either point to a separately
+// allocated buffer (if body is 0 - pos points to the dxPosR object) or to
+// the pos and R of the body (if body nonzero).
+// a dGeomID is a pointer to this object.
+
+struct dxGeom : public dBase {
+    int type;		// geom type number, set by subclass constructor
+    int gflags;		// flags used by geom and space
+    void *data;		// user-defined data pointer
+    dBodyID body;		// dynamics body associated with this object (if any)
+    dxGeom *body_next;	// next geom in body's linked list of associated geoms
+    dxPosR *final_posr;	// final position of the geom in world coordinates
+    dxPosR *offset_posr;	// offset from body in local coordinates
+
+    // information used by spaces
+    dxGeom *next;		// next geom in linked list of geoms
+    dxGeom **tome;	// linked list backpointer
+    dxGeom *next_ex;	// next geom in extra linked list of geoms (for higher level structures)
+    dxGeom **tome_ex;	// extra linked list backpointer (for higher level structures)
+    dxSpace *parent_space;// the space this geom is contained in, 0 if none
+    dReal aabb[6];	// cached AABB for this space
+    unsigned long category_bits,collide_bits;
+
+    dxGeom (dSpaceID _space, int is_placeable);
+    virtual ~dxGeom();
+
+    // Set or clear GEOM_ZERO_SIZED flag
+    void updateZeroSizedFlag(bool is_zero_sized) { gflags = is_zero_sized ? (gflags | GEOM_ZERO_SIZED) : (gflags & ~GEOM_ZERO_SIZED); }
+    // Get parent space TLS kind
+    unsigned getParentSpaceTLSKind() const;
+
+    const dVector3 &buildUpdatedPosition()
+    {
+        dIASSERT(gflags & GEOM_PLACEABLE);
+        
+        recomputePosr();
+        return final_posr->pos;
+    }
+
+    const dMatrix3 &buildUpdatedRotation()
+    {
+        dIASSERT(gflags & GEOM_PLACEABLE);
+
+        recomputePosr();
+        return final_posr->R;
+    }
+
+    // recalculate our new final position if needed
+    void recomputePosr()
+    {
+        if (gflags & GEOM_POSR_BAD) {
+            computePosr();
+            gflags &= ~GEOM_POSR_BAD;
+        }
+    }
+
+    // calculate our new final position from our offset and body
+    void computePosr();
+
+    bool checkControlValueSizeValidity(void *dataValue, int *dataSize, int iRequiresSize) { return (*dataSize == iRequiresSize && dataValue != 0) ? true : !(*dataSize = iRequiresSize); } // Here it is the intent to return true for 0 required size in any case
+    virtual bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
+
+    virtual void computeAABB()=0;
+    // compute the AABB for this object and put it in aabb. this function
+    // always performs a fresh computation, it does not inspect the
+    // GEOM_AABB_BAD flag.
+
+    virtual int AABBTest (dxGeom *o, dReal aabb[6]);
+    // test whether the given AABB object intersects with this object, return
+    // 1=yes, 0=no. this is used as an early-exit test in the space collision
+    // functions. the default implementation returns 1, which is the correct
+    // behavior if no more detailed implementation can be provided.
+
+    // utility functions
+
+    // compute the AABB only if it is not current. this function manipulates
+    // the GEOM_AABB_BAD flag.
+
+    void recomputeAABB() {
+        if (gflags & GEOM_AABB_BAD) {
+            // our aabb functions assume final_posr is up to date
+            recomputePosr(); 
+            computeAABB();
+            gflags &= ~GEOM_AABB_BAD;
+        }
+    }
+
+    inline void markAABBBad();
+
+    // add and remove this geom from a linked list maintained by a space.
+
+    void spaceAdd (dxGeom **first_ptr) {
+        next = *first_ptr;
+        tome = first_ptr;
+        if (*first_ptr) (*first_ptr)->tome = &next;
+        *first_ptr = this;
+    }
+    void spaceRemove() {
+        if (next) next->tome = tome;
+        *tome = next;
+    }
+
+    // add and remove this geom from a linked list maintained by a body.
+
+    void bodyAdd (dxBody *b) {
+        body = b;
+        body_next = b->geom;
+        b->geom = this;
+    }
+    void bodyRemove();
+};
+
+//****************************************************************************
+// the base space class
+//
+// the contained geoms are divided into two kinds: clean and dirty.
+// the clean geoms have not moved since they were put in the list,
+// and their AABBs are valid. the dirty geoms have changed position, and
+// their AABBs may not be valid. the two types are distinguished by the
+// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list.
+
+#if dTLS_ENABLED
+#define dSPACE_TLS_KIND_INIT_VALUE OTK__DEFAULT
+#define dSPACE_TLS_KIND_MANUAL_VALUE OTK_MANUALCLEANUP
+#else
+#define dSPACE_TLS_KIND_INIT_VALUE 0
+#define dSPACE_TLS_KIND_MANUAL_VALUE 0
+#endif
+
+struct dxSpace : public dxGeom {
+    int count;			// number of geoms in this space
+    dxGeom *first;		// first geom in list
+    int cleanup;			// cleanup mode, 1=destroy geoms on exit
+    int sublevel;         // space sublevel (used in dSpaceCollide2). NOT TRACKED AUTOMATICALLY!!!
+    unsigned tls_kind;	// space TLS kind to be used for global caches retrieval
+
+    // cached state for getGeom()
+    int current_index;		// only valid if current_geom != 0
+    dxGeom *current_geom;		// if 0 then there is no information
+
+    // locking stuff. the space is locked when it is currently traversing its
+    // internal data structures, e.g. in collide() and collide2(). operations
+    // that modify the contents of the space are not permitted when the space
+    // is locked.
+    int lock_count;
+
+    dxSpace (dSpaceID _space);
+    ~dxSpace();
+
+    void computeAABB();
+
+    void setCleanup (int mode) { cleanup = (mode != 0); }
+    int getCleanup() const { return cleanup; }
+    void setSublevel(int value) { sublevel = value; }
+    int getSublevel() const { return sublevel; }
+    void setManulCleanup(int value) { tls_kind = (value ? dSPACE_TLS_KIND_MANUAL_VALUE : dSPACE_TLS_KIND_INIT_VALUE); }
+    int getManualCleanup() const { return (tls_kind == dSPACE_TLS_KIND_MANUAL_VALUE) ? 1 : 0; }
+    int query (dxGeom *geom) const { dAASSERT(geom); return (geom->parent_space == this); }
+    int getNumGeoms() const { return count; }
+
+    virtual dxGeom *getGeom (int i);
+
+    virtual void add (dxGeom *);
+    virtual void remove (dxGeom *);
+    virtual void dirty (dxGeom *);
+
+    virtual void cleanGeoms()=0;
+    // turn all dirty geoms into clean geoms by computing their AABBs and any
+    // other space data structures that are required. this should clear the
+    // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms.
+
+    virtual void collide (void *data, dNearCallback *callback)=0;
+    virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0;
+};
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*inline */
+void dxGeom::markAABBBad() {
+    gflags |= (GEOM_DIRTY | GEOM_AABB_BAD);
+    CHECK_NOT_LOCKED(parent_space);
+}
+
+
+//****************************************************************************
+// Initialization and finalization functions
+
+void dInitColliders();
+void dFinitColliders();
+
+void dClearPosrCache(void);
+void dFinitUserClasses();
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/collision_libccd.h

@@ -0,0 +1,44 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _LIBCCD_COLLISION_H_
+#define _LIBCCD_COLLISION_H_
+
+int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip);
+
+#endif /* _LIBCCD_COLLISION_H_ */

thirdparty/ode-0.16.5/ode/src/collision_quadtreespace.cpp

@@ -0,0 +1,616 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// QuadTreeSpace by Erwin de Vries.
+// With math corrections by Oleh Derevenko. ;)
+
+#include <ode/common.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include "config.h"
+#include "matrix.h"
+#include "collision_kernel.h"
+
+#include "collision_space_internal.h"
+
+
+#define AXIS0 0
+#define AXIS1 1
+#define UP 2
+
+//#define DRAWBLOCKS
+
+const int SPLITAXIS = 2;
+const int SPLITS = SPLITAXIS * SPLITAXIS;
+
+#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
+
+class Block{
+public:
+    dReal mMinX, mMaxX;
+    dReal mMinZ, mMaxZ;
+
+    dGeomID mFirst;
+    int mGeomCount;
+
+    Block* mParent;
+    Block* mChildren;
+
+    void Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks);
+
+    void Collide(void* UserData, dNearCallback* Callback);
+    void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback);
+
+    void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback);
+
+    void AddObject(dGeomID Object);
+    void DelObject(dGeomID Object);
+    void Traverse(dGeomID Object);
+
+    bool Inside(const dReal* AABB);
+
+    Block* GetBlock(const dReal* AABB);
+    Block* GetBlockChild(const dReal* AABB);
+};
+
+
+#ifdef DRAWBLOCKS
+#include "..\..\Include\drawstuff\\drawstuff.h"
+
+static void DrawBlock(Block* Block){
+    dVector3 v[8];
+    v[0][AXIS0] = Block->mMinX;
+    v[0][UP] = REAL(-1.0);
+    v[0][AXIS1] = Block->mMinZ;
+
+    v[1][AXIS0] = Block->mMinX;
+    v[1][UP] = REAL(-1.0);
+    v[1][AXIS1] = Block->mMaxZ;
+
+    v[2][AXIS0] = Block->mMaxX;
+    v[2][UP] = REAL(-1.0);
+    v[2][AXIS1] = Block->mMinZ;
+
+    v[3][AXIS0] = Block->mMaxX;
+    v[3][UP] = REAL(-1.0);
+    v[3][AXIS1] = Block->mMaxZ;
+
+    v[4][AXIS0] = Block->mMinX;
+    v[4][UP] = REAL(1.0);
+    v[4][AXIS1] = Block->mMinZ;
+
+    v[5][AXIS0] = Block->mMinX;
+    v[5][UP] = REAL(1.0);
+    v[5][AXIS1] = Block->mMaxZ;
+
+    v[6][AXIS0] = Block->mMaxX;
+    v[6][UP] = REAL(1.0);
+    v[6][AXIS1] = Block->mMinZ;
+
+    v[7][AXIS0] = Block->mMaxX;
+    v[7][UP] = REAL(1.0);
+    v[7][AXIS1] = Block->mMaxZ;
+
+    // Bottom
+    dsDrawLine(v[0], v[1]);
+    dsDrawLine(v[1], v[3]);
+    dsDrawLine(v[3], v[2]);
+    dsDrawLine(v[2], v[0]);
+
+    // Top
+    dsDrawLine(v[4], v[5]);
+    dsDrawLine(v[5], v[7]);
+    dsDrawLine(v[7], v[6]);
+    dsDrawLine(v[6], v[4]);
+
+    // Sides
+    dsDrawLine(v[0], v[4]);
+    dsDrawLine(v[1], v[5]);
+    dsDrawLine(v[2], v[6]);
+    dsDrawLine(v[3], v[7]);
+}
+#endif	//DRAWBLOCKS
+
+
+void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){
+    dIASSERT(MinX <= MaxX);
+    dIASSERT(MinZ <= MaxZ);
+
+    mGeomCount = 0;
+    mFirst = 0;
+
+    mMinX = MinX;
+    mMaxX = MaxX;
+
+    mMinZ = MinZ;
+    mMaxZ = MaxZ;
+
+    this->mParent = Parent;
+
+    if (Depth > 0){
+        mChildren = Blocks;
+        Blocks += SPLITS;
+
+        const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS;
+        const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS;
+
+        const int ChildDepth = Depth - 1;
+        int Index = 0;
+
+        dReal ChildRightX = MinX;
+        for (int i = 0; i < SPLITAXIS; i++){
+            const dReal ChildLeftX = ChildRightX;
+            ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;
+
+            dReal ChildRightZ = MinZ;
+            for (int j = 0; j < SPLITAXIS; j++){
+                const dReal ChildLeftZ = ChildRightZ;
+                ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;
+
+                mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks);
+                ++Index;
+            }
+        }
+    }
+    else mChildren = 0;
+}
+
+void Block::Collide(void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+    DrawBlock(this);
+#endif
+    // Collide the local list
+    dxGeom* g = mFirst;
+    while (g){
+        if (GEOM_ENABLED(g)){
+            Collide(g, g->next_ex, UserData, Callback);
+        }
+        g = g->next_ex;
+    }
+
+    // Recurse for children
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            if (CurrentChild.mGeomCount <= 1){	// Early out
+                continue;
+            }
+            CurrentChild.Collide(UserData, Callback);
+        }
+    }
+}
+
+// Note: g2 is assumed to be in this Block
+void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+    DrawBlock(this);
+#endif
+    // Collide against local list
+    while (g2){
+        if (GEOM_ENABLED(g2)){
+            collideAABBs (g1, g2, UserData, Callback);
+        }
+        g2 = g2->next_ex;
+    }
+
+    // Collide against children
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            // Early out for empty blocks
+            if (CurrentChild.mGeomCount == 0){
+                continue;
+            }
+
+            // Does the geom's AABB collide with the block?
+            // Don't do AABB tests for single geom blocks.
+            if (CurrentChild.mGeomCount == 1){
+                //
+            }
+            else if (true){
+                if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
+                    g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
+                    g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
+                    g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue;
+            }
+            CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
+        }
+    }
+}
+
+void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
+    // Collide against local list
+    dxGeom* g1 = mFirst;
+    while (g1){
+        if (GEOM_ENABLED(g1)){
+            collideAABBs (g1, g2, UserData, Callback);
+        }
+        g1 = g1->next_ex;
+    }
+}
+
+void Block::AddObject(dGeomID Object){
+    // Add the geom
+    Object->next_ex = mFirst;
+    mFirst = Object;
+    Object->tome_ex = (dxGeom**)this;
+
+    // Now traverse upwards to tell that we have a geom
+    Block* Block = this;
+    do{
+        Block->mGeomCount++;
+        Block = Block->mParent;
+    }
+    while (Block);
+}
+
+void Block::DelObject(dGeomID Object){
+    // Del the geom
+    dxGeom *Last, *g = mFirst;
+    bool Found = false;
+
+    if (g == Object){
+        mFirst = g->next_ex;
+        Found = true;
+    }
+    else {
+        Last = g;
+        g = g->next_ex;
+    }
+
+    for (; !Found && g; Found = false){
+        if (g == Object){
+            Last->next_ex = g->next_ex;
+            break;
+        }
+        Last = g;
+        g = g->next_ex;
+    }
+
+    Object->tome_ex = 0;
+    dUASSERT((Object->next_ex = 0, true), "Needed for an assertion check only");
+
+    // Now traverse upwards to tell that we have lost a geom
+    Block* Block = this;
+    do{
+        Block->mGeomCount--;
+        Block = Block->mParent;
+    }
+    while (Block);
+}
+
+void Block::Traverse(dGeomID Object){
+    Block* NewBlock = GetBlock(Object->aabb);
+
+    if (NewBlock != this){
+        // Remove the geom from the old block and add it to the new block.
+        // This could be more optimal, but the loss should be very small.
+        DelObject(Object);
+        NewBlock->AddObject(Object);
+    }
+}
+
+bool Block::Inside(const dReal* AABB){
+    return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ;
+}
+
+Block* Block::GetBlock(const dReal* AABB){
+    if (Inside(AABB)){
+        return GetBlockChild(AABB);	// Child or this will have a good block
+    }
+    else if (mParent){
+        return mParent->GetBlock(AABB);	// Parent has a good block
+    }
+    else return this;	// We are at the root, so we have little choice
+}
+
+Block* Block::GetBlockChild(const dReal* AABB){
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            if (CurrentChild.Inside(AABB)){
+                return CurrentChild.GetBlockChild(AABB);	// Child will have good block
+            }
+        }
+    }
+    return this;	// This is the best block
+}
+
+//****************************************************************************
+// quadtree space
+
+struct dxQuadTreeSpace : public dxSpace{
+    Block* Blocks;	// Blocks[0] is the root
+
+    dArray<dxGeom*> DirtyList;
+
+    dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth);
+    ~dxQuadTreeSpace();
+
+    dxGeom* getGeom(int i);
+
+    void add(dxGeom* g);
+    void remove(dxGeom* g);
+    void dirty(dxGeom* g);
+
+    void computeAABB();
+
+    void cleanGeoms();
+    void collide(void* UserData, dNearCallback* Callback);
+    void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
+
+    // Temp data
+    Block* CurrentBlock;	// Only used while enumerating
+    int* CurrentChild;	// Only used while enumerating
+    int CurrentLevel;	// Only used while enumerating
+    dxGeom* CurrentObject;	// Only used while enumerating
+    int CurrentIndex;
+};
+
+namespace {
+
+    inline
+    sizeint numNodes(int depth) 
+    {
+        // A 4-ary tree has (4^(depth+1) - 1)/3 nodes
+        // Note: split up into multiple constant expressions for readability
+        const int k = depth+1;
+        const sizeint fourToNthPlusOne = (sizeint)1 << (2*k); // 4^k = 2^(2k)
+        return (fourToNthPlusOne - 1) / 3;
+    }
+
+}
+
+
+
+dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
+    type = dQuadTreeSpaceClass;
+
+    sizeint BlockCount = numNodes(Depth);
+
+    Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
+    Block* Blocks = this->Blocks + 1;	// This pointer gets modified!
+
+    dReal MinX = Center[AXIS0] - Extents[AXIS0];
+    dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
+    dReal MinZ = Center[AXIS1] - Extents[AXIS1];
+    dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
+    this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);
+
+    CurrentBlock = 0;
+    CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
+    CurrentLevel = 0;
+    CurrentObject = 0;
+    CurrentIndex = -1;
+
+    // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
+    aabb[0] = -dInfinity;
+    aabb[1] = dInfinity;
+    aabb[2] = -dInfinity;
+    aabb[3] = dInfinity;
+    aabb[4] = -dInfinity;
+    aabb[5] = dInfinity;
+}
+
+dxQuadTreeSpace::~dxQuadTreeSpace(){
+    int Depth = 0;
+    Block* Current = &Blocks[0];
+    while (Current){
+        Depth++;
+        Current = Current->mChildren;
+    }
+
+    sizeint BlockCount = numNodes(Depth);
+
+    dFree(Blocks, BlockCount * sizeof(Block));
+    dFree(CurrentChild, (Depth + 1) * sizeof(int));
+}
+
+dxGeom* dxQuadTreeSpace::getGeom(int Index){
+    dUASSERT(Index >= 0 && Index < count, "index out of range");
+
+    //@@@
+    dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
+
+    return 0;
+
+    // This doesnt work
+/*
+    if (CurrentIndex == Index){
+        // Loop through all objects in the local list
+CHILDRECURSE:
+        if (CurrentObject){
+            dGeomID g = CurrentObject;
+            CurrentObject = CurrentObject->next_ex;
+            CurrentIndex++;
+
+#ifdef DRAWBLOCKS
+            DrawBlock(CurrentBlock);
+#endif	//DRAWBLOCKS
+            return g;
+        }
+        else{
+            // Now lets loop through our children. Starting at index 0.
+            if (CurrentBlock->Children){
+                CurrentChild[CurrentLevel] = 0;
+PARENTRECURSE:
+                for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
+                    if (CurrentBlock->Children[i].GeomCount == 0){
+                        continue;
+                    }
+                    CurrentBlock = &CurrentBlock->Children[i];
+                    CurrentObject = CurrentBlock->First;
+
+                    i++;
+
+                    CurrentLevel++;
+                    goto CHILDRECURSE;
+                }
+            }
+        }
+
+        // Now lets go back to the parent so it can continue processing its other children.
+        if (CurrentBlock->Parent){
+            CurrentBlock = CurrentBlock->Parent;
+            CurrentLevel--;
+            goto PARENTRECURSE;
+        }
+    }
+    else{
+        CurrentBlock = &Blocks[0];
+        CurrentLevel = 0;
+        CurrentObject = CurrentObject;
+        CurrentIndex = 0;
+
+        // Other states are already set
+        CurrentObject = CurrentBlock->First;
+    }
+
+
+    if (current_geom && current_index == Index - 1){
+        //current_geom = current_geom->next_ex; // next
+        current_index = Index;
+        return current_geom;
+    }
+    else for (int i = 0; i < Index; i++){	// this will be verrrrrrry slow
+        getGeom(i);
+    }
+*/
+
+    return 0;
+}
+
+void dxQuadTreeSpace::add(dxGeom* g){
+    CHECK_NOT_LOCKED (this);
+    dAASSERT(g);
+    dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
+
+    DirtyList.push(g);
+    Blocks[0].GetBlock(g->aabb)->AddObject(g);	// Add to best block
+
+    dxSpace::add(g);
+}
+
+void dxQuadTreeSpace::remove(dxGeom* g){
+    CHECK_NOT_LOCKED(this);
+    dAASSERT(g);
+    dUASSERT(g->parent_space == this,"object is not in this space");
+
+    // remove
+    ((Block*)g->tome_ex)->DelObject(g);
+
+    for (int i = 0; i < DirtyList.size(); i++){
+        if (DirtyList[i] == g){
+            DirtyList.remove(i);
+            // (mg) there can be multiple instances of a dirty object on stack  be sure to remove ALL and not just first, for this we decrement i
+            --i;
+        }
+    }
+
+    dxSpace::remove(g);
+}
+
+void dxQuadTreeSpace::dirty(dxGeom* g){
+    DirtyList.push(g);
+}
+
+void dxQuadTreeSpace::computeAABB(){
+    //
+}
+
+void dxQuadTreeSpace::cleanGeoms(){
+    // compute the AABBs of all dirty geoms, and clear the dirty flags
+    lock_count++;
+
+    for (int i = 0; i < DirtyList.size(); i++){
+        dxGeom* g = DirtyList[i];
+        if (IS_SPACE(g)){
+            ((dxSpace*)g)->cleanGeoms();
+        }
+        
+        g->recomputeAABB();
+        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+
+        g->gflags &= ~GEOM_DIRTY;
+
+        ((Block*)g->tome_ex)->Traverse(g);
+    }
+    DirtyList.setSize(0);
+
+    lock_count--;
+}
+
+void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
+    dAASSERT(Callback);
+
+    lock_count++;
+    cleanGeoms();
+
+    Blocks[0].Collide(UserData, Callback);
+
+    lock_count--;
+}
+
+
+struct DataCallback {
+    void *data;
+    dNearCallback *callback;
+};
+// Invokes the callback with arguments swapped
+static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
+{
+    DataCallback *dc = (DataCallback*)data;
+    dc->callback(dc->data, g2, g1);
+}
+
+
+void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){
+    dAASSERT(g2 && Callback);
+
+    lock_count++;
+    cleanGeoms();
+    g2->recomputeAABB();
+
+    if (g2->parent_space == this){
+        // The block the geom is in
+        Block* CurrentBlock = (Block*)g2->tome_ex;
+
+        // Collide against block and its children
+        DataCallback dc = {UserData, Callback};
+        CurrentBlock->Collide(g2, CurrentBlock->mFirst, &dc, swap_callback);
+
+        // Collide against parents
+        while ((CurrentBlock = CurrentBlock->mParent))
+            CurrentBlock->CollideLocal(g2, UserData, Callback);
+
+    }
+    else {
+        DataCallback dc = {UserData, Callback};
+        Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback);
+    }
+
+    lock_count--;
+}
+
+dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){
+    return new dxQuadTreeSpace(space, Center, Extents, Depth);
+}

thirdparty/ode-0.16.5/ode/src/collision_sapspace.cpp

@@ -0,0 +1,856 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ *  Sweep and Prune adaptation/tweaks for ODE by Aras Pranckevicius.
+ *  Additional work by David Walters
+ *  Original code:
+ *    OPCODE - Optimized Collision Detection
+ *    Copyright (C) 2001 Pierre Terdiman
+ *    Homepage: http://www.codercorner.com/Opcode.htm
+ *
+ *  This version does complete radix sort, not "classical" SAP. So, we
+ *  have no temporal coherence, but are able to handle any movement
+ *  velocities equally well.
+ */
+
+#include <ode/common.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+
+#include "config.h"
+#include "matrix.h"
+#include "collision_kernel.h"
+#include "collision_space_internal.h"
+
+// Reference counting helper for radix sort global data.
+//static void RadixSortRef();
+//static void RadixSortDeref();
+
+
+// --------------------------------------------------------------------------
+//  Radix Sort Context
+// --------------------------------------------------------------------------
+
+struct RaixSortContext
+{
+public:
+    RaixSortContext(): mCurrentSize(0), mCurrentUtilization(0), mRanksValid(false), mRanksBuffer(NULL), mPrimaryRanks(NULL) {}
+    ~RaixSortContext() { FreeRanks(); }
+
+    // OPCODE's Radix Sorting, returns a list of indices in sorted order
+    const uint32* RadixSort( const float* input2, uint32 nb );
+
+private:
+    void FreeRanks();
+    void AllocateRanks(sizeint nNewSize);
+
+    void ReallocateRanksIfNecessary(sizeint nNewSize);
+
+private:
+    void SetCurrentSize(sizeint nValue) { mCurrentSize = nValue; }
+    sizeint GetCurrentSize() const { return mCurrentSize; }
+
+    void SetCurrentUtilization(sizeint nValue) { mCurrentUtilization = nValue; }
+    sizeint GetCurrentUtilization() const { return mCurrentUtilization; }
+
+    uint32 *GetRanks1() const { return mPrimaryRanks; }
+    uint32 *GetRanks2() const { return mRanksBuffer + ((mRanksBuffer + mCurrentSize) - mPrimaryRanks); }
+    void SwapRanks() { mPrimaryRanks = GetRanks2(); }
+
+    bool AreRanksValid() const { return mRanksValid; }
+    void InvalidateRanks() { mRanksValid = false; }
+    void ValidateRanks() { mRanksValid = true; }
+
+private:
+    sizeint mCurrentSize;						//!< Current size of the indices list
+    sizeint mCurrentUtilization;					//!< Current utilization of the indices list
+    bool mRanksValid;
+    uint32* mRanksBuffer;						//!< Two lists allocated sequentially in a single block
+    uint32* mPrimaryRanks;
+};
+
+void RaixSortContext::AllocateRanks(sizeint nNewSize)
+{
+    dIASSERT(GetCurrentSize() == 0);
+
+    mRanksBuffer = new uint32[2 * nNewSize];
+    mPrimaryRanks = mRanksBuffer;
+
+    SetCurrentSize(nNewSize);
+}
+
+void RaixSortContext::FreeRanks()
+{
+    SetCurrentSize(0);
+
+    delete[] mRanksBuffer;
+}
+
+void RaixSortContext::ReallocateRanksIfNecessary(sizeint nNewSize)
+{
+    sizeint nCurUtilization = GetCurrentUtilization();
+
+    if (nNewSize != nCurUtilization)
+    {
+        sizeint nCurSize = GetCurrentSize();
+
+        if ( nNewSize > nCurSize )
+        {
+            // Free previously used ram
+            FreeRanks();
+
+            // Get some fresh one
+            AllocateRanks(nNewSize);
+        }
+
+        InvalidateRanks();
+        SetCurrentUtilization(nNewSize);
+    }
+}
+
+// --------------------------------------------------------------------------
+//  SAP space code
+// --------------------------------------------------------------------------
+
+struct dxSAPSpace : public dxSpace
+{
+    // Constructor / Destructor
+    dxSAPSpace( dSpaceID _space, int sortaxis );
+    ~dxSAPSpace();
+
+    // dxSpace
+    virtual dxGeom* getGeom(int i);
+    virtual void add(dxGeom* g);
+    virtual void remove(dxGeom* g);
+    virtual void dirty(dxGeom* g);
+    virtual void computeAABB();
+    virtual void cleanGeoms();
+    virtual void collide( void *data, dNearCallback *callback );
+    virtual void collide2( void *data, dxGeom *geom, dNearCallback *callback );
+
+private:
+
+    //--------------------------------------------------------------------------
+    // Local Declarations
+    //--------------------------------------------------------------------------
+
+    //! A generic couple structure
+    struct Pair
+    {
+        uint32 id0;	//!< First index of the pair
+        uint32 id1;	//!< Second index of the pair
+
+        // Default and Value Constructor
+        Pair() {}
+        Pair( uint32 i0, uint32 i1 ) : id0( i0 ), id1( i1 ) {}
+    };
+
+    //--------------------------------------------------------------------------
+    // Helpers
+    //--------------------------------------------------------------------------
+
+    /**
+    *	Complete box pruning.
+    *  Returns a list of overlapping pairs of boxes, each box of the pair
+    *  belongs to the same set.
+    *
+    *	@param	count	[in] number of boxes.
+    *	@param	geoms	[in] geoms of boxes.
+    *	@param	pairs	[out] array of overlapping pairs.
+    */
+    void BoxPruning( int count, const dxGeom** geoms, dArray< Pair >& pairs );
+
+
+    //--------------------------------------------------------------------------
+    // Implementation Data
+    //--------------------------------------------------------------------------
+
+    // We have two lists (arrays of pointers) to dirty and clean
+    // geoms. Each geom knows it's index into the corresponding list
+    // (see macros above).
+    dArray<dxGeom*> DirtyList; // dirty geoms
+    dArray<dxGeom*> GeomList;	// clean geoms
+
+    // For SAP, we ultimately separate "normal" geoms and the ones that have
+    // infinite AABBs. No point doing SAP on infinite ones (and it doesn't handle
+    // infinite geoms anyway).
+    dArray<dxGeom*> TmpGeomList;	// temporary for normal geoms
+    dArray<dxGeom*> TmpInfGeomList;	// temporary for geoms with infinite AABBs
+
+    // Our sorting axes. (X,Z,Y is often best). Stored *2 for minor speedup
+    // Axis indices into geom's aabb are: min=idx, max=idx+1
+    uint32 ax0idx;
+    uint32 ax1idx;
+    uint32 ax2idx;
+
+    // pruning position array scratch pad
+    // NOTE: this is float not dReal because of the OPCODE radix sorter
+    dArray< float > poslist;
+    RaixSortContext	sortContext;
+};
+
+// Creation
+dSpaceID dSweepAndPruneSpaceCreate( dxSpace* space, int axisorder ) {
+    return new dxSAPSpace( space, axisorder );
+}
+
+
+//==============================================================================
+
+#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
+
+// HACK: We abuse 'next' and 'tome' members of dxGeom to store indices into dirty/geom lists.
+#define GEOM_SET_DIRTY_IDX(g,idx) { (g)->next_ex = (dxGeom*)(sizeint)(idx); }
+#define GEOM_SET_GEOM_IDX(g,idx) { (g)->tome_ex = (dxGeom**)(sizeint)(idx); }
+#define GEOM_GET_DIRTY_IDX(g) ((int)(sizeint)(g)->next_ex)
+#define GEOM_GET_GEOM_IDX(g) ((int)(sizeint)(g)->tome_ex)
+#define GEOM_INVALID_IDX (-1)
+
+
+/*
+*  A bit of repetitive work - similar to collideAABBs, but doesn't check
+*  if AABBs intersect (because SAP returns pairs with overlapping AABBs).
+*/
+static void collideGeomsNoAABBs( dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback )
+{
+    dIASSERT( (g1->gflags & GEOM_AABB_BAD)==0 );
+    dIASSERT( (g2->gflags & GEOM_AABB_BAD)==0 );
+
+    // no contacts if both geoms on the same body, and the body is not 0
+    if (g1->body == g2->body && g1->body) return;
+
+    // test if the category and collide bitfields match
+    if ( ((g1->category_bits & g2->collide_bits) ||
+        (g2->category_bits & g1->collide_bits)) == 0) {
+            return;
+    }
+
+    dReal *bounds1 = g1->aabb;
+    dReal *bounds2 = g2->aabb;
+
+    // check if either object is able to prove that it doesn't intersect the
+    // AABB of the other
+    if (g1->AABBTest (g2,bounds2) == 0) return;
+    if (g2->AABBTest (g1,bounds1) == 0) return;
+
+    // the objects might actually intersect - call the space callback function
+    callback (data,g1,g2);
+}
+
+
+dxSAPSpace::dxSAPSpace( dSpaceID _space, int axisorder ) : dxSpace( _space )
+{
+    type = dSweepAndPruneSpaceClass;
+
+    // Init AABB to infinity
+    aabb[0] = -dInfinity;
+    aabb[1] = dInfinity;
+    aabb[2] = -dInfinity;
+    aabb[3] = dInfinity;
+    aabb[4] = -dInfinity;
+    aabb[5] = dInfinity;
+
+    ax0idx = ( ( axisorder ) & 3 ) << 1;
+    ax1idx = ( ( axisorder >> 2 ) & 3 ) << 1;
+    ax2idx = ( ( axisorder >> 4 ) & 3 ) << 1;
+}
+
+dxSAPSpace::~dxSAPSpace()
+{
+    CHECK_NOT_LOCKED(this);
+    if ( cleanup ) {
+        // note that destroying each geom will call remove()
+        for ( ; DirtyList.size(); dGeomDestroy( DirtyList[ 0 ] ) ) {}
+        for ( ; GeomList.size(); dGeomDestroy( GeomList[ 0 ] ) ) {}
+    }
+    else {
+        // just unhook them
+        for ( ; DirtyList.size(); remove( DirtyList[ 0 ] ) ) {}
+        for ( ; GeomList.size(); remove( GeomList[ 0 ] ) ) {}
+    }
+}
+
+dxGeom* dxSAPSpace::getGeom( int i )
+{
+    dUASSERT( i >= 0 && i < count, "index out of range" );
+    int dirtySize = DirtyList.size();
+    if( i < dirtySize )
+        return DirtyList[i];
+    else
+        return GeomList[i-dirtySize];
+}
+
+void dxSAPSpace::add( dxGeom* g )
+{
+    CHECK_NOT_LOCKED (this);
+    dAASSERT(g);
+    dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
+
+
+    // add to dirty list
+    GEOM_SET_DIRTY_IDX( g, DirtyList.size() );
+    GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX );
+    DirtyList.push( g );
+
+    dxSpace::add(g);
+}
+
+void dxSAPSpace::remove( dxGeom* g )
+{
+    CHECK_NOT_LOCKED(this);
+    dAASSERT(g);
+    dUASSERT(g->parent_space == this,"object is not in this space");
+
+    // remove
+    int dirtyIdx = GEOM_GET_DIRTY_IDX(g);
+    int geomIdx = GEOM_GET_GEOM_IDX(g);
+    // must be in one list, not in both
+    dUASSERT(
+        (dirtyIdx==GEOM_INVALID_IDX && geomIdx>=0 && geomIdx<GeomList.size()) ||
+        (geomIdx==GEOM_INVALID_IDX && dirtyIdx>=0 && dirtyIdx<DirtyList.size()),
+        "geom indices messed up" );
+    if( dirtyIdx != GEOM_INVALID_IDX ) {
+        // we're in dirty list, remove
+        int dirtySize = DirtyList.size();
+        if (dirtyIdx != dirtySize-1) {
+            dxGeom* lastG = DirtyList[dirtySize-1];
+            DirtyList[dirtyIdx] = lastG;
+            GEOM_SET_DIRTY_IDX(lastG,dirtyIdx);
+        }
+        GEOM_SET_DIRTY_IDX(g,GEOM_INVALID_IDX);
+        DirtyList.setSize( dirtySize-1 );
+    } else {
+        // we're in geom list, remove
+        int geomSize = GeomList.size();
+        if (geomIdx != geomSize-1) {
+            dxGeom* lastG = GeomList[geomSize-1];
+            GeomList[geomIdx] = lastG;
+            GEOM_SET_GEOM_IDX(lastG,geomIdx);
+        }
+        GEOM_SET_GEOM_IDX(g,GEOM_INVALID_IDX);
+        GeomList.setSize( geomSize-1 );
+    }
+
+    g->tome_ex = 0;
+    dUASSERT((g->next_ex = 0, true), "Needed for an assertion check only");
+
+    dxSpace::remove(g);
+}
+
+void dxSAPSpace::dirty( dxGeom* g )
+{
+    dAASSERT(g);
+    dUASSERT(g->parent_space == this, "object is not in this space");
+
+    // check if already dirtied
+    int dirtyIdx = GEOM_GET_DIRTY_IDX(g);
+    if( dirtyIdx != GEOM_INVALID_IDX )
+        return;
+
+    int geomIdx = GEOM_GET_GEOM_IDX(g);
+    dUASSERT( geomIdx>=0 && geomIdx<GeomList.size(), "geom indices messed up" );
+
+    // remove from geom list, place last in place of this
+    int geomSize = GeomList.size();
+    if (geomIdx != geomSize-1) {
+        dxGeom* lastG = GeomList[geomSize-1];
+        GeomList[geomIdx] = lastG;
+        GEOM_SET_GEOM_IDX(lastG,geomIdx);
+    }
+    GeomList.setSize( geomSize-1 );
+
+    // add to dirty list
+    GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX );
+    GEOM_SET_DIRTY_IDX( g, DirtyList.size() );
+    DirtyList.push( g );
+}
+
+void dxSAPSpace::computeAABB()
+{
+    // TODO?
+}
+
+void dxSAPSpace::cleanGeoms()
+{
+    int dirtySize = DirtyList.size();
+    if( !dirtySize )
+        return;
+
+    // compute the AABBs of all dirty geoms, clear the dirty flags,
+    // remove from dirty list, place into geom list
+    lock_count++;
+
+    int geomSize = GeomList.size();
+    GeomList.setSize( geomSize + dirtySize ); // ensure space in geom list
+
+    for( int i = 0; i < dirtySize; ++i ) {
+        dxGeom* g = DirtyList[i];
+        if( IS_SPACE(g) ) {
+            ((dxSpace*)g)->cleanGeoms();
+        }
+        
+        g->recomputeAABB();
+        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+        
+        g->gflags &= ~GEOM_DIRTY;
+        
+        // remove from dirty list, add to geom list
+        GEOM_SET_DIRTY_IDX( g, GEOM_INVALID_IDX );
+        GEOM_SET_GEOM_IDX( g, geomSize + i );
+        GeomList[geomSize+i] = g;
+    }
+    // clear dirty list
+    DirtyList.setSize( 0 );
+
+    lock_count--;
+}
+
+void dxSAPSpace::collide( void *data, dNearCallback *callback )
+{
+    dAASSERT (callback);
+
+    lock_count++;
+
+    cleanGeoms();
+
+    // by now all geoms are in GeomList, and DirtyList must be empty
+    int geom_count = GeomList.size();
+    dUASSERT( geom_count == count, "geom counts messed up" );
+
+    // separate all ENABLED geoms into infinite AABBs and normal AABBs
+    TmpGeomList.setSize(0);
+    TmpInfGeomList.setSize(0);
+    int axis0max = ax0idx + 1;
+    for( int i = 0; i < geom_count; ++i ) {
+        dxGeom* g = GeomList[i];
+        if( !GEOM_ENABLED(g) ) // skip disabled ones
+            continue;
+        const dReal& amax = g->aabb[axis0max];
+        if( amax == dInfinity ) // HACK? probably not...
+            TmpInfGeomList.push( g );
+        else
+            TmpGeomList.push( g );
+    }
+
+    // do SAP on normal AABBs
+    dArray< Pair > overlapBoxes;
+    int tmp_geom_count = TmpGeomList.size();
+    if ( tmp_geom_count > 0 )
+    {
+        // Generate a list of overlapping boxes
+        BoxPruning( tmp_geom_count, (const dxGeom**)TmpGeomList.data(), overlapBoxes );
+    }
+
+    // collide overlapping
+    int overlapCount = overlapBoxes.size();
+    for( int j = 0; j < overlapCount; ++j )
+    {
+        const Pair& pair = overlapBoxes[ j ];
+        dxGeom* g1 = TmpGeomList[ pair.id0 ];
+        dxGeom* g2 = TmpGeomList[ pair.id1 ];
+        collideGeomsNoAABBs( g1, g2, data, callback );
+    }
+
+    int infSize = TmpInfGeomList.size();
+    int normSize = TmpGeomList.size();
+    int m, n;
+
+    for ( m = 0; m < infSize; ++m )
+    {
+        dxGeom* g1 = TmpInfGeomList[ m ];
+
+        // collide infinite ones
+        for( n = m+1; n < infSize; ++n ) {
+            dxGeom* g2 = TmpInfGeomList[n];
+            collideGeomsNoAABBs( g1, g2, data, callback );
+        }
+
+        // collide infinite ones with normal ones
+        for( n = 0; n < normSize; ++n ) {
+            dxGeom* g2 = TmpGeomList[n];
+            collideGeomsNoAABBs( g1, g2, data, callback );
+        }
+    }
+
+    lock_count--;
+}
+
+void dxSAPSpace::collide2( void *data, dxGeom *geom, dNearCallback *callback )
+{
+    dAASSERT (geom && callback);
+
+    // TODO: This is just a simple N^2 implementation
+
+    lock_count++;
+
+    cleanGeoms();
+    geom->recomputeAABB();
+
+    // intersect bounding boxes
+    int geom_count = GeomList.size();
+    for ( int i = 0; i < geom_count; ++i ) {
+        dxGeom* g = GeomList[i];
+        if ( GEOM_ENABLED(g) )
+            collideAABBs (g,geom,data,callback);
+    }
+
+    lock_count--;
+}
+
+
+void dxSAPSpace::BoxPruning( int count, const dxGeom** geoms, dArray< Pair >& pairs )
+{
+    // Size the poslist (+1 for infinity end cap)
+    poslist.setSize( count );
+
+    // 1) Build main list using the primary axis
+    //  NOTE: uses floats instead of dReals because that's what radix sort wants
+    for( int i = 0; i < count; ++i )
+        poslist[ i ] = (float)TmpGeomList[i]->aabb[ ax0idx ];
+
+    // 2) Sort the list
+    const uint32* Sorted = sortContext.RadixSort( poslist.data(), count );
+
+    // 3) Prune the list
+    const uint32* const LastSorted = Sorted + count;
+    const uint32* RunningAddress = Sorted;
+
+    bool bExitLoop;
+    Pair IndexPair;
+    while ( Sorted < LastSorted )
+    {
+        IndexPair.id0 = *Sorted++;
+
+        // empty, this loop just advances RunningAddress
+        for (bExitLoop = false; poslist[*RunningAddress++] < poslist[IndexPair.id0]; )
+        {
+            if (RunningAddress == LastSorted)
+            {
+                bExitLoop = true;
+                break;
+            }
+        }
+
+        if ( bExitLoop || RunningAddress == LastSorted) // Not a bug!!!
+        {
+            break;
+        }
+
+        const float idx0ax0max = (float)geoms[IndexPair.id0]->aabb[ax0idx+1]; // To avoid wrong decisions caused by rounding errors, cast the AABB element to float similarly as we did at the function beginning
+        const dReal idx0ax1max = geoms[IndexPair.id0]->aabb[ax1idx+1];
+        const dReal idx0ax2max = geoms[IndexPair.id0]->aabb[ax2idx+1];
+
+        for (const uint32* RunningAddress2 = RunningAddress; poslist[ IndexPair.id1 = *RunningAddress2++ ] <= idx0ax0max; )
+        {
+            const dReal* aabb0 = geoms[ IndexPair.id0 ]->aabb;
+            const dReal* aabb1 = geoms[ IndexPair.id1 ]->aabb;
+
+            // Intersection?
+            if ( idx0ax1max >= aabb1[ax1idx] && aabb1[ax1idx+1] >= aabb0[ax1idx] 
+                && idx0ax2max >= aabb1[ax2idx] && aabb1[ax2idx+1] >= aabb0[ax2idx] )
+            {
+                pairs.push( IndexPair );
+            }
+
+            if (RunningAddress2 == LastSorted)
+            {
+                break;
+            }
+        }
+
+    } // while ( RunningAddress < LastSorted && Sorted < LastSorted )
+}
+
+
+//==============================================================================
+
+//------------------------------------------------------------------------------
+// Radix Sort
+//------------------------------------------------------------------------------
+
+
+
+#define CHECK_PASS_VALIDITY(pass)															\
+    /* Shortcut to current counters */														\
+    const uint32* CurCount = &mHistogram[pass<<8];												\
+    \
+    /* Reset flag. The sorting pass is supposed to be performed. (default) */				\
+    bool PerformPass = true;																\
+    \
+    /* Check pass validity */																\
+    \
+    /* If all values have the same byte, sorting is useless. */								\
+    /* It may happen when sorting bytes or words instead of dwords. */						\
+    /* This routine actually sorts words faster than dwords, and bytes */					\
+    /* faster than words. Standard running time (O(4*n))is reduced to O(2*n) */				\
+    /* for words and O(n) for bytes. Running time for floats depends on actual values... */	\
+    \
+    /* Get first byte */																	\
+    uint8 UniqueVal = *(((const uint8*)input)+pass);												\
+    \
+    /* Check that byte's counter */															\
+    if(CurCount[UniqueVal]==nb)	PerformPass=false;
+
+// WARNING ONLY SORTS IEEE FLOATING-POINT VALUES
+const uint32* RaixSortContext::RadixSort( const float* input2, uint32 nb )
+{
+    union _type_cast_union
+    {
+        _type_cast_union(const float *floats): asFloats(floats) {}
+        _type_cast_union(const uint32 *uints32): asUInts32(uints32) {}
+
+        const float *asFloats;
+        const uint32 *asUInts32;
+        const uint8 *asUInts8;
+    };
+
+    const uint32* input = _type_cast_union(input2).asUInts32;
+
+    // Resize lists if needed
+    ReallocateRanksIfNecessary(nb);
+
+    // Allocate histograms & offsets on the stack
+    uint32 mHistogram[256*4];
+    uint32* mLink[256];
+
+    // Create histograms (counters). Counters for all passes are created in one run.
+    // Pros:	read input buffer once instead of four times
+    // Cons:	mHistogram is 4Kb instead of 1Kb
+    // Floating-point values are always supposed to be signed values, so there's only one code path there.
+    // Please note the floating point comparison needed for temporal coherence! Although the resulting asm code
+    // is dreadful, this is surprisingly not such a performance hit - well, I suppose that's a big one on first
+    // generation Pentiums....We can't make comparison on integer representations because, as Chris said, it just
+    // wouldn't work with mixed positive/negative values....
+    {
+        /* Clear counters/histograms */
+        memset(mHistogram, 0, 256*4*sizeof(uint32));
+
+        /* Prepare to count */
+        const uint8* p = _type_cast_union(input).asUInts8;
+        const uint8* pe = &p[nb*4];
+        uint32* h0= &mHistogram[0];		/* Histogram for first pass (LSB)	*/
+        uint32* h1= &mHistogram[256];	/* Histogram for second pass		*/
+        uint32* h2= &mHistogram[512];	/* Histogram for third pass			*/
+        uint32* h3= &mHistogram[768];	/* Histogram for last pass (MSB)	*/
+
+        bool AlreadySorted = true;	/* Optimism... */
+
+        if (!AreRanksValid())
+        {
+            /* Prepare for temporal coherence */
+            const float* Running = input2;
+            float PrevVal = *Running;
+
+            while(p!=pe)
+            {
+                /* Read input input2 in previous sorted order */
+                float Val = *Running++;
+                /* Check whether already sorted or not */
+                if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */
+                /* Update for next iteration */
+                PrevVal = Val;
+
+                /* Create histograms */
+                h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
+            }
+
+            /* If all input values are already sorted, we just have to return and leave the */
+            /* previous list unchanged. That way the routine may take advantage of temporal */
+            /* coherence, for example when used to sort transparent faces.					*/
+            if(AlreadySorted)
+            {
+                uint32* const Ranks1 = GetRanks1();
+                for(uint32 i=0;i<nb;i++)	Ranks1[i] = i;
+                return Ranks1;
+            }
+        }
+        else
+        {
+            /* Prepare for temporal coherence */
+            uint32* const Ranks1 = GetRanks1();
+
+            uint32* Indices = Ranks1;
+            float PrevVal = input2[*Indices];
+
+            while(p!=pe)
+            {
+                /* Read input input2 in previous sorted order */
+                float Val = input2[*Indices++];
+                /* Check whether already sorted or not */
+                if(Val<PrevVal)	{ AlreadySorted = false; break; } /* Early out */
+                /* Update for next iteration */
+                PrevVal = Val;
+
+                /* Create histograms */
+                h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
+            }
+
+            /* If all input values are already sorted, we just have to return and leave the */
+            /* previous list unchanged. That way the routine may take advantage of temporal */
+            /* coherence, for example when used to sort transparent faces.					*/
+            if(AlreadySorted)	{ return Ranks1;	}
+        }
+
+        /* Else there has been an early out and we must finish computing the histograms */
+        while(p!=pe)
+        {
+            /* Create histograms without the previous overhead */
+            h0[*p++]++;	h1[*p++]++;	h2[*p++]++;	h3[*p++]++;
+        }
+    }
+
+    // Compute #negative values involved if needed
+    uint32 NbNegativeValues = 0;
+
+    // An efficient way to compute the number of negatives values we'll have to deal with is simply to sum the 128
+    // last values of the last histogram. Last histogram because that's the one for the Most Significant Byte,
+    // responsible for the sign. 128 last values because the 128 first ones are related to positive numbers.
+    uint32* h3= &mHistogram[768];
+    for(uint32 i=128;i<256;i++)	NbNegativeValues += h3[i];	// 768 for last histogram, 128 for negative part
+
+    // Radix sort, j is the pass number (0=LSB, 3=MSB)
+    for(uint32 j=0;j<4;j++)
+    {
+        // Should we care about negative values?
+        if(j!=3)
+        {
+            // Here we deal with positive values only
+            CHECK_PASS_VALIDITY(j);
+
+            if(PerformPass)
+            {
+                uint32* const Ranks2 = GetRanks2();
+                // Create offsets
+                mLink[0] = Ranks2;
+                for(uint32 i=1;i<256;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];
+
+                // Perform Radix Sort
+                const uint8* InputBytes = _type_cast_union(input).asUInts8;
+                InputBytes += j;
+                if (!AreRanksValid())
+                {
+                    for(uint32 i=0;i<nb;i++)
+                    {
+                        *mLink[InputBytes[i<<2]]++ = i;
+                    }
+
+                    ValidateRanks();
+                }
+                else
+                {
+                    uint32* const Ranks1 = GetRanks1();
+
+                    uint32* Indices				= Ranks1;
+                    uint32* const IndicesEnd	= Ranks1 + nb;
+                    while(Indices!=IndicesEnd)
+                    {
+                        uint32 id = *Indices++;
+                        *mLink[InputBytes[id<<2]]++ = id;
+                    }
+                }
+
+                // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+                SwapRanks();
+            }
+        }
+        else
+        {
+            // This is a special case to correctly handle negative values
+            CHECK_PASS_VALIDITY(j);
+
+            if(PerformPass)
+            {
+                uint32* const Ranks2 = GetRanks2();
+
+                // Create biased offsets, in order for negative numbers to be sorted as well
+                mLink[0] = Ranks2 + NbNegativeValues;										// First positive number takes place after the negative ones
+                for(uint32 i=1;i<128;i++)		mLink[i] = mLink[i-1] + CurCount[i-1];		// 1 to 128 for positive numbers
+
+                // We must reverse the sorting order for negative numbers!
+                mLink[255] = Ranks2;
+                for(uint32 i=0;i<127;i++)	mLink[254-i] = mLink[255-i] + CurCount[255-i];		// Fixing the wrong order for negative values
+                for(uint32 i=128;i<256;i++)	mLink[i] += CurCount[i];							// Fixing the wrong place for negative values
+
+                // Perform Radix Sort
+                if (!AreRanksValid())
+                {
+                    for(uint32 i=0;i<nb;i++)
+                    {
+                        uint32 Radix = input[i]>>24;							// Radix byte, same as above. AND is useless here (uint32).
+                        // ### cmp to be killed. Not good. Later.
+                        if(Radix<128)		*mLink[Radix]++ = i;		// Number is positive, same as above
+                        else				*(--mLink[Radix]) = i;		// Number is negative, flip the sorting order
+                    }
+
+                    ValidateRanks();
+                }
+                else
+                {
+                    uint32* const Ranks1 = GetRanks1();
+
+                    for(uint32 i=0;i<nb;i++)
+                    {
+                        uint32 Radix = input[Ranks1[i]]>>24;							// Radix byte, same as above. AND is useless here (uint32).
+                        // ### cmp to be killed. Not good. Later.
+                        if(Radix<128)		*mLink[Radix]++ = Ranks1[i];		// Number is positive, same as above
+                        else				*(--mLink[Radix]) = Ranks1[i];		// Number is negative, flip the sorting order
+                    }
+                }
+                // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+                SwapRanks();
+            }
+            else
+            {
+                // The pass is useless, yet we still have to reverse the order of current list if all values are negative.
+                if(UniqueVal>=128)
+                {
+                    if (!AreRanksValid())
+                    {
+                        uint32* const Ranks2 = GetRanks2();
+                        // ###Possible?
+                        for(uint32 i=0;i<nb;i++)
+                        {
+                            Ranks2[i] = nb-i-1;
+                        }
+
+                        ValidateRanks();
+                    }
+                    else
+                    {
+                        uint32* const Ranks1 = GetRanks1();
+                        uint32* const Ranks2 = GetRanks2();
+                        for(uint32 i=0;i<nb;i++)	Ranks2[i] = Ranks1[nb-i-1];
+                    }
+
+                    // Swap pointers for next pass. Valid indices - the most recent ones - are in mRanks after the swap.
+                    SwapRanks();
+                }
+            }
+        }
+    }
+
+    // Return indices
+    uint32* const Ranks1 = GetRanks1();
+    return Ranks1;
+}
+

thirdparty/ode-0.16.5/ode/src/collision_space.cpp

@@ -0,0 +1,864 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+spaces
+
+*/
+
+#include <vector>
+
+#include <ode/common.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include "config.h"
+#include "matrix.h"
+#include "collision_kernel.h"
+#include "collision_space_internal.h"
+#include "util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
+// and moving it to the front of the space's list. all the parents of a
+// dirty geom also become dirty.
+
+void dGeomMoved (dxGeom *geom)
+{
+    dAASSERT (geom);
+
+    // if geom is offset, mark it as needing a calculate
+    if (geom->offset_posr) {
+        geom->gflags |= GEOM_POSR_BAD;
+    }
+
+    // from the bottom of the space heirarchy up, process all clean geoms
+    // turning them into dirty geoms.
+    dxSpace *parent = geom->parent_space;
+
+    while (parent && (geom->gflags & GEOM_DIRTY)==0) {
+        geom->markAABBBad();
+        parent->dirty (geom);
+        geom = parent;
+        parent = parent->parent_space;
+    }
+
+    // all the remaining dirty geoms must have their AABB_BAD flags set, to
+    // ensure that their AABBs get recomputed
+    while (geom) {
+        geom->markAABBBad();
+        geom = geom->parent_space;
+    }
+}
+
+#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
+
+//****************************************************************************
+// dxSpace
+
+dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
+{
+    count = 0;
+    first = 0;
+    cleanup = 1;
+    sublevel = 0;
+    tls_kind = dSPACE_TLS_KIND_INIT_VALUE;
+    current_index = 0;
+    current_geom = 0;
+    lock_count = 0;
+}
+
+
+dxSpace::~dxSpace()
+{
+    CHECK_NOT_LOCKED (this);
+    if (cleanup) {
+        // note that destroying each geom will call remove()
+        dxGeom *g,*n;
+        for (g = first; g; g=n) {
+            n = g->next;
+            dGeomDestroy (g);
+        }
+    }
+    else {
+        dxGeom *g,*n;
+        for (g = first; g; g=n) {
+            n = g->next;
+            remove (g);
+        }
+    }
+}
+
+
+void dxSpace::computeAABB()
+{
+    if (first) {
+        int i;
+        dReal a[6];
+        a[0] = dInfinity;
+        a[1] = -dInfinity;
+        a[2] = dInfinity;
+        a[3] = -dInfinity;
+        a[4] = dInfinity;
+        a[5] = -dInfinity;
+        for (dxGeom *g=first; g; g=g->next) {
+            g->recomputeAABB();
+            for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
+            for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
+        }
+        memcpy(aabb,a,6*sizeof(dReal));
+    }
+    else {
+        dSetZero (aabb,6);
+    }
+}
+
+
+// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
+
+dxGeom *dxSpace::getGeom (int i)
+{
+    dUASSERT (i >= 0 && i < count,"index out of range");
+    if (current_geom && current_index == i-1) {
+        current_geom = current_geom->next;
+        current_index = i;
+        return current_geom;
+    }
+    else {
+        dxGeom *g=first;
+        for (int j=0; j<i; j++) {
+            if (g) g = g->next; else return 0;
+        }
+        current_geom = g;
+        current_index = i;
+        return g;
+    }
+}
+
+
+void dxSpace::add (dxGeom *geom)
+{
+    CHECK_NOT_LOCKED (this);
+    dAASSERT (geom);
+    dUASSERT (geom->parent_space == 0 && geom->next == 0,
+        "geom is already in a space");
+
+    // add
+    geom->parent_space = this;
+    geom->spaceAdd (&first);
+    count++;
+
+    // enumerator has been invalidated
+    current_geom = 0;
+
+    dGeomMoved (this);
+}
+
+
+void dxSpace::remove (dxGeom *geom)
+{
+    CHECK_NOT_LOCKED (this);
+    dAASSERT (geom);
+    dUASSERT (geom->parent_space == this,"object is not in this space");
+
+    // remove
+    geom->spaceRemove();
+    count--;
+
+    // safeguard
+    geom->next = 0;
+    geom->tome = 0;
+    geom->parent_space = 0;
+
+    // enumerator has been invalidated
+    current_geom = 0;
+
+    // the bounding box of this space (and that of all the parents) may have
+    // changed as a consequence of the removal.
+    dGeomMoved (this);
+}
+
+
+void dxSpace::dirty (dxGeom *geom)
+{
+    geom->spaceRemove();
+    geom->spaceAdd (&first);
+}
+
+//****************************************************************************
+// simple space - reports all n^2 object intersections
+
+struct dxSimpleSpace : public dxSpace {
+    dxSimpleSpace (dSpaceID _space);
+    void cleanGeoms();
+    void collide (void *data, dNearCallback *callback);
+    void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+};
+
+
+dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
+{
+    type = dSimpleSpaceClass;
+}
+
+
+void dxSimpleSpace::cleanGeoms()
+{
+    // compute the AABBs of all dirty geoms, and clear the dirty flags
+    lock_count++;
+    for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
+        if (IS_SPACE(g)) {
+            ((dxSpace*)g)->cleanGeoms();
+        }
+
+        g->recomputeAABB();
+        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+
+        g->gflags &= ~GEOM_DIRTY;
+    }
+    lock_count--;
+}
+
+
+void dxSimpleSpace::collide (void *data, dNearCallback *callback)
+{
+    dAASSERT (callback);
+
+    lock_count++;
+    cleanGeoms();
+
+    // intersect all bounding boxes
+    for (dxGeom *g1=first; g1; g1=g1->next) {
+        if (GEOM_ENABLED(g1)){
+            for (dxGeom *g2=g1->next; g2; g2=g2->next) {
+                if (GEOM_ENABLED(g2)){
+                    collideAABBs (g1,g2,data,callback);
+                }
+            }
+        }
+    }
+
+    lock_count--;
+}
+
+
+void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
+                              dNearCallback *callback)
+{
+    dAASSERT (geom && callback);
+
+    lock_count++;
+    cleanGeoms();
+    geom->recomputeAABB();
+
+    // intersect bounding boxes
+    for (dxGeom *g=first; g; g=g->next) {
+        if (GEOM_ENABLED(g)){
+            collideAABBs (g,geom,data,callback);
+        }
+    }
+
+    lock_count--;
+}
+
+//****************************************************************************
+// utility stuff for hash table space
+
+// kind of silly, but oh well...
+#ifndef MAXINT
+#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
+#endif
+
+
+// prime[i] is the largest prime smaller than 2^i
+#define NUM_PRIMES 31
+static const unsigned long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
+1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
+524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
+67108859L,134217689L,268435399L,536870909L,1073741789L};
+
+
+// an axis aligned bounding box in the hash table
+struct dxAABB {
+    int level;		// the level this is stored in (cell size = 2^level)
+    int dbounds[6];	// AABB bounds, discretized to cell size
+    dxGeom *geom;		// corresponding geometry object (AABB stored here)
+    sizeint index;		// index of this AABB, starting from 0
+};
+
+
+// a hash table node that represents an AABB that intersects a particular cell
+// at a particular level
+struct Node {
+    Node *next;		// next node in hash table collision list, 0 if none
+    int x,y,z;		// cell position in space, discretized to cell size
+    dxAABB *aabb;		// axis aligned bounding box that intersects this cell
+};
+
+
+// return the `level' of an AABB. the AABB will be put into cells at this
+// level - the cell size will be 2^level. the level is chosen to be the
+// smallest value such that the AABB occupies no more than 8 cells, regardless
+// of its placement. this means that:
+//	size/2 < q <= size
+// where q is the maximum AABB dimension.
+
+static int findLevel (dReal bounds[6])
+{
+    if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
+        bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
+        bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
+            return MAXINT;
+    }
+
+    // compute q
+    dReal q,q2;
+    q = bounds[1] - bounds[0];	// x bounds
+    q2 = bounds[3] - bounds[2];	// y bounds
+    if (q2 > q) q = q2;
+    q2 = bounds[5] - bounds[4];	// z bounds
+    if (q2 > q) q = q2;
+
+    // find level such that 0.5 * 2^level < q <= 2^level
+    int level;
+    frexp (q,&level);	// q = (0.5 .. 1.0) * 2^level (definition of frexp)
+    return level;
+}
+
+
+// find a virtual memory address for a cell at the given level and x,y,z
+// position.
+// @@@ currently this is not very sophisticated, e.g. the scaling
+// factors could be better designed to avoid collisions, and they should
+// probably depend on the hash table physical size.
+
+static unsigned long getVirtualAddressBase (unsigned int level, unsigned int x, unsigned int y)
+{
+    return level * 1000UL + x * 100UL + y * 10UL;
+}
+
+//****************************************************************************
+// hash space
+
+struct dxHashSpace : public dxSpace {
+    int global_minlevel;	// smallest hash table level to put AABBs in
+    int global_maxlevel;	// objects that need a level larger than this will be
+    // put in a "big objects" list instead of a hash table
+
+    dxHashSpace (dSpaceID _space);
+    void setLevels (int minlevel, int maxlevel);
+    void getLevels (int *minlevel, int *maxlevel);
+    void cleanGeoms();
+    void collide (void *data, dNearCallback *callback);
+    void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+};
+
+
+dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
+{
+    type = dHashSpaceClass;
+    global_minlevel = -3;
+    global_maxlevel = 10;
+}
+
+
+void dxHashSpace::setLevels (int minlevel, int maxlevel)
+{
+    dAASSERT (minlevel <= maxlevel);
+    global_minlevel = minlevel;
+    global_maxlevel = maxlevel;
+}
+
+
+void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
+{
+    if (minlevel) *minlevel = global_minlevel;
+    if (maxlevel) *maxlevel = global_maxlevel;
+}
+
+
+void dxHashSpace::cleanGeoms()
+{
+    // compute the AABBs of all dirty geoms, and clear the dirty flags
+    lock_count++;
+    for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
+        if (IS_SPACE(g)) {
+            ((dxSpace*)g)->cleanGeoms();
+        }
+
+        g->recomputeAABB();
+        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+        
+        g->gflags &= ~GEOM_DIRTY;
+    }
+    lock_count--;
+}
+
+
+void dxHashSpace::collide (void *data, dNearCallback *callback)
+{
+    dAASSERT(this && callback);
+    dxGeom *geom;
+    int i,maxlevel;
+
+    // 0 or 1 geoms can't collide with anything
+    if (count < 2) return;
+
+    lock_count++;
+    cleanGeoms();
+
+    // create a list of auxiliary information for all geom axis aligned bounding
+    // boxes. set the level for all AABBs. put AABBs larger than the space's
+    // global_maxlevel in the big_boxes list, check everything else against
+    // that list at the end. for AABBs that are not too big, record the maximum
+    // level that we need.
+
+    typedef std::vector<dxAABB> AABBlist;
+    AABBlist hash_boxes; // list of AABBs in hash table
+    AABBlist big_boxes; // list of AABBs too big for hash table
+    maxlevel = global_minlevel - 1;
+    for (geom = first; geom; geom=geom->next) {
+        if (!GEOM_ENABLED(geom)){
+            continue;
+        }
+        dxAABB aabb;
+        aabb.geom = geom;
+        // compute level, but prevent cells from getting too small
+        int level = findLevel (geom->aabb);
+        if (level < global_minlevel) level = global_minlevel;
+        if (level <= global_maxlevel) {
+            aabb.level = level;
+            if (level > maxlevel) maxlevel = level;
+            // cellsize = 2^level
+            dReal cellSizeRecip = dRecip(ldexp(REAL(1.0), level)); // No computational errors here!
+            // discretize AABB position to cell size
+            for (i=0; i < 6; i++) {
+                dReal aabbBound = geom->aabb[i] * cellSizeRecip; // No computational errors so far!
+                dICHECK(aabbBound >= dMinIntExact && aabbBound </*=*/ dMaxIntExact); // Otherwise the scene is too large for integer types used 
+
+                aabb.dbounds[i] = (int) dFloor(aabbBound);
+            }
+            // set AABB index
+            aabb.index = hash_boxes.size();
+            // aabb goes in main list
+            hash_boxes.push_back(aabb);
+        }
+        else {
+            // aabb is too big, put it in the big_boxes list. we don't care about
+            // setting level, dbounds, index, or the maxlevel
+            big_boxes.push_back(aabb);
+        }
+    }
+
+    sizeint n = hash_boxes.size(); // number of AABBs in main list
+
+    // for `n' objects, an n*n array of bits is used to record if those objects
+    // have been intersection-tested against each other yet. this array can
+    // grow large with high n, but oh well...
+    int tested_rowsize = (n+7) >> 3;	// number of bytes needed for n bits
+    std::vector<uint8> tested(n * tested_rowsize);
+
+    // create a hash table to store all AABBs. each AABB may take up to 8 cells.
+    // we use chaining to resolve collisions, but we use a relatively large table
+    // to reduce the chance of collisions.
+
+    // compute hash table size sz to be a prime > 8*n
+    for (i=0; i<NUM_PRIMES; i++) {
+        if ((sizeint)prime[i] >= (8*n)) break;
+    }
+    if (i >= NUM_PRIMES) {
+        i = NUM_PRIMES-1;	// probably pointless
+    }
+
+    const unsigned long sz = prime[i];
+
+    // allocate and initialize hash table node pointers
+    typedef std::vector<Node*> HashTable;
+    HashTable table(sz);
+
+    // add each AABB to the hash table (may need to add it to up to 8 cells)
+    const AABBlist::iterator hashend = hash_boxes.end();
+    for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) {
+        const int *dbounds = aabb->dbounds;
+        const int xend = dbounds[1];
+        for (int xi = dbounds[0]; xi <= xend; xi++) {
+            const int yend = dbounds[3];
+            for (int yi = dbounds[2]; yi <= yend; yi++) {
+                int zbegin = dbounds[4];
+                unsigned long hi = (getVirtualAddressBase(aabb->level,xi,yi) + zbegin) % sz;
+                const int zend = dbounds[5];
+                for (int zi = zbegin; zi <= zend; (hi = hi + 1U != sz ? hi + 1U : 0UL), zi++) {
+                    // get the hash index
+                    // add a new node to the hash table
+                    Node *node = new Node;
+                    node->x = xi;
+                    node->y = yi;
+                    node->z = zi;
+                    node->aabb = &*aabb;
+                    node->next = table[hi];
+                    table[hi] = node;
+                }
+            }
+        }
+    }
+
+    // now that all AABBs are loaded into the hash table, we do the actual
+    // collision detection. for all AABBs, check for other AABBs in the
+    // same cells for collisions, and then check for other AABBs in all
+    // intersecting higher level cells.
+
+    int db[6];			// discrete bounds at current level
+    for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) {
+        // we are searching for collisions with aabb
+        for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
+        for (int level = aabb->level; ; ) {
+            dIASSERT(level <= maxlevel);
+            const int xend = db[1];
+            for (int xi = db[0]; xi <= xend; xi++) {
+                const int yend = db[3];
+                for (int yi = db[2]; yi <= yend; yi++) {
+                    int zbegin = db[4];
+                    // get the hash index
+                    unsigned long hi = (getVirtualAddressBase(level, xi, yi) + zbegin) % sz;
+                    const int zend = db[5];
+                    for (int zi = zbegin; zi <= zend; (hi = hi + 1U != sz ? hi + 1U : 0UL), zi++) {
+                        // search all nodes at this index
+                        for (Node* node = table[hi]; node; node=node->next) {
+                            // node points to an AABB that may intersect aabb
+                            if (node->aabb == &*aabb)
+                                continue;
+                            if (node->aabb->level == level &&
+                                node->x == xi && node->y == yi && node->z == zi) {
+                                    // see if aabb and node->aabb have already been tested
+                                    // against each other
+                                    unsigned char mask;
+                                    if (aabb->index <= node->aabb->index) {
+                                        i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
+                                        mask = 1 << (node->aabb->index & 7);
+                                    }
+                                    else {
+                                        i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
+                                        mask = 1 << (aabb->index & 7);
+                                    }
+                                    dIASSERT (i >= 0 && (sizeint)i < (tested_rowsize*n));
+                                    if ((tested[i] & mask)==0) {
+                                        tested[i] |= mask;
+                                        collideAABBs (aabb->geom,node->aabb->geom,data,callback);
+                                    }
+                            }
+                        }
+                    }
+                }
+            }
+
+            if (level == maxlevel) {
+                break;
+            }
+            ++level;
+            // get the discrete bounds for the next level up
+            for (i=0; i<6; i++) db[i] >>= 1;
+        }
+    }
+
+    // every AABB in the normal list must now be intersected against every
+    // AABB in the big_boxes list. so let's hope there are not too many objects
+    // in the big_boxes list.
+    const AABBlist::iterator bigend = big_boxes.end();
+    for (AABBlist::iterator aabb = hash_boxes.begin(); aabb != hashend; ++aabb) {
+        for (AABBlist::iterator aabb2 = big_boxes.begin(); aabb2 != bigend; ++aabb2) {
+            collideAABBs (aabb->geom, aabb2->geom, data, callback);
+        }
+    }
+
+    // intersected all AABBs in the big_boxes list together
+    for (AABBlist::iterator aabb = big_boxes.begin(); aabb != bigend; ++aabb) {
+        AABBlist::iterator aabb2 = aabb;
+        while (++aabb2 != bigend) {
+            collideAABBs (aabb->geom, aabb2->geom, data, callback);
+        }
+    }
+
+    // deallocate table
+    const HashTable::iterator tableend = table.end();
+    for (HashTable::iterator el = table.begin(); el != tableend; ++el)
+        for (Node* node = *el; node; ) {
+            Node* next = node->next;
+            delete node;
+            node = next;
+        }
+
+    lock_count--;
+}
+
+
+void dxHashSpace::collide2 (void *data, dxGeom *geom,
+                            dNearCallback *callback)
+{
+    dAASSERT (geom && callback);
+
+    // this could take advantage of the hash structure to avoid
+    // O(n2) complexity, but it does not yet.
+
+    lock_count++;
+    cleanGeoms();
+    geom->recomputeAABB();
+
+    // intersect bounding boxes
+    for (dxGeom *g=first; g; g=g->next) {
+        if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback);
+    }
+
+    lock_count--;
+}
+
+//****************************************************************************
+// space functions
+
+dxSpace *dSimpleSpaceCreate (dxSpace *space)
+{
+    return new dxSimpleSpace (space);
+}
+
+
+dxSpace *dHashSpaceCreate (dxSpace *space)
+{
+    return new dxHashSpace (space);
+}
+
+
+void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
+{
+    dAASSERT (space);
+    dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
+    dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
+    dxHashSpace *hspace = (dxHashSpace*) space;
+    hspace->setLevels (minlevel,maxlevel);
+}
+
+
+void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
+{
+    dAASSERT (space);
+    dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
+    dxHashSpace *hspace = (dxHashSpace*) space;
+    hspace->getLevels (minlevel,maxlevel);
+}
+
+
+void dSpaceDestroy (dxSpace *space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    dGeomDestroy (space);
+}
+
+
+void dSpaceSetCleanup (dxSpace *space, int mode)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    space->setCleanup (mode);
+}
+
+
+int dSpaceGetCleanup (dxSpace *space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->getCleanup();
+}
+
+
+void dSpaceSetSublevel (dSpaceID space, int sublevel)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    space->setSublevel (sublevel);
+}
+
+
+int dSpaceGetSublevel (dSpaceID space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->getSublevel();
+}
+
+void dSpaceSetManualCleanup (dSpaceID space, int mode)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    space->setManulCleanup(mode);
+}
+
+int dSpaceGetManualCleanup (dSpaceID space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->getManualCleanup();
+}
+
+void dSpaceAdd (dxSpace *space, dxGeom *g)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    CHECK_NOT_LOCKED (space);
+    space->add (g);
+}
+
+
+void dSpaceRemove (dxSpace *space, dxGeom *g)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    CHECK_NOT_LOCKED (space);
+    space->remove (g);
+}
+
+
+int dSpaceQuery (dxSpace *space, dxGeom *g)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->query (g);
+}
+
+void dSpaceClean (dxSpace *space){
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+
+    space->cleanGeoms();
+}
+
+int dSpaceGetNumGeoms (dxSpace *space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->getNumGeoms();
+}
+
+
+dGeomID dSpaceGetGeom (dxSpace *space, int i)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->getGeom (i);
+}
+
+int dSpaceGetClass (dxSpace *space)
+{
+    dAASSERT (space);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    return space->type;
+}
+
+
+void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
+{
+    dAASSERT (space && callback);
+    dUASSERT (dGeomIsSpace(space),"argument not a space");
+    space->collide (data,callback);
+}
+
+
+struct DataCallback {
+    void *data;
+    dNearCallback *callback;
+};
+// Invokes the callback with arguments swapped
+static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
+{
+    DataCallback *dc = (DataCallback*)data;
+    dc->callback(dc->data, g2, g1);
+}
+
+
+void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
+                     dNearCallback *callback)
+{
+    dAASSERT (g1 && g2 && callback);
+    dxSpace *s1,*s2;
+
+    // see if either geom is a space
+    if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
+    if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
+
+    if (s1 && s2) {
+        int l1 = s1->getSublevel();
+        int l2 = s2->getSublevel();
+        if (l1 != l2) {
+            if (l1 > l2) {
+                s2 = 0;
+            } else {
+                s1 = 0;
+            }
+        }
+    }
+
+    // handle the four space/geom cases
+    if (s1) {
+        if (s2) {
+            // g1 and g2 are spaces.
+            if (s1==s2) {
+                // collide a space with itself --> interior collision
+                s1->collide (data,callback);
+            }
+            else {
+                // iterate through the space that has the fewest geoms, calling
+                // collide2 in the other space for each one.
+                if (s1->count < s2->count) {
+                    DataCallback dc = {data, callback};
+                    for (dxGeom *g = s1->first; g; g=g->next) {
+                        s2->collide2 (&dc,g,swap_callback);
+                    }
+                }
+                else {
+                    for (dxGeom *g = s2->first; g; g=g->next) {
+                        s1->collide2 (data,g,callback);
+                    }
+                }
+            }
+        }
+        else {
+            // g1 is a space, g2 is a geom
+            s1->collide2 (data,g2,callback);
+        }
+    }
+    else {
+        if (s2) {
+            // g1 is a geom, g2 is a space
+            DataCallback dc = {data, callback};
+            s2->collide2 (&dc,g1,swap_callback);
+        }
+        else {
+            // g1 and g2 are geoms
+            // make sure they have valid AABBs
+            g1->recomputeAABB();
+            g2->recomputeAABB();
+            collideAABBs(g1,g2, data, callback);
+        }
+    }
+}

thirdparty/ode-0.16.5/ode/src/collision_space_internal.h

@@ -0,0 +1,80 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+stuff common to all spaces
+
+*/
+
+#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_
+#define _ODE_COLLISION_SPACE_INTERNAL_H_
+
+#define ALLOCA(x) dALLOCA16(x)
+
+
+// collide two geoms together. for the hash table space, this is
+// called if the two AABBs inhabit the same hash table cells.
+// this only calls the callback function if the AABBs actually
+// intersect. if a geom has an AABB test function, that is called to
+// provide a further refinement of the intersection.
+//
+// NOTE: this assumes that the geom AABBs are valid on entry
+// and that both geoms are enabled.
+
+static inline void collideAABBs (dxGeom *g1, dxGeom *g2,
+                                 void *data, dNearCallback *callback)
+{
+    dIASSERT((g1->gflags & GEOM_AABB_BAD)==0);
+    dIASSERT((g2->gflags & GEOM_AABB_BAD)==0);
+
+    // no contacts if both geoms on the same body, and the body is not 0
+    if (g1->body == g2->body && g1->body) return;
+
+    // test if the category and collide bitfields match
+    if ( ((g1->category_bits & g2->collide_bits) ||
+        (g2->category_bits & g1->collide_bits)) == 0) {
+            return;
+    }
+
+    // if the bounding boxes are disjoint then don't do anything
+    dReal *bounds1 = g1->aabb;
+    dReal *bounds2 = g2->aabb;
+    if (bounds1[0] > bounds2[1] ||
+        bounds1[1] < bounds2[0] ||
+        bounds1[2] > bounds2[3] ||
+        bounds1[3] < bounds2[2] ||
+        bounds1[4] > bounds2[5] ||
+        bounds1[5] < bounds2[4]) {
+            return;
+    }
+
+    // check if either object is able to prove that it doesn't intersect the
+    // AABB of the other
+    if (g1->AABBTest (g2,bounds2) == 0) return;
+    if (g2->AABBTest (g1,bounds1) == 0) return;
+
+    // the objects might actually intersect - call the space callback function
+    callback (data,g1,g2);
+}
+
+#endif

thirdparty/ode-0.16.5/ode/src/collision_std.h

@@ -0,0 +1,238 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+the standard ODE geometry primitives.
+
+*/
+
+#ifndef _ODE_COLLISION_STD_H_
+#define _ODE_COLLISION_STD_H_
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+
+// primitive collision functions - these have the dColliderFn interface, i.e.
+// the same interface as dCollide(). the first and second geom arguments must
+// have the specified types.
+
+int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip);
+int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
+                       dContactGeom *contact, int skip);
+int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
+                         dContactGeom *contact, int skip);
+int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
+                    dContactGeom *contact, int skip);
+int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
+                      int flags, dContactGeom *contact, int skip);
+int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int flags,
+                           dContactGeom *contact, int skip);
+int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int flags,
+                        dContactGeom *contact, int skip);
+int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,
+                            int flags, dContactGeom *contact, int skip);
+int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip);
+int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
+                       dContactGeom *contact, int skip);
+int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
+                    dContactGeom *contact, int skip);
+int dCollideRayCapsule (dxGeom *o1, dxGeom *o2,
+                        int flags, dContactGeom *contact, int skip);
+int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
+                      dContactGeom *contact, int skip);
+int dCollideRayCylinder (dxGeom *o1, dxGeom *o2, int flags,
+                         dContactGeom *contact, int skip);
+
+// Cylinder - Box/Sphere by (C) CroTeam
+// Ported by Nguyen Binh
+int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, 
+                        int flags, dContactGeom *contact, int skip);
+int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, 
+                           int flags, dContactGeom *contact, int skip); 
+int dCollideCylinderPlane(dxGeom *gCylinder, dxGeom *gPlane, 
+                          int flags, dContactGeom *contact, int skip); 
+
+//--> Convex Collision
+int dCollideConvexPlane (dxGeom *o1, dxGeom *o2, int flags,
+                         dContactGeom *contact, int skip);
+int dCollideSphereConvex (dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip);
+int dCollideConvexBox (dxGeom *o1, dxGeom *o2, int flags,
+                       dContactGeom *contact, int skip);
+int dCollideConvexCapsule (dxGeom *o1, dxGeom *o2,
+                           int flags, dContactGeom *contact, int skip);
+int dCollideConvexConvex (dxGeom *o1, dxGeom *o2, int flags, 
+                          dContactGeom *contact, int skip);
+int dCollideRayConvex (dxGeom *o1, dxGeom *o2, int flags, 
+                       dContactGeom *contact, int skip);
+//<-- Convex Collision
+
+// dHeightfield
+int dCollideHeightfield( dxGeom *o1, dxGeom *o2, 
+                        int flags, dContactGeom *contact, int skip );
+
+//****************************************************************************
+// the basic geometry objects
+
+struct dxSphere : public dxGeom {
+    dReal radius;		// sphere radius
+    dxSphere (dSpaceID space, dReal _radius);
+    void computeAABB();
+};
+
+
+struct dxBox : public dxGeom {
+    dVector3 side;	// side lengths (x,y,z)
+    dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
+    void computeAABB();
+};
+
+
+struct dxCapsule : public dxGeom {
+    dReal radius,lz;	// radius, length along z axis
+    dxCapsule (dSpaceID space, dReal _radius, dReal _length);
+    void computeAABB();
+};
+
+
+struct dxCylinder : public dxGeom {
+    dReal radius,lz;        // radius, length along z axis
+    dxCylinder (dSpaceID space, dReal _radius, dReal _length);
+    void computeAABB();
+};
+
+
+struct dxPlane : public dxGeom {
+    dReal p[4];
+    dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
+    void computeAABB();
+};
+
+
+struct dxRay : public dxGeom {
+    dReal length;
+    dxRay (dSpaceID space, dReal _length);
+    void computeAABB();
+};
+
+struct dxConvex : public dxGeom 
+{  
+    const dReal *planes; /*!< An array of planes in the form:
+                   normal X, normal Y, normal Z,Distance
+                   */
+    const dReal *points; /*!< An array of points X,Y,Z */  
+    const unsigned int *polygons; /*! An array of indices to the points of each polygon, it should be the number of vertices followed by that amount of indices to "points" in counter clockwise order*/
+    unsigned int planecount; /*!< Amount of planes in planes */
+    unsigned int pointcount;/*!< Amount of points in points */
+    unsigned int edgecount;/*!< Amount of edges in convex */
+    dReal saabb[6];/*!< Static AABB */
+    dxConvex(dSpaceID space,
+        const dReal *planes,
+        unsigned int planecount,
+        const dReal *points,
+        unsigned int pointcount,
+        const unsigned int *polygons);
+    ~dxConvex()
+    {
+        if((edgecount!=0)&&(edges!=NULL)) delete[] edges;
+    }
+    void computeAABB();
+    struct edge
+    {
+        unsigned int first;
+        unsigned int second;
+    };
+    edge* edges;
+
+    /*! \brief A Support mapping function for convex shapes
+    \param dir [IN] direction to find the Support Point for
+    \return the index of the support vertex.
+    */
+    inline unsigned int SupportIndex(dVector3 dir)
+    {
+        dVector3 rdir;
+        unsigned int index=0;
+        dMultiply1_331 (rdir,final_posr->R,dir);
+        dReal max = dCalcVectorDot3(points,rdir);
+        dReal tmp;
+        for (unsigned int i = 1; i < pointcount; ++i) 
+        {
+            tmp = dCalcVectorDot3(points+(i*3),rdir);
+            if (tmp > max) 
+            {
+                index=i;
+                max = tmp; 
+            }
+        }
+        return index;
+    }
+
+private:
+    // For Internal Use Only
+    /*! \brief Fills the edges dynamic array based on points and polygons.
+    */
+    void FillEdges();
+#if 0
+    /*
+    What this does is the same as the Support function by doing some preprocessing
+    for optimization. Not complete yet.
+    */
+    // Based on Eberly's Game Physics Book page 307
+    struct Arc
+    {
+        // indices of polyhedron normals that form the spherical arc
+        int normals[2];
+        // index of edge shared by polyhedron faces
+        int edge;
+    };
+    struct Polygon
+    {
+        // indices of polyhedron normals that form the spherical polygon
+        std::vector<int> normals;
+        // index of extreme vertex corresponding to this polygon
+        int vertex;
+    };
+    // This is for extrem feature query and not the usual level BSP structure (that comes later)
+    struct BSPNode
+    {
+        // Normal index (interior node), vertex index (leaf node)
+        int normal;
+        // if Dot (E,D)>=0, D gets propagated to this child
+        BSPNode* right;
+        // if Dot (E,D)<0, D gets propagated to this child
+        BSPNode* left;
+    };
+    void CreateTree();
+    BSPNode* CreateNode(std::vector<Arc> Arcs,std::vector<Polygon> Polygons);
+    void GetFacesSharedByVertex(int i, std::vector<int> f);
+    void GetFacesSharedByEdge(int i, int* f);
+    void GetFaceNormal(int i, dVector3 normal);
+    BSPNode* tree;
+#endif
+};
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/collision_transform.cpp

@@ -0,0 +1,234 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+geom transform
+
+*/
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_transform.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// dxGeomTransform class
+
+struct dxGeomTransform : public dxGeom {
+    dxGeom *obj;		// object that is being transformed
+    int cleanup;		// 1 to destroy obj when destroyed
+    int infomode;		// 1 to put Tx geom in dContactGeom g1
+
+    // cached final object transform (body tx + relative tx). this is set by
+    // computeAABB(), and it is valid while the AABB is valid.
+    dxPosR transform_posr;
+
+    dxGeomTransform (dSpaceID space);
+    ~dxGeomTransform();
+    void computeAABB();
+    void computeFinalTx();
+};
+/*
+void RunMe()
+{
+printf("sizeof body = %i\n", sizeof(dxBody));
+printf("sizeof geom = %i\n", sizeof(dxGeom));
+printf("sizeof geomtransform = %i\n", sizeof(dxGeomTransform));
+printf("sizeof posr = %i\n", sizeof(dxPosR));
+}
+*/
+
+dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
+{
+    type = dGeomTransformClass;
+    obj = 0;
+    cleanup = 0;
+    infomode = 0;
+    dSetZero (transform_posr.pos,4);
+    dRSetIdentity (transform_posr.R);
+}
+
+
+dxGeomTransform::~dxGeomTransform()
+{
+    if (obj && cleanup) delete obj;
+}
+
+
+void dxGeomTransform::computeAABB()
+{
+    if (!obj) {
+        dSetZero (aabb,6);
+        return;
+    }
+
+    // backup the relative pos and R pointers of the encapsulated geom object
+    dxPosR* posr_bak = obj->final_posr;
+
+    // compute temporary pos and R for the encapsulated geom object
+    computeFinalTx();
+    obj->final_posr = &transform_posr;
+
+    // compute the AABB
+    obj->computeAABB();
+    memcpy (aabb,obj->aabb,6*sizeof(dReal));
+
+    // restore the pos and R
+    obj->final_posr = posr_bak;
+}
+
+
+// utility function for dCollideTransform() : compute final pos and R
+// for the encapsulated geom object
+
+void dxGeomTransform::computeFinalTx()
+{
+    dMultiply0_331 (transform_posr.pos,final_posr->R,obj->final_posr->pos);
+    transform_posr.pos[0] += final_posr->pos[0];
+    transform_posr.pos[1] += final_posr->pos[1];
+    transform_posr.pos[2] += final_posr->pos[2];
+    dMultiply0_333 (transform_posr.R,final_posr->R,obj->final_posr->R);
+}
+
+//****************************************************************************
+// collider function:
+// this collides a transformed geom with another geom. the other geom can
+// also be a transformed geom, but this case is not handled specially.
+
+int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
+                       dContactGeom *contact, int skip)
+{
+    dIASSERT (skip >= (int)sizeof(dContactGeom));
+    dIASSERT (o1->type == dGeomTransformClass);
+
+    dxGeomTransform *tr = (dxGeomTransform*) o1;
+    if (!tr->obj) return 0;
+    dUASSERT (tr->obj->parent_space==0,
+        "GeomTransform encapsulated object must not be in a space");
+    dUASSERT (tr->obj->body==0,
+        "GeomTransform encapsulated object must not be attached "
+        "to a body");
+
+    // backup the relative pos and R pointers of the encapsulated geom object,
+    // and the body pointer
+    dxPosR *posr_bak = tr->obj->final_posr;
+    dxBody *bodybak = tr->obj->body;
+
+    // compute temporary pos and R for the encapsulated geom object.
+    // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
+    // because computeFinalTx() will have already been called in
+    // dxGeomTransform::computeAABB()
+
+    if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
+    tr->obj->final_posr = &tr->transform_posr;
+    tr->obj->body = o1->body;
+
+    // do the collision
+    int n = dCollide (tr->obj,o2,flags,contact,skip);
+
+    // if required, adjust the 'g1' values in the generated contacts so that
+    // thay indicated the GeomTransform object instead of the encapsulated
+    // object.
+    if (tr->infomode) {
+        for (int i=0; i<n; i++) {
+            dContactGeom *c = CONTACT(contact,skip*i);
+            c->g1 = o1;
+        }
+    }
+
+    // restore the pos, R and body
+    tr->obj->final_posr = posr_bak;
+    tr->obj->body = bodybak;
+    return n;
+}
+
+//****************************************************************************
+// public API
+
+dGeomID dCreateGeomTransform (dSpaceID space)
+{
+    return new dxGeomTransform (space);
+}
+
+
+void dGeomTransformSetGeom (dGeomID g, dGeomID obj)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    if (tr->obj && tr->cleanup) delete tr->obj;
+    tr->obj = obj;
+}
+
+
+dGeomID dGeomTransformGetGeom (dGeomID g)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    return tr->obj;
+}
+
+
+void dGeomTransformSetCleanup (dGeomID g, int mode)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    tr->cleanup = mode;
+}
+
+
+int dGeomTransformGetCleanup (dGeomID g)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    return tr->cleanup;
+}
+
+
+void dGeomTransformSetInfo (dGeomID g, int mode)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    tr->infomode = mode;
+}
+
+
+int dGeomTransformGetInfo (dGeomID g)
+{
+    dUASSERT (g && g->type == dGeomTransformClass,
+        "argument not a geom transform");
+    dxGeomTransform *tr = (dxGeomTransform*) g;
+    return tr->infomode;
+}
+

thirdparty/ode-0.16.5/ode/src/collision_transform.h

@@ -0,0 +1,39 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+geom transform
+
+*/
+
+#ifndef _ODE_COLLISION_TRANSFORM_H_
+#define _ODE_COLLISION_TRANSFORM_H_
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+
+int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/collision_trimesh_colliders.h

@@ -0,0 +1,47 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_COLLISION_TRIMESH_COLLIDERS_H_
+#define _ODE_COLLISION_TRIMESH_COLLIDERS_H_
+
+
+int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTrimeshPlane(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideConvexTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+ODE_PURE_INLINE int dCollideRayTrimesh( dxGeom *ray, dxGeom *trimesh, int flags,
+                                       dContactGeom *contact, int skip )
+{
+    // Swapped case, for code that needs it (heightfield initially)
+    // The other ray-geom colliders take geoms in a swapped order to the
+    // dCollideRTL function which is annoying when using function pointers.
+    return dCollideRTL( trimesh, ray, flags, contact, skip );
+}
+
+
+#endif // _ODE_COLLISION_TRIMESH_COLLIDERS_H_

thirdparty/ode-0.16.5/ode/src/collision_trimesh_disabled.cpp

@@ -0,0 +1,302 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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 <ode/collision.h>
+#include "config.h"
+#include "matrix.h"
+
+
+#if !dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_trimesh_internal.h"
+
+
+static const dMatrix4 identity = 
+{
+    REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ),
+    REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ),
+    REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ),
+    REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ), REAL( 0.0 ) 
+};
+
+
+typedef dxMeshBase dxDisabledTriMesh_Parent;
+struct dxDisabledTriMesh: 
+    public dxDisabledTriMesh_Parent
+{
+public:
+    // Functions
+    dxDisabledTriMesh(dxSpace *Space, 
+        dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback):
+        dxDisabledTriMesh_Parent(Space, NULL, Callback, ArrayCallback, RayCallback, false)
+    {
+    }
+
+    virtual void computeAABB(); // This is an abstract method in the base class
+};
+
+/*virtual */
+void dxDisabledTriMesh::computeAABB()
+{
+    // Do nothing
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Stub functions for trimesh calls
+
+/*extern */
+dTriMeshDataID dGeomTriMeshDataCreate(void)
+{
+    return NULL;
+}
+
+/*extern */
+void dGeomTriMeshDataDestroy(dTriMeshDataID g)
+{
+    // Do nothing
+}
+
+
+/*extern */
+void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
+{
+    // Do nothing
+}
+
+/*extern */
+void *dGeomTriMeshDataGet(dTriMeshDataID g, int data_id)
+{
+    return NULL;
+}
+
+/*extern */
+void *dGeomTriMeshDataGet2(dTriMeshDataID g, int data_id, sizeint *pout_size/*=NULL*/)
+{
+    if (pout_size != NULL)
+    {
+        *pout_size = 0;
+    }
+
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshSetLastTransform( dGeomID g, const dMatrix4 last_trans )
+{
+    // Do nothing
+}
+
+/*extern */
+const dReal *dGeomTriMeshGetLastTransform( dGeomID g )
+{
+    return identity;
+}
+
+
+/*extern */
+dGeomID dCreateTriMesh(dSpaceID space, 
+    dTriMeshDataID Data,
+    dTriCallback* Callback,
+    dTriArrayCallback* ArrayCallback,
+    dTriRayCallback* RayCallback)
+{
+    return new dxDisabledTriMesh(space, Callback, ArrayCallback, RayCallback); // Oleh_Derevenko: I'm not sure if a NULL can be returned here -- keep on returning an object for backward compatibility
+}
+
+
+/*extern */
+void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
+{
+    // Do nothing
+}
+
+/*extern */
+dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride)
+{
+    // Do nothing
+}
+
+/*extern */
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    // Do nothing
+}
+
+/*extern */
+void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, 
+    const void* Vertices,  int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride)
+{
+    // Do nothing
+}
+
+/*extern */
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, 
+    const void* Vertices,  int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    // Do nothing
+}
+
+/*extern */
+void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
+    const dReal* Vertices, int VertexCount,
+    const dTriIndex* Indices, int IndexCount)
+{
+    // Do nothing
+}
+
+/*extern */
+void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
+    const dReal* Vertices, int VertexCount,
+    const dTriIndex* Indices, int IndexCount,
+    const int* Normals)
+{
+    // Do nothing
+}
+
+
+/*extern ODE_API */
+int dGeomTriMeshDataPreprocess(dTriMeshDataID g)
+{
+    // Do nothing
+    return 1;
+}
+
+/*extern ODE_API */
+int dGeomTriMeshDataPreprocess2(dTriMeshDataID g, unsigned int buildRequestFlags, const intptr *requestExtraData/*=NULL | const intptr (*)[dTRIDATAPREPROCESS_BUILD__MAX]*/)
+{
+    // Do nothing
+    return 1;
+}
+
+/*extern */
+void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
+{
+    // Do nothing
+}
+
+/*extern */
+dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
+{
+    // Do nothing
+}
+
+/*extern */
+dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
+{
+    // Do nothing
+}
+
+/*extern */
+dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshSetTriMergeCallback(dGeomID g, dTriTriMergeCallback* Callback)
+{
+    // Do nothing
+}
+
+/*extern */
+dTriTriMergeCallback* dGeomTriMeshGetTriMergeCallback(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
+{
+    // Do nothing
+}
+
+/*extern */
+int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
+{
+    return 0;
+}
+
+
+/*extern */
+void dGeomTriMeshClearTCCache(dGeomID g)
+{
+    // Do nothing
+}
+
+
+/*extern */
+dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g)
+{
+    return NULL;
+}
+
+
+/*extern */
+int dGeomTriMeshGetTriangleCount (dGeomID g)
+{
+    return 0;
+}
+
+/*extern */
+void dGeomTriMeshDataUpdate(dTriMeshDataID g)
+{
+    // Do nothing
+}
+
+
+#endif // !dTRIMESH_ENABLED
+
+

thirdparty/ode-0.16.5/ode/src/collision_trimesh_gimpact.cpp

@@ -0,0 +1,424 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2017
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "util.h"
+
+
+#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+
+#include "collision_util.h"
+#include "collision_trimesh_gimpact.h"
+#include "collision_trimesh_internal_impl.h"
+
+
+//////////////////////////////////////////////////////////////////////////
+// dxTriMeshData
+
+bool dxTriMeshData::preprocessData(bool /*buildUseFlags*//*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/)
+{
+    FaceAngleStorageMethod faceAndgesRequirementToUse = faceAndgesRequirement;
+
+    if (faceAndgesRequirement != ASM__INVALID && haveFaceAnglesBeenBuilt())
+    {
+        dUASSERT(false, "Another request to build face angles after they had already been built");
+
+        faceAndgesRequirementToUse = ASM__INVALID;
+    }
+
+    // If this mesh has already been preprocessed, exit
+    bool result = faceAndgesRequirementToUse == ASM__INVALID || retrieveTriangleCount() == 0 
+        || meaningfulPreprocessData(faceAndgesRequirementToUse);
+    return result;
+}
+
+struct TrimeshDataVertexIndexAccessor_GIMPACT
+{
+    enum
+    {
+        TRIANGLEINDEX_STRIDE = dxTriMesh::TRIANGLEINDEX_STRIDE,
+    };
+
+    explicit TrimeshDataVertexIndexAccessor_GIMPACT(dxTriMeshData *meshData):
+        m_TriangleVertexIndices(meshData->retrieveTriangleVertexIndices())
+    {
+        dIASSERT(meshData->retrieveTriangleStride() == TRIANGLEINDEX_STRIDE);
+    }
+
+    void getTriangleVertexIndices(unsigned out_VertexIndices[dMTV__MAX], unsigned triangleIdx) const
+    {
+        const GUINT32 *triIndicesBegin = m_TriangleVertexIndices;
+        const unsigned triStride = TRIANGLEINDEX_STRIDE;
+
+        const GUINT32 *triIndicesOfInterest = (const GUINT32 *)((const uint8 *)triIndicesBegin + (sizeint)triangleIdx * triStride);
+        std::copy(triIndicesOfInterest, triIndicesOfInterest + dMTV__MAX, out_VertexIndices);
+    }
+
+    const GUINT32           *m_TriangleVertexIndices;
+};
+
+struct TrimeshDataTrianglePointAccessor_GIMPACT
+{
+    enum
+    {
+        VERTEXINSTANCE_STRIDE = dxTriMesh::VERTEXINSTANCE_STRIDE,
+        TRIANGLEINDEX_STRIDE = dxTriMesh::TRIANGLEINDEX_STRIDE,
+    };
+
+    TrimeshDataTrianglePointAccessor_GIMPACT(dxTriMeshData *meshData):
+        m_VertexInstances(meshData->retrieveVertexInstances()),
+        m_TriangleVertexIndices(meshData->retrieveTriangleVertexIndices())
+    {
+        dIASSERT((unsigned)meshData->retrieveVertexStride() == (unsigned)VERTEXINSTANCE_STRIDE);
+        dIASSERT((unsigned)meshData->retrieveTriangleStride() == (unsigned)TRIANGLEINDEX_STRIDE);
+    }
+
+    void getTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex) const
+    {
+        dxTriMeshData::retrieveTriangleVertexPoints(out_Points, triangleIndex, 
+            &m_VertexInstances[0][0], VERTEXINSTANCE_STRIDE, m_TriangleVertexIndices, TRIANGLEINDEX_STRIDE);
+    }
+
+    const vec3f             *m_VertexInstances;
+    const GUINT32           *m_TriangleVertexIndices;
+};
+
+bool dxTriMeshData::meaningfulPreprocessData(FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/)
+{
+    const bool buildFaceAngles = true; dIASSERT(faceAndgesRequirement != ASM__INVALID);
+    // dIASSERT(buildFaceAngles);
+    dIASSERT(/*!buildFaceAngles || */!haveFaceAnglesBeenBuilt());
+
+    bool result = false;
+
+    bool anglesAllocated = false;
+
+    do 
+    {
+        if (buildFaceAngles)
+        {
+            if (!allocateFaceAngles(faceAndgesRequirement))
+            {
+                break;
+            }
+        }
+
+        anglesAllocated = true;
+
+        const unsigned int numTris = retrieveTriangleCount();
+        const unsigned int numVertices = retrieveVertexCount();
+        sizeint numEdges = (sizeint)numTris * dMTV__MAX;
+        dIASSERT(numVertices <= numEdges); // Edge records are going to be used for vertex data as well
+
+        const sizeint recordsMemoryRequired = dEFFICIENT_SIZE(numEdges * sizeof(EdgeRecord));
+        const sizeint verticesMemoryRequired = /*dEFFICIENT_SIZE*/(numVertices * sizeof(VertexRecord)); // Skip alignment for the last chunk
+        const sizeint totalTempMemoryRequired = recordsMemoryRequired + verticesMemoryRequired;
+        void *tempBuffer = dAlloc(totalTempMemoryRequired);
+
+        if (tempBuffer == NULL)
+        {
+            break;
+        }
+
+        EdgeRecord *edges = (EdgeRecord *)tempBuffer;
+        VertexRecord *vertices = (VertexRecord *)((uint8 *)tempBuffer + recordsMemoryRequired);
+
+        TrimeshDataVertexIndexAccessor_GIMPACT indexAccessor(this);
+        meaningfulPreprocess_SetupEdgeRecords(edges, numEdges, indexAccessor);
+
+        // Sort the edges, so the ones sharing the same verts are beside each other
+        std::sort(edges, edges + numEdges);
+
+        TrimeshDataTrianglePointAccessor_GIMPACT pointAccessor(this);
+        const dReal *const externalNormals = retrieveNormals();
+        IFaceAngleStorageControl *faceAngles = retrieveFaceAngles();
+        meaningfulPreprocess_buildEdgeFlags(NULL, faceAngles, edges, numEdges, vertices, externalNormals, pointAccessor);
+
+        dFree(tempBuffer, totalTempMemoryRequired);
+
+        result = true;
+    }
+    while (false);
+
+    if (!result)
+    {
+        if (anglesAllocated)
+        {
+            if (buildFaceAngles)
+            {
+                freeFaceAngles();
+            }
+        }
+    }
+
+    return result;
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Trimesh
+
+dxTriMesh::~dxTriMesh()
+{
+    //Terminate Trimesh
+    gim_trimesh_destroy(&m_collision_trimesh);
+    gim_terminate_buffer_managers(m_buffer_managers);
+}
+
+
+/*virtual */
+void dxTriMesh::computeAABB()
+{
+    //update trimesh transform
+    mat4f transform;
+    IDENTIFY_MATRIX_4X4(transform);
+    MakeMatrix(this, transform);
+    gim_trimesh_set_tranform(&m_collision_trimesh, transform);
+
+    //Update trimesh boxes
+    gim_trimesh_update(&m_collision_trimesh);
+
+    GIM_AABB_COPY( &m_collision_trimesh.m_aabbset.m_global_bound, aabb );
+}
+
+
+void dxTriMesh::assignMeshData(dxTriMeshData *Data)
+{
+    // GIMPACT only supports stride 12, so we need to catch the error early.
+    dUASSERT(
+        (unsigned int)Data->retrieveVertexStride() == (unsigned)VERTEXINSTANCE_STRIDE 
+        && (unsigned int)Data->retrieveTriangleStride() == (unsigned)TRIANGLEINDEX_STRIDE,
+        "Gimpact trimesh only supports a stride of 3 float/int\n"
+        "This means that you cannot use dGeomTriMeshDataBuildSimple() with Gimpact.\n"
+        "Change the stride, or use Opcode trimeshes instead.\n"
+    );
+
+    dxTriMesh_Parent::assignMeshData(Data);
+
+    //Create trimesh
+    const vec3f *vertexInstances = Data->retrieveVertexInstances();
+    if ( vertexInstances != NULL )
+    {
+        const GUINT32 *triangleVertexIndices = Data->retrieveTriangleVertexIndices();
+
+        sizeint vertexInstanceCount = Data->retrieveVertexCount();
+        sizeint triangleVertexCount = (sizeint)Data->retrieveTriangleCount() * dMTV__MAX;
+
+        gim_trimesh_create_from_data(
+            m_buffer_managers,
+            &m_collision_trimesh,                           // gimpact mesh
+            const_cast<vec3f *>(vertexInstances),           // vertices
+            dCAST_TO_SMALLER(GUINT32, vertexInstanceCount), // nr of verts
+            0,                                              // copy verts?
+            const_cast<GUINT32 *>(triangleVertexIndices),   // indices
+            dCAST_TO_SMALLER(GUINT32, triangleVertexCount), // nr of indices
+            0,                                              // copy indices?
+            1                                               // transformed reply
+        );
+    }
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern */
+dTriMeshDataID dGeomTriMeshDataCreate()
+{
+    return new dxTriMeshData();
+}
+
+/*extern */
+void dGeomTriMeshDataDestroy(dTriMeshDataID g)
+{
+    dxTriMeshData *data = g;
+    delete data;
+}
+
+/*extern */
+void dGeomTriMeshDataSet(dTriMeshDataID g, int dataId, void *pDataLocation) 
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    dxTriMeshData *data = g;
+
+    switch (dataId)
+    {
+        case dTRIMESHDATA_FACE_NORMALS:
+        {
+            data->assignNormals((const dReal *)pDataLocation);
+            break;
+        }
+
+        case dTRIMESHDATA_USE_FLAGS: // Not used for GIMPACT
+        {
+            break;
+        }
+
+        // case dTRIMESHDATA__MAX: -- To be located by Find in Files
+        default:
+        {
+            dUASSERT(dataId, "invalid data type");
+            break;
+        }
+    }
+}
+
+static void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) ;
+
+/*extern */
+void *dGeomTriMeshDataGet(dTriMeshDataID g, int dataId) 
+{
+    return geomTriMeshDataGet(g, dataId, NULL);
+}
+
+/*extern */
+void *dGeomTriMeshDataGet2(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) 
+{
+    return geomTriMeshDataGet(g, dataId, pOutDataSize);
+}
+
+static 
+void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize) 
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    const dxTriMeshData *data = g;
+
+    void *result = NULL;
+
+    switch (dataId)
+    {
+        case dTRIMESHDATA_FACE_NORMALS:
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = data->calculateNormalsMemoryRequirement();
+            }
+
+            result = (void *)data->retrieveNormals();
+            break;
+        }
+
+        case dTRIMESHDATA_USE_FLAGS: // Not not used for GIMPACT
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = 0;
+            }
+
+            break;
+        }
+
+        // case dTRIMESHDATA__MAX: -- To be located by Find in Files
+        default:
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = 0;
+            }
+
+            dUASSERT(dataId, "invalid data type");
+            break;
+        }
+    }
+
+    return result;
+}
+
+/*extern */
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount,
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+    dAASSERT(Vertices);
+    dAASSERT(Indices);
+
+    dxTriMeshData *data = g;
+
+    data->buildData(Vertices, VertexStride, VertexCount,
+        Indices, IndexCount, TriStride,
+        Normals,
+        true);
+}
+
+/*extern */
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount,
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+    dAASSERT(Vertices);
+    dAASSERT(Indices);
+
+    dxTriMeshData *data = g;
+
+    data->buildData(Vertices, VertexStride, VertexCount,
+        Indices, IndexCount, TriStride,
+        Normals,
+        false);
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern */
+dGeomID dCreateTriMesh(dSpaceID space,
+    dTriMeshDataID Data,
+    dTriCallback* Callback,
+    dTriArrayCallback* ArrayCallback,
+    dTriRayCallback* RayCallback)
+{
+    dxTriMesh *mesh = new dxTriMesh(space, Data, Callback, ArrayCallback, RayCallback);
+    return mesh;
+}
+
+
+/*extern */
+void dGeomTriMeshSetLastTransform(dGeomID g, const dMatrix4 last_trans ) 
+{
+    dAASSERT(g);
+    dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh");
+
+    //stub
+}
+
+/*extern */
+const dReal *dGeomTriMeshGetLastTransform(dGeomID g)
+{
+    dAASSERT(g);
+    dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh");
+
+    return NULL; // stub
+}
+
+
+#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+

thirdparty/ode-0.16.5/ode/src/collision_trimesh_gimpact.h

@@ -0,0 +1,278 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
+// Trimesh caches separation by Oleh Derevenko
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2024
+
+
+#ifndef _ODE_COLLISION_TRIMESH_GIMPACT_H_
+#define _ODE_COLLISION_TRIMESH_GIMPACT_H_
+
+
+#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+
+
+//****************************************************************************
+// dxTriMesh class
+
+
+#include "collision_kernel.h"
+#include "collision_trimesh_colliders.h"
+#include "collision_util.h"
+#include <ode/collision_trimesh.h>
+
+#include "collision_trimesh_internal.h"
+#include <GIMPACT/gimpact.h>
+
+
+struct TrimeshCollidersCache // Required for compatibility with OPCODE
+{
+};
+
+
+typedef dxTriDataBase dxTriMeshData_Parent;
+struct dxTriMeshData:
+    public dxTriMeshData_Parent
+{
+public:
+    dxTriMeshData():
+        dxTriMeshData_Parent()
+    {
+    }
+
+    ~dxTriMeshData() { /* Do nothing */ }
+
+    using dxTriMeshData_Parent::buildData;
+    
+    /* Setup the UseFlags array and/or build face angles*/
+    bool preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
+    
+private:
+    bool meaningfulPreprocessData(FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
+
+public:
+    /* For when app changes the vertices */
+    void updateData() { /* Do nothing */ }
+
+public:
+    const vec3f *retrieveVertexInstances() const { return (const vec3f *)dxTriMeshData_Parent::retrieveVertexInstances(); }
+    const GUINT32 *retrieveTriangleVertexIndices() const { return (const GUINT32 *)dxTriMeshData_Parent::retrieveTriangleVertexIndices(); }
+
+public:
+    void assignNormals(const dReal *normals) { dxTriMeshData_Parent::assignNormals(normals); }
+    const dReal *retrieveNormals() const { return (const dReal *)dxTriMeshData_Parent::retrieveNormals(); }
+    sizeint calculateNormalsMemoryRequirement() const { return retrieveTriangleCount() * (sizeof(dReal) * dSA__MAX); }
+};
+
+
+
+#ifdef dDOUBLE
+// To use GIMPACT with doubles, we need to patch a couple of the GIMPACT functions to 
+// convert arguments to floats before sending them in
+
+
+/// Convert an gimpact vec3f to a ODE dVector3d:   dVector3[i] = vec3f[i]
+#define dVECTOR3_VEC3F_COPY(b,a) { \
+    (b)[0] = (a)[0];              \
+    (b)[1] = (a)[1];              \
+    (b)[2] = (a)[2];              \
+    (b)[3] = 0;                   \
+}
+
+static inline 
+void gim_trimesh_get_triangle_verticesODE(GIM_TRIMESH * trimesh, GUINT32 triangle_index, dVector3 v1, dVector3 v2, dVector3 v3)
+{
+    vec3f src1, src2, src3;
+    GREAL *psrc1 = v1 != NULL ? src1 : NULL;
+    GREAL *psrc2 = v2 != NULL ? src2 : NULL;
+    GREAL *psrc3 = v3 != NULL ? src3 : NULL;
+    gim_trimesh_get_triangle_vertices(trimesh, triangle_index, psrc1, psrc2, psrc3);
+
+    if (v1 != NULL)
+    {
+        dVECTOR3_VEC3F_COPY(v1, src1);
+    }
+
+    if (v2 != NULL)
+    {
+        dVECTOR3_VEC3F_COPY(v2, src2);
+    }
+
+    if (v3 != NULL)
+    {
+        dVECTOR3_VEC3F_COPY(v3, src3);
+    }
+}
+
+// Anything calling gim_trimesh_get_triangle_vertices from within ODE 
+// should be patched through to the dDOUBLE version above
+
+#define gim_trimesh_get_triangle_vertices gim_trimesh_get_triangle_verticesODE
+
+static inline 
+int gim_trimesh_ray_closest_collisionODE( GIM_TRIMESH *mesh, dVector3 origin, dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact )
+{
+    vec3f dir_vec3f    = { (GREAL)dir[ 0 ],    (GREAL)dir[ 1 ],    (GREAL)dir[ 2 ]    };
+    vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] };
+
+    return gim_trimesh_ray_closest_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact );
+}
+
+static inline 
+int gim_trimesh_ray_collisionODE( GIM_TRIMESH *mesh, const dVector3 origin, const dVector3 dir, dReal tmax, GIM_TRIANGLE_RAY_CONTACT_DATA *contact )
+{
+    vec3f dir_vec3f    = { (GREAL)dir[ 0 ],    (GREAL)dir[ 1 ],    (GREAL)dir[ 2 ]    };
+    vec3f origin_vec3f = { (GREAL)origin[ 0 ], (GREAL)origin[ 1 ], (GREAL)origin[ 2 ] };
+
+    return gim_trimesh_ray_collision( mesh, origin_vec3f, dir_vec3f, (GREAL)tmax, contact );
+}
+
+static inline 
+void gim_trimesh_sphere_collisionODE( GIM_TRIMESH *mesh, const dVector3 Position, dReal Radius, GDYNAMIC_ARRAY *contact )
+{
+    vec3f pos_vec3f = { (GREAL)Position[ 0 ], (GREAL)Position[ 1 ], (GREAL)Position[ 2 ] };
+    gim_trimesh_sphere_collision( mesh, pos_vec3f, (GREAL)Radius, contact );
+}
+
+static inline 
+void gim_trimesh_plane_collisionODE( GIM_TRIMESH *mesh, const dVector4 plane, GDYNAMIC_ARRAY *contact )
+{
+    vec4f plane_vec4f = { (GREAL)plane[ 0 ], (GREAL)plane[ 1 ], (GREAL)plane[ 2 ], (GREAL)plane[ 3 ] }; \
+    gim_trimesh_plane_collision( mesh, plane_vec4f, contact );	    \
+}
+
+#define GIM_AABB_COPY( src, dst ) {		\
+    (dst)[ 0 ]= (src) -> minX;			\
+    (dst)[ 1 ]= (src) -> maxX;			\
+    (dst)[ 2 ]= (src) -> minY;			\
+    (dst)[ 3 ]= (src) -> maxY;			\
+    (dst)[ 4 ]= (src) -> minZ;			\
+    (dst)[ 5 ]= (src) -> maxZ;			\
+}
+
+
+#else // #ifdef !dDOUBLE
+
+// With single precision, we can pass native ODE vectors directly to GIMPACT
+
+#define gim_trimesh_ray_closest_collisionODE 	gim_trimesh_ray_closest_collision
+#define gim_trimesh_ray_collisionODE 			gim_trimesh_ray_collision
+#define gim_trimesh_sphere_collisionODE 		gim_trimesh_sphere_collision
+#define gim_trimesh_plane_collisionODE 			gim_trimesh_plane_collision
+
+#define GIM_AABB_COPY( src, dst ) 	memcpy( dst, src, 6 * sizeof( GREAL ) )
+
+
+#endif // #ifdef !dDOUBLE
+
+
+typedef dxMeshBase dxTriMesh_Parent;
+struct dxTriMesh: 
+    public dxTriMesh_Parent
+{
+public:
+    // Functions
+    dxTriMesh(dxSpace *Space, dxTriMeshData *Data,
+        dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback):
+        dxTriMesh_Parent(Space, NULL, Callback, ArrayCallback, RayCallback, true) // TC has speed/space 'issues' that don't make it a clear win by default on spheres/boxes.
+    {
+        gim_init_buffer_managers(m_buffer_managers);
+        assignMeshData(Data);
+    }
+
+    ~dxTriMesh();
+
+    void clearTCCache() { /* do nothing */ }
+
+    virtual void computeAABB();
+
+public:
+    dxTriMeshData *retrieveMeshData() const { return getMeshData(); }
+
+    unsigned getMeshTriangleCount() const { return gim_trimesh_get_triangle_count(const_cast<GIM_TRIMESH *>(&m_collision_trimesh)); }
+
+    void fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index)
+    {
+        gim_trimesh_locks_work_data(&m_collision_trimesh);
+        gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, *pout_triangle[0], *pout_triangle[1], *pout_triangle[2]);
+        gim_trimesh_unlocks_work_data(&m_collision_trimesh);
+    }
+
+    void fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index)
+    {
+        gim_trimesh_locks_work_data(&m_collision_trimesh);
+        gim_trimesh_get_triangle_vertices(&m_collision_trimesh, (GUINT32)index, out_triangle[0], out_triangle[1], out_triangle[2]);
+        gim_trimesh_unlocks_work_data(&m_collision_trimesh);
+    }
+
+private:
+    dxTriMeshData *getMeshData() const { return static_cast<dxTriMeshData *>(dxTriMesh_Parent::getMeshData()); }
+
+public:
+    enum
+    {
+        VERTEXINSTANCE_STRIDE = sizeof(vec3f),
+        TRIANGLEINDEX_STRIDE = sizeof(GUINT32) * dMTV__MAX,
+    };
+
+    void assignMeshData(dxTriMeshData *Data);
+
+public:
+    GIM_TRIMESH  m_collision_trimesh;
+    GBUFFER_MANAGER_DATA m_buffer_managers[G_BUFFER_MANAGER__MAX];
+};
+
+
+static inline 
+void MakeMatrix(const dVector3 position, const dMatrix3 rotation, mat4f m)
+{
+    m[0][0] = (GREAL)rotation[dM3E_XX];
+    m[0][1] = (GREAL)rotation[dM3E_XY];
+    m[0][2] = (GREAL)rotation[dM3E_XZ];
+
+    m[1][0] = (GREAL)rotation[dM3E_YX];
+    m[1][1] = (GREAL)rotation[dM3E_YY];
+    m[1][2] = (GREAL)rotation[dM3E_YZ];
+
+    m[2][0] = (GREAL)rotation[dM3E_ZX];
+    m[2][1] = (GREAL)rotation[dM3E_ZY];
+    m[2][2] = (GREAL)rotation[dM3E_ZZ];
+
+    m[0][3] = (GREAL)position[dV3E_X];
+    m[1][3] = (GREAL)position[dV3E_Y];
+    m[2][3] = (GREAL)position[dV3E_Z];
+}
+
+static inline 
+void MakeMatrix(dxGeom *g, mat4f m)
+{
+    const dVector3 &position = g->buildUpdatedPosition();
+    const dMatrix3 &rotation = g->buildUpdatedRotation();
+    MakeMatrix(position, rotation, m);
+}
+
+
+#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+
+#endif	//_ODE_COLLISION_TRIMESH_GIMPACT_H_

thirdparty/ode-0.16.5/ode/src/collision_trimesh_internal.cpp

@@ -0,0 +1,804 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2024
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_trimesh_internal.h"
+#include "odeou.h"
+
+#include <algorithm>
+
+
+
+//////////////////////////////////////////////////////////////////////////
+
+enum EdgeStorageSignInclusion
+{
+    SSI__MIN,
+
+    SSI_SIGNED_STORED = SSI__MIN,
+    SSI_POSITIVE_STORED,
+
+    SSI__MAX,
+};
+
+template<typename TStorageType, EdgeStorageSignInclusion t_SignInclusion>
+class FaceAngleStorageCodec;
+
+template<typename TStorageType>
+class FaceAngleStorageCodec<TStorageType, SSI_SIGNED_STORED>
+{
+public:
+    typedef typename _make_signed<TStorageType>::type storage_type;
+    enum
+    {
+        STORAGE_TYPE_MAX = (typename _make_unsigned<TStorageType>::type)(~(typename _make_unsigned<TStorageType>::type)0) >> 1,
+    };
+
+    static bool areNegativeAnglesCoded()
+    {
+        return true;
+    }
+
+    static storage_type encodeForStorage(dReal angleValue)
+    {
+        unsigned angleAsInt = (unsigned)dFloor(dFabs(angleValue) * (dReal)(STORAGE_TYPE_MAX / M_PI));
+        unsigned limitedAngleAsInt = dMACRO_MIN(angleAsInt, STORAGE_TYPE_MAX);
+        storage_type result = angleValue < REAL(0.0) ? -(storage_type)limitedAngleAsInt : (storage_type)limitedAngleAsInt; 
+        return  result;
+    }
+
+    static FaceAngleDomain classifyStorageValue(storage_type storedValue)
+    {
+        dSASSERT(EAD__MAX == 3);
+
+        return storedValue < 0 ? FAD_CONCAVE : (storedValue == 0 ? FAD_FLAT : FAD_CONVEX);
+    }
+
+    static bool isAngleDomainStored(FaceAngleDomain domainValue)
+    {
+        return !dTMPL_IN_RANGE(domainValue, FAD__SIGNSTORED_IMPLICITVALUE_MIN, FAD__SIGNSTORED_IMPLICITVALUE_MAX);
+    }
+
+    static dReal decodeStorageValue(storage_type storedValue)
+    {
+        return storedValue * (dReal)(M_PI / STORAGE_TYPE_MAX);
+    }
+};
+
+template<typename TStorageType>
+class FaceAngleStorageCodec<TStorageType, SSI_POSITIVE_STORED>
+{
+public:
+    typedef typename _make_unsigned<TStorageType>::type storage_type;
+    enum
+    {
+        STORAGE_TYPE_MIN = 0,
+        STORAGE_TYPE_MAX = (storage_type)(~(storage_type)0),
+    };
+
+    static bool areNegativeAnglesCoded()
+    {
+        return false;
+    }
+
+    static storage_type encodeForStorage(dReal angleValue)
+    {
+        storage_type result = STORAGE_TYPE_MIN;
+
+        if (angleValue >= REAL(0.0))
+        {
+            unsigned angleAsInt = (unsigned)dFloor(angleValue * (dReal)(((STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1) / M_PI)));
+            result = (STORAGE_TYPE_MIN + 1) + dMACRO_MIN(angleAsInt, STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1); 
+        }
+
+        return  result;
+    }
+
+    static FaceAngleDomain classifyStorageValue(storage_type storedValue)
+    {
+        dSASSERT(EAD__MAX == 3);
+
+        return storedValue < STORAGE_TYPE_MIN + 1 ? FAD_CONCAVE : (storedValue == STORAGE_TYPE_MIN + 1 ? FAD_FLAT : FAD_CONVEX);
+    }
+
+    static bool isAngleDomainStored(FaceAngleDomain domainValue)
+    {
+        return dTMPL_IN_RANGE(domainValue, FAD__BYTEPOS_STORED_MIN, FAD__BYTEPOS_STORED_MAX);
+    }
+
+    static dReal decodeStorageValue(storage_type storedValue)
+    {
+        dIASSERT(storedValue >= (STORAGE_TYPE_MIN + 1));
+
+        return (storedValue - (STORAGE_TYPE_MIN + 1)) * (dReal)(M_PI / (STORAGE_TYPE_MAX - STORAGE_TYPE_MIN - 1));
+    }
+};
+
+template<class TStorageCodec>
+class FaceAnglesWrapper:
+    public IFaceAngleStorageControl,
+    public IFaceAngleStorageView
+{
+protected:
+    FaceAnglesWrapper(unsigned triangleCount) { setAllocatedTriangleCount(triangleCount); }
+
+public:
+    virtual ~FaceAnglesWrapper();
+
+    static IFaceAngleStorageControl *allocateInstance(unsigned triangleCount, IFaceAngleStorageView *&out_storageView);
+
+    static bool calculateInstanceSizeRequired(sizeint &out_sizeRequired, unsigned triangleCount);
+
+private:
+    void freeInstance();
+
+private:
+    typedef typename TStorageCodec::storage_type storage_type;
+    typedef storage_type TriangleFaceAngles[dMTV__MAX];
+
+    struct StorageRecord
+    {
+        StorageRecord(): m_triangleCount(0) {}
+
+        unsigned        m_triangleCount;
+        TriangleFaceAngles  m_triangleFaceAngles[1];
+    };
+
+    static sizeint calculateStorageSizeForTriangleCount(unsigned triangleCount)
+    {
+        const unsigned baseIncludedTriangleCount = dSTATIC_ARRAY_SIZE(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles);
+        const sizeint singleTriangleSize = membersize(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles[0]);
+        return sizeof(FaceAnglesWrapper<TStorageCodec>) + (triangleCount > baseIncludedTriangleCount ? (triangleCount - baseIncludedTriangleCount) * singleTriangleSize : 0U);
+    }
+
+    static sizeint calculateTriangleCountForStorageSize(sizeint storageSize)
+    {
+        dIASSERT(storageSize >= sizeof(FaceAnglesWrapper<TStorageCodec>));
+
+        const unsigned baseIncludedTriangleCount = dSTATIC_ARRAY_SIZE(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles);
+        const sizeint singleTriangleSize = membersize(FaceAnglesWrapper<TStorageCodec>::StorageRecord, m_triangleFaceAngles[0]);
+        return (storageSize - sizeof(FaceAnglesWrapper<TStorageCodec>)) / singleTriangleSize + baseIncludedTriangleCount;
+    }
+
+private: // IFaceAngleStorageControl
+    virtual void disposeStorage();
+
+    virtual bool areNegativeAnglesStored() const;
+
+    virtual void assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue);
+
+private: // IFaceAngleStorageView
+    virtual FaceAngleDomain retrieveFacesAngleFromStorage(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex);
+
+public:
+    void setFaceAngle(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue)
+    {
+        dIASSERT(dTMPL_IN_RANGE(triangleIndex, 0, getAllocatedTriangleCount()));
+        dIASSERT(dTMPL_IN_RANGE(vertexIndex, dMTV__MIN, dMTV__MAX));
+
+        m_record.m_triangleFaceAngles[triangleIndex][vertexIndex] = TStorageCodec::encodeForStorage(dAngleValue);
+    }
+
+    FaceAngleDomain getFaceAngle(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex) const
+    {
+        dIASSERT(dTMPL_IN_RANGE(triangleIndex, 0, getAllocatedTriangleCount()));
+        dIASSERT(dTMPL_IN_RANGE(vertexIndex, dMTV__MIN, dMTV__MAX));
+
+        storage_type storedValue = m_record.m_triangleFaceAngles[triangleIndex][vertexIndex];
+        FaceAngleDomain resultDomain = TStorageCodec::classifyStorageValue(storedValue);
+
+        out_angleValue = TStorageCodec::isAngleDomainStored(resultDomain) ? TStorageCodec::decodeStorageValue(storedValue) : REAL(0.0);
+        return resultDomain;
+    }
+
+private:
+    unsigned getAllocatedTriangleCount() const { return m_record.m_triangleCount; }
+    void setAllocatedTriangleCount(unsigned triangleCount) { m_record.m_triangleCount = triangleCount; }
+
+private:
+    StorageRecord       m_record;
+};
+
+
+template<class TStorageCodec>
+FaceAnglesWrapper<TStorageCodec>::~FaceAnglesWrapper()
+{
+}
+
+
+template<class TStorageCodec>
+/*static */
+IFaceAngleStorageControl *FaceAnglesWrapper<TStorageCodec>::allocateInstance(unsigned triangleCount, IFaceAngleStorageView *&out_storageView)
+{
+    FaceAnglesWrapper<TStorageCodec> *result = NULL;
+
+    do
+    {
+        sizeint sizeRequired;
+        if (!FaceAnglesWrapper<TStorageCodec>::calculateInstanceSizeRequired(sizeRequired, triangleCount))
+        {
+            break;
+        }
+
+        void *bufferPointer = dAlloc(sizeRequired);
+        if (bufferPointer == NULL)
+        {
+            break;
+        }
+
+        result = (FaceAnglesWrapper<TStorageCodec> *)bufferPointer;
+        new(result) FaceAnglesWrapper<TStorageCodec>(triangleCount);
+
+        out_storageView = result;
+    }
+    while (false);
+
+    return result;
+}
+
+template<class TStorageCodec>
+/*static */
+bool FaceAnglesWrapper<TStorageCodec>::calculateInstanceSizeRequired(sizeint &out_sizeRequired, unsigned triangleCount)
+{
+    bool result = false;
+
+    do
+    {
+        sizeint triangleMaximumCount = calculateTriangleCountForStorageSize(SIZE_MAX);
+        dIASSERT(triangleCount <= triangleMaximumCount);
+
+        if (triangleCount > triangleMaximumCount) // Check for overflow
+        {
+            break;
+        }
+
+        out_sizeRequired = calculateStorageSizeForTriangleCount(triangleCount); // Trailing alignment is going to be added by memory manager automatically
+        result = true;
+    }
+    while (false);
+
+    return result;
+}
+
+template<class TStorageCodec>
+void FaceAnglesWrapper<TStorageCodec>::freeInstance()
+{
+    unsigned triangleCount = getAllocatedTriangleCount();
+
+    this->FaceAnglesWrapper<TStorageCodec>::~FaceAnglesWrapper();
+
+    sizeint memoryBlockSize = calculateStorageSizeForTriangleCount(triangleCount);
+    dFree(this, memoryBlockSize);
+}
+
+
+template<class TStorageCodec>
+/*virtual */
+void FaceAnglesWrapper<TStorageCodec>::disposeStorage()
+{
+    freeInstance();
+}
+
+template<class TStorageCodec>
+/*virtual */
+bool FaceAnglesWrapper<TStorageCodec>::areNegativeAnglesStored() const
+{
+    return TStorageCodec::areNegativeAnglesCoded();
+}
+
+template<class TStorageCodec>
+/*virtual */
+void FaceAnglesWrapper<TStorageCodec>::assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue)
+{
+    setFaceAngle(triangleIndex, vertexIndex, dAngleValue);
+}
+
+template<class TStorageCodec>
+/*virtual */
+FaceAngleDomain FaceAnglesWrapper<TStorageCodec>::retrieveFacesAngleFromStorage(dReal &out_angleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex)
+{
+    return getFaceAngle(out_angleValue, triangleIndex, vertexIndex);
+}
+
+
+typedef IFaceAngleStorageControl *(FAngleStorageAllocProc)(unsigned triangleCount, IFaceAngleStorageView *&out_storageView);
+
+BEGIN_NAMESPACE_OU();
+template<>
+FAngleStorageAllocProc *const CEnumUnsortedElementArray<FaceAngleStorageMethod, ASM__MAX, FAngleStorageAllocProc *, 0x161211AD>::m_aetElementArray[] =
+{
+    &FaceAnglesWrapper<FaceAngleStorageCodec<uint8, SSI_SIGNED_STORED> >::allocateInstance, // ASM_BYTE_SIGNED,
+    &FaceAnglesWrapper<FaceAngleStorageCodec<uint8, SSI_POSITIVE_STORED> >::allocateInstance, // ASM_BYTE_POSITIVE,
+    &FaceAnglesWrapper<FaceAngleStorageCodec<uint16, SSI_SIGNED_STORED> >::allocateInstance, // ASM_WORD_SIGNED,
+};
+END_NAMESPACE_OU();
+static const CEnumUnsortedElementArray<FaceAngleStorageMethod, ASM__MAX, FAngleStorageAllocProc *, 0x161211AD> g_AngleStorageAllocProcs;
+
+
+//////////////////////////////////////////////////////////////////////////
+
+dxTriDataBase::~dxTriDataBase()
+{
+    freeFaceAngles();
+}
+
+
+void dxTriDataBase::buildData(const void *vertices, int vertexStride, unsigned vertexCount,
+    const void *indices, unsigned indexCount, int triStride,
+    const void *normals,
+    bool single)
+{
+    dIASSERT(vertices);
+    dIASSERT(indices);
+    dIASSERT(vertexStride);
+    dIASSERT(triStride);
+    dIASSERT(indexCount);
+    dIASSERT(indexCount % dMTV__MAX == 0);
+
+    m_vertices = vertices;
+    m_vertexStride = vertexStride;
+    m_vertexCount = vertexCount;
+    m_indices = indices;
+    m_triangleCount = indexCount / dMTV__MAX;
+    m_triStride = triStride;
+    m_single = single;
+
+    m_normals = normals;
+}
+
+
+bool dxTriDataBase::allocateFaceAngles(FaceAngleStorageMethod storageMethod)
+{
+    bool result = false;
+
+    dIASSERT(m_faceAngles == NULL);
+    
+    IFaceAngleStorageView *storageView;
+
+    unsigned triangleCount = m_triangleCount;
+
+    FAngleStorageAllocProc *allocProc = g_AngleStorageAllocProcs.Encode(storageMethod);
+    IFaceAngleStorageControl *storageInstance = allocProc(triangleCount, storageView);
+
+    if (storageInstance != NULL)
+    {
+        m_faceAngles = storageInstance;
+        m_faceAngleView = storageView;
+        result = true;
+    }
+
+    return result;
+}
+
+void dxTriDataBase::freeFaceAngles()
+{
+    if (m_faceAngles != NULL)
+    {
+        m_faceAngles->disposeStorage();
+        m_faceAngles = NULL;
+        m_faceAngleView = NULL;
+    }
+}
+
+
+void dxTriDataBase::EdgeRecord::setupEdge(dMeshTriangleVertex edgeIdx, int triIdx, const unsigned vertexIndices[dMTV__MAX])
+{
+    if (edgeIdx < dMTV_SECOND)
+    {
+        dIASSERT(edgeIdx == dMTV_FIRST);
+
+        m_edgeFlags  = dxTriMeshData::CUF_USE_FIRST_EDGE;
+        m_vert1Flags = dxTriMeshData::CUF_USE_FIRST_VERTEX;
+        m_vert2Flags = dxTriMeshData::CUF_USE_SECOND_VERTEX;
+        m_vertIdx1 = vertexIndices[dMTV_FIRST];
+        m_vertIdx2 = vertexIndices[dMTV_SECOND];
+    }
+    else if (edgeIdx == dMTV_SECOND)
+    {
+        m_edgeFlags  = dxTriMeshData::CUF_USE_SECOND_EDGE;
+        m_vert1Flags = dxTriMeshData::CUF_USE_SECOND_VERTEX;
+        m_vert2Flags = dxTriMeshData::CUF_USE_THIRD_VERTEX;
+        m_vertIdx1 = vertexIndices[dMTV_SECOND];
+        m_vertIdx2 = vertexIndices[dMTV_THIRD];
+    }
+    else
+    {
+        dIASSERT(edgeIdx == dMTV_THIRD);
+
+        m_edgeFlags  = dxTriMeshData::CUF_USE_THIRD_EDGE;
+        m_vert1Flags = dxTriMeshData::CUF_USE_THIRD_VERTEX;
+        m_vert2Flags = dxTriMeshData::CUF_USE_FIRST_VERTEX;
+        m_vertIdx1 = vertexIndices[dMTV_THIRD];
+        m_vertIdx2 = vertexIndices[dMTV_FIRST];
+    }
+
+    // Make sure vertex index 1 is less than index 2 (for easier sorting)
+    if (m_vertIdx1 > m_vertIdx2)
+    {
+        dxSwap(m_vertIdx1, m_vertIdx2);
+        dxSwap(m_vert1Flags, m_vert2Flags);
+    }
+
+    m_triIdx = triIdx;
+    m_absVertexFlags = 0;
+}
+
+
+BEGIN_NAMESPACE_OU();
+template<>
+const dMeshTriangleVertex CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC>::m_aetElementArray[] = 
+{
+    dMTV_FIRST, // kVert0 / kVert_Base
+    dMTV_SECOND, // kVert1 / kVert_Base
+    dMTV__MAX,
+    dMTV_THIRD, // kVert2 / kVert_Base
+};
+END_NAMESPACE_OU();
+/*extern */const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC> g_VertFlagOppositeIndices;
+
+BEGIN_NAMESPACE_OU();
+template<>
+const dMeshTriangleVertex CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9>::m_aetElementArray[] = 
+{
+    dMTV_SECOND, // kVert0 / kVert_Base
+    dMTV_THIRD, // kVert1 / kVert_Base
+    dMTV__MAX,
+    dMTV_FIRST, // kVert2 / kVert_Base
+};
+END_NAMESPACE_OU();
+/*extern */const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9> g_VertFlagEdgeStartIndices;
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern ODE_API */
+void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
+    const dReal* Vertices, int VertexCount, 
+    const dTriIndex* Indices, int IndexCount,
+    const int *Normals)
+{
+#ifdef dSINGLE
+    dGeomTriMeshDataBuildSingle1(g,
+        Vertices, 4 * sizeof(dReal), VertexCount, 
+        Indices, IndexCount, 3 * sizeof(dTriIndex),
+        Normals);
+#else
+    dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, 
+        Indices, IndexCount, 3 * sizeof(dTriIndex),
+        Normals);
+#endif
+}
+
+
+/*extern ODE_API */
+void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride)
+{
+    dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
+        Indices, IndexCount, TriStride, (const void *)NULL);
+}
+
+/*extern ODE_API */
+void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride)
+{
+    dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
+        Indices, IndexCount, TriStride, NULL);
+}
+
+/*extern ODE_API */
+void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
+    const dReal* Vertices, int VertexCount, 
+    const dTriIndex* Indices, int IndexCount)
+{
+    dGeomTriMeshDataBuildSimple1(g,
+        Vertices, VertexCount, Indices, IndexCount,
+        (int *)NULL);
+}
+
+
+/*extern ODE_API */
+int dGeomTriMeshDataPreprocess(dTriMeshDataID g)
+{
+    unsigned buildRequestFlags = (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES);
+    return dGeomTriMeshDataPreprocess2(g, buildRequestFlags, NULL);
+}
+
+
+BEGIN_NAMESPACE_OU();
+template<>
+const FaceAngleStorageMethod CEnumUnsortedElementArray<unsigned, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX, FaceAngleStorageMethod, 0x17010902>::m_aetElementArray[] = 
+{
+    ASM_BYTE_POSITIVE, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_BYTE_POSITIVE,
+    ASM_BYTE_SIGNED, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_BYTE_ALL,
+    ASM_WORD_SIGNED, // dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA_WORD_ALL,
+};
+END_NAMESPACE_OU();
+static const CEnumUnsortedElementArray<unsigned, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX, FaceAngleStorageMethod, 0x17010902> g_TriMeshDataPreprocess_FaceAndlesExtraDataAngleStorageMethods;
+
+/*extern ODE_API */
+int dGeomTriMeshDataPreprocess2(dTriMeshDataID g, unsigned int buildRequestFlags, const intptr *requestExtraData/*=NULL | const intptr (*)[dTRIDATAPREPROCESS_BUILD__MAX]*/)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+    dAASSERT((buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES)) == 0 || requestExtraData == NULL || dIN_RANGE(requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES], dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MIN, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX));
+
+    dxTriMeshData *data = g;
+
+    bool buildUseFlags = (buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_CONCAVE_EDGES)) != 0;
+    FaceAngleStorageMethod faceAnglesRequirement = (buildRequestFlags & (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES)) != 0
+        ? g_TriMeshDataPreprocess_FaceAndlesExtraDataAngleStorageMethods.Encode(requestExtraData != NULL && dIN_RANGE(requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES], dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MIN, dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__MAX) ? (unsigned)requestExtraData[dTRIDATAPREPROCESS_BUILD_FACE_ANGLES] : dTRIDATAPREPROCESS_FACE_ANGLES_EXTRA__DEFAULT)
+        : ASM__INVALID;
+    return data->preprocessData(buildUseFlags, faceAnglesRequirement);
+}
+
+/*extern ODE_API */
+void dGeomTriMeshDataUpdate(dTriMeshDataID g) 
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    dxTriMeshData *data = g;
+    data->updateData();
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern ODE_API */
+void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignCallback(Callback);
+}
+
+/*extern ODE_API */
+dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveCallback();
+}
+
+/*extern ODE_API */
+void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignArrayCallback(ArrayCallback);
+}
+
+/*extern ODE_API */
+dTriArrayCallback *dGeomTriMeshGetArrayCallback(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveArrayCallback();
+}
+
+/*extern ODE_API */
+void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignRayCallback(Callback);
+}
+
+/*extern ODE_API */
+dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");	
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveRayCallback();
+}
+
+/*extern ODE_API */
+void dGeomTriMeshSetTriMergeCallback(dGeomID g, dTriTriMergeCallback* Callback)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignTriMergeCallback(Callback);
+}
+
+/*extern ODE_API */
+dTriTriMergeCallback *dGeomTriMeshGetTriMergeCallback(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");	
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveTriMergeCallback();
+}
+
+/*extern ODE_API */
+void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignMeshData(Data);
+}
+
+/*extern ODE_API */
+dTriMeshDataID dGeomTriMeshGetData(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveMeshData();
+}
+
+
+BEGIN_NAMESPACE_OU();
+template<>
+const int CEnumSortedElementArray<dxTriMesh::TRIMESHTC, dxTriMesh::TTC__MAX, int, 0x161003D5>::m_aetElementArray[] =
+{
+    dSphereClass, // TTC_SPHERE,
+    dBoxClass, // TTC_BOX,
+    dCapsuleClass, // TTC_CAPSULE,
+};
+END_NAMESPACE_OU();
+static const CEnumSortedElementArray<dxTriMesh::TRIMESHTC, dxTriMesh::TTC__MAX, int, 0x161003D5> g_asiMeshTCGeomClasses;
+
+/*extern ODE_API */
+void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+
+    dxTriMesh::TRIMESHTC tc = g_asiMeshTCGeomClasses.Decode(geomClass);
+
+    if (g_asiMeshTCGeomClasses.IsValidDecode(tc))
+    {
+        mesh->assignDoTC(tc, enable != 0);
+    }
+}
+
+/*extern ODE_API */
+int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+
+    dxTriMesh::TRIMESHTC tc = g_asiMeshTCGeomClasses.Decode(geomClass);
+
+    bool result = g_asiMeshTCGeomClasses.IsValidDecode(tc) 
+        && mesh->retrieveDoTC(tc);
+    return result;
+}
+
+
+/*extern ODE_API */
+dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveMeshData();
+}
+
+
+/*extern ODE_API */
+void dGeomTriMeshClearTCCache(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->clearTCCache();
+}
+
+
+/*extern ODE_API */
+int dGeomTriMeshGetTriangleCount(dGeomID g)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    const dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    unsigned result = mesh->getMeshTriangleCount();
+    return result;
+}
+
+
+/*extern ODE_API */
+void dGeomTriMeshGetTriangle(dGeomID g, int index, dVector3 *v0/*=NULL*/, dVector3 *v1/*=NULL*/, dVector3 *v2/*=NULL*/)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+    dUASSERT(v0 != NULL || v1 != NULL || v2 != NULL, "A meaningless call");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+
+    dVector3 *pv[3] = { v0, v1, v2 };
+    mesh->fetchMeshTransformedTriangle(pv, index);
+}
+
+/*extern ODE_API */
+void dGeomTriMeshGetPoint(dGeomID g, int index, dReal u, dReal v, dVector3 Out)
+{
+    dUASSERT(g && g->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+
+    dVector3 dv[3];
+    mesh->fetchMeshTransformedTriangle(dv, index);
+
+    GetPointFromBarycentric(dv, u, v, Out);
+}
+
+
+/*extern */
+IFaceAngleStorageView *dxGeomTriMeshGetFaceAngleView(dxGeom *triMeshGeom)
+{
+    dUASSERT(triMeshGeom && triMeshGeom->type == dTriMeshClass, "The argument is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(triMeshGeom);
+    return mesh->retrieveFaceAngleView();
+}
+
+
+#endif // #if dTRIMESH_ENABLED
+
+
+//////////////////////////////////////////////////////////////////////////
+// Deprecated functions
+
+/*extern */
+void dGeomTriMeshDataGetBuffer(dTriMeshDataID g, unsigned char **buf, int *bufLen)
+{
+    sizeint dataSizeStorage;
+    void *dataPointer = dGeomTriMeshDataGet2(g, dTRIMESHDATA_USE_FLAGS, (bufLen != NULL ? &dataSizeStorage : NULL));
+
+    if (bufLen != NULL)
+    {
+        *bufLen = (int)dataSizeStorage;
+    }
+
+    if (buf != NULL)
+    {
+        *buf = (unsigned char *)dataPointer;
+    }
+}
+
+/*extern */
+void dGeomTriMeshDataSetBuffer(dTriMeshDataID g, unsigned char* buf)
+{
+    dGeomTriMeshDataSet(g, dTRIMESHDATA_USE_FLAGS, (void *)buf);
+}
+

thirdparty/ode-0.16.5/ode/src/collision_trimesh_internal.h

@@ -0,0 +1,399 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
+// TriMesh caches separation by Oleh Derevenko
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2024
+
+
+#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
+#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
+
+
+//****************************************************************************
+// dxTriMesh class
+
+
+#include "collision_kernel.h"
+#include "collision_trimesh_colliders.h"
+#include "collision_util.h"
+#include <ode/collision_trimesh.h>
+
+#if dTLS_ENABLED
+#include "odetls.h"
+#endif
+
+
+struct TrimeshCollidersCache;
+struct dxTriMeshData;
+
+
+static inline 
+TrimeshCollidersCache *GetTrimeshCollidersCache(unsigned uiTLSKind)
+{
+#if dTLS_ENABLED
+    EODETLSKIND tkTLSKind = (EODETLSKIND)uiTLSKind;
+    return COdeTls::GetTrimeshCollidersCache(tkTLSKind);
+#else // dTLS_ENABLED
+    (void)uiTLSKind; // unused
+    extern TrimeshCollidersCache g_ccTrimeshCollidersCache;
+    return &g_ccTrimeshCollidersCache;
+#endif // dTLS_ENABLED
+}
+
+
+enum FaceAngleStorageMethod
+{
+    ASM__MIN,
+
+    ASM_BYTE_SIGNED = ASM__MIN,
+    ASM_BYTE_POSITIVE,
+    ASM_WORD_SIGNED,
+
+    ASM__MAX,
+
+    ASM__INVALID = ASM__MAX,
+};
+
+enum FaceAngleDomain
+{
+    FAD__MIN,
+
+    FAD_CONCAVE = FAD__MIN,
+
+    FAD__SIGNSTORED_IMPLICITVALUE_MIN,
+
+    FAD_FLAT = FAD__SIGNSTORED_IMPLICITVALUE_MIN,
+
+    FAD__SIGNSTORED_IMPLICITVALUE_MAX,
+
+    FAD__BYTEPOS_STORED_MIN = FAD__SIGNSTORED_IMPLICITVALUE_MAX,
+
+    FAD_CONVEX = FAD__BYTEPOS_STORED_MIN,
+
+    FAD__BYTEPOS_STORED_MAX,
+
+    EAD__MAX = FAD__BYTEPOS_STORED_MAX,
+};
+
+class IFaceAngleStorageControl
+{
+public:
+    virtual void disposeStorage() = 0;
+
+    virtual bool areNegativeAnglesStored() const = 0;
+
+    // This is to store angles between neighbor triangle normals as positive value for convex and negative for concave edges
+    virtual void assignFacesAngleIntoStorage(unsigned triangleIndex, dMeshTriangleVertex vertexIndex, dReal dAngleValue) = 0;
+};
+
+class IFaceAngleStorageView
+{
+public:
+    virtual FaceAngleDomain retrieveFacesAngleFromStorage(dReal &out_AngleValue, unsigned triangleIndex, dMeshTriangleVertex vertexIndex) = 0;
+};
+
+
+typedef dBase dxTriDataBase_Parent;
+struct dxTriDataBase:
+    public dxTriDataBase_Parent
+{
+public:
+    dxTriDataBase():
+        dxTriDataBase_Parent(),
+        m_vertices(NULL),
+        m_vertexStride(0),
+        m_vertexCount(0),
+        m_indices(NULL),
+        m_triangleCount(0),
+        m_triStride(0),
+        m_single(false),
+        m_normals(NULL),
+        m_faceAngles(NULL),
+        m_faceAngleView(NULL)
+    {
+#if !dTRIMESH_ENABLED
+        dUASSERT(false, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
+#endif
+    }
+
+    ~dxTriDataBase();
+
+    void buildData(const void *Vertices, int VertexStide, unsigned VertexCount, 
+        const void *Indices, unsigned IndexCount, int TriStride, 
+        const void *Normals, 
+        bool Single);
+
+
+public:
+    unsigned retrieveVertexCount() const { return m_vertexCount; }
+    int retrieveVertexStride() const { return m_vertexStride; }
+
+    unsigned retrieveTriangleCount() const { return m_triangleCount; }
+    int retrieveTriangleStride() const { return m_triStride; }
+
+protected:
+    const void *retrieveVertexInstances() const { return m_vertices; }
+    const void *retrieveTriangleVertexIndices() const { return m_indices; }
+    bool isSingle() const { return m_single; }
+
+public:
+    template<typename tcoordfloat, typename tindexint>
+    static void retrieveTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex,
+        const tcoordfloat *vertexInstances, int vertexStride, const tindexint *triangleVertexIndices, int triangleStride);
+
+public:
+    void assignNormals(const void *normals) { m_normals = normals; }
+    const void *retrieveNormals() const { return m_normals; }
+
+    IFaceAngleStorageControl *retrieveFaceAngles() const { return m_faceAngles; }
+    IFaceAngleStorageView *retrieveFaceAngleView() const { return m_faceAngleView; }
+
+protected:
+    bool allocateFaceAngles(FaceAngleStorageMethod storageMethod);
+    void freeFaceAngles();
+
+    bool haveFaceAnglesBeenBuilt() const { return m_faceAngles != NULL; }
+
+public:
+    enum MeshComponentUseFlags
+    {
+        CUF__USE_EDGES_MIN = 0x01,
+        CUF_USE_FIRST_EDGE = CUF__USE_EDGES_MIN << dMTV_FIRST,
+        CUF_USE_SECOND_EDGE = CUF__USE_EDGES_MIN << dMTV_SECOND,
+        CUF_USE_THIRD_EDGE = CUF__USE_EDGES_MIN << dMTV_THIRD,
+        CUF__USE_EDGES_MAX = CUF__USE_EDGES_MIN << dMTV__MAX,
+        CUF__USE_ALL_EDGES = CUF_USE_FIRST_EDGE | CUF_USE_SECOND_EDGE | CUF_USE_THIRD_EDGE,
+
+        CUF__USE_VERTICES_MIN = CUF__USE_EDGES_MAX,
+        CUF_USE_FIRST_VERTEX = CUF__USE_VERTICES_MIN << dMTV_FIRST,
+        CUF_USE_SECOND_VERTEX = CUF__USE_VERTICES_MIN << dMTV_SECOND,
+        CUF_USE_THIRD_VERTEX = CUF__USE_VERTICES_MIN << dMTV_THIRD,
+        CUF__USE_VERTICES_LAST = CUF__USE_VERTICES_MIN << (dMTV__MAX - 1),
+        CUF__USE_VERTICES_MAX = CUF__USE_VERTICES_MIN << dMTV__MAX,
+        CUF__USE_ALL_VERTICES = CUF_USE_FIRST_VERTEX | CUF_USE_SECOND_VERTEX | CUF_USE_THIRD_VERTEX,
+
+        CUF__USE_ALL_COMPONENTS = CUF__USE_ALL_VERTICES | CUF__USE_ALL_EDGES,
+    };
+
+    // Make sure that the flags match the values declared in public interface
+    dSASSERT((unsigned)CUF_USE_FIRST_EDGE == dMESHDATAUSE_EDGE1);
+    dSASSERT((unsigned)CUF_USE_SECOND_EDGE == dMESHDATAUSE_EDGE2);
+    dSASSERT((unsigned)CUF_USE_THIRD_EDGE == dMESHDATAUSE_EDGE3);
+    dSASSERT((unsigned)CUF_USE_FIRST_VERTEX == dMESHDATAUSE_VERTEX1);
+    dSASSERT((unsigned)CUF_USE_SECOND_VERTEX == dMESHDATAUSE_VERTEX2);
+    dSASSERT((unsigned)CUF_USE_THIRD_VERTEX == dMESHDATAUSE_VERTEX3);
+
+protected:
+    struct EdgeRecord
+    {
+    public:
+        void setupEdge(dMeshTriangleVertex edgeIdx, int triIdx, const unsigned vertexIndices[dMTV__MAX]);
+
+        // Get the vertex opposite this edge in the triangle
+        dMeshTriangleVertex getOppositeVertexIndex() const
+        {
+            extern const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161116DC> g_VertFlagOppositeIndices;
+
+            dMeshTriangleVertex oppositeIndex = g_VertFlagOppositeIndices.Encode(((m_vert1Flags | m_vert2Flags) ^ CUF__USE_ALL_VERTICES) / CUF__USE_VERTICES_MIN - 1);
+            dIASSERT(dIN_RANGE(oppositeIndex, dMTV__MIN, dMTV__MAX));
+
+            return oppositeIndex;
+        }
+
+        dMeshTriangleVertex getEdgeStartVertexIndex() const
+        {
+            extern const CEnumUnsortedElementArray<unsigned, dxTriDataBase::CUF__USE_VERTICES_LAST / dxTriDataBase::CUF__USE_VERTICES_MIN, dMeshTriangleVertex, 0x161225E9> g_VertFlagEdgeStartIndices;
+
+            dMeshTriangleVertex startIndex = g_VertFlagEdgeStartIndices.Encode(((m_vert1Flags | m_vert2Flags) ^ CUF__USE_ALL_VERTICES) / CUF__USE_VERTICES_MIN - 1);
+            dIASSERT(dIN_RANGE(startIndex, dMTV__MIN, dMTV__MAX));
+
+            return startIndex;
+        }
+
+    public:
+        bool operator <(const EdgeRecord &anotherEdge) const { return m_vertIdx1 < anotherEdge.m_vertIdx1 || (m_vertIdx1 == anotherEdge.m_vertIdx1 && m_vertIdx2 < anotherEdge.m_vertIdx2); }
+
+    public:
+        enum
+        {
+            AVF_VERTEX_USED             = 0x01,
+            AVF_VERTEX_HAS_CONCAVE_EDGE = 0x02,
+        };
+
+    public:
+        unsigned m_vertIdx1;	// Index into vertex array for this edges vertices
+        unsigned m_vertIdx2;
+        unsigned m_triIdx;		// Index into triangle array for triangle this edge belongs to
+
+        uint8 m_edgeFlags;	
+        uint8 m_vert1Flags;
+        uint8 m_vert2Flags;
+        uint8 m_absVertexFlags;
+    };
+
+    struct VertexRecord
+    {
+        unsigned m_UsedFromEdgeIndex;
+    };
+
+    template<class TMeshDataAccessor>
+    static void meaningfulPreprocess_SetupEdgeRecords(EdgeRecord *edges, sizeint numEdges, const TMeshDataAccessor &dataAccessor);
+    template<class TMeshDataAccessor>
+    static void meaningfulPreprocess_buildEdgeFlags(uint8 *useFlags/*=NULL*/, IFaceAngleStorageControl *faceAngles/*=NULL*/, 
+        EdgeRecord *edges, sizeint numEdges, VertexRecord *vertices, 
+        const dReal *externalNormals, const TMeshDataAccessor &dataAccessor);
+    static void buildBoundaryEdgeAngle(IFaceAngleStorageControl *faceAngles, EdgeRecord *currEdge);
+    template<class TMeshDataAccessor>
+    static void buildConcaveEdgeAngle(IFaceAngleStorageControl *faceAngles, bool negativeAnglesStored, 
+        EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+        const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+        const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+        const TMeshDataAccessor &dataAccessor);
+    template<class TMeshDataAccessor>
+    static 
+    void buildConvexEdgeAngle(IFaceAngleStorageControl *faceAngles, 
+        EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+        const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+        const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+        const TMeshDataAccessor &dataAccessor);
+    template<class TMeshDataAccessor>
+    static dReal calculateEdgeAngleValidated(unsigned firstVertexStartIndex,
+        EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+        const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+        const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+        const TMeshDataAccessor &dataAccessor);
+
+private:
+    const void *m_vertices;
+    int m_vertexStride;
+    unsigned m_vertexCount;
+    const void *m_indices;
+    unsigned m_triangleCount;
+    int m_triStride;
+    bool m_single;
+
+private:
+    const void *m_normals;
+    IFaceAngleStorageControl *m_faceAngles;
+    IFaceAngleStorageView *m_faceAngleView; 
+};
+
+
+typedef dxGeom dxMeshBase_Parent;
+struct dxMeshBase:
+    public dxMeshBase_Parent
+{
+public:
+    dxMeshBase(dxSpace *Space, dxTriDataBase *Data, 
+        dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback, 
+        bool doTCs=false):
+        dxMeshBase_Parent(Space, 1),
+        m_Callback(Callback),
+        m_ArrayCallback(ArrayCallback),
+        m_RayCallback(RayCallback),
+        m_TriMergeCallback(NULL),
+        m_Data(Data)
+    {
+        std::fill(m_DoTCs, m_DoTCs + dARRAY_SIZE(m_DoTCs), doTCs);
+        type = dTriMeshClass;
+    }
+
+    bool invokeCallback(dxGeom *Object, int TriIndex)
+    {
+        return m_Callback == NULL || m_Callback(this, Object, TriIndex) != 0;
+    }
+
+public:
+    enum TRIMESHTC
+    {
+        TTC__MIN,
+
+        TTC_SPHERE = TTC__MIN,
+        TTC_BOX,
+        TTC_CAPSULE,
+
+        TTC__MAX,
+    };
+
+public:
+    void assignCallback(dTriCallback *value) { m_Callback = value; }
+    dTriCallback *retrieveCallback() const { return m_Callback; }
+
+    void assignArrayCallback(dTriArrayCallback *value) { m_ArrayCallback = value; }
+    dTriArrayCallback *retrieveArrayCallback() const { return m_ArrayCallback; }
+
+    void assignRayCallback(dTriRayCallback *value) { m_RayCallback = value; }
+    dTriRayCallback *retrieveRayCallback() const { return m_RayCallback; }
+
+    void assignTriMergeCallback(dTriTriMergeCallback *value) { m_TriMergeCallback = value; }
+    dTriTriMergeCallback *retrieveTriMergeCallback() const { return m_TriMergeCallback; }
+
+    void assignMeshData(dxTriDataBase *instance)
+    {
+        setMeshData(instance);
+        // I changed my data -- I know nothing about my own AABB anymore.
+        markAABBBad();
+    }
+    dxTriDataBase *retrieveMeshData() const { return getMeshData(); }
+
+    IFaceAngleStorageControl *retrieveFaceAngleStorage() const { return m_Data->retrieveFaceAngles(); }
+    IFaceAngleStorageView *retrieveFaceAngleView() const { return m_Data->retrieveFaceAngleView(); }
+
+    void assignDoTC(TRIMESHTC tc, bool value) { setDoTC(tc, value); }
+    bool retrieveDoTC(TRIMESHTC tc) const { return getDoTC(tc); }
+
+public:
+    void setDoTC(TRIMESHTC tc, bool value) { dIASSERT(dIN_RANGE(tc, TTC__MIN, TTC__MAX)); m_DoTCs[tc] = value; }
+    bool getDoTC(TRIMESHTC tc) const { dIASSERT(dIN_RANGE(tc, TTC__MIN, TTC__MAX)); return m_DoTCs[tc]; }
+
+private:
+    void setMeshData(dxTriDataBase *Data) { m_Data = Data; }
+
+protected:
+    dxTriDataBase *getMeshData() const { return m_Data; }
+
+public:
+    // Callbacks
+    dTriCallback *m_Callback;
+    dTriArrayCallback *m_ArrayCallback;
+    dTriRayCallback *m_RayCallback;
+    dTriTriMergeCallback *m_TriMergeCallback;
+
+private:
+    // Data types
+    dxTriDataBase *m_Data;
+
+public:
+    bool m_DoTCs[TTC__MAX];
+};
+
+
+IFaceAngleStorageView *dxGeomTriMeshGetFaceAngleView(dxGeom *triMeshGeom);
+
+
+#include "collision_trimesh_gimpact.h"
+#include "collision_trimesh_opcode.h"
+
+
+#endif	//_ODE_COLLISION_TRIMESH_INTERNAL_H_

thirdparty/ode-0.16.5/ode/src/collision_trimesh_internal_impl.h

@@ -0,0 +1,463 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh base template method implementations by Oleh Derevenko (C) 2016-2024
+
+
+#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_
+#define _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_
+
+
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_ENABLED
+
+
+template<typename tcoordfloat, typename tindexint>
+/*static */
+void dxTriDataBase::retrieveTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex,
+    const tcoordfloat *vertexInstances, int vertexStride, const tindexint *triangleVertexIndices, int triangleStride)
+{
+    const tindexint *triangleIndicesOfInterest = (const tindexint *)((uint8 *)triangleVertexIndices + (sizeint)triangleIndex * triangleStride);
+    for (unsigned trianglePoint = dMTV__MIN; trianglePoint != dMTV__MAX; ++trianglePoint)
+    {
+        unsigned vertexIndex = triangleIndicesOfInterest[trianglePoint];
+        tcoordfloat *pointVertex = (tcoordfloat *)((uint8 *)vertexInstances + (sizeint)vertexIndex * vertexStride);
+        dAssignVector3(out_Points[trianglePoint], (dReal)pointVertex[dSA_X], (dReal)pointVertex[dSA_Y], (dReal)pointVertex[dSA_Z]);
+        dSASSERT(dSA_X == 0);
+        dSASSERT(dSA_Y == 1);
+        dSASSERT(dSA_Z == 2);
+    }
+}
+
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::meaningfulPreprocess_SetupEdgeRecords(EdgeRecord *edges, sizeint numEdges, const TMeshDataAccessor &dataAccessor)
+{
+    unsigned vertexIndices[dMTV__MAX];
+    // Make a list of every edge in the mesh
+    unsigned triangleIdx = 0;
+    for (sizeint edgeIdx = 0; edgeIdx != numEdges; ++triangleIdx, edgeIdx += dMTV__MAX)
+    {
+        dataAccessor.getTriangleVertexIndices(vertexIndices, triangleIdx);
+        edges[edgeIdx + dMTV_FIRST].setupEdge(dMTV_FIRST, triangleIdx, vertexIndices);
+        edges[edgeIdx + dMTV_SECOND].setupEdge(dMTV_SECOND, triangleIdx, vertexIndices);
+        edges[edgeIdx + dMTV_THIRD].setupEdge(dMTV_THIRD, triangleIdx, vertexIndices);
+    }
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::meaningfulPreprocess_buildEdgeFlags(uint8 *useFlags/*=NULL*/, IFaceAngleStorageControl *faceAngles/*=NULL*/, 
+    EdgeRecord *edges, sizeint numEdges, VertexRecord *vertices, 
+    const dReal *externalNormals/*=NULL*/, const TMeshDataAccessor &dataAccessor)
+{
+    dIASSERT(useFlags != NULL || faceAngles != NULL);
+    dIASSERT(numEdges != 0);
+
+    const bool negativeAnglesStored = faceAngles != NULL && faceAngles->areNegativeAnglesStored();
+
+    // Go through the sorted list of edges and flag all the edges and vertices that we need to use
+    EdgeRecord *const lastEdge = edges + (numEdges - 1);
+    for (EdgeRecord *currEdge = edges; ; ++currEdge)
+    {
+        // Handle the last edge separately to have an optimizer friendly loop
+        if (currEdge >= lastEdge)
+        {
+            // This is a boundary edge
+            if (currEdge == lastEdge)
+            {
+                if (faceAngles != NULL)
+                {
+                    buildBoundaryEdgeAngle(faceAngles, currEdge);
+                }
+
+                if (useFlags != NULL)
+                {
+                    // For the last element EdgeRecord::kAbsVertexUsed assignment can be skipped as noone is going to need it anymore
+                    useFlags[currEdge[0].m_triIdx] |= ((edges[currEdge[0].m_vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert1Flags : 0) 
+                        | ((edges[currEdge[0].m_vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0 ? currEdge[0].m_vert2Flags : 0)
+                        | currEdge[0].m_edgeFlags;
+                }
+            }
+
+            break;
+        }
+
+        unsigned vertIdx1 = currEdge[0].m_vertIdx1;
+        unsigned vertIdx2 = currEdge[0].m_vertIdx2;
+
+        if (vertIdx2 == currEdge[1].m_vertIdx2 // Check second vertex first as it is more likely to change taking the sorting rules into account
+            && vertIdx1 == currEdge[1].m_vertIdx1)
+        {
+            // We let the dot threshold for concavity get slightly negative to allow for rounding errors
+            const float kConcaveThreshold = 0.000001f;
+
+            const dVector3 *pSecondTriangleEdgeToUse = NULL, *pFirstTriangleToUse = NULL;
+            dVector3 secondTriangleMatchingEdge;
+            dVector3 firstTriangle[dMTV__MAX];
+            dVector3 secondOppositeVertexSegment, triangleNormal;
+            dReal lengthSquareProduct, secondOppositeSegmentLengthSquare;
+
+            // Calculate orthogonal vector from the matching edge of the second triangle to its opposite point
+            {
+                dVector3 secondTriangle[dMTV__MAX];
+                dataAccessor.getTriangleVertexPoints(secondTriangle, currEdge[1].m_triIdx);
+
+                // Get the vertex opposite this edge in the second triangle
+                dMeshTriangleVertex secondOppositeVertex = currEdge[1].getOppositeVertexIndex();
+                dMeshTriangleVertex secondEdgeStart = secondOppositeVertex + 1 != dMTV__MAX ? (dMeshTriangleVertex)(secondOppositeVertex + 1) : dMTV__MIN;
+                dMeshTriangleVertex secondEdgeEnd = (dMeshTriangleVertex)(dMTV_FIRST + dMTV_SECOND + dMTV_THIRD - secondEdgeStart - secondOppositeVertex);
+
+                dSubtractVectors3(secondTriangleMatchingEdge, secondTriangle[secondEdgeEnd], secondTriangle[secondEdgeStart]);
+
+                if (dSafeNormalize3(secondTriangleMatchingEdge))
+                {
+                    pSecondTriangleEdgeToUse = &secondTriangleMatchingEdge;
+
+                    dVector3 secondTriangleOppositeEdge;
+                    dSubtractVectors3(secondTriangleOppositeEdge, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]);
+                    dReal dProjectionLength = dCalcVectorDot3(secondTriangleOppositeEdge, secondTriangleMatchingEdge);
+                    dAddVectorScaledVector3(secondOppositeVertexSegment, secondTriangleOppositeEdge, secondTriangleMatchingEdge, -dProjectionLength);
+                }
+                else
+                {
+                    dSubtractVectors3(secondOppositeVertexSegment, secondTriangle[secondOppositeVertex], secondTriangle[secondEdgeStart]);
+                }
+
+                secondOppositeSegmentLengthSquare = dCalcVectorLengthSquare3(secondOppositeVertexSegment);
+            }
+
+            // Either calculate the normal from triangle vertices...
+            if (externalNormals == NULL)
+            {
+                // Get the normal of the first triangle
+                dataAccessor.getTriangleVertexPoints(firstTriangle, currEdge[0].m_triIdx);
+                pFirstTriangleToUse = &firstTriangle[dMTV__MIN];
+
+                dVector3 firstEdge, secondEdge;
+                dSubtractVectors3(secondEdge, firstTriangle[dMTV_THIRD], firstTriangle[dMTV_SECOND]);
+                dSubtractVectors3(firstEdge, firstTriangle[dMTV_FIRST], firstTriangle[dMTV_SECOND]);
+                dCalcVectorCross3(triangleNormal, secondEdge, firstEdge);
+                dReal normalLengthSuqare = dCalcVectorLengthSquare3(triangleNormal);
+                lengthSquareProduct = secondOppositeSegmentLengthSquare * normalLengthSuqare;
+            }
+            // ...or use the externally supplied normals
+            else
+            {
+                const dReal *pTriangleExternalNormal = externalNormals + currEdge[0].m_triIdx * dSA__MAX;
+                dAssignVector3(triangleNormal, pTriangleExternalNormal[dSA_X], pTriangleExternalNormal[dSA_Y], pTriangleExternalNormal[dSA_Z]);
+                // normalLengthSuqare = REAL(1.0);
+                dUASSERT(dFabs(dCalcVectorLengthSquare3(triangleNormal) - REAL(1.0)) < REAL(0.25) * kConcaveThreshold * kConcaveThreshold, "Mesh triangle normals must be normalized");
+
+                lengthSquareProduct = secondOppositeSegmentLengthSquare/* * normalLengthSuqare*/;
+            }
+
+            dReal normalSegmentDot = dCalcVectorDot3(triangleNormal, secondOppositeVertexSegment);
+
+            // This is a concave edge, leave it for the next pass
+            // OD: This is the "dot >= kConcaveThresh" check, but since the vectors were not normalized to save on roots and divisions,
+            // the check against zero is performed first and then the dot product is squared and compared against the threshold multiplied by lengths' squares
+            // OD: Originally, there was dot > -kConcaveThresh check, but this does not seem to be a good idea
+            // as it can mark all edges on potentially large (nearly) flat surfaces concave.
+            if (normalSegmentDot > REAL(0.0) && normalSegmentDot * normalSegmentDot >= kConcaveThreshold * kConcaveThreshold * lengthSquareProduct)
+            {
+                if (faceAngles != NULL)
+                {
+                    buildConcaveEdgeAngle(faceAngles, negativeAnglesStored, currEdge, normalSegmentDot, lengthSquareProduct,
+                        triangleNormal, secondOppositeVertexSegment,
+                        pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor);
+                }
+
+                if (useFlags != NULL)
+                {
+                    // Mark the vertices of a concave edge to prevent their use
+                    unsigned absVertexFlags1 = edges[vertIdx1].m_absVertexFlags;
+                    edges[vertIdx1].m_absVertexFlags |= absVertexFlags1 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED;
+
+                    if ((absVertexFlags1 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED)
+                    {
+                        // If the vertex was already used from other triangles but then discovered 
+                        // to have a concave edge, unmark the previous use
+                        unsigned usedFromEdgeIndex = vertices[vertIdx1].m_UsedFromEdgeIndex;
+                        const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex;
+                        unsigned usedInTriangleIndex = usedFromEdge->m_triIdx;
+                        uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx1 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags;
+                        useFlags[usedInTriangleIndex] ^= usedVertFlags;
+                        dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0);
+                    }
+
+                    unsigned absVertexFlags2 = edges[vertIdx2].m_absVertexFlags;
+                    edges[vertIdx2].m_absVertexFlags = absVertexFlags2 | EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED;
+
+                    if ((absVertexFlags2 & (EdgeRecord::AVF_VERTEX_HAS_CONCAVE_EDGE | EdgeRecord::AVF_VERTEX_USED)) == EdgeRecord::AVF_VERTEX_USED)
+                    {
+                        // Similarly unmark the possible previous use of the edge's second vertex
+                        unsigned usedFromEdgeIndex = vertices[vertIdx2].m_UsedFromEdgeIndex;
+                        const EdgeRecord *usedFromEdge = edges + usedFromEdgeIndex;
+                        unsigned usedInTriangleIndex = usedFromEdge->m_triIdx;
+                        uint8 usedVertFlags = usedFromEdge->m_vertIdx1 == vertIdx2 ? usedFromEdge->m_vert1Flags : usedFromEdge->m_vert2Flags;
+                        useFlags[usedInTriangleIndex] ^= usedVertFlags;
+                        dIASSERT((useFlags[usedInTriangleIndex] & usedVertFlags) == 0);
+                    }
+                }
+            }
+            // If this is a convex edge, mark its vertices and edge as used
+            else
+            {
+                if (faceAngles != NULL)
+                {
+                    buildConvexEdgeAngle(faceAngles, currEdge, normalSegmentDot, lengthSquareProduct,
+                        triangleNormal, secondOppositeVertexSegment,
+                        pSecondTriangleEdgeToUse, pFirstTriangleToUse, dataAccessor);
+                }
+
+                if (useFlags != NULL)
+                {
+                    EdgeRecord *edgeToUse = currEdge;
+                    unsigned triIdx = edgeToUse[0].m_triIdx;
+                    unsigned triIdx1 = edgeToUse[1].m_triIdx;
+                    
+                    unsigned triUseFlags = useFlags[triIdx];
+                    unsigned triUseFlags1 = useFlags[triIdx1];
+
+                    // Choose to add flags to the bitmask that already has more edges
+                    // (to group flags in selected triangles rather than scattering them evenly)
+                    if ((triUseFlags1 & CUF__USE_ALL_EDGES) > (triUseFlags & CUF__USE_ALL_EDGES))
+                    {
+                        triIdx = triIdx1;
+                        triUseFlags = triUseFlags1;
+                        edgeToUse = edgeToUse + 1;
+                    }
+
+                    if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                    {
+                        // Only add each vertex once and set a mark to prevent further additions
+                        edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                        // Also remember the index the vertex flags are going to be applied to 
+                        // to allow easily clear the vertex from the use flags if any concave edges are found to connect to it
+                        vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges);
+                        triUseFlags |= edgeToUse[0].m_vert1Flags;
+                    }
+
+                    // Same processing for the second vertex...
+                    if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                    {
+                        edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                        vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(edgeToUse - edges);
+                        triUseFlags |= edgeToUse[0].m_vert2Flags;
+                    }
+
+                    // And finally store the use flags adding the edge flags in
+                    useFlags[triIdx] = triUseFlags | edgeToUse[0].m_edgeFlags;
+                }
+            }
+
+            // Skip the second edge
+            ++currEdge;
+        }
+        // This is a boundary edge
+        else
+        {
+            if (faceAngles != NULL)
+            {
+                buildBoundaryEdgeAngle(faceAngles, currEdge);
+            }
+
+            if (useFlags != NULL)
+            {
+                unsigned triIdx = currEdge[0].m_triIdx;
+                unsigned triUseExtraFlags = 0;
+                
+                if ((edges[vertIdx1].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                {
+                    edges[vertIdx1].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                    vertices[vertIdx1].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges);
+                    triUseExtraFlags |= currEdge[0].m_vert1Flags;
+                }
+
+                if ((edges[vertIdx2].m_absVertexFlags & EdgeRecord::AVF_VERTEX_USED) == 0)
+                {
+                    edges[vertIdx2].m_absVertexFlags |= EdgeRecord::AVF_VERTEX_USED;
+                    vertices[vertIdx2].m_UsedFromEdgeIndex = (unsigned)(currEdge - edges);
+                    triUseExtraFlags |= currEdge[0].m_vert2Flags;
+                }
+
+                useFlags[triIdx] |= triUseExtraFlags | currEdge[0].m_edgeFlags;
+            }
+        }
+    }
+}
+
+/*static */
+void dxTriDataBase::buildBoundaryEdgeAngle(IFaceAngleStorageControl *faceAngles, 
+    EdgeRecord *currEdge)
+{
+    const dReal faceAngle = REAL(0.0);
+
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    // -- For boundary edges, only the first element is valid
+    // dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    // faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_TriIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::buildConcaveEdgeAngle(IFaceAngleStorageControl *faceAngles, bool negativeAnglesStored, 
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dReal faceAngle;
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+
+    // Check if concave angles are stored at all
+    if (negativeAnglesStored)
+    {
+        // The length square product can become zero due to precision loss
+        // when both the normal and the opposite edge vectors are very small.
+        if (lengthSquareProduct != REAL(0.0))
+        {
+            faceAngle = -calculateEdgeAngleValidated(firstVertexStartIndex,
+                currEdge, normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment,
+                pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor);
+        }
+        else
+        {
+            faceAngle = REAL(0.0);
+        }
+    }
+    else
+    {
+        // If concave angles ate not stored, set an arbitrary negative value
+        faceAngle = -(dReal)M_PI;
+    }
+
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+void dxTriDataBase::buildConvexEdgeAngle(IFaceAngleStorageControl *faceAngles, 
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dReal faceAngle;
+    dMeshTriangleVertex firstVertexStartIndex = currEdge[0].getEdgeStartVertexIndex();
+
+    // The length square product can become zero due to precision loss
+    // when both the normal and the opposite edge vectors are very small.
+    if (normalSegmentDot < REAL(0.0) && lengthSquareProduct != REAL(0.0))
+    {
+        faceAngle = calculateEdgeAngleValidated(firstVertexStartIndex,
+            currEdge, -normalSegmentDot, lengthSquareProduct, triangleNormal, secondOppositeVertexSegment,
+            pSecondTriangleMatchingEdge, pFirstTriangle, dataAccessor);
+    }
+    else
+    {
+        faceAngle = REAL(0.0);
+    }
+
+    faceAngles->assignFacesAngleIntoStorage(currEdge[0].m_triIdx, firstVertexStartIndex, faceAngle);
+    dMeshTriangleVertex secondVertexStartIndex = currEdge[1].getEdgeStartVertexIndex();
+    faceAngles->assignFacesAngleIntoStorage(currEdge[1].m_triIdx, secondVertexStartIndex, faceAngle);
+}
+
+template<class TMeshDataAccessor>
+/*static */
+dReal dxTriDataBase::calculateEdgeAngleValidated(unsigned firstVertexStartIndex,
+    EdgeRecord *currEdge, const dReal &normalSegmentDot, const dReal &lengthSquareProduct,
+    const dVector3 &triangleNormal, const dVector3 &secondOppositeVertexSegment,
+    const dVector3 *pSecondTriangleMatchingEdge/*=NULL*/, const dVector3 *pFirstTriangle/*=NULL*/, 
+    const TMeshDataAccessor &dataAccessor)
+{
+    dIASSERT(lengthSquareProduct >= REAL(0.0));
+
+    dReal result;
+    dReal angleCosine = normalSegmentDot / dSqrt(lengthSquareProduct);
+
+    if (angleCosine < REAL(1.0))
+    {
+        dVector3 normalSecondOppositeSegmentCross;
+        dCalcVectorCross3(normalSecondOppositeSegmentCross, triangleNormal, secondOppositeVertexSegment);
+
+        dReal secondTriangleEdgeDirectionCheck;
+
+        if (pSecondTriangleMatchingEdge != NULL)
+        {
+            // Check the cross product against the second triangle edge, if possible...
+            secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, *pSecondTriangleMatchingEdge);
+        }
+        else
+        {
+            // ...if not, calculate the supposed direction of the second triangle's edge 
+            // as negative of first triangle edge. For that cross-multiply the precomputed
+            // first triangle normal by vector from the degenerate edge to its opposite vertex.
+
+            // Retrieve the first triangle points if necessary
+            dVector3 firstTriangleStorage[dMTV__MAX];
+            const dVector3 *pFirstTriangleToUse = pFirstTriangle;
+
+            if (pFirstTriangle == NULL)
+            {
+                dataAccessor.getTriangleVertexPoints(firstTriangleStorage, currEdge[0].m_triIdx);
+                pFirstTriangleToUse = &firstTriangleStorage[dMTV__MIN];
+            }
+
+            // Calculate the opposite vector
+            unsigned firstTriangleOppositeIndex = firstVertexStartIndex != dMTV__MIN ? firstVertexStartIndex - 1 : dMTV__MAX - 1;
+
+            dVector3 firstOppositeVertexSegment;
+            dSubtractVectors3(firstOppositeVertexSegment, pFirstTriangleToUse[firstTriangleOppositeIndex], pFirstTriangleToUse[firstVertexStartIndex]);
+
+            dVector3 normalFirstOppositeSegmentCross;
+            dCalcVectorCross3(normalFirstOppositeSegmentCross, triangleNormal, firstOppositeVertexSegment);
+
+            // And finally calculate the dot product to compare vector directions
+            secondTriangleEdgeDirectionCheck = dCalcVectorDot3(normalSecondOppositeSegmentCross, normalFirstOppositeSegmentCross);
+        }
+
+        // Negative product means the angle absolute value is less than M_PI_2, positive - greater.
+        result = secondTriangleEdgeDirectionCheck < REAL(0.0) ? dAsin(angleCosine) : (dReal)M_PI_2 + dAcos(angleCosine);
+    }
+    else
+    {
+        result = (dReal)M_PI_2;
+        dIASSERT(angleCosine - REAL(1.0) < 1e-4); // The computational error can not be too high because the dot product had been verified to be greater than the concave threshold above
+    }
+
+    return result;
+}
+
+
+#endif // #if dTRIMESH_ENABLED
+
+
+#endif // #ifndef _ODE_COLLISION_TRIMESH_INTERNAL_IMPL_H_

thirdparty/ode-0.16.5/ode/src/collision_trimesh_opcode.cpp

@@ -0,0 +1,767 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2024
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
+
+#include "collision_util.h"
+#include "collision_trimesh_opcode.h"
+#include "collision_trimesh_internal_impl.h"
+#include <algorithm>
+
+
+//////////////////////////////////////////////////////////////////////////
+// TrimeshCollidersCache
+
+void TrimeshCollidersCache::initOPCODECaches()
+{
+    m_RayCollider.SetDestination(&m_Faces);
+
+    /* -- not used
+    _PlanesCollider.SetTemporalCoherence(true);
+    */
+
+    m_SphereCollider.SetTemporalCoherence(true);
+    m_SphereCollider.SetPrimitiveTests(false);
+
+    m_OBBCollider.SetTemporalCoherence(true);
+
+    // no first-contact test (i.e. return full contact info)
+    m_AABBTreeCollider.SetFirstContact( false );     
+    // temporal coherence only works with "first contact" tests
+    m_AABBTreeCollider.SetTemporalCoherence(false);
+    // Perform full BV-BV tests (true) or SAT-lite tests (false)
+    m_AABBTreeCollider.SetFullBoxBoxTest( true );
+    // Perform full Primitive-BV tests (true) or SAT-lite tests (false)
+    m_AABBTreeCollider.SetFullPrimBoxTest( true );
+    const char* msg;
+    if ((msg =m_AABBTreeCollider.ValidateSettings()))
+    {
+        dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
+    }
+
+    /* -- not used
+    _LSSCollider.SetTemporalCoherence(false);
+    _LSSCollider.SetPrimitiveTests(false);
+    _LSSCollider.SetFirstContact(false);
+    */
+}
+
+void TrimeshCollidersCache::clearOPCODECaches()
+{
+    m_Faces.Empty();
+    m_DefaultSphereCache.TouchedPrimitives.Empty();
+    m_DefaultBoxCache.TouchedPrimitives.Empty();
+    m_DefaultCapsuleCache.TouchedPrimitives.Empty();
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Trimesh data
+
+dxTriMeshData::~dxTriMeshData()
+{
+    if ( m_InternalUseFlags != NULL )
+    {
+        sizeint flagsMemoryRequired = calculateUseFlagsMemoryRequirement();
+        dFree(m_InternalUseFlags, flagsMemoryRequired);
+    }
+}
+
+void dxTriMeshData::buildData(const Point *Vertices, int VertexStide, unsigned VertexCount,
+    const IndexedTriangle *Indices, unsigned IndexCount, int TriStride,
+    const dReal *in_Normals,
+    bool Single)
+{
+    dxTriMeshData_Parent::buildData(Vertices, VertexStide, VertexCount, Indices, IndexCount, TriStride, in_Normals, Single);
+    dAASSERT(IndexCount % dMTV__MAX == 0);
+
+    m_Mesh.SetNbTriangles(IndexCount / dMTV__MAX);
+    m_Mesh.SetNbVertices(VertexCount);
+    m_Mesh.SetPointers(Indices, Vertices);
+    m_Mesh.SetStrides(TriStride, VertexStide);
+    m_Mesh.SetSingle(Single);
+
+    // Build tree
+    // recommended in Opcode User Manual
+    //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
+    // used in ODE, why?
+    //Settings.mRules = SPLIT_BEST_AXIS;
+    // best compromise?
+    BuildSettings Settings(SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER);
+
+    OPCODECREATE TreeBuilder(&m_Mesh, Settings, true, false);
+
+    m_BVTree.Build(TreeBuilder);
+
+    // compute model space AABB
+    dVector3 AABBMax, AABBMin;
+    calculateDataAABB(AABBMax, AABBMin);
+
+    dAddVectors3(m_AABBCenter, AABBMin, AABBMax);
+    dScaleVector3(m_AABBCenter, REAL(0.5));
+
+    dSubtractVectors3(m_AABBExtents, AABBMax, m_AABBCenter);
+
+    // user data (not used by OPCODE)
+    dIASSERT(m_InternalUseFlags == NULL);
+}
+
+
+void dxTriMeshData::calculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin)
+{
+    if (isSingle()) 
+    {
+        templateCalculateDataAABB<float>(AABBMax, AABBMin);
+    } 
+    else 
+    {
+        templateCalculateDataAABB<double>(AABBMax, AABBMin);
+    }
+}
+
+template<typename treal>
+void dxTriMeshData::templateCalculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin)
+{
+    dIASSERT(isSingle() == (sizeof(treal) == sizeof(float)));
+
+    const Point *vertices = retrieveVertexInstances();
+    const int vertexStide = retrieveVertexStride();
+    const unsigned vertexCount = retrieveVertexCount();
+
+    AABBMax[dV3E_X] = AABBMax[dV3E_Y] = AABBMax[dV3E_Z] = -dInfinity;
+    AABBMin[dV3E_X] = AABBMin[dV3E_Y] = AABBMin[dV3E_Z] = dInfinity;
+    dSASSERT(dV3E__AXES_COUNT == 3);
+
+    const uint8 *verts = (const uint8 *)vertices;
+    for( unsigned i = 0; i < vertexCount; ++i ) 
+    {
+        const treal *v = (const treal *)verts;
+        if( v[dSA_X] > AABBMax[dV3E_X] ) AABBMax[dV3E_X] = (dReal)v[dSA_X];
+        if( v[dSA_X] < AABBMin[dV3E_X] ) AABBMin[dV3E_X] = (dReal)v[dSA_X];
+        if( v[dSA_Y] > AABBMax[dV3E_Y] ) AABBMax[dV3E_Y] = (dReal)v[dSA_Y];
+        if( v[dSA_Y] < AABBMin[dV3E_Y] ) AABBMin[dV3E_Y] = (dReal)v[dSA_Y];
+        if( v[dSA_Z] > AABBMax[dV3E_Z] ) AABBMax[dV3E_Z] = (dReal)v[dSA_Z];
+        if( v[dSA_Z] < AABBMin[dV3E_Z] ) AABBMin[dV3E_Z] = (dReal)v[dSA_Z];
+        verts += vertexStide;
+    }
+}
+
+
+bool dxTriMeshData::preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/)
+{
+    bool buildUseFlagsToUse = buildUseFlags;
+    FaceAngleStorageMethod faceAndgesRequirementToUse = faceAndgesRequirement;
+
+    if (buildUseFlags && haveUseFlagsBeenBuilt())
+    {
+        dUASSERT(false, "Another request to build edge/vertex use flags after they had already been built");
+
+        buildUseFlagsToUse = false;
+    }
+
+    if (faceAndgesRequirement != ASM__INVALID && haveFaceAnglesBeenBuilt())
+    {
+        dUASSERT(false, "Another request to build face angles after they had already been built");
+
+        faceAndgesRequirementToUse = ASM__INVALID;
+    }
+
+    // If this mesh has already been preprocessed, exit
+    bool result = (!buildUseFlagsToUse && faceAndgesRequirementToUse == ASM__INVALID) || m_Mesh.GetNbTriangles() == 0 
+        || meaningfulPreprocessData(buildUseFlagsToUse, faceAndgesRequirementToUse);
+    return result;
+}
+
+struct TrimeshDataVertexIndexAccessor_OPCODE
+{
+    TrimeshDataVertexIndexAccessor_OPCODE(const IndexedTriangle *triIndicesBegin, unsigned triStride):
+        m_TriIndicesBegin(triIndicesBegin),
+        m_TriStride(triStride)
+    {
+    }
+
+    void getTriangleVertexIndices(unsigned out_VertexIndices[dMTV__MAX], unsigned triangleIdx) const
+    {
+        const IndexedTriangle *triIndicesBegin = m_TriIndicesBegin;
+        const unsigned triStride = m_TriStride;
+
+        const IndexedTriangle *triIndicesOfInterest = (const IndexedTriangle *)((const uint8 *)triIndicesBegin + triangleIdx * (sizeint)triStride);
+        std::copy(triIndicesOfInterest->mVRef, triIndicesOfInterest->mVRef + dMTV__MAX, out_VertexIndices);
+        dSASSERT(dMTV__MAX == dARRAY_SIZE(triIndicesOfInterest->mVRef));
+        dSASSERT(dMTV_FIRST == 0);
+        dSASSERT(dMTV_SECOND == 1);
+        dSASSERT(dMTV_THIRD == 2);
+        dSASSERT(dMTV__MAX == 3);
+    }
+
+
+    const IndexedTriangle   *m_TriIndicesBegin;
+    unsigned                m_TriStride;
+};
+
+struct TrimeshDataTrianglePointAccessor_OPCODE
+{
+    TrimeshDataTrianglePointAccessor_OPCODE(const MeshInterface &mesh):
+        m_Mesh(mesh)
+    {
+    }
+
+    void getTriangleVertexPoints(dVector3 out_Points[dMTV__MAX], unsigned triangleIndex) const
+    {
+        VertexPointers vpTriangle;
+        ConversionArea vc;
+        m_Mesh.GetTriangle(vpTriangle, triangleIndex, vc);
+
+        for (unsigned pointIndex = 0; pointIndex != 3; ++pointIndex)
+        {
+            dAssignVector3(out_Points[pointIndex], vpTriangle.Vertex[pointIndex]->x, vpTriangle.Vertex[pointIndex]->y, vpTriangle.Vertex[pointIndex]->z);
+        }
+        dSASSERT(dMTV_FIRST == 0);
+        dSASSERT(dMTV_SECOND == 1);
+        dSASSERT(dMTV_THIRD == 2);
+        dSASSERT(dMTV__MAX == 3);
+    }
+
+    const MeshInterface     &m_Mesh;
+};
+
+bool dxTriMeshData::meaningfulPreprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/)
+{
+    const bool buildFaceAngles = faceAndgesRequirement != ASM__INVALID;
+    dIASSERT(buildUseFlags || buildFaceAngles);
+    dIASSERT(!buildUseFlags || !haveUseFlagsBeenBuilt());
+    dIASSERT(!buildFaceAngles || !haveFaceAnglesBeenBuilt());
+
+    bool result = false;
+
+    uint8 *useFlags = NULL;
+    sizeint flagsMemoryRequired = 0;
+    bool flagsAllocated = false, anglesAllocated = false;
+
+    do 
+    {
+        if (buildUseFlags)
+        {
+            flagsMemoryRequired = calculateUseFlagsMemoryRequirement();
+            useFlags = (uint8 *)dAlloc(flagsMemoryRequired);
+
+            if (useFlags == NULL)
+            {
+                break;
+            }
+        }
+
+        flagsAllocated = true;
+
+        if (buildFaceAngles)
+        {
+            if (!allocateFaceAngles(faceAndgesRequirement))
+            {
+                break;
+            }
+        }
+
+        anglesAllocated = true;
+
+        const unsigned int numTris = m_Mesh.GetNbTriangles();
+        const unsigned int numVertices = m_Mesh.GetNbVertices();
+        sizeint numEdges = (sizeint)numTris * dMTV__MAX;
+        dIASSERT(numVertices <= numEdges); // Edge records are going to be used for vertex data as well
+
+        const sizeint recordsMemoryRequired = dEFFICIENT_SIZE(numEdges * sizeof(EdgeRecord));
+        const sizeint verticesMemoryRequired = /*dEFFICIENT_SIZE*/(numVertices * sizeof(VertexRecord)); // Skip alignment for the last chunk
+        const sizeint totalTempMemoryRequired = recordsMemoryRequired + verticesMemoryRequired;
+        void *tempBuffer = dAlloc(totalTempMemoryRequired);
+        
+        if (tempBuffer == NULL)
+        {
+            break;
+        }
+
+        EdgeRecord *edges = (EdgeRecord *)tempBuffer;
+        VertexRecord *vertices = (VertexRecord *)((uint8 *)tempBuffer + recordsMemoryRequired);
+
+        // Delay zero-filling until all the allocations succeed
+        if (useFlags != NULL)
+        {
+            memset(useFlags, 0, flagsMemoryRequired);
+        }
+
+        const IndexedTriangle *triIndicesBegin = m_Mesh.GetTris();
+        unsigned triStride = m_Mesh.GetTriStride();
+        TrimeshDataVertexIndexAccessor_OPCODE indexAccessor(triIndicesBegin, triStride);
+        meaningfulPreprocess_SetupEdgeRecords(edges, numEdges, indexAccessor);
+
+        // Sort the edges, so the ones sharing the same verts are beside each other
+        std::sort(edges, edges + numEdges);
+
+        TrimeshDataTrianglePointAccessor_OPCODE pointAccessor(m_Mesh);
+        const dReal *const externalNormals = retrieveNormals();
+        IFaceAngleStorageControl *faceAngles = retrieveFaceAngles();
+        meaningfulPreprocess_buildEdgeFlags(useFlags, faceAngles, edges, numEdges, vertices, externalNormals, pointAccessor);
+
+        dFree(tempBuffer, totalTempMemoryRequired);
+    	
+        if (buildUseFlags)
+        {
+            m_InternalUseFlags = useFlags;
+        }
+
+        result = true;
+    }
+    while (false);
+
+    if (!result)
+    {
+        if (flagsAllocated)
+        {
+            if (anglesAllocated)
+            {
+                if (buildFaceAngles)
+                {
+                    freeFaceAngles();
+                }
+            }
+
+            if (buildUseFlags)
+            {
+                dFree(useFlags, flagsMemoryRequired);
+            }
+        }
+    }
+
+    return result;
+}
+
+
+void dxTriMeshData::updateData()
+{
+    m_BVTree.Refit();
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////
+// dxTriMesh
+
+dxTriMesh::~dxTriMesh()
+{
+    //
+}
+
+void dxTriMesh::clearTCCache()
+{
+    /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
+    but the destructor isn't called when doing this, so we would leak.
+    So, call the previous caches' containers' destructors by hand first. */
+    int i, n;
+
+    n = m_SphereTCCache.size();
+    for( i = 0; i != n; ++i ) 
+    {
+        m_SphereTCCache[i].~SphereTC();
+    }
+    m_SphereTCCache.setSize(0);
+
+    n = m_BoxTCCache.size();
+    for( i = 0; i != n; ++i ) 
+    {
+        m_BoxTCCache[i].~BoxTC();
+    }
+    m_BoxTCCache.setSize(0);
+
+    n = m_CapsuleTCCache.size();
+    for( i = 0; i != n; ++i ) 
+    {
+        m_CapsuleTCCache[i].~CapsuleTC();
+    }
+    m_CapsuleTCCache.setSize(0);
+}
+
+
+bool dxTriMesh::controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize)
+{
+    if (controlClass == dGeomColliderControlClass) 
+    {
+        if (controlCode == dGeomCommonAnyControlCode) 
+        {
+            return checkControlValueSizeValidity(dataValue, dataSize, 0);
+        }
+        else if (controlCode == dGeomColliderSetMergeSphereContactsControlCode) 
+        {
+            return checkControlValueSizeValidity(dataValue, dataSize, sizeof(int)) 
+                && controlGeometry_SetMergeSphereContacts(*(int *)dataValue);
+        }
+        else if (controlCode == dGeomColliderGetMergeSphereContactsControlCode) 
+        {
+            return checkControlValueSizeValidity(dataValue, dataSize, sizeof(int)) 
+                && controlGeometry_GetMergeSphereContacts(*(int *)dataValue);
+        }
+    }
+
+    return dxTriMesh_Parent::controlGeometry(controlClass, controlCode, dataValue, dataSize);
+}
+
+bool dxTriMesh::controlGeometry_SetMergeSphereContacts(int dataValue)
+{
+    if (dataValue == dGeomColliderMergeContactsValue__Default) 
+    {
+        m_SphereContactsMergeOption = (dxContactMergeOptions)MERGE_NORMALS__SPHERE_DEFAULT;
+    }
+    else if (dataValue == dGeomColliderMergeContactsValue_None) 
+    {
+        m_SphereContactsMergeOption = DONT_MERGE_CONTACTS;
+    }
+    else if (dataValue == dGeomColliderMergeContactsValue_Normals) 
+    {
+        m_SphereContactsMergeOption = MERGE_CONTACT_NORMALS;
+    }
+    else if (dataValue == dGeomColliderMergeContactsValue_Full) 
+    {
+        m_SphereContactsMergeOption = MERGE_CONTACTS_FULLY;
+    }
+    else 
+    {
+        dAASSERT(false && "Invalid contact merge control value");
+        return false;
+    }
+
+    return true;
+}
+
+bool dxTriMesh::controlGeometry_GetMergeSphereContacts(int &returnValue)
+{
+    if (m_SphereContactsMergeOption == DONT_MERGE_CONTACTS) {
+        returnValue = dGeomColliderMergeContactsValue_None;
+    }
+    else if (m_SphereContactsMergeOption == MERGE_CONTACT_NORMALS) {
+        returnValue = dGeomColliderMergeContactsValue_Normals;
+    }
+    else if (m_SphereContactsMergeOption == MERGE_CONTACTS_FULLY) {
+        returnValue = dGeomColliderMergeContactsValue_Full;
+    }
+    else {
+        dIASSERT(false && "Internal error: unexpected contact merge option field value");
+        return false;
+    }
+
+    return true;
+}
+
+
+/*virtual */
+void dxTriMesh::computeAABB() 
+{
+    const dxTriMeshData *meshData = getMeshData();
+    dVector3 c;
+    const dMatrix3& R = final_posr->R;
+    const dVector3& pos = final_posr->pos;
+
+    dMultiply0_331( c, R, meshData->m_AABBCenter );
+
+    dReal xrange = dFabs(R[0] * meshData->m_AABBExtents[0]) +
+        dFabs(R[1] * meshData->m_AABBExtents[1]) + 
+        dFabs(R[2] * meshData->m_AABBExtents[2]);
+    dReal yrange = dFabs(R[4] * meshData->m_AABBExtents[0]) +
+        dFabs(R[5] * meshData->m_AABBExtents[1]) + 
+        dFabs(R[6] * meshData->m_AABBExtents[2]);
+    dReal zrange = dFabs(R[8] * meshData->m_AABBExtents[0]) +
+        dFabs(R[9] * meshData->m_AABBExtents[1]) + 
+        dFabs(R[10] * meshData->m_AABBExtents[2]);
+
+    aabb[0] = c[0] + pos[0] - xrange;
+    aabb[1] = c[0] + pos[0] + xrange;
+    aabb[2] = c[1] + pos[1] - yrange;
+    aabb[3] = c[1] + pos[1] + yrange;
+    aabb[4] = c[2] + pos[2] - zrange;
+    aabb[5] = c[2] + pos[2] + zrange;
+}
+
+
+void dxTriMesh::fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index)
+{
+    const dVector3 &position = buildUpdatedPosition();
+    const dMatrix3 &rotation = buildUpdatedRotation();
+    fetchMeshTriangle(pout_triangle, index, position, rotation);
+}
+
+void dxTriMesh::fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index)
+{
+    const dVector3 &position = buildUpdatedPosition();
+    const dMatrix3 &rotation = buildUpdatedRotation();
+    fetchMeshTriangle(out_triangle, index, position, rotation);
+}
+
+void dxTriMesh::fetchMeshTriangle(dVector3 *const pout_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const
+{
+    dIASSERT(dIN_RANGE(index, 0, getMeshTriangleCount()));
+
+    VertexPointers VP;
+    ConversionArea VC;
+
+    const dxTriMeshData *meshData = getMeshData();
+    meshData->m_Mesh.GetTriangle(VP, index, VC);
+
+    for (unsigned i = 0; i != 3; ++i)
+    {
+        if (pout_triangle[i] != NULL)
+        {
+            dVector3 v;
+            v[dV3E_X] = VP.Vertex[i]->x;
+            v[dV3E_Y] = VP.Vertex[i]->y;
+            v[dV3E_Z] = VP.Vertex[i]->z;
+
+            dVector3 &out_triangle = *(pout_triangle[i]);
+            dMultiply0_331(out_triangle, rotation, v);
+            dAddVectors3(out_triangle, out_triangle, position);
+            out_triangle[dV3E_PAD] = REAL(0.0);
+        }
+    }
+}
+
+void dxTriMesh::fetchMeshTriangle(dVector3 out_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const
+{
+    dIASSERT(dIN_RANGE(index, 0, getMeshTriangleCount()));
+
+    VertexPointers VP;
+    ConversionArea VC;
+
+    const dxTriMeshData *meshData = getMeshData();
+    meshData->m_Mesh.GetTriangle(VP, index, VC);
+
+    for (unsigned i = 0; i != 3; ++i)
+    {
+        dVector3 v;
+        v[dV3E_X] = VP.Vertex[i]->x;
+        v[dV3E_Y] = VP.Vertex[i]->y;
+        v[dV3E_Z] = VP.Vertex[i]->z;
+
+        dMultiply0_331(out_triangle[i], rotation, v);
+        dAddVectors3(out_triangle[i], out_triangle[i], position);
+        out_triangle[i][dV3E_PAD] = REAL(0.0);
+    }
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern */
+dTriMeshDataID dGeomTriMeshDataCreate()
+{
+    return new dxTriMeshData();
+}
+
+/*extern */
+void dGeomTriMeshDataDestroy(dTriMeshDataID g)
+{
+    dxTriMeshData *mesh = g;
+    delete mesh;
+}
+
+
+/*extern */
+void dGeomTriMeshDataSet(dTriMeshDataID g, int dataId, void *pDataLocation)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    dxTriMeshData *data = g;
+
+    switch (dataId)
+    {
+        case dTRIMESHDATA_FACE_NORMALS:
+        {
+            data->assignNormals((const dReal *)pDataLocation);
+            break;
+        }
+
+        case dTRIMESHDATA_USE_FLAGS:
+        {
+            data->assignExternalUseFlagsBuffer((uint8 *)pDataLocation);
+            break;
+        }
+
+        // case dTRIMESHDATA__MAX: -- To be located by Find in Files
+        default:
+        {
+            dUASSERT(dataId, "invalid data type");
+            break;
+        }
+    }
+}
+
+static void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize);
+
+/*extern */
+void *dGeomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize)
+{
+    return geomTriMeshDataGet(g, dataId, NULL);
+}
+
+/*extern */
+void *dGeomTriMeshDataGet2(dTriMeshDataID g, int dataId, sizeint *pOutDataSize)
+{
+    return geomTriMeshDataGet(g, dataId, pOutDataSize);
+}
+
+static 
+void *geomTriMeshDataGet(dTriMeshDataID g, int dataId, sizeint *pOutDataSize)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    const dxTriMeshData *data = g;
+
+    void *result = NULL;
+
+    switch (dataId)
+    {
+        case dTRIMESHDATA_FACE_NORMALS:
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = data->calculateNormalsMemoryRequirement();
+            }
+
+            result = (void *)data->retrieveNormals();
+            break;
+        }
+
+        case dTRIMESHDATA_USE_FLAGS:
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = data->calculateUseFlagsMemoryRequirement();
+            }
+
+            result = const_cast<uint8 *>(data->smartRetrieveUseFlags());
+            break;
+        }
+
+        // case dTRIMESHDATA__MAX: -- To be located by Find in Files
+        default:
+        {
+            if (pOutDataSize != NULL)
+            {
+                *pOutDataSize = 0;
+            }
+
+            dUASSERT(dataId, "invalid data type");
+            break;
+        }
+    }
+
+    return result;
+}
+
+
+/*extern */
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    dxTriMeshData *data = g;
+    data->buildData((const Point *)Vertices, VertexStride, VertexCount, 
+        (const IndexedTriangle *)Indices, IndexCount, TriStride, 
+        (const dReal *)Normals, 
+        true);
+}
+
+/*extern */
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
+    const void* Vertices, int VertexStride, int VertexCount, 
+    const void* Indices, int IndexCount, int TriStride,
+    const void* Normals)
+{
+    dUASSERT(g, "The argument is not a trimesh data");
+
+    g->buildData((const Point *)Vertices, VertexStride, VertexCount, 
+        (const IndexedTriangle *)Indices, IndexCount, TriStride, 
+        (const dReal *)Normals, 
+        false);
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+/*extern */
+dGeomID dCreateTriMesh(dSpaceID space, 
+    dTriMeshDataID Data,
+    dTriCallback* Callback,
+    dTriArrayCallback* ArrayCallback,
+    dTriRayCallback* RayCallback)
+{
+    dxTriMesh *mesh = new dxTriMesh(space, Data, Callback, ArrayCallback, RayCallback);
+    return mesh;
+}
+
+
+/*extern */
+void dGeomTriMeshSetLastTransform(dGeomID g, const dMatrix4 last_trans )
+{
+    dAASSERT(g);
+    dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    mesh->assignLastTransform(last_trans);
+}
+
+/*extern */
+const dReal *dGeomTriMeshGetLastTransform(dGeomID g)
+{
+    dAASSERT(g);
+    dUASSERT(g->type == dTriMeshClass, "The geom is not a trimesh");
+
+    dxTriMesh *mesh = static_cast<dxTriMesh *>(g);
+    return mesh->retrieveLastTransform();
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+
+// Cleanup for allocations when shutting down ODE
+/*extern */
+void opcode_collider_cleanup()
+{
+#if !dTLS_ENABLED
+
+    // Clear TC caches
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(0);
+    pccColliderCache->clearOPCODECaches();
+
+#endif // dTLS_ENABLED
+}
+
+
+#endif // dTRIMESH_ENABLED && dTRIMESH_OPCODE
+

thirdparty/ode-0.16.5/ode/src/collision_trimesh_opcode.h

@@ -0,0 +1,333 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+// Modified for FreeSOLID Compatibility by Rodrigo Hernandez
+// Trimesh caches separation by Oleh Derevenko
+// TriMesh storage classes refactoring and face angle computation code by Oleh Derevenko (C) 2016-2024
+
+
+#ifndef _ODE_COLLISION_TRIMESH_OPCODE_H_
+#define _ODE_COLLISION_TRIMESH_OPCODE_H_
+
+
+#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
+
+//****************************************************************************
+// dxTriMesh class
+
+
+#include "collision_kernel.h"
+#include "collision_trimesh_colliders.h"
+#include "collision_util.h"
+#include <ode/collision_trimesh.h>
+
+#include "collision_trimesh_internal.h"
+
+#define BAN_OPCODE_AUTOLINK
+#include "Opcode.h"
+using namespace Opcode;
+
+
+#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+
+// New trimesh collider hash table types
+enum
+{
+    MAXCONTACT_X_NODE = 4,
+    CONTACTS_HASHSIZE = 256
+};
+
+struct CONTACT_KEY
+{
+    dContactGeom * m_contact;
+    unsigned int m_key;
+};
+
+struct CONTACT_KEY_HASH_NODE
+{
+    CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
+    int m_keycount;
+};
+
+struct CONTACT_KEY_HASH_TABLE
+{
+public:
+    CONTACT_KEY_HASH_NODE &operator[](unsigned int index) { return m_storage[index]; }
+
+private:
+    CONTACT_KEY_HASH_NODE m_storage[CONTACTS_HASHSIZE];
+};
+
+#endif // !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+
+
+struct VertexUseCache
+{
+public:
+    VertexUseCache(): m_VertexUseBits(NULL), m_VertexUseElements(0) {}
+    ~VertexUseCache() { freeVertexUSEDFlags();  }
+
+    bool resizeAndResetVertexUSEDFlags(unsigned VertexCount)
+    {
+        bool Result = false;
+        sizeint VertexNewElements = (VertexCount + 7) / 8;
+        if (VertexNewElements <= m_VertexUseElements || reallocVertexUSEDFlags(VertexNewElements)) {
+            memset(m_VertexUseBits, 0, VertexNewElements);
+            Result = true;
+        }
+        return Result;
+    }
+
+    bool getVertexUSEDFlag(unsigned VertexIndex) const { return (m_VertexUseBits[VertexIndex / 8] & (1 << (VertexIndex % 8))) != 0; }
+    void setVertexUSEDFlag(unsigned VertexIndex) { m_VertexUseBits[VertexIndex / 8] |= (1 << (VertexIndex % 8)); }
+
+private:
+    bool reallocVertexUSEDFlags(sizeint VertexNewElements)
+    {
+        bool Result = false;
+        uint8 *VertexNewBits = (uint8 *)dRealloc(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]), VertexNewElements * sizeof(m_VertexUseBits[0]));
+        if (VertexNewBits) {
+            m_VertexUseBits = VertexNewBits;
+            m_VertexUseElements = VertexNewElements;
+            Result = true;
+        }
+        return Result;
+    }
+
+    void freeVertexUSEDFlags()
+    {
+        dFree(m_VertexUseBits, m_VertexUseElements * sizeof(m_VertexUseBits[0]));
+        m_VertexUseBits = NULL;
+        m_VertexUseElements = 0;
+    }
+
+private:
+    uint8 *m_VertexUseBits;
+    sizeint m_VertexUseElements;
+};
+
+
+struct TrimeshCollidersCache
+{
+    TrimeshCollidersCache()
+    {
+        initOPCODECaches();
+    }
+
+    void initOPCODECaches();
+    void clearOPCODECaches();
+
+    // Collider caches
+    BVTCache ColCache;
+
+#if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+    CONTACT_KEY_HASH_TABLE m_hashcontactset;
+#endif
+
+    // Colliders
+    /* -- not used -- also uncomment in InitOPCODECaches()
+    PlanesCollider _PlanesCollider; -- not used 
+    */
+    SphereCollider m_SphereCollider;
+    OBBCollider m_OBBCollider;
+    RayCollider m_RayCollider;
+    AABBTreeCollider m_AABBTreeCollider;
+    /* -- not used -- also uncomment in InitOPCODECaches()
+    LSSCollider _LSSCollider;
+    */
+    // Trimesh caches
+    CollisionFaces m_Faces;
+    SphereCache m_DefaultSphereCache;
+    OBBCache m_DefaultBoxCache;
+    LSSCache m_DefaultCapsuleCache;
+
+    // Trimesh-plane collision vertex use cache
+    VertexUseCache m_VertexUses;
+};
+
+
+typedef dxTriDataBase dxTriMeshData_Parent;
+struct dxTriMeshData:
+    public dxTriMeshData_Parent
+{
+public:
+    dxTriMeshData():
+        dxTriMeshData_Parent(),
+        m_ExternalUseFlags(NULL),
+        m_InternalUseFlags(NULL)
+    {
+    }
+
+    ~dxTriMeshData();
+
+    void buildData(const Point *Vertices, int VertexStide, unsigned VertexCount,
+        const IndexedTriangle *Indices, unsigned IndexCount, int TriStride,
+        const dReal *in_Normals,
+        bool Single);
+
+private:
+    void calculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin);
+    template<typename treal>
+    void templateCalculateDataAABB(dVector3 &AABBMax, dVector3 &AABBMin);
+
+public:
+    /* Setup the UseFlags array and/or build face angles*/
+    bool preprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
+
+private:
+    bool meaningfulPreprocessData(bool buildUseFlags/*=false*/, FaceAngleStorageMethod faceAndgesRequirement/*=ASM__INVALID*/);
+
+public:
+    /* For when app changes the vertices */
+    void updateData();
+
+public:
+    const Point *retrieveVertexInstances() const { return (const Point *)dxTriMeshData_Parent::retrieveVertexInstances(); }
+
+public:
+    void assignNormals(const dReal *normals) { dxTriMeshData_Parent::assignNormals(normals); }
+    const dReal *retrieveNormals() const { return (const dReal *)dxTriMeshData_Parent::retrieveNormals(); }
+    sizeint calculateNormalsMemoryRequirement() const { return retrieveTriangleCount() * (sizeof(dReal) * dSA__MAX); }
+
+public:
+    void assignExternalUseFlagsBuffer(uint8 *buffer) { m_ExternalUseFlags = buffer != m_InternalUseFlags ? buffer : NULL; }
+    const uint8 *smartRetrieveUseFlags() const { return m_ExternalUseFlags != NULL ? m_ExternalUseFlags : m_InternalUseFlags; }
+    bool haveUseFlagsBeenBuilt() const { return m_InternalUseFlags != NULL; }
+    sizeint calculateUseFlagsMemoryRequirement() const { return m_Mesh.GetNbTriangles() * sizeof(m_InternalUseFlags[0]); }
+
+public:
+    Model m_BVTree;
+    MeshInterface m_Mesh;
+
+    /* aabb in model space */
+    dVector3 m_AABBCenter;
+    dVector3 m_AABBExtents;
+
+    // data for use in collision resolution
+    uint8 *m_ExternalUseFlags;
+    uint8 *m_InternalUseFlags;
+
+};
+
+
+typedef dxMeshBase dxTriMesh_Parent;
+struct dxTriMesh: 
+    public dxTriMesh_Parent
+{
+public:
+    // Functions
+    dxTriMesh(dxSpace *Space, dxTriMeshData *Data, 
+        dTriCallback *Callback, dTriArrayCallback *ArrayCallback, dTriRayCallback *RayCallback):
+        dxTriMesh_Parent(Space, Data, Callback, ArrayCallback, RayCallback, false)
+    {
+        m_SphereContactsMergeOption = (dxContactMergeOptions)MERGE_NORMALS__SPHERE_DEFAULT;
+
+        dZeroMatrix4(m_last_trans);
+    }
+
+    ~dxTriMesh();
+
+    void clearTCCache();
+
+    bool controlGeometry(int controlClass, int controlCode, void *dataValue, int *dataSize);
+
+    virtual void computeAABB();
+
+public:
+    dxTriMeshData *retrieveMeshData() const { return getMeshData(); }
+    const dReal *retrieveMeshNormals() const { return getMeshData()->retrieveNormals(); }
+    Model &retrieveMeshBVTreeRef() const { return getMeshData()->m_BVTree; }
+    const uint8 *retrieveMeshSmartUseFlags() const { return getMeshData()->smartRetrieveUseFlags(); }
+
+    unsigned getMeshTriangleCount() const { return getMeshData()->m_Mesh.GetNbTriangles(); }
+    void fetchMeshTransformedTriangle(dVector3 *const pout_triangle[3], unsigned index)/* const*/;
+    void fetchMeshTransformedTriangle(dVector3 out_triangle[3], unsigned index)/* const*/;
+    void fetchMeshTriangle(dVector3 *const pout_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const;
+    void fetchMeshTriangle(dVector3 out_triangle[3], unsigned index, const dVector3 position, const dMatrix3 rotation) const;
+
+public:
+    void assignLastTransform(const dMatrix4 last_trans) { dCopyMatrix4x4(m_last_trans, last_trans); }
+    const dReal *retrieveLastTransform() const { return m_last_trans; }
+
+private:
+    enum
+    {
+        MERGE_NORMALS__SPHERE_DEFAULT = DONT_MERGE_CONTACTS
+    };
+
+    bool controlGeometry_SetMergeSphereContacts(int dataValue);
+    bool controlGeometry_GetMergeSphereContacts(int &returnValue);
+
+private:
+    dxTriMeshData *getMeshData() const { return static_cast<dxTriMeshData *>(dxTriMesh_Parent::getMeshData()); }
+
+public:
+    // Some constants
+    // Temporal coherence
+    struct SphereTC : public SphereCache{
+        dxGeom* Geom;
+    };
+
+    struct BoxTC : public OBBCache{
+        dxGeom* Geom;
+    };
+
+    struct CapsuleTC : public LSSCache{
+        dxGeom* Geom;
+    };
+
+public:
+    // Contact merging option
+    dxContactMergeOptions m_SphereContactsMergeOption;
+    // Instance data for last transform.
+    dMatrix4 m_last_trans;
+
+    dArray<SphereTC> m_SphereTCCache;
+    dArray<BoxTC> m_BoxTCCache;
+    dArray<CapsuleTC> m_CapsuleTCCache;
+};
+
+
+static inline 
+Matrix4x4 &MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4 &Out)
+{
+    return Out.Set(
+        Rotation[0], Rotation[4], Rotation[8], 0.0f,
+        Rotation[1], Rotation[5], Rotation[9], 0.0f,
+        Rotation[2], Rotation[6], Rotation[10],0.0f,
+        Position[0], Position[1], Position[2], 1.0f);
+}
+
+static inline 
+Matrix4x4 &MakeMatrix(dxGeom* g, Matrix4x4 &Out)
+{
+    const dVector3 &position = g->buildUpdatedPosition();
+    const dMatrix3 &rotation = g->buildUpdatedRotation();
+    return MakeMatrix(position, rotation, Out);
+}
+
+
+#endif // #if dTRIMESH_ENABLED && dTRIMESH_OPCODE
+
+
+#endif	//_ODE_COLLISION_TRIMESH_OPCODE_H_

thirdparty/ode-0.16.5/ode/src/collision_trimesh_plane.cpp

@@ -0,0 +1,226 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh - Plane collider by David Walters, July 2006
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_OPCODE
+
+int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
+{
+    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
+    dIASSERT( o1->type == dTriMeshClass );
+    dIASSERT( o2->type == dPlaneClass );
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    // Alias pointers to the plane and trimesh
+    dxTriMesh* trimesh = (dxTriMesh*)( o1 );
+    dxPlane* plane = (dxPlane*)( o2 );
+
+    int contact_count = 0;
+
+    // Cache the maximum contact count.
+    const int contact_max = ( flags & NUMC_MASK );
+
+    // Cache trimesh position and rotation.
+    const dVector3& trimesh_pos = *(const dVector3*)dGeomGetPosition( trimesh );
+    const dMatrix3& trimesh_R = *(const dMatrix3*)dGeomGetRotation( trimesh );
+
+    //
+    // For all triangles.
+    //
+
+    VertexPointersEx VPE;
+    VertexPointers &VP = VPE.vp;
+    ConversionArea VC;
+    dReal alpha;
+    dVector3 vertex;
+
+#if !defined(dSINGLE) || 1
+    dVector3 int_vertex;		// Intermediate vertex for double precision mode.
+#endif // dSINGLE
+
+    const unsigned uiTLSKind = trimesh->getParentSpaceTLSKind();
+    dIASSERT(uiTLSKind == plane->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+    VertexUseCache &vertex_use_cache = pccColliderCache->m_VertexUses;
+
+    // Reallocate vertex use cache if necessary
+    const dxTriMeshData *meshData = trimesh->retrieveMeshData();
+    const int vertex_count = meshData->m_Mesh.GetNbVertices();
+    const bool cache_status = vertex_use_cache.resizeAndResetVertexUSEDFlags(vertex_count);
+
+    // Cache the triangle count.
+    const int tri_count = meshData->m_Mesh.GetNbTriangles();
+
+    // For each triangle
+    for ( int t = 0; t < tri_count; ++t )
+    {
+        // Get triangle, which should also use callback.
+        bool ex_avail = meshData->m_Mesh.GetExTriangle( VPE, t, VC);
+
+        // For each vertex.
+        for ( int v = 0; v < 3; ++v )
+        {
+            // point already used ?
+            if (cache_status && ex_avail)
+            {
+                unsigned VIndex = VPE.Index[v];
+                if (vertex_use_cache.getVertexUSEDFlag(VIndex))
+                    continue;
+                // mark this point as used
+                vertex_use_cache.setVertexUSEDFlag(VIndex);
+            }
+
+            //
+            // Get Vertex
+            //
+
+#if defined(dSINGLE) && 0 // Always assign via intermediate array as otherwise it is an incapsulation violation
+
+            dMultiply0_331( vertex, trimesh_R, (float*)( VP.Vertex[ v ] ) );
+
+#else // dDOUBLE || 1
+
+            // OPCODE data is in single precision format.
+            int_vertex[ 0 ] = VP.Vertex[ v ]->x;
+            int_vertex[ 1 ] = VP.Vertex[ v ]->y;
+            int_vertex[ 2 ] = VP.Vertex[ v ]->z;
+
+            dMultiply0_331( vertex, trimesh_R, int_vertex );
+
+#endif // dSINGLE/dDOUBLE
+
+            vertex[ 0 ] += trimesh_pos[ 0 ];
+            vertex[ 1 ] += trimesh_pos[ 1 ];
+            vertex[ 2 ] += trimesh_pos[ 2 ];
+
+
+            //
+            // Collision?
+            //
+
+            // If alpha < 0 then point is in front of plane. i.e. no contact
+            // If alpha = 0 then the point is on the plane
+            alpha = plane->p[ 3 ] - dCalcVectorDot3( plane->p, vertex );
+
+            // If alpha > 0 the point is behind the plane. CONTACT!
+            if ( alpha > 0 )
+            {
+                // Alias the contact
+                dContactGeom* contact = SAFECONTACT( flags, contacts, contact_count, skip );
+
+                contact->pos[ 0 ] = vertex[ 0 ];
+                contact->pos[ 1 ] = vertex[ 1 ];
+                contact->pos[ 2 ] = vertex[ 2 ];
+
+                contact->normal[ 0 ] = plane->p[ 0 ];
+                contact->normal[ 1 ] = plane->p[ 1 ];
+                contact->normal[ 2 ] = plane->p[ 2 ];
+
+                contact->depth = alpha;
+                contact->g1 = trimesh;
+                contact->g2 = plane;
+                contact->side1 = t;
+                contact->side2 = -1;
+
+                ++contact_count;
+
+                // All contact slots are full?
+                if ( contact_count >= contact_max )
+                    return contact_count; // <=== STOP HERE
+            }
+        }
+    }
+
+    // Return contact count.
+    return contact_count;
+}
+
+
+#endif // dTRIMESH_OPCODE
+
+
+#if dTRIMESH_GIMPACT
+
+#include "gimpact_contact_export_helper.h"
+#include "gimpact_plane_contact_accessor.h"
+
+
+int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip )
+{
+    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
+    dIASSERT( o1->type == dTriMeshClass );
+    dIASSERT( o2->type == dPlaneClass );
+    dIASSERT ((flags & NUMC_MASK) >= 1);
+
+    // Alias pointers to the plane and trimesh
+    dxTriMesh* trimesh = (dxTriMesh*)( o1 );
+    dVector4 plane;
+    dGeomPlaneGetParams(o2, plane);
+
+    o1 -> recomputeAABB();
+    o2 -> recomputeAABB();
+
+    //Find collision
+
+    GDYNAMIC_ARRAY collision_result;
+    GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result);
+
+    gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result);
+
+    if(collision_result.m_size == 0 )
+    {
+        GIM_DYNARRAY_DESTROY(collision_result);
+        return 0;
+    }
+
+
+    vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f, collision_result);
+    unsigned int contactcount = collision_result.m_size;
+    
+    dxPlaneContactAccessor contactaccessor(planecontact_results, plane, o1, o2);
+    contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, flags, contacts, skip);
+
+    GIM_DYNARRAY_DESTROY(collision_result);
+
+    return (int)contactcount;
+}
+
+
+#endif // dTRIMESH_GIMPACT
+
+
+#endif // dTRIMESH_ENABLED
+

thirdparty/ode-0.16.5/ode/src/collision_trimesh_ray.cpp

@@ -0,0 +1,207 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_trimesh_internal.h"
+
+#if dTRIMESH_OPCODE
+int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (RayGeom->type == dRayClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
+    dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+    RayCollider& Collider = pccColliderCache->m_RayCollider;
+
+    dReal Length = dGeomRayGetLength(RayGeom);
+
+    int FirstContact = dGeomRayGetFirstContact(RayGeom);
+    int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom);
+    int ClosestHit = dGeomRayGetClosestHit(RayGeom);
+
+    Collider.SetFirstContact(FirstContact != 0);
+    Collider.SetClosestHit(ClosestHit != 0);
+    Collider.SetCulling(BackfaceCull != 0);
+    Collider.SetMaxDist(Length);
+
+    const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+    const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+    Matrix4x4 MeshMatrix;
+    const dVector3 ZeroVector3 = { REAL(0.0), };
+    MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
+
+    dVector3 Origin, Direction;
+    dGeomRayGet(RayGeom, Origin, Direction);
+
+    dVector3 OffsetOrigin;
+    dSubtractVectors3(OffsetOrigin, Origin, TLPosition);
+
+    /* Make Ray */
+    Ray WorldRay;
+    WorldRay.mOrig.Set(OffsetOrigin[0], OffsetOrigin[1], OffsetOrigin[2]);
+    WorldRay.mDir.Set(Direction[0], Direction[1], Direction[2]);
+
+    /* Intersect */
+    int TriCount = 0;
+    if (Collider.Collide(WorldRay, TriMesh->retrieveMeshBVTreeRef(), &MeshMatrix)) {
+        TriCount = pccColliderCache->m_Faces.GetNbFaces();
+    }
+
+    if (TriCount == 0) {
+        return 0;
+    }
+
+    const CollisionFace* Faces = pccColliderCache->m_Faces.GetFaces();
+
+    int OutTriCount = 0;
+    for (int i = 0; i < TriCount; i++) {
+        if (TriMesh->m_RayCallback == null ||
+            TriMesh->m_RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
+            Faces[i].mU, Faces[i].mV)) {
+                const int& TriIndex = Faces[i].mFaceID;
+                if (!TriMesh->invokeCallback(RayGeom, TriIndex)) {
+                    continue;
+                }
+
+                dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
+
+                dVector3 dv[3];
+                TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation);
+
+                dVector3 vu;
+                vu[0] = dv[1][0] - dv[0][0];
+                vu[1] = dv[1][1] - dv[0][1];
+                vu[2] = dv[1][2] - dv[0][2];
+                vu[3] = REAL(0.0);
+
+                dVector3 vv;
+                vv[0] = dv[2][0] - dv[0][0];
+                vv[1] = dv[2][1] - dv[0][1];
+                vv[2] = dv[2][2] - dv[0][2];
+                vv[3] = REAL(0.0);
+
+                dCalcVectorCross3(Contact->normal, vv, vu);	// Reversed
+
+                // Even though all triangles might be initially valid, 
+                // a triangle may degenerate into a segment after applying 
+                // space transformation.
+                if (dSafeNormalize3(Contact->normal))
+                {
+                    // No sense to save on single type conversion in algorithm of this size.
+                    // If there would be a custom typedef for distance type it could be used 
+                    // instead of dReal. However using float directly is the loss of abstraction 
+                    // and possible loss of precision in future.
+                    /*float*/ dReal T = Faces[i].mDistance;
+                    Contact->pos[0] = Origin[0] + (Direction[0] * T);
+                    Contact->pos[1] = Origin[1] + (Direction[1] * T);
+                    Contact->pos[2] = Origin[2] + (Direction[2] * T);
+                    Contact->pos[3] = REAL(0.0);
+
+                    Contact->depth = T;
+                    Contact->g1 = TriMesh;
+                    Contact->g2 = RayGeom;
+                    Contact->side1 = TriIndex;
+                    Contact->side2 = -1;
+
+                    OutTriCount++;
+
+                    // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
+                    if (OutTriCount >= (Flags & NUMC_MASK)) {
+                        break;
+                    }
+                }
+        }
+    }
+    return OutTriCount;
+}
+#endif // dTRIMESH_OPCODE
+
+#if dTRIMESH_GIMPACT
+int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride)
+{
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (RayGeom->type == dRayClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    dReal Length = dGeomRayGetLength(RayGeom);
+    int FirstContact = dGeomRayGetFirstContact(RayGeom);
+    int BackfaceCull = dGeomRayGetBackfaceCull(RayGeom);
+    int ClosestHit = dGeomRayGetClosestHit(RayGeom);
+    dVector3 Origin, Direction;
+    dGeomRayGet(RayGeom, Origin, Direction);
+
+    char intersect=0;
+    GIM_TRIANGLE_RAY_CONTACT_DATA contact_data;
+
+    if(ClosestHit)
+    {
+        intersect = gim_trimesh_ray_closest_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
+    }
+    else
+    {
+        intersect = gim_trimesh_ray_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data);
+    }
+
+    if(intersect == 0)
+    {
+        return 0;
+    }
+
+
+    if(!TriMesh->m_RayCallback || 
+        TriMesh->m_RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v))
+    {
+        dContactGeom* Contact = &( Contacts[ 0 ] );
+        VEC_COPY(Contact->pos,contact_data.m_point);
+        VEC_COPY(Contact->normal,contact_data.m_normal);
+        Contact->depth = contact_data.tparam;
+        Contact->g1 = TriMesh;
+        Contact->g2 = RayGeom;
+        Contact->side1 = contact_data.m_face_id;
+        Contact->side2 = -1;
+        return 1;
+    }
+
+    return 0;
+}
+#endif  // dTRIMESH_GIMPACT
+
+#endif // dTRIMESH_ENABLED

thirdparty/ode-0.16.5/ode/src/collision_trimesh_sphere.cpp

@@ -0,0 +1,596 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_util.h"
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_OPCODE
+
+// Ripped from Opcode 1.1.
+static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){
+
+    // now onto the bulk of the collision...
+
+    dVector3 Diff;
+    Diff[0] = Origin[0] - Center[0];
+    Diff[1] = Origin[1] - Center[1];
+    Diff[2] = Origin[2] - Center[2];
+    Diff[3] = Origin[3] - Center[3];
+
+    dReal A00 = dCalcVectorDot3(Edge0, Edge0);
+    dReal A01 = dCalcVectorDot3(Edge0, Edge1);
+    dReal A11 = dCalcVectorDot3(Edge1, Edge1);
+
+    dReal B0 = dCalcVectorDot3(Diff, Edge0);
+    dReal B1 = dCalcVectorDot3(Diff, Edge1);
+
+    dReal C = dCalcVectorDot3(Diff, Diff);
+
+    dReal Det = dFabs(A00 * A11 - A01 * A01);
+    u = A01 * B1 - A11 * B0;
+    v = A01 * B0 - A00 * B1;
+
+    dReal DistSq;
+
+    if (u + v <= Det){
+        if(u < REAL(0.0)){
+            if(v < REAL(0.0)){  // region 4
+                if(B0 < REAL(0.0)){
+                    v = REAL(0.0);
+                    if (-B0 >= A00){
+                        u = REAL(1.0);
+                        DistSq = A00 + REAL(2.0) * B0 + C;
+                    }
+                    else{
+                        u = -B0 / A00;
+                        DistSq = B0 * u + C;
+                    }
+                }
+                else{
+                    u = REAL(0.0);
+                    if(B1 >= REAL(0.0)){
+                        v = REAL(0.0);
+                        DistSq = C;
+                    }
+                    else if(-B1 >= A11){
+                        v = REAL(1.0);
+                        DistSq = A11 + REAL(2.0) * B1 + C;
+                    }
+                    else{
+                        v = -B1 / A11;
+                        DistSq = B1 * v + C;
+                    }
+                }
+            }
+            else{  // region 3
+                u = REAL(0.0);
+                if(B1 >= REAL(0.0)){
+                    v = REAL(0.0);
+                    DistSq = C;
+                }
+                else if(-B1 >= A11){
+                    v = REAL(1.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else{
+                    v = -B1 / A11;
+                    DistSq = B1 * v + C;
+                }
+            }
+        }
+        else if(v < REAL(0.0)){  // region 5
+            v = REAL(0.0);
+            if (B0 >= REAL(0.0)){
+                u = REAL(0.0);
+                DistSq = C;
+            }
+            else if (-B0 >= A00){
+                u = REAL(1.0);
+                DistSq = A00 + REAL(2.0) * B0 + C;
+            }
+            else{
+                u = -B0 / A00;
+                DistSq = B0 * u + C;
+            }
+        }
+        else{  // region 0
+            // minimum at interior point
+            if (Det == REAL(0.0)){
+                u = REAL(0.0);
+                v = REAL(0.0);
+                DistSq = FLT_MAX;
+            }
+            else{
+                dReal InvDet = REAL(1.0) / Det;
+                u *= InvDet;
+                v *= InvDet;
+                DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+            }
+        }
+    }
+    else{
+        dReal Tmp0, Tmp1, Numer, Denom;
+
+        if(u < REAL(0.0)){  // region 2
+            Tmp0 = A01 + B0;
+            Tmp1 = A11 + B1;
+            if (Tmp1 > Tmp0){
+                Numer = Tmp1 - Tmp0;
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    u = REAL(1.0);
+                    v = REAL(0.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else{
+                    u = Numer / Denom;
+                    v = REAL(1.0) - u;
+                    DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+            else{
+                u = REAL(0.0);
+                if(Tmp1 <= REAL(0.0)){
+                    v = REAL(1.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else if(B1 >= REAL(0.0)){
+                    v = REAL(0.0);
+                    DistSq = C;
+                }
+                else{
+                    v = -B1 / A11;
+                    DistSq = B1 * v + C;
+                }
+            }
+        }
+        else if(v < REAL(0.0)){  // region 6
+            Tmp0 = A01 + B1;
+            Tmp1 = A00 + B0;
+            if (Tmp1 > Tmp0){
+                Numer = Tmp1 - Tmp0;
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    v = REAL(1.0);
+                    u = REAL(0.0);
+                    DistSq = A11 + REAL(2.0) * B1 + C;
+                }
+                else{
+                    v = Numer / Denom;
+                    u = REAL(1.0) - v;
+                    DistSq =  u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+            else{
+                v = REAL(0.0);
+                if (Tmp1 <= REAL(0.0)){
+                    u = REAL(1.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else if(B0 >= REAL(0.0)){
+                    u = REAL(0.0);
+                    DistSq = C;
+                }
+                else{
+                    u = -B0 / A00;
+                    DistSq = B0 * u + C;
+                }
+            }
+        }
+        else{  // region 1
+            Numer = A11 + B1 - A01 - B0;
+            if (Numer <= REAL(0.0)){
+                u = REAL(0.0);
+                v = REAL(1.0);
+                DistSq = A11 + REAL(2.0) * B1 + C;
+            }
+            else{
+                Denom = A00 - REAL(2.0) * A01 + A11;
+                if (Numer >= Denom){
+                    u = REAL(1.0);
+                    v = REAL(0.0);
+                    DistSq = A00 + REAL(2.0) * B0 + C;
+                }
+                else{
+                    u = Numer / Denom;
+                    v = REAL(1.0) - u;
+                    DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+                }
+            }
+        }
+    }
+
+    Dist = dSqrt(dFabs(DistSq));
+
+    if (Dist <= Radius){
+        Dist = Radius - Dist;
+        return true;
+    }
+    else return false;
+}
+
+int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (SphereGeom->type == dSphereClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+    const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
+    dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+    TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+    SphereCollider& Collider = pccColliderCache->m_SphereCollider;
+
+    const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+    const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+    Matrix4x4 MeshMatrix;
+    const dVector3 ZeroVector3 = { REAL(0.0), };
+    MakeMatrix(ZeroVector3, TLRotation, MeshMatrix);
+
+    const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
+    dReal Radius = dGeomSphereGetRadius(SphereGeom);
+
+    dVector3 OffsetPosition;
+    dSubtractVectors3(OffsetPosition, Position, TLPosition);
+
+    // Sphere
+    Sphere Sphere;
+    Sphere.mCenter.Set(OffsetPosition[0], OffsetPosition[1], OffsetPosition[2]);
+    Sphere.mRadius = Radius;
+
+
+    // TC results
+    if (TriMesh->getDoTC(dxTriMesh::TTC_SPHERE)) {
+        dxTriMesh::SphereTC* sphereTC = 0;
+        const int sphereCacheSize = TriMesh->m_SphereTCCache.size();
+        for (int i = 0; i != sphereCacheSize; i++){
+            if (TriMesh->m_SphereTCCache[i].Geom == SphereGeom){
+                sphereTC = &TriMesh->m_SphereTCCache[i];
+                break;
+            }
+        }
+
+        if (!sphereTC) {
+            TriMesh->m_SphereTCCache.push(dxTriMesh::SphereTC());
+
+            sphereTC = &TriMesh->m_SphereTCCache[TriMesh->m_SphereTCCache.size() - 1];
+            sphereTC->Geom = SphereGeom;
+        }
+
+        // Intersect
+        Collider.SetTemporalCoherence(true);
+        Collider.Collide(*sphereTC, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+    else {
+        Collider.SetTemporalCoherence(false);
+        Collider.Collide(pccColliderCache->m_DefaultSphereCache, Sphere, TriMesh->retrieveMeshBVTreeRef(), null, &MeshMatrix);
+    }
+
+    if (! Collider.GetContactStatus()) {
+        // no collision occurred
+        return 0;
+    }
+
+    // get results
+    int TriCount = Collider.GetNbTouchedPrimitives();
+    const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
+
+    if (TriCount != 0){
+        if (TriMesh->m_ArrayCallback != null){
+            TriMesh->m_ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
+        }
+
+        int OutTriCount = 0;
+        for (int i = 0; i < TriCount; i++){
+            if (OutTriCount == (Flags & NUMC_MASK)){
+                break;
+            }
+
+            const int TriIndex = Triangles[i];
+
+            dVector3 dv[3];
+            if (!TriMesh->invokeCallback(SphereGeom, TriIndex))
+                continue;
+
+            TriMesh->fetchMeshTriangle(dv, TriIndex, TLPosition, TLRotation);
+
+            dVector3& v0 = dv[0];
+            dVector3& v1 = dv[1];
+            dVector3& v2 = dv[2];
+
+            dVector3 vu;
+            vu[0] = v1[0] - v0[0];
+            vu[1] = v1[1] - v0[1];
+            vu[2] = v1[2] - v0[2];
+            vu[3] = REAL(0.0);
+
+            dVector3 vv;
+            vv[0] = v2[0] - v0[0];
+            vv[1] = v2[1] - v0[1];
+            vv[2] = v2[2] - v0[2];
+            vv[3] = REAL(0.0);
+
+            // Get plane coefficients
+            dVector4 Plane;
+            dCalcVectorCross3(Plane, vu, vv);
+
+            // Even though all triangles might be initially valid, 
+            // a triangle may degenerate into a segment after applying 
+            // space transformation.
+            if (!dSafeNormalize3(Plane)) {
+                continue;
+            }
+
+            /* If the center of the sphere is within the positive halfspace of the
+            * triangle's plane, allow a contact to be generated.
+            * If the center of the sphere made it into the positive halfspace of a
+            * back-facing triangle, then the physics update and/or velocity needs
+            * to be adjusted (penetration has occured anyway).
+            */
+
+            dReal side = dCalcVectorDot3(Plane,Position) - dCalcVectorDot3(Plane, v0);
+
+            if(side < REAL(0.0)) {
+                continue;
+            }
+
+            dReal Depth;
+            dReal u, v;
+            if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
+                continue;	// Sphere doesn't hit triangle
+            }
+
+            if (Depth < REAL(0.0)){
+                continue; // Negative depth does not produce a contact
+            }
+
+            dVector3 ContactPos;
+
+            dReal w = REAL(1.0) - u - v;
+            ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
+            ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
+            ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
+
+            // Depth returned from GetContactData is depth along 
+            // contact point - sphere center direction
+            // we'll project it to contact normal
+            dVector3 dir;
+            dir[0] = Position[0]-ContactPos[0];
+            dir[1] = Position[1]-ContactPos[1];
+            dir[2] = Position[2]-ContactPos[2];
+            dReal dirProj = dCalcVectorDot3(dir, Plane) / dSqrt(dCalcVectorDot3(dir, dir));
+
+            // Since Depth already had a requirement to be non-negative,
+            // negative direction projections should not be allowed as well,
+            // as otherwise the multiplication will result in negative contact depth.
+            if (dirProj < REAL(0.0)){
+                continue; // Zero contact depth could be ignored
+            }
+
+            dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
+
+            Contact->pos[0] = ContactPos[0];
+            Contact->pos[1] = ContactPos[1];
+            Contact->pos[2] = ContactPos[2];
+            Contact->pos[3] = REAL(0.0);
+
+            // Using normal as plane (reversed)
+            Contact->normal[0] = -Plane[0];
+            Contact->normal[1] = -Plane[1];
+            Contact->normal[2] = -Plane[2];
+            Contact->normal[3] = REAL(0.0);
+
+            Contact->depth = Depth * dirProj;
+            //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
+
+            // We need to set these unconditionally, as the merging may fail! - Bram
+            Contact->g1 = TriMesh;
+            Contact->g2 = SphereGeom;
+            Contact->side2 = -1;
+
+            Contact->side1 = TriIndex;
+
+            OutTriCount++;
+        }
+        if (OutTriCount > 0){
+            if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACTS_FULLY) {
+                dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
+                Contact->g1 = TriMesh;
+                Contact->g2 = SphereGeom;
+                Contact->side2 = -1;
+
+                if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
+                    dVector3 pos;
+                    pos[0] = Contact->pos[0];
+                    pos[1] = Contact->pos[1];
+                    pos[2] = Contact->pos[2];
+
+                    dVector3 normal;
+                    normal[0] = Contact->normal[0] * Contact->depth;
+                    normal[1] = Contact->normal[1] * Contact->depth;
+                    normal[2] = Contact->normal[2] * Contact->depth;
+                    normal[3] = REAL(0.0);
+
+                    int TriIndex = Contact->side1;
+
+                    for (int i = 1; i < OutTriCount; i++){
+                        dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        pos[0] += TempContact->pos[0];
+                        pos[1] += TempContact->pos[1];
+                        pos[2] += TempContact->pos[2];
+
+                        normal[0] += TempContact->normal[0] * TempContact->depth;
+                        normal[1] += TempContact->normal[1] * TempContact->depth;
+                        normal[2] += TempContact->normal[2] * TempContact->depth;
+
+                        TriIndex = (TriMesh->m_TriMergeCallback) ? TriMesh->m_TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
+                    }
+
+                    Contact->side1 = TriIndex;
+
+                    Contact->pos[0] = pos[0] / OutTriCount;
+                    Contact->pos[1] = pos[1] / OutTriCount;
+                    Contact->pos[2] = pos[2] / OutTriCount;
+
+                    if ( !dSafeNormalize3(normal) )
+                        return OutTriCount;	// Cannot merge in this pathological case
+
+                    // Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
+                    // That is why we need to correct this by increasing the depth for each intersection.
+                    // The maximum of the adjusted depths is our newly merged depth value - Bram.
+
+                    dReal mergedDepth = REAL(0.0);
+                    dReal minEffectiveness = REAL(0.5);
+                    for ( int i = 0; i < OutTriCount; ++i )
+                    {
+                        dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
+                        dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
+                        if ( effectiveness < dEpsilon )
+                            return OutTriCount; // Cannot merge this pathological case
+                        // Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
+                        effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
+                        dReal adjusted = TempContact->depth / effectiveness;
+                        mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
+                    }
+                    Contact->depth = mergedDepth;
+                    Contact->normal[0] = normal[0];
+                    Contact->normal[1] = normal[1];
+                    Contact->normal[2] = normal[2];
+                    Contact->normal[3] = normal[3];
+                }
+
+                return 1;
+            }
+            else if (TriMesh->m_SphereContactsMergeOption == MERGE_CONTACT_NORMALS) {
+                if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
+                    dVector3 Normal;
+
+                    dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
+                    Normal[0] = FirstContact->normal[0] * FirstContact->depth;
+                    Normal[1] = FirstContact->normal[1] * FirstContact->depth;
+                    Normal[2] = FirstContact->normal[2] * FirstContact->depth;
+                    Normal[3] = FirstContact->normal[3] * FirstContact->depth;
+
+                    for (int i = 1; i < OutTriCount; i++){
+                        dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        Normal[0] += Contact->normal[0] * Contact->depth;
+                        Normal[1] += Contact->normal[1] * Contact->depth;
+                        Normal[2] += Contact->normal[2] * Contact->depth;
+                        Normal[3] += Contact->normal[3] * Contact->depth;
+                    }
+
+                    dNormalize3(Normal);
+
+                    for (int i = 0; i < OutTriCount; i++){
+                        dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+                        Contact->normal[0] = Normal[0];
+                        Contact->normal[1] = Normal[1];
+                        Contact->normal[2] = Normal[2];
+                        Contact->normal[3] = Normal[3];
+                    }
+                }
+
+                return OutTriCount;
+            }
+            else {
+                dIASSERT(TriMesh->m_SphereContactsMergeOption == DONT_MERGE_CONTACTS);
+                return OutTriCount;
+            }
+        }
+        else return 0;
+    }
+    else return 0;
+}
+
+
+#endif // dTRIMESH_OPCODE
+
+
+#if dTRIMESH_GIMPACT
+
+#include "gimpact_contact_export_helper.h"
+#include "gimpact_gim_contact_accessor.h"
+
+
+int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride)
+{
+    dIASSERT (Stride >= (int)sizeof(dContactGeom));
+    dIASSERT (g1->type == dTriMeshClass);
+    dIASSERT (SphereGeom->type == dSphereClass);
+    dIASSERT ((Flags & NUMC_MASK) >= 1);
+
+    dxTriMesh* TriMesh = (dxTriMesh*)g1;
+    dVector3& Position = *(dVector3*)dGeomGetPosition(SphereGeom);
+    dReal Radius = dGeomSphereGetRadius(SphereGeom);
+    //Create contact list
+    GDYNAMIC_ARRAY trimeshcontacts;
+    GIM_CREATE_CONTACT_LIST(trimeshcontacts);
+
+    g1 -> recomputeAABB();
+    SphereGeom -> recomputeAABB();
+
+    //Collide trimeshes
+    gim_trimesh_sphere_collisionODE(&TriMesh->m_collision_trimesh,Position,Radius,&trimeshcontacts);
+
+    if(trimeshcontacts.m_size == 0)
+    {
+        GIM_DYNARRAY_DESTROY(trimeshcontacts);
+        return 0;
+    }
+
+    GIM_CONTACT * ptrimeshcontacts = GIM_DYNARRAY_POINTER(GIM_CONTACT,trimeshcontacts);
+    unsigned contactcount = trimeshcontacts.m_size;
+
+    dxGIMCContactAccessor contactaccessor(ptrimeshcontacts, g1, SphereGeom, -1);
+    contactcount = dxGImpactContactsExportHelper::ExportMaxDepthGImpactContacts(contactaccessor, contactcount, Flags, Contacts, Stride);
+
+    GIM_DYNARRAY_DESTROY(trimeshcontacts);
+
+    return (int)contactcount;
+}
+
+
+#endif // dTRIMESH_GIMPACT
+
+
+#endif // dTRIMESH_ENABLED

thirdparty/ode-0.16.5/ode/src/collision_util.cpp

@@ -0,0 +1,613 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+some useful collision utility stuff. this includes some API utility
+functions that are defined in the public header files.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include "config.h"
+#include "odemath.h"
+#include "collision_util.h"
+
+//****************************************************************************
+
+int dCollideSpheres (dVector3 p1, dReal r1,
+                     dVector3 p2, dReal r2, dContactGeom *c)
+{
+    // printf ("d=%.2f  (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
+    //	  d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
+
+    dReal d = dCalcPointsDistance3(p1,p2);
+    if (d > (r1 + r2)) return 0;
+    if (d <= 0) {
+        c->pos[0] = p1[0];
+        c->pos[1] = p1[1];
+        c->pos[2] = p1[2];
+        c->normal[0] = 1;
+        c->normal[1] = 0;
+        c->normal[2] = 0;
+        c->depth = r1 + r2;
+    }
+    else {
+        dReal d1 = dRecip (d);
+        c->normal[0] = (p1[0]-p2[0])*d1;
+        c->normal[1] = (p1[1]-p2[1])*d1;
+        c->normal[2] = (p1[2]-p2[2])*d1;
+        dReal k = REAL(0.5) * (r2 - r1 - d);
+        c->pos[0] = p1[0] + c->normal[0]*k;
+        c->pos[1] = p1[1] + c->normal[1]*k;
+        c->pos[2] = p1[2] + c->normal[2]*k;
+        c->depth = r1 + r2 - d;
+    }
+    return 1;
+}
+
+
+void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
+                           const dVector3 pb, const dVector3 ub,
+                           dReal *alpha, dReal *beta)
+{
+    dVector3 p;
+    p[0] = pb[0] - pa[0];
+    p[1] = pb[1] - pa[1];
+    p[2] = pb[2] - pa[2];
+    dReal uaub = dCalcVectorDot3(ua,ub);
+    dReal q1 =  dCalcVectorDot3(ua,p);
+    dReal q2 = -dCalcVectorDot3(ub,p);
+    dReal d = 1-uaub*uaub;
+    if (d <= REAL(0.0001)) {
+        // @@@ this needs to be made more robust
+        *alpha = 0;
+        *beta  = 0;
+    }
+    else {
+        d = dRecip(d);
+        *alpha = (q1 + uaub*q2)*d;
+        *beta  = (uaub*q1 + q2)*d;
+    }
+}
+
+
+// given two line segments A and B with endpoints a1-a2 and b1-b2, return the
+// points on A and B that are closest to each other (in cp1 and cp2).
+// in the case of parallel lines where there are multiple solutions, a
+// solution involving the endpoint of at least one line will be returned.
+// this will work correctly for zero length lines, e.g. if a1==a2 and/or
+// b1==b2.
+//
+// the algorithm works by applying the voronoi clipping rule to the features
+// of the line segments. the three features of each line segment are the two
+// endpoints and the line between them. the voronoi clipping rule states that,
+// for feature X on line A and feature Y on line B, the closest points PA and
+// PB between X and Y are globally the closest points if PA is in V(Y) and
+// PB is in V(X), where V(X) is the voronoi region of X.
+
+void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
+                                const dVector3 b1, const dVector3 b2,
+                                dVector3 cp1, dVector3 cp2)
+{
+    dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
+    dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
+
+#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
+#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
+
+    // check vertex-vertex features
+
+    SET3 (a1a2,a2,-,a1);
+    SET3 (b1b2,b2,-,b1);
+    SET3 (a1b1,b1,-,a1);
+    da1 = dCalcVectorDot3(a1a2,a1b1);
+    db1 = dCalcVectorDot3(b1b2,a1b1);
+    if (da1 <= 0 && db1 >= 0) {
+        SET2 (cp1,a1);
+        SET2 (cp2,b1);
+        return;
+    }
+
+    SET3 (a1b2,b2,-,a1);
+    da2 = dCalcVectorDot3(a1a2,a1b2);
+    db2 = dCalcVectorDot3(b1b2,a1b2);
+    if (da2 <= 0 && db2 <= 0) {
+        SET2 (cp1,a1);
+        SET2 (cp2,b2);
+        return;
+    }
+
+    SET3 (a2b1,b1,-,a2);
+    da3 = dCalcVectorDot3(a1a2,a2b1);
+    db3 = dCalcVectorDot3(b1b2,a2b1);
+    if (da3 >= 0 && db3 >= 0) {
+        SET2 (cp1,a2);
+        SET2 (cp2,b1);
+        return;
+    }
+
+    SET3 (a2b2,b2,-,a2);
+    da4 = dCalcVectorDot3(a1a2,a2b2);
+    db4 = dCalcVectorDot3(b1b2,a2b2);
+    if (da4 >= 0 && db4 <= 0) {
+        SET2 (cp1,a2);
+        SET2 (cp2,b2);
+        return;
+    }
+
+    // check edge-vertex features.
+    // if one or both of the lines has zero length, we will never get to here,
+    // so we do not have to worry about the following divisions by zero.
+
+    la = dCalcVectorDot3(a1a2,a1a2);
+    if (da1 >= 0 && da3 <= 0) {
+        k = da1 / la;
+        SET3 (n,a1b1,-,k*a1a2);
+        if (dCalcVectorDot3(b1b2,n) >= 0) {
+            SET3 (cp1,a1,+,k*a1a2);
+            SET2 (cp2,b1);
+            return;
+        }
+    }
+
+    if (da2 >= 0 && da4 <= 0) {
+        k = da2 / la;
+        SET3 (n,a1b2,-,k*a1a2);
+        if (dCalcVectorDot3(b1b2,n) <= 0) {
+            SET3 (cp1,a1,+,k*a1a2);
+            SET2 (cp2,b2);
+            return;
+        }
+    }
+
+    lb = dCalcVectorDot3(b1b2,b1b2);
+    if (db1 <= 0 && db2 >= 0) {
+        k = -db1 / lb;
+        SET3 (n,-a1b1,-,k*b1b2);
+        if (dCalcVectorDot3(a1a2,n) >= 0) {
+            SET2 (cp1,a1);
+            SET3 (cp2,b1,+,k*b1b2);
+            return;
+        }
+    }
+
+    if (db3 <= 0 && db4 >= 0) {
+        k = -db3 / lb;
+        SET3 (n,-a2b1,-,k*b1b2);
+        if (dCalcVectorDot3(a1a2,n) <= 0) {
+            SET2 (cp1,a2);
+            SET3 (cp2,b1,+,k*b1b2);
+            return;
+        }
+    }
+
+    // it must be edge-edge
+
+    k = dCalcVectorDot3(a1a2,b1b2);
+    det = la*lb - k*k;
+    if (det <= 0) {
+        // this should never happen, but just in case...
+        SET2(cp1,a1);
+        SET2(cp2,b1);
+        return;
+    }
+    det = dRecip (det);
+    dReal alpha = (lb*da1 -  k*db1) * det;
+    dReal beta  = ( k*da1 - la*db1) * det;
+    SET3 (cp1,a1,+,alpha*a1a2);
+    SET3 (cp2,b1,+,beta*b1b2);
+
+# undef SET2
+# undef SET3
+}
+
+
+// a simple root finding algorithm is used to find the value of 't' that
+// satisfies:
+//		d|D(t)|^2/dt = 0
+// where:
+//		|D(t)| = |p(t)-b(t)|
+// where p(t) is a point on the line parameterized by t:
+//		p(t) = p1 + t*(p2-p1)
+// and b(t) is that same point clipped to the boundary of the box. in box-
+// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
+// each of which looks like this:
+//
+//	    t_lo     /
+//	      ______/    -->t
+//	     /     t_hi
+//	    /
+//
+// t_lo and t_hi are the t values where the line passes through the planes
+// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
+// in a piecewise fashion from t=0 to t=1, stopping at the point where
+// d|D(t)|^2/dt crosses from negative to positive.
+
+void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
+                            const dVector3 c, const dMatrix3 R,
+                            const dVector3 side,
+                            dVector3 lret, dVector3 bret)
+{
+    int i;
+
+    // compute the start and delta of the line p1-p2 relative to the box.
+    // we will do all subsequent computations in this box-relative coordinate
+    // system. we have to do a translation and rotation for each point.
+    dVector3 tmp,s,v;
+    tmp[0] = p1[0] - c[0];
+    tmp[1] = p1[1] - c[1];
+    tmp[2] = p1[2] - c[2];
+    dMultiply1_331 (s,R,tmp);
+    tmp[0] = p2[0] - p1[0];
+    tmp[1] = p2[1] - p1[1];
+    tmp[2] = p2[2] - p1[2];
+    dMultiply1_331 (v,R,tmp);
+
+    // mirror the line so that v has all components >= 0
+    dVector3 sign;
+    for (i=0; i<3; i++) {
+        if (v[i] < 0) {
+            s[i] = -s[i];
+            v[i] = -v[i];
+            sign[i] = -1;
+        }
+        else sign[i] = 1;
+    }
+
+    // compute v^2
+    dVector3 v2;
+    v2[0] = v[0]*v[0];
+    v2[1] = v[1]*v[1];
+    v2[2] = v[2]*v[2];
+
+    // compute the half-sides of the box
+    dReal h[3];
+    h[0] = REAL(0.5) * side[0];
+    h[1] = REAL(0.5) * side[1];
+    h[2] = REAL(0.5) * side[2];
+
+    // region is -1,0,+1 depending on which side of the box planes each
+    // coordinate is on. tanchor is the next t value at which there is a
+    // transition, or the last one if there are no more.
+    int region[3];
+    dReal tanchor[3];
+
+    // Denormals are a problem, because we divide by v[i], and then 
+    // multiply that by 0. Alas, infinity times 0 is infinity (!)
+    // We also use v2[i], which is v[i] squared. Here's how the epsilons 
+    // are chosen:
+    // float epsilon = 1.175494e-038 (smallest non-denormal number)
+    // double epsilon = 2.225074e-308 (smallest non-denormal number)
+    // For single precision, choose an epsilon such that v[i] squared is 
+    // not a denormal; this is for performance.
+    // For double precision, choose an epsilon such that v[i] is not a 
+    // denormal; this is for correctness. (Jon Watte on mailinglist)
+
+#if defined( dSINGLE )
+    const dReal tanchor_eps = REAL(1e-19);
+#else
+    const dReal tanchor_eps = REAL(1e-307);
+#endif
+
+    // find the region and tanchor values for p1
+    for (i=0; i<3; i++) {
+        if (v[i] > tanchor_eps) {
+            if (s[i] < -h[i]) {
+                region[i] = -1;
+                tanchor[i] = (-h[i]-s[i])/v[i];
+            }
+            else {
+                region[i] = (s[i] > h[i]);
+                tanchor[i] = (h[i]-s[i])/v[i];
+            }
+        }
+        else {
+            region[i] = 0;
+            tanchor[i] = 2;		// this will never be a valid tanchor
+        }
+    }
+
+    // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
+    dReal t=0;
+    dReal dd2dt = 0;
+    for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
+    if (dd2dt >= 0) goto got_answer;
+
+    do {
+        // find the point on the line that is at the next clip plane boundary
+        dReal next_t = 1;
+        for (i=0; i<3; i++) {
+            if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
+                next_t = tanchor[i];
+        }
+
+        // compute d|d|^2/dt for the next t
+        dReal next_dd2dt = 0;
+        for (i=0; i<3; i++) {
+            next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
+        }
+
+        // if the sign of d|d|^2/dt has changed, solution = the crossover point
+        if (next_dd2dt >= 0) {
+            dReal m = (next_dd2dt-dd2dt)/(next_t - t);
+            t -= dd2dt/m;
+            goto got_answer;
+        }
+
+        // advance to the next anchor point / region
+        for (i=0; i<3; i++) {
+            if (tanchor[i] == next_t) {
+                tanchor[i] = (h[i]-s[i])/v[i];
+                region[i]++;
+            }
+        }
+        t = next_t;
+        dd2dt = next_dd2dt;
+    }
+    while (t < 1);
+    t = 1;
+
+got_answer:
+
+    // compute closest point on the line
+    for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i];	// note: tmp=p2-p1
+
+    // compute closest point on the box
+    for (i=0; i<3; i++) {
+        tmp[i] = sign[i] * (s[i] + t*v[i]);
+        if (tmp[i] < -h[i]) tmp[i] = -h[i];
+        else if (tmp[i] > h[i]) tmp[i] = h[i];
+    }
+    dMultiply0_331 (s,R,tmp);
+    for (i=0; i<3; i++) bret[i] = s[i] + c[i];
+}
+
+
+// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
+// or 0 if not.
+
+int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
+                    const dVector3 side1, const dVector3 p2,
+                    const dMatrix3 R2, const dVector3 side2)
+{
+    // two boxes are disjoint if (and only if) there is a separating axis
+    // perpendicular to a face from one box or perpendicular to an edge from
+    // either box. the following tests are derived from:
+    //    "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
+    //    S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
+
+    // Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
+    // Qij is abs(Rij)
+    dVector3 p,pp;
+    dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
+        Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
+
+    // get vector from centers of box 1 to box 2, relative to box 1
+    p[0] = p2[0] - p1[0];
+    p[1] = p2[1] - p1[1];
+    p[2] = p2[2] - p1[2];
+    dMultiply1_331 (pp,R1,p);		// get pp = p relative to body 1
+
+    // get side lengths / 2
+    A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
+    B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
+
+    // for the following tests, excluding computation of Rij, in the worst case,
+    // 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
+    // notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
+
+    // separating axis = u1,u2,u3
+    R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
+    Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
+    if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
+    R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
+    Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
+    if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
+    R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);
+    Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
+    if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
+
+    // separating axis = v1,v2,v3
+    if (dFabs(dCalcVectorDot3_41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
+    if (dFabs(dCalcVectorDot3_41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
+    if (dFabs(dCalcVectorDot3_41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
+
+    // separating axis = u1 x (v1,v2,v3)
+    if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
+    if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
+    if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
+
+    // separating axis = u2 x (v1,v2,v3)
+    if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
+    if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
+    if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
+
+    // separating axis = u3 x (v1,v2,v3)
+    if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
+    if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
+    if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
+
+    return 1;
+}
+
+//****************************************************************************
+// other utility functions
+
+/*ODE_API */void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
+{
+    aabb[0] = -dInfinity;
+    aabb[1] = dInfinity;
+    aabb[2] = -dInfinity;
+    aabb[3] = dInfinity;
+    aabb[4] = -dInfinity;
+    aabb[5] = dInfinity;
+}
+
+
+//****************************************************************************
+// Helpers for Croteam's collider - by Nguyen Binh
+
+int dClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
+{
+    // calculate distance of edge points to plane
+    dReal fDistance0 = dPointPlaneDistance(  vEpnt0 ,plPlane );
+    dReal fDistance1 = dPointPlaneDistance(  vEpnt1 ,plPlane );
+
+    // if both points are behind the plane
+    if ( fDistance0 < 0 && fDistance1 < 0 ) 
+    {
+        // do nothing
+        return 0;
+        // if both points in front of the plane
+    } 
+    else if ( fDistance0 > 0 && fDistance1 > 0 ) 
+    {
+        // accept them
+        return 1;
+        // if we have edge/plane intersection
+    } else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) 
+    {
+
+        // find intersection point of edge and plane
+        dVector3 vIntersectionPoint;
+        vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
+        vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
+        vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
+
+        // clamp correct edge to intersection point
+        if ( fDistance0 < 0 ) 
+        {
+            dVector3Copy(vIntersectionPoint,vEpnt0);
+        } else 
+        {
+            dVector3Copy(vIntersectionPoint,vEpnt1);
+        }
+        return 1;
+    }
+    return 1;
+}
+
+// clip polygon with plane and generate new polygon points
+void		 dClipPolyToPlane( const dVector3 avArrayIn[], const int ctIn, 
+                              dVector3 avArrayOut[], int &ctOut, 
+                              const dVector4 &plPlane )
+{
+    // start with no output points
+    ctOut = 0;
+
+    int i0 = ctIn-1;
+
+    // for each edge in input polygon
+    for (int i1=0; i1<ctIn; i0=i1, i1++) {
+
+
+        // calculate distance of edge points to plane
+        dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
+        dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );
+
+        // if first point is in front of plane
+        if( fDistance0 >= 0 ) {
+            // emit point
+            avArrayOut[ctOut][0] = avArrayIn[i0][0];
+            avArrayOut[ctOut][1] = avArrayIn[i0][1];
+            avArrayOut[ctOut][2] = avArrayIn[i0][2];
+            ctOut++;
+        }
+
+        // if points are on different sides
+        if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) {
+
+            // find intersection point of edge and plane
+            dVector3 vIntersectionPoint;
+            vIntersectionPoint[0]= avArrayIn[i0][0] - 
+                (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[1]= avArrayIn[i0][1] - 
+                (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[2]= avArrayIn[i0][2] - 
+                (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
+
+            // emit intersection point
+            avArrayOut[ctOut][0] = vIntersectionPoint[0];
+            avArrayOut[ctOut][1] = vIntersectionPoint[1];
+            avArrayOut[ctOut][2] = vIntersectionPoint[2];
+            ctOut++;
+        }
+    }
+
+}
+
+void		 dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, 
+                               dVector3 avArrayOut[], int &ctOut, 
+                               const dVector4 &plPlane ,dReal fRadius)
+{
+    // start with no output points
+    ctOut = 0;
+
+    int i0 = ctIn-1;
+
+    // for each edge in input polygon
+    for (int i1=0; i1<ctIn; i0=i1, i1++) 
+    {
+        // calculate distance of edge points to plane
+        dReal fDistance0 = dPointPlaneDistance(  avArrayIn[i0],plPlane );
+        dReal fDistance1 = dPointPlaneDistance(  avArrayIn[i1],plPlane );
+
+        // if first point is in front of plane
+        if( fDistance0 >= 0 ) 
+        {
+            // emit point
+            if (dVector3LengthSquare(avArrayIn[i0]) <= fRadius*fRadius)
+            {
+                avArrayOut[ctOut][0] = avArrayIn[i0][0];
+                avArrayOut[ctOut][1] = avArrayIn[i0][1];
+                avArrayOut[ctOut][2] = avArrayIn[i0][2];
+                ctOut++;
+            }
+        }
+
+        // if points are on different sides
+        if( (fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0) ) 
+        {
+
+            // find intersection point of edge and plane
+            dVector3 vIntersectionPoint;
+            vIntersectionPoint[0]= avArrayIn[i0][0] - 
+                (avArrayIn[i0][0]-avArrayIn[i1][0])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[1]= avArrayIn[i0][1] - 
+                (avArrayIn[i0][1]-avArrayIn[i1][1])*fDistance0/(fDistance0-fDistance1);
+            vIntersectionPoint[2]= avArrayIn[i0][2] - 
+                (avArrayIn[i0][2]-avArrayIn[i1][2])*fDistance0/(fDistance0-fDistance1);
+
+            // emit intersection point
+            if (dVector3LengthSquare(avArrayIn[i0]) <= fRadius*fRadius)
+            {
+                avArrayOut[ctOut][0] = vIntersectionPoint[0];
+                avArrayOut[ctOut][1] = vIntersectionPoint[1];
+                avArrayOut[ctOut][2] = vIntersectionPoint[2];
+                ctOut++;
+            }
+        }
+    }	
+}
+

thirdparty/ode-0.16.5/ode/src/collision_util.h

@@ -0,0 +1,358 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+some useful collision utility stuff.
+
+*/
+
+#ifndef _ODE_COLLISION_UTIL_H_
+#define _ODE_COLLISION_UTIL_H_
+
+#include <ode/common.h>
+#include <ode/contact.h>
+#include <ode/rotation.h>
+#include "odemath.h"
+
+
+// given a pointer `p' to a dContactGeom, return the dContactGeom at
+// p + skip bytes.
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+
+#if 1
+#include "collision_kernel.h"
+// Fetches a contact
+static inline 
+dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
+    dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
+    return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
+}
+#endif
+
+
+// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
+// return 1, else return 0.
+
+int dCollideSpheres (dVector3 p1, dReal r1,
+                     dVector3 p2, dReal r2, dContactGeom *c);
+
+
+// given two lines
+//    qa = pa + alpha* ua
+//    qb = pb + beta * ub
+// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
+// beta go from [-inf,inf], return alpha and beta such that qa and qb are
+// as close as possible
+
+void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
+                           const dVector3 pb, const dVector3 ub,
+                           dReal *alpha, dReal *beta);
+
+
+// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
+// vector 'side'), compute the points of closest approach between the box
+// and the line. return these points in 'lret' (the point on the line) and
+// 'bret' (the point on the box). if the line actually penetrates the box
+// then the solution is not unique, but only one solution will be returned.
+// in this case the solution points will coincide.
+
+void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
+                            const dVector3 c, const dMatrix3 R,
+                            const dVector3 side,
+                            dVector3 lret, dVector3 bret);
+
+// 20 Apr 2004
+// Start code by Nguyen Binh
+int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane);
+// clip polygon with plane and generate new polygon points
+void dClipPolyToPlane(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane );
+
+void dClipPolyToCircle(const dVector3 avArrayIn[], const int ctIn, dVector3 avArrayOut[], int &ctOut, const dVector4 &plPlane ,dReal fRadius);
+
+// Some vector math
+static inline 
+void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
+{
+    dSubtractVectors3(c, a, b);
+}
+
+static inline 
+void dVector3Scale(dVector3& a,dReal nScale)
+{
+    dScaleVector3(a, nScale);
+}
+
+static inline 
+void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
+{
+    dAddVectors3(c, a, b);
+}
+
+static inline 
+void dVector3Copy(const dVector3& a,dVector3& c)
+{
+    dCopyVector3(c, a);
+}
+
+static inline 
+void dVector4Copy(const dVector4& a,dVector4& c)
+{
+    dCopyVector4(c, a);
+}
+
+static inline 
+void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
+{
+    dCalcVectorCross3(c, a, b);
+}
+
+static inline 
+dReal dVector3Length(const dVector3& a)
+{
+    return dCalcVectorLength3(a);
+}
+
+static inline 
+dReal dVector3LengthSquare(const dVector3& a)
+{
+    return dCalcVectorLengthSquare3(a);
+}
+
+static inline 
+dReal dVector3Dot(const dVector3& a,const dVector3& b)
+{
+    return dCalcVectorDot3(a, b);
+}
+
+static inline 
+void dVector3Inv(dVector3& a)
+{
+    dNegateVector3(a);
+}
+
+static inline 
+void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
+{
+    dGetMatrixColumn3(v, m, col);
+}
+
+static inline 
+void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+{
+    dCalcVectorCross3_114(r, v, m + col);
+}
+
+static inline 
+void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+{
+    dCalcVectorCross3_141(r, m + col, v);
+}
+
+static inline 
+void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
+{
+    dMultiply0_331(r, m, v);
+}
+
+static inline 
+dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
+{
+    return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
+}
+
+static inline 
+void dConstructPlane(const dVector3& normal,const dReal& distance, dVector4& plane)
+{
+    plane[0] = normal[0];
+    plane[1] = normal[1];
+    plane[2] = normal[2];
+    plane[3] = distance;
+}
+
+static inline 
+void dMatrix3Copy(const dReal* source,dMatrix3& dest)
+{
+    dCopyMatrix4x3(dest, source);
+}
+
+static inline 
+dReal dMatrix3Det( const dMatrix3& mat )
+{
+    dReal det;
+
+    det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
+        - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
+        + mat[2] * ( mat[4]*mat[9]  - mat[8]*mat[5] );
+
+    return( det );
+}
+
+
+static inline 
+void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
+{
+    dReal det = dMatrix3Det( ma );
+
+    if ( dFabs( det ) < REAL(0.0005) )
+    {
+        dRSetIdentity( dst );
+        return;
+    }
+
+    double detRecip = REAL(1.0) / det;
+
+    dst[0] =  (dReal)(( ma[5]*ma[10] - ma[6]*ma[9]  ) * detRecip);
+    dst[1] =  (dReal)(( ma[9]*ma[2]  - ma[1]*ma[10] ) * detRecip);
+    dst[2] =  (dReal)(( ma[1]*ma[6]  - ma[5]*ma[2]  ) * detRecip);
+
+    dst[4] =  (dReal)(( ma[6]*ma[8]  - ma[4]*ma[10] ) * detRecip);
+    dst[5] =  (dReal)(( ma[0]*ma[10] - ma[8]*ma[2]  ) * detRecip);
+    dst[6] =  (dReal)(( ma[4]*ma[2]  - ma[0]*ma[6]  ) * detRecip);
+
+    dst[8] =  (dReal)(( ma[4]*ma[9]  - ma[8]*ma[5]  ) * detRecip);
+    dst[9] =  (dReal)(( ma[8]*ma[1]  - ma[0]*ma[9]  ) * detRecip);
+    dst[10] = (dReal)(( ma[0]*ma[5]  - ma[1]*ma[4]  ) * detRecip);
+}
+
+static inline 
+void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+{
+
+    // Nguyen Binh : this code seem to be the fastest.
+    dReal x0 = 	source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
+    dReal x1 = 	source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
+    dReal x2 = 	source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
+    dReal x3 = 	source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
+
+    dest[0]  = 	quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
+    dest[1]  = 	quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
+    dest[2]  = 	quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
+
+    /*
+    // nVidia SDK implementation
+    dVector3 uv, uuv; 
+    dVector3 qvec;
+    qvec[0] = quat[1];
+    qvec[1] = quat[2];
+    qvec[2] = quat[3];
+
+    dVector3Cross(qvec,source,uv);
+    dVector3Cross(qvec,uv,uuv);
+
+    dVector3Scale(uv,REAL(2.0)*quat[0]);
+    dVector3Scale(uuv,REAL(2.0));
+
+    dest[0] = source[0] + uv[0] + uuv[0];
+    dest[1] = source[1] + uv[1] + uuv[1];
+    dest[2] = source[2] + uv[2] + uuv[2];   
+    */
+}
+
+static inline 
+void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+{
+
+    dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
+
+    if (norm > REAL(0.0))
+    {
+        dQuaternion invQuat;
+        invQuat[0] =  quat[0] / norm;
+        invQuat[1] = -quat[1] / norm;
+        invQuat[2] = -quat[2] / norm;
+        invQuat[3] = -quat[3] / norm;	
+
+        dQuatTransform(invQuat,source,dest);
+
+    }
+    else
+    {
+        // Singular -> return identity
+        dVector3Copy(source,dest);
+    }
+}
+
+static inline 
+void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
+{
+    rY = asin(mRot[0 * 4 + 2]);
+    if (rY < M_PI /2)
+    {
+        if (rY > -M_PI /2)
+        {
+            rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
+            rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
+        }
+        else
+        {
+            // not unique
+            rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
+            rZ = REAL(0.0);
+        }
+    }
+    else
+    {
+        // not unique
+        rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
+        rZ = REAL(0.0);
+    }
+}
+
+static inline
+void dQuatInv(const dQuaternion& source, dQuaternion& dest)
+{
+    dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
+
+    if (norm > 0.0f)
+    {
+        dReal neg_norm_recip = -REAL(1.0) / norm;
+        dest[0] = -source[0] * neg_norm_recip;
+        dest[1] = source[1] * neg_norm_recip;
+        dest[2] = source[2] * neg_norm_recip;
+        dest[3] = source[3] * neg_norm_recip;	
+    }
+    else
+    {
+        // Singular -> return identity
+        dest[0] = REAL(1.0);
+        dest[1] = REAL(0.0);
+        dest[2] = REAL(0.0);
+        dest[3] = REAL(0.0);
+    }
+}
+
+// Finds barycentric
+static inline 
+void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
+    dReal w = REAL(1.0) - u - v;
+
+    Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
+    Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
+    Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
+    Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
+}
+
+
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/common.h

@@ -0,0 +1,351 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_PRIVATE_COMMON_H_
+#define _ODE_PRIVATE_COMMON_H_
+
+
+#include "typedefs.h"
+#include "error.h"
+#include <ode/memory.h>
+#include <algorithm>
+
+
+#ifndef SIZE_MAX
+#define SIZE_MAX  ((sizeint)(-1))
+#endif
+
+#define dMACRO_MAX(a, b) ((a) > (b) ? (a) : (b))
+#define dMACRO_MIN(a, b) ((a) < (b) ? (a) : (b))
+
+
+#ifdef dSINGLE
+#define dEpsilon  FLT_EPSILON
+#else
+#define dEpsilon  DBL_EPSILON
+#endif
+
+
+#ifdef dSINGLE
+
+#if !defined(FLT_MANT_DIG)
+#define FLT_MANT_DIG 24
+#endif
+
+#define dMaxExact   ((float)((1UL << FLT_MANT_DIG) - 1))
+#define dMinExact   ((float)(-dMaxExact))
+
+
+#else // #ifndef dSINGLE
+
+#if !defined(DBL_MANT_DIG)
+#define DBL_MANT_DIG 53
+#endif
+
+#define dMaxExact   (double)((1ULL << DBL_MANT_DIG) - 1)
+#define dMinExact   ((double)(-dMaxExact))
+
+
+#endif // #ifndef dSINGLE
+
+
+#define dMaxIntExact dMACRO_MIN(dMaxExact, (dReal)INT_MAX)
+#define dMinIntExact dMACRO_MAX(dMinExact, (dReal)INT_MIN)
+
+
+#ifndef offsetof
+#define offsetof(s, m) ((sizeint)&(((s *)8)->m) - (sizeint)8)
+#endif
+#ifndef membersize
+#define membersize(s, m) (sizeof(((s *)8)->m))
+#endif
+#ifndef endoffsetof
+#define endoffsetof(s, m)   ((sizeint)((sizeint)&(((s *)8)->m) - (sizeint)8) + sizeof(((s *)8)->m))
+#endif
+
+
+/* the efficient alignment. most platforms align data structures to some
+ * number of bytes, but this is not always the most efficient alignment.
+ * for example, many x86 compilers align to 4 bytes, but on a pentium it
+ * is important to align doubles to 8 byte boundaries (for speed), and
+ * the 4 floats in a SIMD register to 16 byte boundaries. many other
+ * platforms have similar behavior. setting a larger alignment can waste
+ * a (very) small amount of memory. NOTE: this number must be a power of
+ * two. this is set to 16 by default.
+ */
+#ifndef EFFICIENT_ALIGNMENT
+#define EFFICIENT_ALIGNMENT 16
+#endif
+
+#define dALIGN_SIZE(buf_size, alignment) (((buf_size) + (alignment - 1)) & (int)(~(alignment - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes
+#define dALIGN_PTR(buf_ptr, alignment) ((void *)(((uintptr)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes
+
+/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
+#define dEFFICIENT_SIZE(x) dALIGN_SIZE(x, EFFICIENT_ALIGNMENT)
+#define dEFFICIENT_PTR(p) dALIGN_PTR(p, EFFICIENT_ALIGNMENT)
+#define dOFFSET_EFFICIENTLY(p, b) ((void *)((uintptr)(p) + dEFFICIENT_SIZE(b)))
+
+#define dOVERALIGNED_SIZE(size, alignment) dEFFICIENT_SIZE((size) + ((alignment) - EFFICIENT_ALIGNMENT))
+#define dOVERALIGNED_PTR(buf_ptr, alignment) dALIGN_PTR(buf_ptr, alignment)
+#define dOFFSET_OVERALIGNEDLY(buf_ptr, size, alignment) ((void *)((uintptr)(buf_ptr) + dOVERALIGNED_SIZE(size, alignment)))
+
+
+
+#define dDERIVE_SIZE_UNION_PADDING_ELEMENTS(DataSize, ElementType) (((DataSize) + sizeof(ElementType) - 1) / sizeof(ElementType))
+#define dDERIVE_TYPE_UNION_PADDING_ELEMENTS(DataType, ElementType) dDERIVE_SIZE_UNION_PADDING_ELEMENTS(sizeof(DataType), ElementType)
+#define dDERIVE_SIZE_EXTRA_PADDING_ELEMENTS(DataSize, AlignmentSize, ElementType) (((dALIGN_SIZE(DataSize, dMACRO_MAX(AlignmentSize, sizeof(ElementType))) - (DataSize)) / sizeof(ElementType))
+
+
+
+/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
+ * up to 15 bytes per allocation, depending on what alloca() returns.
+ */
+#define dALLOCA16(n) \
+    dEFFICIENT_PTR(alloca((n)+(EFFICIENT_ALIGNMENT)))
+
+
+class dxAlignedAllocation
+{
+public:
+    dxAlignedAllocation(): m_userAreaPointer(NULL), m_bufferAllocated(NULL), m_sizeUsed(0) {}
+    ~dxAlignedAllocation() { freeAllocation(); }
+
+    void *allocAligned(sizeint sizeRequired, unsigned alignmentRequired)
+    {
+        dIASSERT((alignmentRequired & (alignmentRequired - 1)) == 0);
+        dIASSERT(alignmentRequired <= SIZE_MAX - sizeRequired);
+
+        sizeint sizeToUse = sizeRequired + alignmentRequired;
+        void *bufferPointer = dAlloc(sizeToUse);
+        void *userAreaPointer = bufferPointer != NULL && alignmentRequired != 0 ? dALIGN_PTR(bufferPointer, alignmentRequired) : bufferPointer;
+        assignData(userAreaPointer, bufferPointer, sizeToUse);
+
+        return userAreaPointer;
+    }
+
+    void *getUserAreaPointer() const { return m_userAreaPointer; }
+    sizeint getUserAreaSize() const { return m_sizeUsed - ((uint8 *)m_userAreaPointer - (uint8 *)m_bufferAllocated); }
+
+    void freeAllocation()
+    {
+        sizeint sizeUsed;
+        void *bufferPointer = extractData(sizeUsed);
+        
+        if (bufferPointer != NULL)
+        {
+            dFree(bufferPointer, sizeUsed);
+        }
+    }
+
+private:
+    void assignData(void *userAreaPointer, void *bufferAllocated, sizeint sizeUsed)
+    {
+        dIASSERT(m_userAreaPointer == NULL);
+        dIASSERT(m_bufferAllocated == NULL);
+        dIASSERT(m_sizeUsed == 0);
+
+        m_userAreaPointer = userAreaPointer;
+        m_bufferAllocated = bufferAllocated;
+        m_sizeUsed = sizeUsed;
+    }
+
+    void *extractData(sizeint &out_sizeUsed)
+    {
+        void *bufferPointer = m_bufferAllocated;
+
+        if (bufferPointer != NULL)
+        {
+            out_sizeUsed = m_sizeUsed;
+
+            m_userAreaPointer = NULL;
+            m_bufferAllocated = NULL;
+            m_sizeUsed = 0;
+        }
+
+        return bufferPointer;
+    }
+
+private:
+    void *m_userAreaPointer;
+    void *m_bufferAllocated;
+    sizeint m_sizeUsed;
+};
+
+
+template<typename DstType, typename SrcType>
+inline 
+bool _cast_to_smaller(DstType &dtOutResult, const SrcType &stArgument)
+{
+    return (SrcType)(dtOutResult = (DstType)stArgument) == stArgument;
+}
+
+#if defined(__GNUC__)
+
+#define dCAST_TO_SMALLER(TargetType, SourceValue) ({ TargetType ttCastSmallerValue; dIVERIFY(_cast_to_smaller(ttCastSmallerValue, SourceValue)); ttCastSmallerValue; })
+
+
+#else // #if !defined(__GNUC__)
+
+#define dCAST_TO_SMALLER(TargetType, SourceValue) templateCAST_TO_SMALLER<TargetType>(SourceValue)
+
+template <typename TTargetType, typename TSourceType>
+inline TTargetType templateCAST_TO_SMALLER(const TSourceType &stSourceValue)
+{
+    TTargetType ttCastSmallerValue;
+    dIVERIFY(_cast_to_smaller(ttCastSmallerValue, stSourceValue));
+    return ttCastSmallerValue;
+}
+
+
+#endif // #if !defined(__GNUC__)
+
+
+template<typename value_type>
+inline 
+void dxSwap(value_type &one, value_type &another)
+{
+    std::swap(one, another);
+}
+
+template<typename value_type, typename lo_type, typename hi_type>
+inline 
+value_type dxClamp(const value_type &value, const lo_type &lo, const hi_type &hi)
+{
+    return value < lo ? (value_type)lo : value > hi ? (value_type)hi : value;
+}
+
+
+template <typename Type>
+union _const_type_cast_union
+{
+    explicit _const_type_cast_union(const void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {}
+
+    operator const Type *() const { return m_pstTypedPointer; }
+    const Type &operator *() const { return *m_pstTypedPointer; }
+    const Type *operator ->() const { return m_pstTypedPointer; }
+    const Type &operator [](diffint diElementIndex) const { return m_pstTypedPointer[diElementIndex]; }
+    const Type &operator [](sizeint siElementIndex) const { return m_pstTypedPointer[siElementIndex]; }
+
+    const void 		*m_psvCharBuffer;
+    const Type		*m_pstTypedPointer;
+};
+
+template <typename Type>
+union _type_cast_union
+{
+    explicit _type_cast_union(void *psvCharBuffer): m_psvCharBuffer(psvCharBuffer) {}
+
+    operator Type *() const { return m_pstTypedPointer; }
+    Type &operator *() const { return *m_pstTypedPointer; }
+    Type *operator ->() const { return m_pstTypedPointer; }
+    Type &operator [](diffint diElementIndex) const { return m_pstTypedPointer[diElementIndex]; }
+    Type &operator [](sizeint siElementIndex) const { return m_pstTypedPointer[siElementIndex]; }
+
+    void			*m_psvCharBuffer;
+    Type			*m_pstTypedPointer;
+};
+
+
+template<sizeint tsiTypeSize>
+struct _sized_signed;
+
+template<>
+struct _sized_signed<sizeof(uint8)>
+{
+    typedef int8 type;
+};
+
+template<>
+struct _sized_signed<sizeof(uint16)>
+{
+    typedef int16 type;
+};
+
+template<>
+struct _sized_signed<sizeof(uint32)>
+{
+    typedef int32 type;
+};
+
+template<>
+struct _sized_signed<sizeof(uint64)>
+{
+    typedef int64 type;
+};
+
+template<typename tintergraltype>
+struct _make_signed
+{
+    typedef typename _sized_signed<sizeof(tintergraltype)>::type type;
+};
+
+
+template<sizeint tsiTypeSize>
+struct _sized_unsigned;
+
+template<>
+struct _sized_unsigned<sizeof(int8)>
+{
+    typedef uint8 type;
+};
+
+template<>
+struct _sized_unsigned<sizeof(int16)>
+{
+    typedef uint16 type;
+};
+
+template<>
+struct _sized_unsigned<sizeof(int32)>
+{
+    typedef uint32 type;
+};
+
+template<>
+struct _sized_unsigned<sizeof(int64)>
+{
+    typedef uint64 type;
+};
+
+template<typename tintergraltype>
+struct _make_unsigned
+{
+    typedef typename _sized_unsigned<sizeof(tintergraltype)>::type type;
+};
+
+
+// template<typename tvalueint, typename tminint, typename tmaxint>
+// inline 
+// bool dxInRange(tvalueint viValue, tminint miMin, tmaxint miMax)
+// {
+//     return (typename _sized_unsigned<dMACRO_MAX(sizeof(tvalueint), sizeof(tminint))>::type)(viValue - miMin) < (typename _sized_unsigned<dMACRO_MAX(sizeof(tmaxint), sizeof(tminint))>::type)(miMax - miMin);
+// }
+// #define dIN_RANGE(aval, amin, amax) dxInRange(aval, amin, amax)
+
+#define dIN_RANGE(aval, amin, amax) ((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (_sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (_sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin)))
+#define dTMPL_IN_RANGE(aval, amin, amax) ((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(aval) - (typename _sized_unsigned<dMACRO_MAX(sizeof(aval), sizeof(amin))>::type)(amin)) < (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)((typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amax) - (typename _sized_unsigned<dMACRO_MAX(sizeof(amax), sizeof(amin))>::type)(amin)))
+#define dCLAMP(aval, alo, ahi) dxClamp(aval, alo, ahi)
+#define dARRAY_SIZE(aarr) (sizeof(aarr) / sizeof((aarr)[0]))
+#define dSTATIC_ARRAY_SIZE(aclass, aarr) dARRAY_SIZE(((aclass *)sizeof(void *))->aarr)
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/config.h

@@ -0,0 +1,113 @@
+/* This file was autogenerated by Premake */
+#ifndef _ODE_CONFIG_H_
+#define _ODE_CONFIG_H_
+
+
+/******************************************************************
+ * CONFIGURATON SETTINGS - you can change these, and then rebuild
+ *   ODE to modify the behavior of the library.
+ *
+ *   dTRIMESH_ENABLED  - enable/disable trimesh support
+ *   dTRIMESH_OPCODE   - use the OPCODE trimesh engine
+ *   dTRIMESH_GIMPACT  - use the GIMPACT trimesh engine
+ *                       Only one trimesh engine should be enabled.
+ *
+ *   dTRIMESH_16BIT_INDICES (todo: opcode only)
+ *                       Setup the trimesh engine to use 16 bit
+ *                       triangle indices. The default is to use
+ *                       32 bit indices. Use the dTriIndex type to
+ *                       detect the correct index size.
+ *
+ *   dTRIMESH_OPCODE_USE_NEWOLD_TRIMESH_TRIMESH_COLLIDER
+ *                       Use old implementation of trimesh-trimesh collider
+ *                       (for backward compatibility only)
+ *
+ *   dOU_ENABLED       
+ *   dATOMICS_ENABLED
+ *   dTLS_ENABLED
+ *                       Use generic features of OU library, atomic API
+ *                       and TLS API respectively.
+ *                       Generic features and atomic API are always enabled, 
+ *                       unless threading interface support is disabled.
+ *                       Using TLS for global variables allows calling ODE 
+ *                       collision detection functions from multiple threads.
+ *
+ *   dBUILTIN_THREADING_IMPL_ENABLED
+ *                       Include built-in multithreaded threading 
+ *                       implementation (still must be created and assigned
+ *                       to be used).
+ *
+ ******************************************************************/
+
+#define dTRIMESH_ENABLED 1
+#define dTRIMESH_OPCODE 1
+#define dTRIMESH_16BIT_INDICES 0
+
+#define dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER 0
+
+#define dOU_ENABLED 1
+#define dATOMICS_ENABLED 1
+/* #define dTLS_ENABLED 1 */
+
+/* #define dTHREADING_INTF_DISABLED 1 */
+#define dBUILTIN_THREADING_IMPL_ENABLED 1
+
+
+/******************************************************************
+ * SYSTEM SETTINGS - you shouldn't need to change these. If you
+ *   run into an issue with these settings, please report it to
+ *   the ODE bug tracker at:
+ *      http://sf.net/tracker/?group_id=24884&atid=382799
+ ******************************************************************/
+
+/* Try to identify the platform */
+#if defined(_XENON)
+  #define ODE_PLATFORM_XBOX360
+#elif defined(SN_TARGET_PSP_HW)
+  #define ODE_PLATFORM_PSP
+#elif defined(SN_TARGET_PS3)
+  #define ODE_PLATFORM_PS3
+#elif defined(_MSC_VER) || defined(__CYGWIN__) || defined(__MINGW32__)
+  #define ODE_PLATFORM_WINDOWS
+#elif defined(__linux__)
+  #define ODE_PLATFORM_LINUX
+#elif defined(__APPLE__) && defined(__MACH__)
+  #define ODE_PLATFORM_OSX
+#else
+  #error "Need some help identifying the platform!"
+#endif
+
+/* Additional platform defines used in the code */
+#if defined(ODE_PLATFORM_WINDOWS) && !defined(WIN32)
+  #define WIN32
+#endif
+
+#if defined(__CYGWIN__) || defined(__MINGW32__)
+  #define CYGWIN
+#endif
+
+#if defined(ODE_PLATFORM_OSX)
+  #define macintosh
+#endif
+
+#if !defined(ODE_PLATFORM_OSX) && !defined(ODE_PLATFORM_PS3)
+  #include <malloc.h>
+#endif
+
+#if !defined(ODE_PLATFORM_WINDOWS)
+  #include <alloca.h>
+#endif
+
+
+/* Basic OU functionality is required if either atomic API or TLS support
+ * is enabled. */
+#if (dATOMICS_ENABLED || dTLS_ENABLED) && !dOU_ENABLED
+#undef dOU_ENABLED
+#define dOU_ENABLED 1
+#endif
+
+
+#include "typedefs.h"
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/config.h.in

@@ -0,0 +1,329 @@
+/* ode/src/config.h.in.  Generated from configure.ac by autoheader.  */
+
+
+#ifndef ODE_CONFIG_H
+#define ODE_CONFIG_H
+
+
+/* Define if building universal (internal helper macro) */
+#undef AC_APPLE_UNIVERSAL_BUILD
+
+/* Define to one of `_getb67', `GETB67', `getb67' for Cray-2 and Cray-YMP
+   systems. This function is required for `alloca.c' support on those systems.
+   */
+#undef CRAY_STACKSEG_END
+
+/* Define to 1 if using `alloca.c'. */
+#undef C_ALLOCA
+
+/* Define to 1 if you have `alloca', as a function or macro. */
+#undef HAVE_ALLOCA
+
+/* Define to 1 if you have <alloca.h> and it should be used (not on Ultrix).
+   */
+#undef HAVE_ALLOCA_H
+
+/* Use the Apple OpenGL framework. */
+#undef HAVE_APPLE_OPENGL_FRAMEWORK
+
+/* Define to 1 if you have the `atan2f' function. */
+#undef HAVE_ATAN2F
+
+/* Define to 1 if you have the `clock_gettime' function. */
+#undef HAVE_CLOCK_GETTIME
+
+/* Define to 1 if you have the `copysign' function. */
+#undef HAVE_COPYSIGN
+
+/* Define to 1 if you have the `copysignf' function. */
+#undef HAVE_COPYSIGNF
+
+/* Define to 1 if you have the `cosf' function. */
+#undef HAVE_COSF
+
+/* Define to 1 if you have the <dlfcn.h> header file. */
+#undef HAVE_DLFCN_H
+
+/* Define to 1 if you have the `fabsf' function. */
+#undef HAVE_FABSF
+
+/* Define to 1 if you have the <float.h> header file. */
+#undef HAVE_FLOAT_H
+
+/* Define to 1 if you have the `floor' function. */
+#undef HAVE_FLOOR
+
+/* Define to 1 if you have the `fmodf' function. */
+#undef HAVE_FMODF
+
+/* Define to 1 if you have the `gettimeofday' function. */
+#undef HAVE_GETTIMEOFDAY
+
+/* Define to 1 if you have the <GL/glext.h> header file. */
+#undef HAVE_GL_GLEXT_H
+
+/* Define to 1 if you have the <GL/glu.h> header file. */
+#undef HAVE_GL_GLU_H
+
+/* Define to 1 if you have the <GL/gl.h> header file. */
+#undef HAVE_GL_GL_H
+
+/* Define to 1 if you have the <inttypes.h> header file. */
+#undef HAVE_INTTYPES_H
+
+/* Define to 1 if you have the `isnan' function. */
+#undef HAVE_ISNAN
+
+/* Define to 1 if you have the `isnanf' function. */
+#undef HAVE_ISNANF
+
+/* Define to 1 if you have the `m' library (-lm). */
+#undef HAVE_LIBM
+
+/* Define to 1 if you have the `rt' library (-lrt). */
+#undef HAVE_LIBRT
+
+/* Define to 1 if you have the `sunmath' library (-lsunmath). */
+#undef HAVE_LIBSUNMATH
+
+/* Define to 1 if you have the <limits.h> header file. */
+#undef HAVE_LIMITS_H
+
+/* Define to 1 if you have the <malloc.h> header file. */
+#undef HAVE_MALLOC_H
+
+/* Define to 1 if you have the <math.h> header file. */
+#undef HAVE_MATH_H
+
+/* Define to 1 if you have the `memmove' function. */
+#undef HAVE_MEMMOVE
+
+/* Define to 1 if you have the <memory.h> header file. */
+#undef HAVE_MEMORY_H
+
+/* Define to 1 if you have the `memset' function. */
+#undef HAVE_MEMSET
+
+/* Define to 1 if you have the `no_pthread_condattr_setclock' function. */
+#undef HAVE_NO_PTHREAD_CONDATTR_SETCLOCK
+
+/* Define to 1 if libc includes obstacks. */
+#undef HAVE_OBSTACK
+
+/* Define to 1 if you have the `pthread_attr_setinheritsched' function. */
+#undef HAVE_PTHREAD_ATTR_SETINHERITSCHED
+
+/* Define to 1 if you have the `pthread_attr_setstacklazy' function. */
+#undef HAVE_PTHREAD_ATTR_SETSTACKLAZY
+
+/* Define to 1 if you have the `pthread_condattr_setclock' function. */
+#undef HAVE_PTHREAD_CONDATTR_SETCLOCK
+
+/* Define to 1 if you have the `sinf' function. */
+#undef HAVE_SINF
+
+/* Define to 1 if you have the `snprintf' function. */
+#undef HAVE_SNPRINTF
+
+/* Define to 1 if you have the `sqrt' function. */
+#undef HAVE_SQRT
+
+/* Define to 1 if you have the `sqrtf' function. */
+#undef HAVE_SQRTF
+
+/* Define to 1 if you have the <stdarg.h> header file. */
+#undef HAVE_STDARG_H
+
+/* Define to 1 if stdbool.h conforms to C99. */
+#undef HAVE_STDBOOL_H
+
+/* Define to 1 if you have the <stddef.h> header file. */
+#undef HAVE_STDDEF_H
+
+/* Define to 1 if you have the <stdint.h> header file. */
+#undef HAVE_STDINT_H
+
+/* Define to 1 if you have the <stdio.h> header file. */
+#undef HAVE_STDIO_H
+
+/* Define to 1 if you have the <stdlib.h> header file. */
+#undef HAVE_STDLIB_H
+
+/* Define to 1 if you have the `strchr' function. */
+#undef HAVE_STRCHR
+
+/* Define to 1 if you have the <strings.h> header file. */
+#undef HAVE_STRINGS_H
+
+/* Define to 1 if you have the <string.h> header file. */
+#undef HAVE_STRING_H
+
+/* Define to 1 if you have the `strstr' function. */
+#undef HAVE_STRSTR
+
+/* Define to 1 if you have the <sys/stat.h> header file. */
+#undef HAVE_SYS_STAT_H
+
+/* Define to 1 if you have the <sys/time.h> header file. */
+#undef HAVE_SYS_TIME_H
+
+/* Define to 1 if you have the <sys/types.h> header file. */
+#undef HAVE_SYS_TYPES_H
+
+/* Define to 1 if you have the <time.h> header file. */
+#undef HAVE_TIME_H
+
+/* Define to 1 if you have the <unistd.h> header file. */
+#undef HAVE_UNISTD_H
+
+/* Define to 1 if you have the `vsnprintf' function. */
+#undef HAVE_VSNPRINTF
+
+/* Define to 1 if the system has the type `_Bool'. */
+#undef HAVE__BOOL
+
+/* Define to 1 if you have the `_isnan' function. */
+#undef HAVE__ISNAN
+
+/* Define to 1 if you have the `_isnanf' function. */
+#undef HAVE__ISNANF
+
+/* Define to 1 if you have the `__isnan' function. */
+#undef HAVE___ISNAN
+
+/* Define to 1 if you have the `__isnanf' function. */
+#undef HAVE___ISNANF
+
+/* Define to the sub-directory where libtool stores uninstalled libraries. */
+#undef LT_OBJDIR
+
+/* Mac OS X version setting for OU Library */
+#undef MAC_OS_X_VERSION
+
+/* Name of package */
+#undef PACKAGE
+
+/* Define to the address where bug reports for this package should be sent. */
+#undef PACKAGE_BUGREPORT
+
+/* Define to the full name of this package. */
+#undef PACKAGE_NAME
+
+/* Define to the full name and version of this package. */
+#undef PACKAGE_STRING
+
+/* Define to the one symbol short name of this package. */
+#undef PACKAGE_TARNAME
+
+/* Define to the home page for this package. */
+#undef PACKAGE_URL
+
+/* Define to the version of this package. */
+#undef PACKAGE_VERSION
+
+/* compiling for a pentium on a gcc-based platform? */
+#undef PENTIUM
+
+/* If using the C implementation of alloca, define if you know the
+   direction of stack growth for your system; otherwise it will be
+   automatically deduced at runtime.
+	STACK_DIRECTION > 0 => grows toward higher addresses
+	STACK_DIRECTION < 0 => grows toward lower addresses
+	STACK_DIRECTION = 0 => direction of growth unknown */
+#undef STACK_DIRECTION
+
+/* Define to 1 if you have the ANSI C header files. */
+#undef STDC_HEADERS
+
+/* Version number of package */
+#undef VERSION
+
+/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most
+   significant byte first (like Motorola and SPARC, unlike Intel). */
+#if defined AC_APPLE_UNIVERSAL_BUILD
+# if defined __BIG_ENDIAN__
+#  define WORDS_BIGENDIAN 1
+# endif
+#else
+# ifndef WORDS_BIGENDIAN
+#  undef WORDS_BIGENDIAN
+# endif
+#endif
+
+/* compiling for a X86_64 system on a gcc-based platform? */
+#undef X86_64_SYSTEM
+
+/* OU features enabled */
+#undef _OU_FEATURE_SET
+
+/* libou namespace for ODE */
+#undef _OU_NAMESPACE
+
+/* Target OS setting for OU Library */
+#undef _OU_TARGET_OS
+
+/* Define for Solaris 2.5.1 so the uint32_t typedef from <sys/synch.h>,
+   <pthread.h>, or <semaphore.h> is not used. If the typedef were allowed, the
+   #define below would cause a syntax error. */
+#undef _UINT32_T
+
+/* Atomic API of OU is enabled */
+#undef dATOMICS_ENABLED
+
+/* Built-in multithreaded threading implementation is included */
+#undef dBUILTIN_THREADING_IMPL_ENABLED
+
+/* Generic OU features are enabled */
+#undef dOU_ENABLED
+
+/* Threading interface is disabled */
+#undef dTHREADING_INTF_DISABLED
+
+/* Thread Local Storage API of OU is enabled */
+#undef dTLS_ENABLED
+
+/* Use the old trimesh-trimesh collider */
+#undef dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+
+/* Define to `__inline__' or `__inline' if that's what the C compiler
+   calls it, or to nothing if 'inline' is not supported under any name.  */
+#ifndef __cplusplus
+#undef inline
+#endif
+
+/* Define to the type of a signed integer type of width exactly 32 bits if
+   such a type exists and the standard includes do not define it. */
+#undef int32_t
+
+/* Define to `unsigned int' if <sys/types.h> does not define. */
+#undef size_t
+
+/* Define to the type of an unsigned integer type of width exactly 32 bits if
+   such a type exists and the standard includes do not define it. */
+#undef uint32_t
+
+/* Define to empty if the keyword `volatile' does not work. Warning: valid
+   code using `volatile' can become incorrect without. Disable with care. */
+#undef volatile
+
+
+
+#ifdef HAVE_ALLOCA_H
+#include <alloca.h>
+#endif
+#ifdef HAVE_MALLOC_H
+#include <malloc.h>
+#endif
+#ifdef HAVE_STDINT_H
+#include <stdint.h>
+#endif
+#ifdef HAVE_INTTYPES_H
+#include <inttypes.h>
+#endif
+
+
+#include "typedefs.h"
+
+
+#endif /* #define ODE_CONFIG_H */
+

thirdparty/ode-0.16.5/ode/src/coop_matrix_types.h

@@ -0,0 +1,158 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// Cooperative matrix algorithm types
+// Copyright (C) 2017-2024 Oleh Derevenko (odar@eleks.com - change all "a" to "e")
+
+
+#ifndef _ODE_COOP_MATRIX_TYPES_H_
+#define _ODE_COOP_MATRIX_TYPES_H_
+
+
+
+#include "threadingutils.h"
+#include "common.h"
+#include "error.h"
+
+
+#ifndef dCOOPERATIVE_ENABLED
+
+#if dATOMICS_ENABLED && !dTHREADING_INTF_DISABLED
+
+#define dCOOPERATIVE_ENABLED 1
+
+
+#endif // #if dATOMICS_ENABLED && !dTHREADING_INTF_DISABLED
+
+
+#endif // #ifndef dCOOPERATIVE_ENABLED
+
+
+enum
+{
+    COOP_THREAD_DATA_ALIGNMENT_SIZE      = 64, // Typical size of a cache line
+};
+
+
+typedef uintptr cellindexint;
+
+
+enum CellContextInstance
+{
+    CCI__MIN,
+
+    CCI_FIRST = CCI__MIN,
+    CCI_SECOND,
+
+    CCI__MAX,
+    CCI__LOG2_OF_MAX = 1,
+
+    CCI__DEFAULT = CCI__MIN,
+};
+dSASSERT(1 << CCI__LOG2_OF_MAX >= CCI__MAX);
+
+static inline 
+CellContextInstance buildNextContextInstance(CellContextInstance instance)
+{
+    dIASSERT(dIN_RANGE(instance, CCI__MIN, CCI__MAX));
+    dSASSERT(CCI__MAX == 2);
+
+    return (CellContextInstance)(CCI_FIRST + CCI_SECOND - instance);
+}
+
+
+enum
+{
+    CELLDESC_CCI_BITMASK    = (1 << CCI__LOG2_OF_MAX) - 1,
+    CELLDESC_LOCK_BIT       = 1 << CCI__LOG2_OF_MAX,
+    CELLDESC__HELPER_BITS   = CELLDESC_CCI_BITMASK | CELLDESC_LOCK_BIT,
+    CELLDESC__COLINDEX_BASE = CELLDESC__HELPER_BITS + 1,
+};
+
+#define MAKE_CELLDESCRIPTOR(columnIndex, contextInstance, locked) ((cellindexint)((cellindexint)(columnIndex) * CELLDESC__COLINDEX_BASE + (contextInstance) + ((locked) ? CELLDESC_LOCK_BIT : 0)))
+#define MARK_CELLDESCRIPTOR_LOCKED(descriptor) ((cellindexint)((descriptor) | CELLDESC_LOCK_BIT))
+#define GET_CELLDESCRIPTOR_COLUMNINDEX(descriptor) ((unsigned int)((cellindexint)(descriptor) / CELLDESC__COLINDEX_BASE))
+#define GET_CELLDESCRIPTOR_CONTEXTINSTANCE(descriptor) ((CellContextInstance)((descriptor) & CELLDESC_CCI_BITMASK))
+#define GET_CELLDESCRIPTOR_ISLOCKED(descriptor) (((descriptor) & CELLDESC_LOCK_BIT) != 0)
+
+#define INVALID_CELLDESCRIPTOR      MAKE_CELLDESCRIPTOR(GET_CELLDESCRIPTOR_COLUMNINDEX(-1), CCI__MAX - 1, true)
+
+
+enum BlockProcessingState
+{
+    BPS_COMPETING_FOR_A_BLOCK = -1,
+    BPS_NO_BLOCKS_PROCESSED,
+    BPS_SOME_BLOCKS_PROCESSED,
+};
+
+
+class CooperativeAtomics
+{
+public:
+    static atomicord32 AtomicDecrementUint32(volatile atomicord32 *paoDestination)
+    {
+#if dCOOPERATIVE_ENABLED
+        return ::AtomicDecrement(paoDestination);
+#else
+        dIASSERT(false); return 0; // The function is not supposed to be called in this case
+#endif // #if dCOOPERATIVE_ENABLED
+    }
+
+    static bool AtomicCompareExchangeUint32(volatile atomicord32 *paoDestination, atomicord32 aoComparand, atomicord32 aoExchange)
+    {
+#if dCOOPERATIVE_ENABLED
+        return ::AtomicCompareExchange(paoDestination, aoComparand, aoExchange);
+#else
+        dIASSERT(false); return false; // The function is not supposed to be called in this case
+#endif // #if dCOOPERATIVE_ENABLED
+    }
+
+    static bool AtomicCompareExchangeCellindexint(volatile cellindexint *destination, cellindexint comparand, cellindexint exchange)
+    {
+#if dCOOPERATIVE_ENABLED
+        return ::AtomicCompareExchangePointer((volatile atomicptr *)destination, (atomicptr)comparand, (atomicptr)exchange);
+#else
+        dIASSERT(false); return false; // The function is not supposed to be called in this case
+#endif // #if dCOOPERATIVE_ENABLED
+    }
+
+    static void AtomicStoreCellindexint(volatile cellindexint *destination, cellindexint value)
+    {
+#if dCOOPERATIVE_ENABLED
+        ::AtomicStorePointer((volatile atomicptr *)destination, (atomicptr)value);
+#else
+        dIASSERT(false); // The function is not supposed to be called in this case
+#endif // #if dCOOPERATIVE_ENABLED
+    }
+
+    static void AtomicReadReorderBarrier()
+    {
+#if dCOOPERATIVE_ENABLED
+        ::AtomicReadReorderBarrier();
+#else
+        dIASSERT(false); // The function is not supposed to be called in this case
+#endif // #if dCOOPERATIVE_ENABLED
+    }
+};
+
+
+#endif // #ifndef _ODE_COOP_MATRIX_TYPES_H_

thirdparty/ode-0.16.5/ode/src/cylinder.cpp

@@ -0,0 +1,108 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+standard ODE geometry primitives: public API and pairwise collision functions.
+
+the rule is that only the low level primitive collision functions should set
+dContactGeom::g1 and dContactGeom::g2.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+
+#define dMAX(A,B)  ((A)>(B) ? (A) : (B))
+
+
+// flat cylinder public API
+
+dxCylinder::dxCylinder (dSpaceID space, dReal _radius, dReal _length) :
+dxGeom (space,1)
+{
+    dAASSERT (_radius >= 0 && _length >= 0);
+    type = dCylinderClass;
+    radius = _radius;
+    lz = _length;
+    updateZeroSizedFlag(!_radius || !_length);
+}
+
+
+void dxCylinder::computeAABB()
+{
+    const dMatrix3& R = final_posr->R;
+    const dVector3& pos = final_posr->pos;
+
+    dReal dOneMinusR2Square = (dReal)(REAL(1.0) - R[2]*R[2]);
+    dReal xrange = dFabs(R[2]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR2Square));
+    dReal dOneMinusR6Square = (dReal)(REAL(1.0) - R[6]*R[6]);
+    dReal yrange = dFabs(R[6]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR6Square));
+    dReal dOneMinusR10Square = (dReal)(REAL(1.0) - R[10]*R[10]);
+    dReal zrange = dFabs(R[10]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR10Square));
+
+    aabb[0] = pos[0] - xrange;
+    aabb[1] = pos[0] + xrange;
+    aabb[2] = pos[1] - yrange;
+    aabb[3] = pos[1] + yrange;
+    aabb[4] = pos[2] - zrange;
+    aabb[5] = pos[2] + zrange;
+}
+
+
+dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length)
+{
+    return new dxCylinder (space,radius,length);
+}
+
+void dGeomCylinderSetParams (dGeomID cylinder, dReal radius, dReal length)
+{
+    dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
+    dAASSERT (radius >= 0 && length >= 0);
+    dxCylinder *c = (dxCylinder*) cylinder;
+    c->radius = radius;
+    c->lz = length;
+    c->updateZeroSizedFlag(!radius || !length);
+    dGeomMoved (cylinder);
+}
+
+void dGeomCylinderGetParams (dGeomID cylinder, dReal *radius, dReal *length)
+{
+    dUASSERT (cylinder && cylinder->type == dCylinderClass,"argument not a ccylinder");
+    dxCylinder *c = (dxCylinder*) cylinder;
+    *radius = c->radius;
+    *length = c->lz;
+}
+
+

thirdparty/ode-0.16.5/ode/src/default_threading.cpp

@@ -0,0 +1,77 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * Threading base wrapper class header file.                             *
+ * Copyright (C) 2011-2024 Oleh Derevenko. All rights reserved.          *
+ * e-mail: odar@eleks.com (change all "a" to "e")                        *
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * The default threading instance holder class implementation
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+
+#include <ode/common.h>
+#include <ode/threading_impl.h>
+#include "config.h"
+#include "default_threading.h"
+#include "error.h"
+
+
+/*static */dThreadingImplementationID DefaultThreadingHolder::m_defaultThreadingImpl = NULL;
+/*static */const dThreadingFunctionsInfo *DefaultThreadingHolder::m_defaultThreadingFunctions = NULL;
+
+
+/*static */
+bool DefaultThreadingHolder::initializeDefaultThreading()
+{
+    dIASSERT(m_defaultThreadingImpl == NULL);
+
+    bool initResult = false;
+
+    dThreadingImplementationID threadingImpl = dThreadingAllocateSelfThreadedImplementation();
+
+    if (threadingImpl != NULL)
+    {
+        m_defaultThreadingFunctions = dThreadingImplementationGetFunctions(threadingImpl);
+        m_defaultThreadingImpl = threadingImpl;
+
+        initResult = true;
+    }
+
+    return initResult;
+}
+
+/*static */
+void DefaultThreadingHolder::finalizeDefaultThreading()
+{
+    dThreadingImplementationID threadingImpl = m_defaultThreadingImpl;
+
+    if (threadingImpl != NULL)
+    {
+        dThreadingFreeImplementation(threadingImpl);
+
+        m_defaultThreadingFunctions = NULL;
+        m_defaultThreadingImpl = NULL;
+    }
+}
+

thirdparty/ode-0.16.5/ode/src/default_threading.h

@@ -0,0 +1,55 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * Threading base wrapper class header file.                             *
+ * Copyright (C) 2011-2024 Oleh Derevenko. All rights reserved.          *
+ * e-mail: odar@eleks.com (change all "a" to "e")                        *
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * A default threading instance holder class definition
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+
+#ifndef _ODE__PRIVATE_DEFAULT_THREADING_H_
+#define _ODE__PRIVATE_DEFAULT_THREADING_H_
+
+
+#include <ode/threading.h>
+
+
+class DefaultThreadingHolder
+{
+public:
+    static bool initializeDefaultThreading();
+    static void finalizeDefaultThreading();
+
+    static dThreadingImplementationID getDefaultThreadingImpl() { return m_defaultThreadingImpl; }
+    static const dThreadingFunctionsInfo *getDefaultThreadingFunctions() { return m_defaultThreadingFunctions; }
+
+private:
+    static dThreadingImplementationID       m_defaultThreadingImpl;
+    static const dThreadingFunctionsInfo    *m_defaultThreadingFunctions;
+};
+
+
+#endif // #ifndef _ODE__PRIVATE_DEFAULT_THREADING_H_

thirdparty/ode-0.16.5/ode/src/error.cpp

@@ -0,0 +1,179 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include <ode/error.h>
+#include "config.h"
+
+
+static dMessageFunction *error_function = 0;
+static dMessageFunction *debug_function = 0;
+static dMessageFunction *message_function = 0;
+
+
+extern "C" void dSetErrorHandler (dMessageFunction *fn)
+{
+    error_function = fn;
+}
+
+
+extern "C" void dSetDebugHandler (dMessageFunction *fn)
+{
+    debug_function = fn;
+}
+
+
+extern "C" void dSetMessageHandler (dMessageFunction *fn)
+{
+    message_function = fn;
+}
+
+
+extern "C" dMessageFunction *dGetErrorHandler()
+{
+    return error_function;
+}
+
+
+extern "C" dMessageFunction *dGetDebugHandler()
+{
+    return debug_function;
+}
+
+
+extern "C" dMessageFunction *dGetMessageHandler()
+{
+    return message_function;
+}
+
+
+static void printMessage (int num, const char *msg1, const char *msg2,
+                          va_list ap)
+{
+    fflush (stderr);
+    fflush (stdout);
+    if (num) fprintf (stderr,"\n%s %d: ",msg1,num);
+    else fprintf (stderr,"\n%s: ",msg1);
+    vfprintf (stderr,msg2,ap);
+    fprintf (stderr,"\n");
+    fflush (stderr);
+}
+
+//****************************************************************************
+// unix
+
+#ifndef WIN32
+
+extern "C" void dError (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (error_function) error_function (num,msg,ap);
+    else printMessage (num,"ODE Error",msg,ap);
+    va_end (ap);
+    exit (1);
+}
+
+
+extern "C" void dDebug (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (debug_function) debug_function (num,msg,ap);
+    else printMessage (num,"ODE INTERNAL ERROR",msg,ap);
+    va_end (ap);
+    // *((char *)0) = 0;   ... commit SEGVicide
+    abort();
+}
+
+
+extern "C" void dMessage (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (message_function) message_function (num,msg,ap);
+    else printMessage (num,"ODE Message",msg,ap);
+    va_end (ap);
+}
+
+#endif
+
+//****************************************************************************
+// windows
+
+#ifdef WIN32
+
+// isn't cygwin annoying!
+#ifdef CYGWIN
+#define _snprintf snprintf
+#define _vsnprintf vsnprintf
+#endif
+
+
+#include "windows.h"
+
+
+extern "C" void dError (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (error_function) error_function (num,msg,ap);
+    else {
+        char s[1000],title[100];
+        _snprintf (title,sizeof(title),"ODE Error %d",num);
+        _vsnprintf (s,sizeof(s),msg,ap);
+        s[sizeof(s)-1] = 0;
+        MessageBox(0,s,title,MB_OK | MB_ICONWARNING);
+    }
+    va_end (ap);
+    exit (1);
+}
+
+
+extern "C" void dDebug (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (debug_function) debug_function (num,msg,ap);
+    else {
+        char s[1000],title[100];
+        _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num);
+        _vsnprintf (s,sizeof(s),msg,ap);
+        s[sizeof(s)-1] = 0;
+        MessageBox(0,s,title,MB_OK | MB_ICONSTOP);
+    }
+    va_end (ap);
+    abort();
+}
+
+
+extern "C" void dMessage (int num, const char *msg, ...)
+{
+    va_list ap;
+    va_start (ap,msg);
+    if (message_function) message_function (num,msg,ap);
+    else printMessage (num,"ODE Message",msg,ap);
+    va_end (ap);
+}
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/error.h

@@ -0,0 +1,101 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* Library private error handling functions and macros */
+
+#ifndef _ODE__PRIVATE_ERROR_H_
+#define _ODE__PRIVATE_ERROR_H_
+
+#include <ode/error.h>
+#include <ode/common.h>
+
+
+
+/* debugging:
+ *   IASSERT  is an internal assertion, i.e. a consistency check. if it fails
+ *            we want to know where.
+ *   UASSERT  is a user assertion, i.e. if it fails a nice error message
+ *            should be printed for the user.
+ *   AASSERT  is an arguments assertion, i.e. if it fails "bad argument(s)"
+ *            is printed.
+ *   DEBUGMSG just prints out a message
+ */
+
+#  if defined(__STDC__) && __STDC_VERSION__ >= 199901L
+#    define __FUNCTION__ __func__
+#  endif
+#ifndef dNODEBUG
+#  ifdef __GNUC__
+#    define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
+      "assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); } }
+#    define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \
+      msg " in %s()", __FUNCTION__); } }
+#    define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT,				\
+  msg " in %s() [%s:%u]", __FUNCTION__,__FILE__,__LINE__); }
+#  else // not __GNUC__
+#    define dIASSERT(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
+      "assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); } }
+#    define dUASSERT(a,msg) { if (!(a)) { dDebug (d_ERR_UASSERT, \
+      msg " (%s:%u)", __FILE__,__LINE__); } }
+#    define dDEBUGMSG(msg) { dMessage (d_ERR_UASSERT, \
+      msg " (%s:%u)", __FILE__,__LINE__); }
+#  endif
+#  define dIVERIFY(a) dIASSERT(a)
+#  define dUVERIFY(a, msg) dUASSERT(a, msg)
+#else
+#  define dIASSERT(a) ((void)0)
+#  define dUASSERT(a,msg) ((void)0)
+#  define dDEBUGMSG(msg) ((void)0)
+#  define dIVERIFY(a) ((void)(a))
+#  define dUVERIFY(a, msg) ((void)(a))
+#endif
+
+#ifdef __GNUC__
+#define dUNUSED(Name) Name __attribute__((unused))
+#else // not __GNUC__
+#define dUNUSED(Name) Name
+#endif
+
+#if __cplusplus >= 201103L 
+#define dSASSERT(e)  static_assert(e, #e)
+#define dSMSGASSERT(e, message)  static_assert(e, message)
+#else
+#define d_SASSERT_INNER_TOKENPASTE(x, y) x ## y
+#define d_SASSERT_TOKENPASTE(x, y) d_SASSERT_INNER_TOKENPASTE(x, y)
+#define dSASSERT(e) typedef char dUNUSED(d_SASSERT_TOKENPASTE(d_StaticAssertionFailed_, __LINE__)[(e)?1:-1])
+#define dSMSGASSERT(e, message)  dSASSERT(e)
+#endif
+
+#  ifdef __GNUC__
+#    define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
+      "assertion \"" #a "\" failed in %s() [%s:%u]",__FUNCTION__,__FILE__,__LINE__); *(int *)0 = 0; } }
+#  else // not __GNUC__
+#    define dICHECK(a) { if (!(a)) { dDebug (d_ERR_IASSERT, \
+      "assertion \"" #a "\" failed in %s:%u",__FILE__,__LINE__); *(int *)0 = 0; } }
+#  endif
+
+// Argument assert is a special case of user assert
+#define dAASSERT(a) dUASSERT(a, "Bad argument(s)")
+#define dAVERIFY(a) dUVERIFY(a, "Bad argument(s)")
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/export-dif.cpp

@@ -0,0 +1,620 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * Export a DIF (Dynamics Interchange Format) file.
+ */
+
+
+// @@@ TODO:
+//	* export all spaces, and geoms in spaces, not just ones attached to bodies
+//	  (separate export function?)
+//	* say the space each geom is in, so reader can construct space hierarchy
+//	* limot --> separate out into limits and motors?
+//	* make sure ODE-specific parameters divided out
+
+
+#include <ode/ode.h>
+#include "config.h"
+#include "objects.h"
+#include "joints/joints.h"
+#include "collision_kernel.h"
+
+//***************************************************************************
+// utility
+
+struct PrintingContext {
+    FILE *file;		// file to write to
+    int precision;		// digits of precision to print
+    int indent;		// number of levels of indent
+
+    void printIndent();
+    void printReal (dReal x);
+    void print (const char *name, int x);
+    void print (const char *name, unsigned x);
+    void print (const char *name, dReal x);
+    void print (const char *name, const dReal *x, int n=3);
+    void print (const char *name, const char *x=0);
+    void printNonzero (const char *name, dReal x);
+    void printNonzero (const char *name, const dReal x[3]);
+};
+
+
+void PrintingContext::printIndent()
+{
+    for (int i=0; i<indent; i++) fputc ('\t',file);
+}
+
+
+void PrintingContext::print (const char *name, int x)
+{
+    printIndent();
+    fprintf (file,"%s = %d,\n",name,x);
+}
+
+void PrintingContext::print (const char *name, unsigned x)
+{
+    printIndent();
+    fprintf (file,"%s = %u,\n",name,x);
+}
+
+void PrintingContext::printReal (dReal x)
+{
+    if (x==dInfinity) {
+        fprintf (file,"inf");
+    }
+    else if (x==-dInfinity) {
+        fprintf (file,"-inf");
+    }
+    else {
+        fprintf (file,"%.*g",precision,x);
+    }
+}
+
+
+void PrintingContext::print (const char *name, dReal x)
+{
+    printIndent();
+    fprintf (file,"%s = ",name);
+    printReal (x);
+    fprintf (file,",\n");
+}
+
+
+void PrintingContext::print (const char *name, const dReal *x, int n)
+{
+    printIndent();
+    fprintf (file,"%s = {",name);
+    for (int i=0; i<n; i++) {
+        printReal (x[i]);
+        if (i < n-1) fputc (',',file);
+    }
+    fprintf (file,"},\n");
+}
+
+
+void PrintingContext::print (const char *name, const char *x)
+{
+    printIndent();
+    if (x) {
+        fprintf (file,"%s = \"%s\",\n",name,x);
+    }
+    else {
+        fprintf (file,"%s\n",name);
+    }
+}
+
+
+void PrintingContext::printNonzero (const char *name, dReal x)
+{
+    if (x != 0) print (name,x);
+}
+
+
+void PrintingContext::printNonzero (const char *name, const dReal x[3])
+{
+    if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
+}
+
+//***************************************************************************
+// joints
+
+
+static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
+{
+    if (num >= 0) {
+        c.printIndent();
+        fprintf (c.file,"limit%d = {\n",num);
+    }
+    else {
+        c.print ("limit = {");
+    }
+    c.indent++;
+    c.print ("low_stop",limot.lostop);
+    c.print ("high_stop",limot.histop);
+    c.printNonzero ("bounce",limot.bounce);
+    c.print ("ODE = {");
+    c.indent++;
+    c.printNonzero ("stop_erp",limot.stop_erp);
+    c.printNonzero ("stop_cfm",limot.stop_cfm);
+    c.indent--;
+    c.print ("},");
+    c.indent--;
+    c.print ("},");
+
+    if (num >= 0) {
+        c.printIndent();
+        fprintf (c.file,"motor%d = {\n",num);
+    }
+    else {
+        c.print ("motor = {");
+    }
+    c.indent++;
+    c.printNonzero ("vel",limot.vel);
+    c.printNonzero ("fmax",limot.fmax);
+    c.print ("ODE = {");
+    c.indent++;
+    c.printNonzero ("fudge_factor",limot.fudge_factor);
+    c.printNonzero ("normal_cfm",limot.normal_cfm);
+    c.indent--;
+    c.print ("},");
+    c.indent--;
+    c.print ("},");
+}
+
+
+static const char *getJointName (dxJoint *j)
+{
+    switch (j->type()) {
+        case dJointTypeBall: return "ball";
+        case dJointTypeHinge: return "hinge";
+        case dJointTypeSlider: return "slider";
+        case dJointTypeContact: return "contact";
+        case dJointTypeUniversal: return "universal";
+        case dJointTypeHinge2: return "ODE_hinge2";
+        case dJointTypeFixed: return "fixed";
+        case dJointTypeNull: return "null";
+        case dJointTypeAMotor: return "ODE_angular_motor";
+        case dJointTypeLMotor: return "ODE_linear_motor";
+        case dJointTypePR: return "PR";
+        case dJointTypePU: return "PU";
+    case dJointTypePiston: return "piston";
+        default: return "unknown";
+    }
+}
+
+
+static void printBall (PrintingContext &c, dxJoint *j)
+{
+    dxJointBall *b = (dxJointBall*) j;
+    c.print ("anchor1",b->anchor1);
+    c.print ("anchor2",b->anchor2);
+}
+
+
+static void printHinge (PrintingContext &c, dxJoint *j)
+{
+    dxJointHinge *h = (dxJointHinge*) j;
+    c.print ("anchor1",h->anchor1);
+    c.print ("anchor2",h->anchor2);
+    c.print ("axis1",h->axis1);
+    c.print ("axis2",h->axis2);
+    c.print ("qrel",h->qrel,4);
+    printLimot (c,h->limot,-1);
+}
+
+
+static void printSlider (PrintingContext &c, dxJoint *j)
+{
+    dxJointSlider *s = (dxJointSlider*) j;
+    c.print ("axis1",s->axis1);
+    c.print ("qrel",s->qrel,4);
+    c.print ("offset",s->offset);
+    printLimot (c,s->limot,-1);
+}
+
+
+static void printContact (PrintingContext &c, dxJoint *j)
+{
+    dxJointContact *ct = (dxJointContact*) j;
+    int mode = ct->contact.surface.mode;
+    c.print ("pos",ct->contact.geom.pos);
+    c.print ("normal",ct->contact.geom.normal);
+    c.print ("depth",ct->contact.geom.depth);
+    //@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
+    //    to do this we must have written out all geoms in all spaces, not just
+    //    geoms that are attached to bodies.
+    c.print ("mu",ct->contact.surface.mu);
+    if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
+    if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
+    if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
+    if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
+    if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
+    if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
+    if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
+    if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
+    if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
+    int fa = 0;		// friction approximation code
+    if (mode & dContactApprox1_1) fa |= 1;
+    if (mode & dContactApprox1_2) fa |= 2;
+    if (fa) c.print ("friction_approximation",fa);
+    if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
+}
+
+
+static void printUniversal (PrintingContext &c, dxJoint *j)
+{
+    dxJointUniversal *u = (dxJointUniversal*) j;
+    c.print ("anchor1",u->anchor1);
+    c.print ("anchor2",u->anchor2);
+    c.print ("axis1",u->axis1);
+    c.print ("axis2",u->axis2);
+    c.print ("qrel1",u->qrel1,4);
+    c.print ("qrel2",u->qrel2,4);
+    printLimot (c,u->limot1,1);
+    printLimot (c,u->limot2,2);
+}
+
+
+static void printHinge2 (PrintingContext &c, dxJoint *j)
+{
+    dxJointHinge2 *h = (dxJointHinge2*) j;
+    c.print ("anchor1",h->anchor1);
+    c.print ("anchor2",h->anchor2);
+    c.print ("axis1",h->axis1);
+    c.print ("axis2",h->axis2);
+    c.print ("v1",h->v1);	//@@@ much better to write out 'qrel' here, if it's available
+    c.print ("v2",h->v2);
+    c.print ("susp_erp",h->susp_erp);
+    c.print ("susp_cfm",h->susp_cfm);
+    printLimot (c,h->limot1,1);
+    printLimot (c,h->limot2,2);
+}
+
+static void printPR (PrintingContext &c, dxJoint *j)
+{
+    dxJointPR *pr = (dxJointPR*) j;
+    c.print ("anchor2",pr->anchor2);
+    c.print ("axisR1",pr->axisR1);
+    c.print ("axisR2",pr->axisR2);
+    c.print ("axisP1",pr->axisP1);
+    c.print ("qrel",pr->qrel,4);
+    c.print ("offset",pr->offset);
+    printLimot (c,pr->limotP,1);
+    printLimot (c,pr->limotR,2);
+}
+
+static void printPU (PrintingContext &c, dxJoint *j)
+{
+    dxJointPU *pu = (dxJointPU*) j;
+    c.print ("anchor1",pu->anchor1);
+    c.print ("anchor2",pu->anchor2);
+    c.print ("axis1",pu->axis1);
+    c.print ("axis2",pu->axis2);
+    c.print ("axisP",pu->axisP1);
+    c.print ("qrel1",pu->qrel1,4);
+    c.print ("qrel2",pu->qrel2,4);
+    printLimot (c,pu->limot1,1);
+    printLimot (c,pu->limot2,2);
+    printLimot (c,pu->limotP,3);
+}
+
+static void printPiston (PrintingContext &c, dxJoint *j)
+{
+    dxJointPiston *rap = (dxJointPiston*) j;
+    c.print ("anchor1",rap->anchor1);
+    c.print ("anchor2",rap->anchor2);
+    c.print ("axis1",rap->axis1);
+    c.print ("axis2",rap->axis2);
+    c.print ("qrel",rap->qrel,4);
+    printLimot (c,rap->limotP,1);
+    printLimot (c, rap->limotR, 2);
+}
+
+static void printFixed (PrintingContext &c, dxJoint *j)
+{
+    dxJointFixed *f = (dxJointFixed*) j;
+    c.print ("qrel",f->qrel);
+    c.print ("offset",f->offset);
+}
+
+static void printLMotor (PrintingContext &c, dxJoint *j)
+{
+    dxJointLMotor *a = (dxJointLMotor*) j;
+    c.print("num", a->num);
+    c.printIndent();
+    fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
+    c.print ("axis1",a->axis[0]);
+    c.print ("axis2",a->axis[1]);
+    c.print ("axis3",a->axis[2]);
+    for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
+}
+
+struct dxAMotorJointPrinter
+{
+    static void print(PrintingContext &c, dxJointAMotor *a)
+    {
+        c.print ("num",a->m_num);
+        c.print ("mode",a->m_mode);
+        c.printIndent();
+        fprintf (c.file,"rel = {%d,%d,%d},\n",a->m_rel[0],a->m_rel[1],a->m_rel[2]);
+        c.print ("axis1",a->m_axis[0]);
+        c.print ("axis2",a->m_axis[1]);
+        c.print ("axis3",a->m_axis[2]);
+        for (int i=0; i<3; i++) printLimot (c,a->m_limot[i],i+1);
+        c.print ("angle1",a->m_angle[0]);
+        c.print ("angle2",a->m_angle[1]);
+        c.print ("angle3",a->m_angle[2]);
+    }
+};
+
+static void printAMotor (PrintingContext &c, dxJoint *j)
+{
+    dxJointAMotor *a = (dxJointAMotor*) j;
+    dxAMotorJointPrinter::print(c, a);
+}
+
+//***************************************************************************
+// geometry
+
+static void printGeom (PrintingContext &c, dxGeom *g);
+
+static void printSphere (PrintingContext &c, dxGeom *g)
+{
+    c.print ("type","sphere");
+    c.print ("radius",dGeomSphereGetRadius (g));
+}
+
+
+static void printBox (PrintingContext &c, dxGeom *g)
+{
+    dVector3 sides;
+    dGeomBoxGetLengths (g,sides);
+    c.print ("type","box");
+    c.print ("sides",sides);
+}
+
+
+static void printCapsule (PrintingContext &c, dxGeom *g)
+{
+    dReal radius,length;
+    dGeomCapsuleGetParams (g,&radius,&length);
+    c.print ("type","capsule");
+    c.print ("radius",radius);
+    c.print ("length",length);
+}
+
+
+static void printCylinder (PrintingContext &c, dxGeom *g)
+{
+    dReal radius,length;
+    dGeomCylinderGetParams (g,&radius,&length);
+    c.print ("type","cylinder");
+    c.print ("radius",radius);
+    c.print ("length",length);
+}
+
+
+static void printPlane (PrintingContext &c, dxGeom *g)
+{
+    dVector4 e;
+    dGeomPlaneGetParams (g,e);
+    c.print ("type","plane");
+    c.print ("normal",e);
+    c.print ("d",e[3]);
+}
+
+
+static void printRay (PrintingContext &c, dxGeom *g)
+{
+    dReal length = dGeomRayGetLength (g);
+    c.print ("type","ray");
+    c.print ("length",length);
+}
+
+
+static void printConvex (PrintingContext &c, dxGeom * /*g*/)
+{
+    c.print ("type","convex");
+    ///@todo Print information about convex hull
+}
+
+
+
+static void printTriMesh (PrintingContext &c, dxGeom * /*g*/)
+{
+    c.print ("type","trimesh");
+    //@@@ i don't think that the trimesh accessor functions are really
+    //    sufficient to read out all the triangle data, and anyway we
+    //    should have a method of not duplicating trimesh data that is
+    //    shared.
+}
+
+
+static void printHeightfieldClass (PrintingContext &c, dxGeom * /*g*/)
+{
+    c.print ("type","heightfield");
+    ///@todo Print information about heightfield
+}
+
+
+static void printGeom (PrintingContext &c, dxGeom *g)
+{
+    unsigned long category = dGeomGetCategoryBits (g);
+    if (category != (unsigned long)(~0)) {
+        c.printIndent();
+        fprintf (c.file,"category_bits = %lu\n",category);
+    }
+    unsigned long collide = dGeomGetCollideBits (g);
+    if (collide != (unsigned long)(~0)) {
+        c.printIndent();
+        fprintf (c.file,"collide_bits = %lu\n",collide);
+    }
+    if (!dGeomIsEnabled (g)) {
+        c.print ("disabled",1);
+    }
+    switch (g->type) {
+        case dSphereClass: printSphere (c,g); break;
+        case dBoxClass: printBox (c,g); break;
+        case dCapsuleClass: printCapsule (c,g); break;
+        case dCylinderClass: printCylinder (c,g); break;
+        case dPlaneClass: printPlane (c,g); break;
+        case dRayClass: printRay (c,g); break;
+        case dConvexClass: printConvex (c,g); break;
+        case dTriMeshClass: printTriMesh (c,g); break;
+        case dHeightfieldClass: printHeightfieldClass (c,g); break;
+    }
+}
+
+//***************************************************************************
+// world
+
+void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
+{
+    PrintingContext c;
+    c.file = file;
+#if defined(dSINGLE)
+    c.precision = 7;
+#else
+    c.precision = 15;
+#endif
+    c.indent = 1;
+
+    fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
+    c.print ("gravity",w->gravity);
+    c.print ("ODE = {");
+    c.indent++;
+    c.print ("ERP",w->global_erp);
+    c.print ("CFM",w->global_cfm);
+    c.print ("auto_disable = {");
+    c.indent++;
+    c.print ("linear_threshold",w->adis.linear_average_threshold);
+    c.print ("angular_threshold",w->adis.angular_average_threshold);
+    c.print ("average_samples",(int)w->adis.average_samples);
+    c.print ("idle_time",w->adis.idle_time);
+    c.print ("idle_steps",w->adis.idle_steps);
+    fprintf (file,"\t\t},\n\t},\n}\n");
+    c.indent -= 3;
+
+    // bodies
+    int num = 0;
+    fprintf (file,"%sbody = {}\n",prefix);
+    for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
+        b->tag = num;
+        fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
+        c.indent++;
+        c.print ("pos",b->posr.pos);
+        c.print ("q",b->q,4);
+        c.print ("lvel",b->lvel);
+        c.print ("avel",b->avel);
+        c.print ("mass",b->mass.mass);
+        fprintf (file,"\tI = {{");
+        for (int i=0; i<3; i++) {
+            for (int j=0; j<3; j++) {
+                c.printReal (b->mass.I[i*4+j]);
+                if (j < 2) fputc (',',file);
+            }
+            if (i < 2) fprintf (file,"},{");
+        }
+        fprintf (file,"}},\n");
+        c.printNonzero ("com",b->mass.c);
+        c.print ("ODE = {");
+        c.indent++;
+        if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
+        if (b->flags & dxBodyDisabled) c.print ("disabled",1);
+        if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
+        if (b->flags & dxBodyAutoDisable) {
+            c.print ("auto_disable = {");
+            c.indent++;
+            c.print ("linear_threshold",b->adis.linear_average_threshold);
+            c.print ("angular_threshold",b->adis.angular_average_threshold);
+            c.print ("average_samples",(int)b->adis.average_samples);
+            c.print ("idle_time",b->adis.idle_time);
+            c.print ("idle_steps",b->adis.idle_steps);
+            c.print ("time_left",b->adis_timeleft);
+            c.print ("steps_left",b->adis_stepsleft);
+            c.indent--;
+            c.print ("},");
+        }
+        c.printNonzero ("facc",b->facc);
+        c.printNonzero ("tacc",b->tacc);
+        if (b->flags & dxBodyFlagFiniteRotationAxis) {
+            c.print ("finite_rotation_axis",b->finite_rot_axis);
+        }
+        c.indent--;
+        c.print ("},");
+        if (b->geom) {
+            c.print ("geometry = {");
+            c.indent++;
+            for (dxGeom *g=b->geom; g; g=g->body_next) {
+                c.print ("{");
+                c.indent++;
+                printGeom (c,g);
+                c.indent--;
+                c.print ("},");
+            }
+            c.indent--;
+            c.print ("},");
+        }
+        c.indent--;
+        c.print ("}");
+        num++;
+    }
+
+    // joints
+    num = 0;
+    fprintf (file,"%sjoint = {}\n",prefix);
+    for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
+        c.indent++;
+        const char *name = getJointName (j);
+        fprintf (file,
+            "%sjoint[%d] = dynamics.%s_joint {\n"
+            "\tworld = %sworld,\n"
+            "\tbody = {"
+            ,prefix,num,name,prefix);
+
+        if ( j->node[0].body )
+            fprintf (file,"%sbody[%d]",prefix,j->node[0].body->tag);
+        if ( j->node[1].body )
+            fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
+        fprintf (file,"}\n");
+
+        switch (j->type()) {
+            case dJointTypeBall: printBall (c,j); break;
+            case dJointTypeHinge: printHinge (c,j); break;
+            case dJointTypeSlider: printSlider (c,j); break;
+            case dJointTypeContact: printContact (c,j); break;
+            case dJointTypeUniversal: printUniversal (c,j); break;
+            case dJointTypeHinge2: printHinge2 (c,j); break;
+            case dJointTypeFixed: printFixed (c,j); break;
+            case dJointTypeAMotor: printAMotor (c,j); break;
+            case dJointTypeLMotor: printLMotor (c,j); break;
+            case dJointTypePR: printPR (c,j); break;
+            case dJointTypePU: printPU (c,j); break;
+            case dJointTypePiston: printPiston (c,j); break;
+            default: c.print("unknown joint");
+        }
+        c.indent--;
+        c.print ("}");
+        num++;
+    }
+}

thirdparty/ode-0.16.5/ode/src/fastdot.cpp

@@ -0,0 +1,46 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* generated code, do not edit. */
+
+#include <ode/common.h>
+#include "config.h"
+#include "matrix.h"
+
+#include "fastdot_impl.h"
+
+
+/*extern */
+dReal dxDot (const dReal *a, const dReal *b, unsigned n)
+{
+    return calculateLargeVectorDot<1>(a, b, n);
+}
+
+
+#undef dDot
+
+/*extern */
+dReal dDot (const dReal *a, const dReal *b, int n)
+{
+    return dxDot (a, b, n);
+}
+

thirdparty/ode-0.16.5/ode/src/fastdot_impl.h

@@ -0,0 +1,51 @@
+
+
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_FASTDOT_IMPL_H_
+#define _ODE_FASTDOT_IMPL_H_
+
+
+template<unsigned b_stride>
+dReal calculateLargeVectorDot (const dReal *a, const dReal *b, unsigned n)
+{
+    dReal sum = 0;
+    const dReal *a_end = a + (n & (int)(~3));
+    for (; a != a_end; b += 4 * b_stride, a += 4) {
+        dReal p0 = a[0], p1 = a[1], p2 = a[2], p3 = a[3];
+        dReal q0 = b[0 * b_stride], q1 = b[1 * b_stride], q2 = b[2 * b_stride], q3 = b[3 * b_stride];
+        dReal m0 = p0 * q0;
+        dReal m1 = p1 * q1;
+        dReal m2 = p2 * q2;
+        dReal m3 = p3 * q3;
+        sum += m0 + m1 + m2 + m3;
+    }
+    a_end += (n & 3);
+    for (; a != a_end; b += b_stride, ++a) {
+        sum += (*a) * (*b);
+    }
+    return sum;
+}
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/fastldltfactor.cpp

@@ -0,0 +1,462 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* 
+ * LDLT factorization related code of ThreadedEquationSolverLDLT 
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/matrix_coop.h>
+#include "config.h"
+#include "threaded_solver_ldlt.h"
+#include "threading_base.h"
+#include "resource_control.h"
+#include "error.h"
+
+#include "fastldltfactor_impl.h"
+
+
+/*static */
+void ThreadedEquationSolverLDLT::estimateCooperativeFactoringLDLTResourceRequirements(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading();
+    unsigned limitedThreadCount = restrictFactoringLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount > 1)
+    {
+        doEstimateCooperativeFactoringLDLTResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+    }
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::cooperativelyFactorLDLT(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(rowCount != 0);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    unsigned limitedThreadCount = restrictFactoringLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount <= 1)
+    {
+        factorMatrixAsLDLT<FLDLT_D_STRIDE>(A, d, rowCount, rowSkip);
+    }
+    else
+    {
+        doCooperativelyFactorLDLTValidated(resourceContainer, limitedThreadCount, A, d, rowCount, rowSkip);
+    }
+}
+
+
+/*static */
+unsigned ThreadedEquationSolverLDLT::restrictFactoringLDLTAllowedThreadCount(
+    dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount)
+{
+    unsigned limitedThreadCount = 1;
+
+#if dCOOPERATIVE_ENABLED
+    const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned solvingMaximalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep);
+    dIASSERT(deriveSolvingL1StripeThreadCount(FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1);
+
+    if (solvingMaximalBlockCount >= FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+    {
+        limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, false);
+    }
+#endif // #if dCOOPERATIVE_ENABLED
+
+    return limitedThreadCount;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doEstimateCooperativeFactoringLDLTResourceRequirementsValidated(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned solvingTotalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep);
+    dIASSERT(solvingTotalBlockCount >= 1);
+
+    unsigned solvingLastBlockIndex = solvingTotalBlockCount - 1;
+
+    const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS;
+    unsigned factorizingMaximalBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingLastBlockIndex, solvingBlockStep, factorizingBlockARows);
+
+    unsigned blockSolvingMaximumThreads = deriveSolvingL1StripeThreadCount(solvingLastBlockIndex, allowedThreadCount);
+    unsigned blockFactorizingMaximumThreads = deriveScalingAndFactorizingL1StripeThreadCount(factorizingMaximalBlockCount, allowedThreadCount);
+    unsigned simultaneousCallCount = 1 // Final synchronization point
+        + 2 // intermediate synchronization points
+        + dMACRO_MAX(blockSolvingMaximumThreads, blockFactorizingMaximumThreads);
+
+    FactorizationSolvingL1StripeMemoryEstimates solvingMemoryEstimates;
+    FactorizationScalingAndFactorizingL1StripeMemoryEstimates scalingAndFactorizingEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1Stripe_XMemoryRequirement(solvingTotalBlockCount, solvingMemoryEstimates);
+    sizeint factorizingMemoryRequired = estimateCooperativelyScalingAndFactorizingL1Stripe_XMemoryRequirement(blockFactorizingMaximumThreads, scalingAndFactorizingEstimates);
+    sizeint totalSizeRequired = solvingMemoryRequired + factorizingMemoryRequired;
+    const unsigned memoryAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT;
+
+    unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED;
+    summaryRequirementsDescriptor->mergeAnotherDescriptorIn(totalSizeRequired, memoryAlignmentRequired, simultaneousCallCount, featureRequirement);
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doCooperativelyFactorLDLTValidated(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip)
+{
+    dIASSERT(allowedThreadCount > 1);
+
+    const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned solvingTotalBlockCount = deriveSolvingL1StripeBlockCount(rowCount, solvingBlockStep);
+    dIASSERT(solvingTotalBlockCount >= 1);
+
+    unsigned solvingLastBlockIndex = solvingTotalBlockCount - 1;
+
+    const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS;
+    unsigned factorizingMaximalBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingLastBlockIndex, solvingBlockStep, factorizingBlockARows);
+
+    unsigned blockFactorizingMaximumThreads = deriveScalingAndFactorizingL1StripeThreadCount(factorizingMaximalBlockCount, allowedThreadCount);
+
+    dCallWaitID completionWait = resourceContainer->getStockCallWait();
+    dAASSERT(completionWait != NULL);
+
+    FactorizationSolvingL1StripeMemoryEstimates solvingMemoryEstimates;
+    FactorizationScalingAndFactorizingL1StripeMemoryEstimates scalingAndFactorizingEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1Stripe_XMemoryRequirement(solvingTotalBlockCount, solvingMemoryEstimates);
+    sizeint factorizingMemoryRequired = estimateCooperativelyScalingAndFactorizingL1Stripe_XMemoryRequirement(blockFactorizingMaximumThreads, scalingAndFactorizingEstimates);
+    sizeint totalSizeRequired = solvingMemoryRequired + factorizingMemoryRequired;
+    dIASSERT(totalSizeRequired <= resourceContainer->getMemoryBufferSize());
+
+    void *bufferAllocated = resourceContainer->getMemoryBufferPointer();
+    dIASSERT(bufferAllocated != NULL);
+    dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated);
+
+    atomicord32 solvingBlockCompletionProgress;
+    cellindexint *solvingBlockProgressDescriptors;
+    FactorizationSolveL1StripeCellContext *solvingCellContexts;
+
+    FactorizationFactorizeL1StripeContext *factorizingFactorizationContext;
+
+    void *bufferCurrentLocation = bufferAllocated;
+    bufferCurrentLocation = markCooperativelySolvingL1Stripe_XMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, solvingBlockProgressDescriptors, solvingCellContexts);
+    bufferCurrentLocation = markCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructuresOut(bufferCurrentLocation, scalingAndFactorizingEstimates, factorizingFactorizationContext);
+    dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + totalSizeRequired);
+
+    dCallReleaseeID calculationFinishReleasee;
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    threading->PostThreadedCall(NULL, &calculationFinishReleasee, 1, NULL, completionWait, &factotLDLT_completion_callback, NULL, 0, "FactorLDLT Completion");
+
+    FactorLDLTWorkerContext workerContext(threading, allowedThreadCount, A, d, solvingTotalBlockCount, rowCount, rowSkip, 
+        solvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts, 
+        factorizingFactorizationContext,
+        calculationFinishReleasee); // The variable must exist in the outer scope
+
+    dIASSERT(solvingTotalBlockCount >= FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM);
+    dSASSERT(FLDLT_COOPERATIVE_BLOCK_COUNT_MINIMUM > 2);
+
+    scaleAndFactorizeL1FirstRowStripe_2<FLDLT_D_STRIDE>(workerContext.m_ARow, workerContext.m_d, workerContext.m_rowSkip);
+    workerContext.incrementForNextBlock();
+
+    const unsigned blockIndex = 1;
+    dIASSERT(blockIndex == workerContext.m_solvingBlockIndex);
+
+    initializeCooperativelySolvingL1Stripe_XMemoryStructures(blockIndex, solvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts);
+    unsigned secondBlockSolvingThreadCount = deriveSolvingL1StripeThreadCount(blockIndex, allowedThreadCount);
+
+    dCallReleaseeID secondBlockSolvingSyncReleasee;
+    threading->PostThreadedCall(NULL, &secondBlockSolvingSyncReleasee, secondBlockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingCompleteSync_callback, &workerContext, 0, "FactorLDLT Solving Complete Sync");
+    
+    if (secondBlockSolvingThreadCount > 1)
+    {
+        threading->PostThreadedCallsGroup(NULL, secondBlockSolvingThreadCount - 1, secondBlockSolvingSyncReleasee, &factotLDLT_solvingComplete_callback, &workerContext, "FactorLDLT Solving Complete");
+    }
+
+    factotLDLT_solvingComplete(workerContext, secondBlockSolvingThreadCount - 1);
+    threading->AlterThreadedCallDependenciesCount(secondBlockSolvingSyncReleasee, -1);
+
+    threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "FactorLDLT End Wait");
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_solvingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_solvingComplete(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+    
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_solvingComplete(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex)
+{
+    participateSolvingL1Stripe_X<FSL1S_BLOCK_SIZE, FSL1S_REGULAR_B_ROWS>(ref_context.m_A, ref_context.m_ARow, ref_context.m_solvingBlockIndex, ref_context.m_rowSkip, 
+        ref_context.m_refSolvingBlockCompletionProgress, ref_context.m_solvingBlockProgressDescriptors, ref_context.m_solvingCellContexts, ownThreadIndex);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_solvingCompleteSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_solvingCompleteSync(*ptrContext);
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_solvingCompleteSync(FactorLDLTWorkerContext &ref_workerContext)
+{
+    unsigned solvingBlockIndex = ref_workerContext.m_solvingBlockIndex;
+    FactorizationFactorizeL1StripeContext *factorizingFactorizationContext = ref_workerContext.m_factorizingFactorizationContext;
+
+    const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE;
+    const unsigned factorizingBlockARows = FFL1S_REGULAR_A_ROWS;
+    unsigned factorizingBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingBlockIndex, solvingBlockStep, factorizingBlockARows);
+    unsigned blockFactorizingThreadCount = deriveScalingAndFactorizingL1StripeThreadCount(factorizingBlockCount, ref_workerContext.m_allowedThreadCount);
+    initializeCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructures(factorizingFactorizationContext, blockFactorizingThreadCount);
+
+    dCallReleaseeID blockFactorizingSyncReleasee;
+
+    dxThreadingBase *threading = ref_workerContext.m_threading;
+    if (solvingBlockIndex != ref_workerContext.m_totalBlockCount - 1)
+    {
+        threading->PostThreadedCall(NULL, &blockFactorizingSyncReleasee, blockFactorizingThreadCount, NULL, NULL, &factotLDLT_scalingAndFactorizingCompleteSync_callback, &ref_workerContext, 0, "FactorLDLT S'n'F Sync");
+    }
+    else
+    {
+        blockFactorizingSyncReleasee = ref_workerContext.m_calculationFinishReleasee;
+
+        if (blockFactorizingThreadCount > 1)
+        {
+            threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, blockFactorizingThreadCount - 1);
+        }
+    }
+
+    if (blockFactorizingThreadCount > 1)
+    {
+        threading->PostThreadedCallsGroup(NULL, blockFactorizingThreadCount - 1, blockFactorizingSyncReleasee, &factotLDLT_scalingAndFactorizingComplete_callback, &ref_workerContext, "FactorLDLT S'n'F Complete");
+    }
+
+    factotLDLT_scalingAndFactorizingComplete(ref_workerContext, blockFactorizingThreadCount - 1);
+    threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, -1);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingComplete_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_scalingAndFactorizingComplete(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingComplete(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex)
+{
+    unsigned factorizationRow = ref_workerContext.m_solvingBlockIndex * FSL1S_BLOCK_SIZE;
+    participateScalingAndFactorizingL1Stripe_X<FFL1S_REGULAR_A_ROWS, FLDLT_D_STRIDE>(ref_workerContext.m_ARow, ref_workerContext.m_d, factorizationRow, 
+        ref_workerContext.m_rowSkip, ref_workerContext.m_factorizingFactorizationContext, ownThreadIndex);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingCompleteSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_scalingAndFactorizingCompleteSync(*ptrContext);
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingCompleteSync(FactorLDLTWorkerContext &ref_workerContext)
+{
+    ref_workerContext.incrementForNextBlock();
+
+    unsigned blockIndex = ref_workerContext.m_solvingBlockIndex;
+    dIASSERT(blockIndex < ref_workerContext.m_totalBlockCount);
+
+    atomicord32 &refSolvingBlockCompletionProgress = ref_workerContext.m_refSolvingBlockCompletionProgress;
+    cellindexint *solvingBlockProgressDescriptors = ref_workerContext.m_solvingBlockProgressDescriptors;
+    FactorizationSolveL1StripeCellContext *solvingCellContexts = ref_workerContext.m_solvingCellContexts;
+
+    initializeCooperativelySolvingL1Stripe_XMemoryStructures(blockIndex, refSolvingBlockCompletionProgress, solvingBlockProgressDescriptors, solvingCellContexts);
+    unsigned blockSolvingThreadCount = deriveSolvingL1StripeThreadCount(blockIndex, ref_workerContext.m_allowedThreadCount);
+
+    dCallReleaseeID blockSolvingSyncReleasee;
+
+    dxThreadingBase *threading = ref_workerContext.m_threading;
+    if (blockIndex != ref_workerContext.m_totalBlockCount - 1 || ref_workerContext.m_rowCount % FSL1S_REGULAR_B_ROWS == 0)
+    {
+        threading->PostThreadedCall(NULL, &blockSolvingSyncReleasee, blockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingCompleteSync_callback, &ref_workerContext, 0, "FactorLDLT Solving Complete Sync");
+
+        if (blockSolvingThreadCount > 1)
+        {
+            threading->PostThreadedCallsGroup(NULL, blockSolvingThreadCount - 1, blockSolvingSyncReleasee, &factotLDLT_solvingComplete_callback, &ref_workerContext, "FactorLDLT Solving Complete");
+        }
+
+        factotLDLT_solvingComplete(ref_workerContext, blockSolvingThreadCount - 1);
+    }
+    else
+    {
+        dSASSERT(FSL1S_REGULAR_B_ROWS == 2);
+        dSASSERT(FSL1S_FINAL_B_ROWS == 1);
+
+        threading->PostThreadedCall(NULL, &blockSolvingSyncReleasee, blockSolvingThreadCount, NULL, NULL, &factotLDLT_solvingFinalSync_callback, &ref_workerContext, 0, "FactorLDLT Solving Final Sync");
+
+        if (blockSolvingThreadCount > 1)
+        {
+            threading->PostThreadedCallsGroup(NULL, blockSolvingThreadCount - 1, blockSolvingSyncReleasee, &factotLDLT_solvingFinal_callback, &ref_workerContext, "FactorLDLT Solving Final");
+        }
+
+        factotLDLT_solvingFinal(ref_workerContext, blockSolvingThreadCount - 1);
+    }
+
+    threading->AlterThreadedCallDependenciesCount(blockSolvingSyncReleasee, -1);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_solvingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_solvingFinal(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_solvingFinal(FactorLDLTWorkerContext &ref_context, unsigned ownThreadIndex)
+{
+    participateSolvingL1Stripe_X<FSL1S_BLOCK_SIZE, FSL1S_FINAL_B_ROWS>(ref_context.m_A, ref_context.m_ARow, ref_context.m_solvingBlockIndex, ref_context.m_rowSkip, 
+        ref_context.m_refSolvingBlockCompletionProgress, ref_context.m_solvingBlockProgressDescriptors, ref_context.m_solvingCellContexts, ownThreadIndex);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_solvingFinalSync_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_solvingFinalSync(*ptrContext);
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_solvingFinalSync(FactorLDLTWorkerContext &ref_workerContext)
+{
+    unsigned solvingBlockIndex = ref_workerContext.m_solvingBlockIndex;
+    FactorizationFactorizeL1StripeContext *factorizingFactorizationContext = ref_workerContext.m_factorizingFactorizationContext;
+
+    const unsigned int solvingBlockStep = FSL1S_BLOCK_SIZE;
+    const unsigned factorizingBlockARows = FFL1S_FINAL_A_ROWS;
+    unsigned factorizingBlockCount = deriveScalingAndFactorizingL1StripeBlockCountFromSolvingBlockIndex(solvingBlockIndex, solvingBlockStep, factorizingBlockARows);
+    unsigned blockFactorizingThreadCount = deriveScalingAndFactorizingL1StripeThreadCount(factorizingBlockCount, ref_workerContext.m_allowedThreadCount);
+    initializeCooperativelyScalingAndFactorizingL1Stripe_XMemoryStructures(factorizingFactorizationContext, blockFactorizingThreadCount);
+
+    dCallReleaseeID blockFactorizingSyncReleasee = ref_workerContext.m_calculationFinishReleasee;
+    dIASSERT(solvingBlockIndex == ref_workerContext.m_totalBlockCount - 1);
+
+    dxThreadingBase *threading = ref_workerContext.m_threading;
+
+    if (blockFactorizingThreadCount > 1)
+    {
+        threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, blockFactorizingThreadCount - 1);
+        threading->PostThreadedCallsGroup(NULL, blockFactorizingThreadCount - 1, blockFactorizingSyncReleasee, &factotLDLT_scalingAndFactorizingFinal_callback, &ref_workerContext, "FactorLDLT S'n'F Final");
+    }
+
+    factotLDLT_scalingAndFactorizingFinal(ref_workerContext, blockFactorizingThreadCount - 1);
+    threading->AlterThreadedCallDependenciesCount(blockFactorizingSyncReleasee, -1);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingFinal_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    FactorLDLTWorkerContext *ptrContext = (FactorLDLTWorkerContext *)callContext;
+
+    factotLDLT_scalingAndFactorizingFinal(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::factotLDLT_scalingAndFactorizingFinal(FactorLDLTWorkerContext &ref_workerContext, unsigned ownThreadIndex)
+{
+    unsigned factorizationRow = ref_workerContext.m_solvingBlockIndex * FSL1S_BLOCK_SIZE;
+    participateScalingAndFactorizingL1Stripe_X<FFL1S_FINAL_A_ROWS, FLDLT_D_STRIDE>(ref_workerContext.m_ARow, ref_workerContext.m_d, factorizationRow, 
+        ref_workerContext.m_rowSkip, ref_workerContext.m_factorizingFactorizationContext, ownThreadIndex);
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::factotLDLT_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    // Do nothing
+    return 1;
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Public interface functions
+
+
+/*extern ODE_API */
+void dFactorLDLT(dReal *A, dReal *d, int n, int nskip1)
+{
+    factorMatrixAsLDLT<1>(A, d, n, nskip1);
+}
+
+
+/*extern ODE_API */
+void dEstimateCooperativelyFactorLDLTResourceRequirements(dResourceRequirementsID requirements,
+    unsigned maximalAllowedThreadCount, unsigned maximalRowCount)
+{
+    dAASSERT(requirements != NULL);
+
+    dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements;
+    ThreadedEquationSolverLDLT::estimateCooperativeFactoringLDLTResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount);
+}
+
+/*extern ODE_API */
+void dCooperativelyFactorLDLT(dResourceContainerID resources, unsigned allowedThreadCount, 
+    dReal *A, dReal *d, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(resources != NULL);
+
+    dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources;
+    ThreadedEquationSolverLDLT::cooperativelyFactorLDLT(resourceContainer, allowedThreadCount, A, d, rowCount, rowSkip);
+}

thirdparty/ode-0.16.5/ode/src/fastldltsolve.cpp

@@ -0,0 +1,222 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* 
+ * LDLT solving related code of ThreadedEquationSolverLDLT 
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/matrix_coop.h>
+#include "config.h"
+#include "threaded_solver_ldlt.h"
+#include "threading_base.h"
+#include "resource_control.h"
+
+#include "fastldltsolve_impl.h"
+
+
+/*static */
+void ThreadedEquationSolverLDLT::estimateCooperativeSolvingLDLTResourceRequirements(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    unsigned stageBlockCountSifficiencyMask;
+    dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading();
+    unsigned limitedThreadCount = restrictSolvingLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount, stageBlockCountSifficiencyMask);
+
+    if (limitedThreadCount > 1)
+    {
+        if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_STRAIGHT)) != 0)
+        {
+            doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+        }
+
+        if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SCALING_VECTOR)) != 0)
+        {
+            doEstimateCooperativeScalingVectorResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+        }
+
+        if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_TRANSPOSED)) == 0)
+        {
+            doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+        }
+    }
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::cooperativelySolveLDLT(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(rowCount != 0);
+
+    unsigned stageBlockCountSifficiencyMask;
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    unsigned limitedThreadCount = restrictSolvingLDLTAllowedThreadCount(threading, allowedThreadCount, rowCount, stageBlockCountSifficiencyMask);
+
+    if (limitedThreadCount <= 1)
+    {
+        solveEquationSystemWithLDLT<SLDLT_D_STRIDE, SLDLT_B_STRIDE>(L, d, b, rowCount, rowSkip);
+    }
+    else
+    {
+        doCooperativelySolveLDLTValidated(resourceContainer, limitedThreadCount, stageBlockCountSifficiencyMask, L, d, b, rowCount, rowSkip);
+    }
+}
+
+/*static */
+unsigned ThreadedEquationSolverLDLT::restrictSolvingLDLTAllowedThreadCount(
+    dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount, unsigned &out_stageBlockCountSifficiencyMask)
+{
+    unsigned limitedThreadCount = 1;
+    unsigned stageBlockCountSifficiencyMask = 0;
+
+#if dCOOPERATIVE_ENABLED
+    {
+        const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation
+        unsigned solvingStraightBlockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep);
+        dIASSERT(deriveSolvingL1StraightThreadCount(SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1);
+
+        if (solvingStraightBlockCount >= SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+        {
+            stageBlockCountSifficiencyMask |= 1U << SLDLTS_SOLVING_STRAIGHT;
+        }
+    }
+
+    {
+        const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation
+        unsigned scalingBlockCount = deriveScalingVectorBlockCount(rowCount, blockStep);
+        dIASSERT(deriveScalingVectorThreadCount(SV_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1);
+
+        if (scalingBlockCount >= SV_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+        {
+            stageBlockCountSifficiencyMask |= 1U << SLDLTS_SCALING_VECTOR;
+        }
+    }
+
+    {
+        const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation
+        unsigned solvingTransposedBlockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep);
+        dIASSERT(deriveSolvingL1TransposedThreadCount(SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1);
+
+        if (solvingTransposedBlockCount >= SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+        {
+            stageBlockCountSifficiencyMask |= 1U << SLDLTS_SOLVING_TRANSPOSED;
+        }
+    }
+
+    if (stageBlockCountSifficiencyMask != 0)
+    {
+        limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true);
+    }
+#endif // #if dCOOPERATIVE_ENABLED
+
+    out_stageBlockCountSifficiencyMask = stageBlockCountSifficiencyMask;
+    return limitedThreadCount;
+}
+
+
+/*static */
+void ThreadedEquationSolverLDLT::doCooperativelySolveLDLTValidated(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, unsigned stageBlockCountSifficiencyMask, 
+    const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dIASSERT(allowedThreadCount > 1);
+
+    if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_STRAIGHT)) == 0)
+    {
+        solveL1Straight<SLDLT_B_STRIDE>(L, b, rowCount, rowSkip);
+    }
+    else
+    {
+        dSASSERT(SLDLT_B_STRIDE + 0 == SL1S_B_STRIDE);
+
+        doCooperativelySolveL1StraightValidated(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip);
+    }
+
+    if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SCALING_VECTOR)) == 0)
+    {
+        scaleLargeVector<SLDLT_B_STRIDE, SLDLT_D_STRIDE>(b, d, rowCount);
+    }
+    else
+    {
+        dSASSERT(SLDLT_B_STRIDE + 0 == SV_A_STRIDE);
+        dSASSERT(SLDLT_D_STRIDE + 0 == SV_D_STRIDE);
+
+        doCooperativelyScaleVectorValidated(resourceContainer, allowedThreadCount, b, d, rowCount);
+    }
+
+    if ((stageBlockCountSifficiencyMask & (1U << SLDLTS_SOLVING_TRANSPOSED)) == 0)
+    {
+        solveL1Transposed<SLDLT_B_STRIDE>(L, b, rowCount, rowSkip);
+    }
+    else
+    {
+        dSASSERT(SLDLT_B_STRIDE + 0 == SL1T_B_STRIDE);
+
+        doCooperativelySolveL1TransposedValidated(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip);
+    }
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Public interface functions
+
+/*extern ODE_API */
+void dSolveLDLT(const dReal *L, const dReal *d, dReal *b, int n, int nskip)
+{
+    dAASSERT(n != 0);
+
+    if (n != 0)
+    {
+        dAASSERT(L != NULL);
+        dAASSERT(d != NULL);
+        dAASSERT(b != NULL);
+
+        solveEquationSystemWithLDLT<1, 1>(L, d, b, n, nskip);
+    }
+}
+
+
+/*extern ODE_API */
+void dEstimateCooperativelySolveLDLTResourceRequirements(dResourceRequirementsID requirements,
+    unsigned maximalAllowedThreadCount, unsigned maximalRowCount)
+{
+    dAASSERT(requirements != NULL);
+
+    dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements;
+    ThreadedEquationSolverLDLT::estimateCooperativeSolvingLDLTResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount);
+}
+
+/*extern ODE_API */
+void dCooperativelySolveLDLT(dResourceContainerID resources, unsigned allowedThreadCount, 
+    const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(resources != NULL);
+
+    dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources;
+    ThreadedEquationSolverLDLT::cooperativelySolveLDLT(resourceContainer, allowedThreadCount, L, d, b, rowCount, rowSkip);
+}
+

thirdparty/ode-0.16.5/ode/src/fastldltsolve_impl.h

@@ -0,0 +1,49 @@
+
+
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_MATRIX_IMPL_H_
+#define _ODE_MATRIX_IMPL_H_
+
+
+#include "fastlsolve_impl.h"
+#include "fastltsolve_impl.h"
+#include "fastvecscale_impl.h"
+
+
+template<unsigned int d_stride, unsigned int b_stride>
+void solveEquationSystemWithLDLT(const dReal *L, const dReal *d, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(L != NULL);
+    dAASSERT(d != NULL);
+    dAASSERT(b != NULL);
+    dAASSERT(rowCount > 0);
+    dAASSERT(rowSkip >= rowCount);
+
+    solveL1Straight<b_stride>(L, b, rowCount, rowSkip);
+    scaleLargeVector<b_stride, d_stride>(b, d, rowCount);
+    solveL1Transposed<b_stride>(L, b, rowCount, rowSkip);
+}
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/fastlsolve.cpp

@@ -0,0 +1,230 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * L1Straight Equation Solving Routines
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/matrix_coop.h>
+#include "config.h"
+#include "threaded_solver_ldlt.h"
+#include "threading_base.h"
+#include "resource_control.h"
+#include "error.h"
+
+#include "fastlsolve_impl.h"
+
+
+/*static */
+void ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1StraightResourceRequirements(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading();
+    unsigned limitedThreadCount = restrictSolvingL1StraightAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount > 1)
+    {
+        doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+    }
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::cooperativelySolveL1Straight(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(rowCount != 0);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    unsigned limitedThreadCount = restrictSolvingL1StraightAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount <= 1)
+    {
+        solveL1Straight<SL1S_B_STRIDE>(L, b, rowCount, rowSkip);
+    }
+    else
+    {
+        doCooperativelySolveL1StraightValidated(resourceContainer, limitedThreadCount, L, b, rowCount, rowSkip);
+    }
+}
+
+
+/*static */
+unsigned ThreadedEquationSolverLDLT::restrictSolvingL1StraightAllowedThreadCount(
+    dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount)
+{
+    unsigned limitedThreadCount = 1;
+
+#if dCOOPERATIVE_ENABLED
+    const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned solvingBlockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep);
+    dIASSERT(deriveSolvingL1StraightThreadCount(SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1);
+
+    if (solvingBlockCount >= SL1S_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+    {
+        limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true);
+    }
+#endif // #if dCOOPERATIVE_ENABLED
+
+    return limitedThreadCount;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doEstimateCooperativeSolvingL1StraightResourceRequirementsValidated(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor, 
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep);
+    dIASSERT(blockCount >= 1);
+
+    unsigned threadCountToUse = deriveSolvingL1StraightThreadCount(blockCount, allowedThreadCount);
+    dIASSERT(threadCountToUse > 1);
+
+    unsigned simultaneousCallCount = 1 + (threadCountToUse - 1);
+
+    SolvingL1StraightMemoryEstimates solvingMemoryEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1StraightMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates);
+    const unsigned solvingAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT;
+
+    unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED;
+    summaryRequirementsDescriptor->mergeAnotherDescriptorIn(solvingMemoryRequired, solvingAlignmentRequired, simultaneousCallCount, featureRequirement);
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doCooperativelySolveL1StraightValidated(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dIASSERT(allowedThreadCount > 1);
+
+    const unsigned int blockStep = SL1S_BLOCK_SIZE; // Required by the implementation
+    unsigned blockCount = deriveSolvingL1StraightBlockCount(rowCount, blockStep);
+    dIASSERT(blockCount >= 1);
+
+    unsigned threadCountToUse = deriveSolvingL1StraightThreadCount(blockCount, allowedThreadCount);
+    dIASSERT(threadCountToUse > 1);
+
+    dCallWaitID completionWait = resourceContainer->getStockCallWait();
+    dAASSERT(completionWait != NULL);
+
+    atomicord32 blockCompletionProgress;
+    cellindexint *blockProgressDescriptors;
+    SolveL1StraightCellContext *cellContexts;
+
+    SolvingL1StraightMemoryEstimates solvingMemoryEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1StraightMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates);
+    dIASSERT(solvingMemoryRequired <= resourceContainer->getMemoryBufferSize());
+
+    void *bufferAllocated = resourceContainer->getMemoryBufferPointer();
+    dIASSERT(bufferAllocated != NULL);
+    dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated);
+
+    void *bufferCurrentLocation = bufferAllocated;
+    bufferCurrentLocation = markCooperativelySolvingL1StraightMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, blockProgressDescriptors, cellContexts);
+    dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + solvingMemoryRequired);
+
+    initializeCooperativelySolveL1StraightMemoryStructures<blockStep>(rowCount, blockCompletionProgress, blockProgressDescriptors, cellContexts);
+
+    dCallReleaseeID calculationFinishReleasee;
+    SolveL1StraightWorkerContext workerContext; // The variable must exist in the outer scope
+
+    workerContext.init(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &solveL1Straight_completion_callback, NULL, 0, "SolveL1Straight Completion");
+    threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &solveL1Straight_worker_callback, &workerContext, "SolveL1Straight Work");
+
+    participateSolvingL1Straight<blockStep, SL1S_B_STRIDE>(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts, threadCountToUse - 1);
+
+    threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "SolveL1Straight End Wait");
+}
+
+/*static */
+int ThreadedEquationSolverLDLT::solveL1Straight_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    SolveL1StraightWorkerContext *ptrContext = (SolveL1StraightWorkerContext *)callContext;
+
+    solveL1Straight_worker(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::solveL1Straight_worker(SolveL1StraightWorkerContext &ref_context, unsigned ownThreadIndex)
+{
+    const unsigned blockStep = SL1S_BLOCK_SIZE;
+
+    participateSolvingL1Straight<blockStep, SL1S_B_STRIDE>(ref_context.m_L, ref_context.m_b, ref_context.m_rowCount, ref_context.m_rowSkip, 
+        *ref_context.m_ptrBlockCompletionProgress, ref_context.m_blockProgressDescriptors, ref_context.m_cellContexts, ownThreadIndex);
+}
+
+/*static */
+int ThreadedEquationSolverLDLT::solveL1Straight_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    return 1;
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////
+// Public interface functions
+
+/*extern ODE_API */
+void dSolveL1(const dReal *L, dReal *B, int n, int lskip1)
+{
+    dAASSERT(n != 0);
+
+    if (n != 0)
+    {
+        dAASSERT(L != NULL);
+        dAASSERT(B != NULL);
+
+        solveL1Straight<1>(L, B, n, lskip1);
+    }
+}
+
+
+/*extern ODE_API */
+void dEstimateCooperativelySolveL1StraightResourceRequirements(dResourceRequirementsID requirements,
+    unsigned maximalAllowedThreadCount, unsigned maximalRowCount)
+{
+    dAASSERT(requirements != NULL);
+
+    dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements;
+    ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1StraightResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount);
+}
+
+/*extern ODE_API */
+void dCooperativelySolveL1Straight(dResourceContainerID resources, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(resources != NULL);
+
+    dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources;
+    ThreadedEquationSolverLDLT::cooperativelySolveL1Straight(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip);
+}
+

thirdparty/ode-0.16.5/ode/src/fastltsolve.cpp

@@ -0,0 +1,229 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * L1Transposed Equation Solving Routines
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/matrix_coop.h>
+#include "config.h"
+#include "threaded_solver_ldlt.h"
+#include "threading_base.h"
+#include "resource_control.h"
+#include "error.h"
+
+#include "fastltsolve_impl.h"
+
+
+/*static */
+void ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1TransposedResourceRequirements(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading();
+    unsigned limitedThreadCount = restrictSolvingL1TransposedAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount > 1)
+    {
+        doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, rowCount);
+    }
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::cooperativelySolveL1Transposed(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dIASSERT(rowCount != 0);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    unsigned limitedThreadCount = restrictSolvingL1TransposedAllowedThreadCount(threading, allowedThreadCount, rowCount);
+
+    if (limitedThreadCount <= 1)
+    {
+        solveL1Transposed<SL1T_B_STRIDE>(L, b, rowCount, rowSkip);
+    }
+    else
+    {
+        doCooperativelySolveL1TransposedValidated(resourceContainer, limitedThreadCount, L, b, rowCount, rowSkip);
+    }
+}
+
+
+/*static */
+unsigned ThreadedEquationSolverLDLT::restrictSolvingL1TransposedAllowedThreadCount(
+    dxThreadingBase *threading, unsigned allowedThreadCount, unsigned rowCount)
+{
+    unsigned limitedThreadCount = 1;
+
+#if dCOOPERATIVE_ENABLED
+    const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation
+    unsigned solvingBlockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep);
+    dIASSERT(deriveSolvingL1TransposedThreadCount(SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM, 2) > 1);
+
+    if (solvingBlockCount >= SL1T_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+    {
+        limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true);
+    }
+#endif // #if dCOOPERATIVE_ENABLED
+
+    return limitedThreadCount;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doEstimateCooperativeSolvingL1TransposedResourceRequirementsValidated(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned rowCount)
+{
+    const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation
+    unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep);
+    dIASSERT(blockCount >= 1);
+
+    unsigned threadCountToUse = deriveSolvingL1TransposedThreadCount(blockCount, allowedThreadCount);
+    dIASSERT(threadCountToUse > 1);
+
+    unsigned simultaneousCallCount = 1 + (threadCountToUse - 1);
+
+    SolvingL1TransposedMemoryEstimates solvingMemoryEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1TransposedMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates);
+    const unsigned solvingAlignmentRequired = ALLOCATION_DEFAULT_ALIGNMENT;
+
+    unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED;
+    summaryRequirementsDescriptor->mergeAnotherDescriptorIn(solvingMemoryRequired, solvingAlignmentRequired, simultaneousCallCount, featureRequirement);
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doCooperativelySolveL1TransposedValidated(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dIASSERT(allowedThreadCount > 1);
+
+    const unsigned int blockStep = SL1T_BLOCK_SIZE; // Required by the implementation
+    unsigned blockCount = deriveSolvingL1TransposedBlockCount(rowCount, blockStep);
+    dIASSERT(blockCount >= 1);
+
+    unsigned threadCountToUse = deriveSolvingL1TransposedThreadCount(blockCount, allowedThreadCount);
+    dIASSERT(threadCountToUse > 1);
+
+    dCallWaitID completionWait = resourceContainer->getStockCallWait();
+    dAASSERT(completionWait != NULL);
+
+    atomicord32 blockCompletionProgress;
+    cellindexint *blockProgressDescriptors;
+    SolveL1TransposedCellContext *cellContexts;
+
+    SolvingL1TransposedMemoryEstimates solvingMemoryEstimates;
+    sizeint solvingMemoryRequired = estimateCooperativelySolvingL1TransposedMemoryRequirement<blockStep>(rowCount, solvingMemoryEstimates);
+    dIASSERT(solvingMemoryRequired <= resourceContainer->getMemoryBufferSize());
+
+    void *bufferAllocated = resourceContainer->getMemoryBufferPointer();
+    dIASSERT(bufferAllocated != NULL);
+    dIASSERT(dALIGN_PTR(bufferAllocated, ALLOCATION_DEFAULT_ALIGNMENT) == bufferAllocated);
+
+    void *bufferCurrentLocation = bufferAllocated;
+    bufferCurrentLocation = markCooperativelySolvingL1TransposedMemoryStructuresOut(bufferCurrentLocation, solvingMemoryEstimates, blockProgressDescriptors, cellContexts);
+    dIVERIFY(bufferCurrentLocation <= (uint8 *)bufferAllocated + solvingMemoryRequired);
+
+    initializeCooperativelySolveL1TransposedMemoryStructures<blockStep>(rowCount, blockCompletionProgress, blockProgressDescriptors, cellContexts);
+
+    dCallReleaseeID calculationFinishReleasee;
+    SolveL1TransposedWorkerContext workerContext; // The variable must exist in the outer scope
+
+    workerContext.init(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &solveL1Transposed_completion_callback, NULL, 0, "SolveL1Transposed Completion");
+    threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &solveL1Transposed_worker_callback, &workerContext, "SolveL1Transposed Work");
+
+    participateSolvingL1Transposed<blockStep, SL1T_B_STRIDE>(L, b, rowCount, rowSkip, blockCompletionProgress, blockProgressDescriptors, cellContexts, threadCountToUse - 1);
+
+    threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "SolveL1Transposed End Wait");
+}
+
+/*static */
+int ThreadedEquationSolverLDLT::solveL1Transposed_worker_callback(void *callContext, dcallindex_t callInstanceIndex, dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    SolveL1TransposedWorkerContext *ptrContext = (SolveL1TransposedWorkerContext *)callContext;
+
+    solveL1Transposed_worker(*ptrContext, dCAST_TO_SMALLER(unsigned, callInstanceIndex));
+    
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::solveL1Transposed_worker(SolveL1TransposedWorkerContext &ref_context, unsigned ownThreadIndex)
+{
+    const unsigned blockStep = SL1T_BLOCK_SIZE;
+    participateSolvingL1Transposed<blockStep, SL1T_B_STRIDE>(ref_context.m_L, ref_context.m_b, ref_context.m_rowCount, ref_context.m_rowSkip, 
+        *ref_context.m_ptrBlockCompletionProgress, ref_context.m_blockProgressDescriptors, ref_context.m_cellContexts, ownThreadIndex);
+}
+
+/*static */
+int ThreadedEquationSolverLDLT::solveL1Transposed_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    return 1;
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////
+// Public interface functions
+
+/*extern ODE_API */
+void dSolveL1T(const dReal *L, dReal *B, int rowCount, int rowSkip)
+{
+    dAASSERT(rowCount != 0);
+
+    if (rowCount != 0)
+    {
+        dAASSERT(L != NULL);
+        dAASSERT(B != NULL);
+
+        solveL1Transposed<1>(L, B, rowCount, rowSkip);
+    }
+}
+
+
+/*extern ODE_API */
+void dEstimateCooperativelySolveL1TransposedResourceRequirements(dResourceRequirementsID requirements, 
+    unsigned maximalAllowedThreadCount, unsigned maximalRowCount)
+{
+    dAASSERT(requirements != NULL);
+
+    dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements;
+    ThreadedEquationSolverLDLT::estimateCooperativeSolvingL1TransposedResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalRowCount);
+}
+
+/*extern ODE_API */
+void dCooperativelySolveL1Transposed(dResourceContainerID resources, unsigned allowedThreadCount, 
+    const dReal *L, dReal *b, unsigned rowCount, unsigned rowSkip)
+{
+    dAASSERT(resources != NULL);
+
+    dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources;
+    ThreadedEquationSolverLDLT::cooperativelySolveL1Transposed(resourceContainer, allowedThreadCount, L, b, rowCount, rowSkip);
+}
+

thirdparty/ode-0.16.5/ode/src/fastvecscale.cpp

@@ -0,0 +1,204 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* 
+ * Vector scaling related code of ThreadedEquationSolverLDLT 
+ * Copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")
+ */
+
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/matrix_coop.h>
+#include "config.h"
+#include "threaded_solver_ldlt.h"
+#include "threading_base.h"
+#include "resource_control.h"
+#include "error.h"
+
+#include "fastvecscale_impl.h"
+
+
+/*static */
+void ThreadedEquationSolverLDLT::estimateCooperativeScalingVectorResourceRequirements(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned elementCount)
+{
+    dxThreadingBase *threading = summaryRequirementsDescriptor->getrelatedThreading();
+    unsigned limitedThreadCount = restrictScalingVectorAllowedThreadCount(threading, allowedThreadCount, elementCount);
+
+    if (limitedThreadCount > 1)
+    {
+        doEstimateCooperativeScalingVectorResourceRequirementsValidated(summaryRequirementsDescriptor, allowedThreadCount, elementCount);
+    }
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::cooperativelyScaleVector(dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    dReal *vectorData, const dReal *scaleData, unsigned elementCount)
+{
+    dAASSERT(elementCount != 0);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    unsigned limitedThreadCount = restrictScalingVectorAllowedThreadCount(threading, allowedThreadCount, elementCount);
+
+    if (limitedThreadCount <= 1)
+    {
+        scaleLargeVector<SV_A_STRIDE, SV_D_STRIDE>(vectorData, scaleData, elementCount);
+    }
+    else
+    {
+        doCooperativelyScaleVectorValidated(resourceContainer, limitedThreadCount, vectorData, scaleData, elementCount);
+    }
+}
+
+/*static */
+unsigned ThreadedEquationSolverLDLT::restrictScalingVectorAllowedThreadCount(
+    dxThreadingBase *threading, unsigned allowedThreadCount, unsigned elementCount)
+{
+    unsigned limitedThreadCount = 1;
+
+#if dCOOPERATIVE_ENABLED
+    const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation
+    unsigned scalingBlockCount = deriveScalingVectorBlockCount(elementCount, blockStep);
+    dIASSERT(deriveScalingVectorThreadCount(SV_COOPERATIVE_BLOCK_COUNT_MINIMUM - 1, 2) > 1);
+
+    if (scalingBlockCount >= SV_COOPERATIVE_BLOCK_COUNT_MINIMUM)
+    {
+        limitedThreadCount = threading->calculateThreadingLimitedThreadCount(allowedThreadCount, true);
+    }
+#endif // #if dCOOPERATIVE_ENABLED
+
+    return limitedThreadCount;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doEstimateCooperativeScalingVectorResourceRequirementsValidated(
+    dxResourceRequirementDescriptor *summaryRequirementsDescriptor,
+    unsigned allowedThreadCount, unsigned elementCount)
+{
+    unsigned simultaneousCallCount = 1 + (allowedThreadCount - 1);
+
+    sizeint scalingMemoryRequired = 0;
+    const unsigned scalingAlignmentRequired = 0;
+
+    unsigned featureRequirement = dxResourceRequirementDescriptor::STOCK_CALLWAIT_REQUIRED;
+    summaryRequirementsDescriptor->mergeAnotherDescriptorIn(scalingMemoryRequired, scalingAlignmentRequired, simultaneousCallCount, featureRequirement);
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::doCooperativelyScaleVectorValidated(
+    dxRequiredResourceContainer *resourceContainer, unsigned allowedThreadCount, 
+    dReal *vectorData, const dReal *scaleData, unsigned elementCount)
+{
+    dIASSERT(allowedThreadCount > 1);
+
+    const unsigned int blockStep = SV_BLOCK_SIZE; // Required by the implementation
+    unsigned scalingBlockCount = deriveScalingVectorBlockCount(elementCount, blockStep);
+    dIASSERT(scalingBlockCount > 0U);
+
+    unsigned threadCountToUse = deriveScalingVectorThreadCount(scalingBlockCount - 1, allowedThreadCount);
+    dIASSERT(threadCountToUse > 1);
+
+    dCallWaitID completionWait = resourceContainer->getStockCallWait();
+    dAASSERT(completionWait != NULL);
+
+    atomicord32 blockCompletionProgress;
+
+    initializeCooperativelyScaleVectorMemoryStructures(blockCompletionProgress);
+
+    dCallReleaseeID calculationFinishReleasee;
+    ScaleVectorWorkerContext workerContext; // The variable must exist in the outer scope
+
+    workerContext.init(vectorData, scaleData, elementCount, blockCompletionProgress);
+
+    dxThreadingBase *threading = resourceContainer->getThreadingInstance();
+    threading->PostThreadedCall(NULL, &calculationFinishReleasee, threadCountToUse - 1, NULL, completionWait, &scaleVector_completion_callback, NULL, 0, "ScaleVector Completion");
+    threading->PostThreadedCallsGroup(NULL, threadCountToUse - 1, calculationFinishReleasee, &scaleVector_worker_callback, &workerContext, "ScaleVector Work");
+
+    participateScalingVector<blockStep, SV_A_STRIDE, SV_D_STRIDE>(vectorData, scaleData, elementCount, blockCompletionProgress);
+
+    threading->WaitThreadedCallExclusively(NULL, completionWait, NULL, "ScaleVector End Wait");
+}
+
+
+/*static */
+int ThreadedEquationSolverLDLT::scaleVector_worker_callback(void *callContext, dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    ScaleVectorWorkerContext *ptrContext = (ScaleVectorWorkerContext *)callContext;
+
+    scaleVector_worker(*ptrContext);
+
+    return 1;
+}
+
+/*static */
+void ThreadedEquationSolverLDLT::scaleVector_worker(ScaleVectorWorkerContext &ref_context)
+{
+    const unsigned blockStep = SV_BLOCK_SIZE;
+
+    participateScalingVector<blockStep, SV_A_STRIDE, SV_D_STRIDE>(ref_context.m_vectorData, ref_context.m_scaleData, ref_context.m_elementCount, *ref_context.m_ptrBlockCompletionProgress);
+}
+
+/*static */
+int ThreadedEquationSolverLDLT::scaleVector_completion_callback(void *dUNUSED(callContext), dcallindex_t dUNUSED(callInstanceIndex), dCallReleaseeID dUNUSED(callThisReleasee))
+{
+    return 1;
+}
+
+
+//////////////////////////////////////////////////////////////////////////
+// Public interface functions
+
+/*extern ODE_API */
+void dScaleVector(dReal *a, const dReal *d, int n)
+{
+    scaleLargeVector<1, 1>(a, d, n);
+}
+
+/*extern ODE_API_DEPRECATED ODE_API */
+void dVectorScale(dReal *a, const dReal *d, int n)
+{
+    scaleLargeVector<1, 1>(a, d, n);
+}
+
+
+/*extern ODE_API */
+void dEstimateCooperativelyScaleVectorResourceRequirements(dResourceRequirementsID requirements,
+    unsigned maximalAllowedThreadCount, unsigned maximalElementCount)
+{
+    dAASSERT(requirements != NULL);
+
+    dxResourceRequirementDescriptor *requirementsDescriptor = (dxResourceRequirementDescriptor *)requirements;
+    ThreadedEquationSolverLDLT::estimateCooperativeScalingVectorResourceRequirements(requirementsDescriptor, maximalAllowedThreadCount, maximalElementCount);
+}
+
+/*extern ODE_API */
+void dCooperativelyScaleVector(dResourceContainerID resources, unsigned allowedThreadCount, 
+    dReal *dataVector, const dReal *scaleVector, unsigned elementCount)
+{
+    dAASSERT(resources != NULL);
+
+    dxRequiredResourceContainer *resourceContainer = (dxRequiredResourceContainer *)resources;
+    ThreadedEquationSolverLDLT::cooperativelyScaleVector(resourceContainer, allowedThreadCount, dataVector, scaleVector, elementCount);
+}
+

thirdparty/ode-0.16.5/ode/src/fastvecscale_impl.h

@@ -0,0 +1,171 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * Vector scaling function implementation
+ * Improvements and cooperative implementation copyright (c) 2017-2024 Oleh Derevenko, odar@eleks.com (change all "a" to "e")  
+ */
+
+#ifndef _ODE_FASTVECSCALE_IMPL_H_
+#define _ODE_FASTVECSCALE_IMPL_H_
+
+
+
+template<unsigned int a_stride, unsigned int d_stride>
+void scaleLargeVector(dReal *aStart, const dReal *dStart, unsigned elementCount)
+{
+    dAASSERT (aStart && dStart && elementCount >= 0);
+    
+    const unsigned step = 4;
+
+    dReal *ptrA = aStart;
+    const dReal *ptrD = dStart;
+    const dReal *const dStepsEnd = dStart + (sizeint)(elementCount & ~(step - 1)) * d_stride;
+    for (; ptrD != dStepsEnd; ptrA += step * a_stride, ptrD += step * d_stride) 
+    {
+        dReal a0 = ptrA[0], a1 = ptrA[1 * a_stride], a2 = ptrA[2 * a_stride], a3 = ptrA[3 * a_stride];
+        dReal d0 = ptrD[0], d1 = ptrD[1 * d_stride], d2 = ptrD[2 * d_stride], d3 = ptrD[3 * d_stride];
+        a0 *= d0;
+        a1 *= d1;
+        a2 *= d2;
+        a3 *= d3;
+        ptrA[0] = a0; ptrA[1 * a_stride] = a1; ptrA[2 * a_stride] = a2; ptrA[3 * a_stride] = a3;
+        dSASSERT(step == 4);
+    }
+
+    switch (elementCount & (step - 1))
+    {
+        case 3:
+        {
+            dReal a2 = ptrA[2 * a_stride];
+            dReal d2 = ptrD[2 * d_stride];
+            ptrA[2 * a_stride] = a2 * d2;
+            // break; -- proceed to case 2
+        }
+
+        case 2:
+        {
+            dReal a1 = ptrA[1 * a_stride];
+            dReal d1 = ptrD[1 * d_stride];
+            ptrA[1 * a_stride] = a1 * d1;
+            // break; -- proceed to case 1
+        }
+
+        case 1:
+        {
+            dReal a0 = ptrA[0];
+            dReal d0 = ptrD[0];
+            ptrA[0] = a0 * d0;
+            break;
+        }
+    }
+    dSASSERT(step == 4);
+}
+
+
+template<unsigned int block_step, unsigned int a_stride, unsigned int d_stride>
+/*static */
+void ThreadedEquationSolverLDLT::participateScalingVector(dReal *ptrAStart, const dReal *ptrDStart, const unsigned elementCount,
+    volatile atomicord32 &refBlockCompletionProgress/*=0*/)
+{
+    dAASSERT (ptrAStart != NULL);
+    dAASSERT(ptrDStart != NULL);
+    dAASSERT(elementCount >= 0);
+
+    const unsigned wrapSize = 4;
+    dSASSERT(block_step % wrapSize == 0);
+
+    const unsigned completeBlockCount = elementCount / block_step;
+    const unsigned trailingBlockElements = elementCount % block_step;
+
+    unsigned blockIndex;
+    while ((blockIndex = ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, completeBlockCount)) != completeBlockCount)
+    {
+        dReal *ptrAElement = ptrAStart + (sizeint)(blockIndex * block_step) * a_stride;
+        const dReal *ptrDElement = ptrDStart + (sizeint)(blockIndex * block_step) * d_stride;
+        const dReal *const ptrDBlockEnd = ptrDElement + block_step * d_stride;
+        dSASSERT((sizeint)block_step * a_stride < UINT_MAX);
+        dSASSERT((sizeint)block_step * d_stride < UINT_MAX);
+
+        for (; ptrDElement != ptrDBlockEnd; ptrAElement += wrapSize * a_stride, ptrDElement += wrapSize * d_stride)
+        {
+            dReal a0 = ptrAElement[0], a1 = ptrAElement[1 * a_stride], a2 = ptrAElement[2 * a_stride], a3 = ptrAElement[3 * a_stride];
+            dReal d0 = ptrDElement[0], d1 = ptrDElement[1 * d_stride], d2 = ptrDElement[2 * d_stride], d3 = ptrDElement[3 * d_stride];
+            a0 *= d0;
+            a1 *= d1;
+            a2 *= d2;
+            a3 *= d3;
+            ptrAElement[0] = a0; ptrAElement[1 * a_stride] = a1; ptrAElement[2 * a_stride] = a2; ptrAElement[3 * a_stride] = a3;
+            dSASSERT(wrapSize == 4);
+        }
+    }
+
+    if (trailingBlockElements != 0 && (blockIndex = ThrsafeIncrementIntUpToLimit(&refBlockCompletionProgress, completeBlockCount + 1)) != completeBlockCount + 1)
+    {
+        dReal *ptrAElement = ptrAStart + (sizeint)(completeBlockCount * block_step) * a_stride;
+        const dReal *ptrDElement = ptrDStart + (sizeint)(completeBlockCount * block_step) * d_stride;
+        const dReal *const ptrDBlockEnd = ptrDElement + (trailingBlockElements & ~(wrapSize - 1)) * d_stride;
+
+        for (; ptrDElement != ptrDBlockEnd; ptrAElement += wrapSize * a_stride, ptrDElement += wrapSize * d_stride)
+        {
+            dReal a0 = ptrAElement[0], a1 = ptrAElement[1 * a_stride], a2 = ptrAElement[2 * a_stride], a3 = ptrAElement[3 * a_stride];
+            dReal d0 = ptrDElement[0], d1 = ptrDElement[1 * d_stride], d2 = ptrDElement[2 * d_stride], d3 = ptrDElement[3 * d_stride];
+            a0 *= d0;
+            a1 *= d1;
+            a2 *= d2;
+            a3 *= d3;
+            ptrAElement[0] = a0; ptrAElement[1 * a_stride] = a1; ptrAElement[2 * a_stride] = a2; ptrAElement[3 * a_stride] = a3;
+            dSASSERT(wrapSize == 4);
+        }
+
+        switch (trailingBlockElements & (wrapSize - 1))
+        {
+            case 3:
+            {
+                dReal a2 = ptrAElement[2 * a_stride];
+                dReal d2 = ptrDElement[2 * d_stride];
+                ptrAElement[2 * a_stride] = a2 * d2;
+                // break; -- proceed to case 2
+            }
+
+            case 2:
+            {
+                dReal a1 = ptrAElement[1 * a_stride];
+                dReal d1 = ptrDElement[1 * d_stride];
+                ptrAElement[1 * a_stride] = a1 * d1;
+                // break; -- proceed to case 1
+            }
+
+            case 1:
+            {
+                dReal a0 = ptrAElement[0];
+                dReal d0 = ptrDElement[0];
+                ptrAElement[0] = a0 * d0;
+                break;
+            }
+        }
+        dSASSERT(wrapSize == 4);
+    }
+}
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/gimpact_contact_export_helper.cpp

@@ -0,0 +1,95 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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 <ode/collision.h>
+#include "config.h"
+
+
+#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+
+#include "gimpact_contact_export_helper.h"
+#include "error.h"
+
+
+/*static */
+dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth)
+{
+    dReal result;
+
+    while (true)
+    {
+        dReal firstdepth = REAL(0.5) * (mindepth + maxdepth);
+        dReal lowdepth = maxdepth, highdepth = mindepth;
+
+        unsigned marginindex = 0;
+        unsigned highindex = marginindex;
+        dIASSERT(contactcount != 0);
+
+        for (unsigned i = 0; i < contactcount; i++)
+        {
+            dReal depth = pdepths[i];
+
+            if (depth < firstdepth)
+            {
+                dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth;
+                if (highdepth < depth) { highdepth = depth; }
+            }
+            else if (depth > firstdepth)
+            {
+                pdepths[highindex++] = depth;
+                if (depth < lowdepth) { lowdepth = depth; }
+            }
+        }
+
+        unsigned countabove = highindex - marginindex;
+        if (maxcontacts < countabove)
+        {
+            contactcount = countabove;
+            pdepths += marginindex;
+            mindepth = lowdepth;
+        }
+        else if (maxcontacts == countabove)
+        {
+            result = dNextAfter(firstdepth, dInfinity);
+            break;
+        }
+        else
+        {
+            unsigned countbelow = marginindex;
+            if (maxcontacts <= contactcount - countbelow)
+            {
+                result = firstdepth;
+                break;
+            }
+
+            maxcontacts -= contactcount - countbelow;
+            contactcount = countbelow;
+            maxdepth = highdepth;
+        }
+    }
+
+    return result;
+}
+
+
+#endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+

thirdparty/ode-0.16.5/ode/src/gimpact_contact_export_helper.h

@@ -0,0 +1,177 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+#ifndef _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_
+#define _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_
+
+
+#include "collision_kernel.h"
+#include "collision_util.h"
+#include "util.h"
+
+
+#ifndef ALLOCA
+#define ALLOCA(x) dALLOCA16(x)
+#endif
+
+
+struct dxGImpactContactsExportHelper
+{
+public:
+    template<class dxGImpactContactAccessor>
+    static unsigned ExportMaxDepthGImpactContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+        int Flags, dContactGeom* Contacts, int Stride)
+    {
+        unsigned result;
+
+        unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
+        if (contactcount > maxcontacts)
+        {
+            ExportExcesssiveContacts(srccontacts, contactcount, Flags, Contacts, Stride);
+            result = maxcontacts;
+        }
+        else
+        {
+            ExportFitContacts(srccontacts, contactcount, Flags, Contacts, Stride);
+            result = contactcount;
+        }
+
+        return result;
+    }
+
+private:
+    template<class dxGImpactContactAccessor>
+    static void ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+        int Flags, dContactGeom* Contacts, int Stride);
+    template<class dxGImpactContactAccessor>
+    static void ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+        int Flags, dContactGeom* Contacts, int Stride);
+    template<class dxGImpactContactAccessor>
+    static dReal FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts);
+    static dReal FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth);
+};
+
+
+template<class dxGImpactContactAccessor> 
+/*static */
+void dxGImpactContactsExportHelper::ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+    int Flags, dContactGeom* Contacts, int Stride)
+{
+    unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK);
+    dReal marginaldepth = FindContactsMarginalDepth(srccontacts, contactcount, maxcontacts);
+
+    unsigned contactshead = 0, contacttail = maxcontacts;
+    for (unsigned i = 0; i < contactcount; i++)
+    {
+        dReal depth = srccontacts.RetrieveDepthByIndex(i);
+
+        if (depth > marginaldepth)
+        {
+            dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contactshead, Stride);
+            srccontacts.ExportContactGeomByIndex(pcontact, i);
+
+            if (++contactshead == maxcontacts)
+            {
+                break;
+            }
+        }
+        else if (depth == marginaldepth && contactshead < contacttail)
+        {
+            --contacttail;
+
+            dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contacttail, Stride);
+            srccontacts.ExportContactGeomByIndex(pcontact, i);
+        }
+    }
+}
+
+template<class dxGImpactContactAccessor>
+/*static */
+void dxGImpactContactsExportHelper::ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount,
+    int Flags, dContactGeom* Contacts, int Stride)
+{
+    for (unsigned i = 0; i < contactcount; i++)
+    {
+        dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+        srccontacts.ExportContactGeomByIndex(pcontact, i);
+    }
+}
+
+template<class dxGImpactContactAccessor>
+/*static */
+dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts)
+{
+    dReal result;
+
+    dReal *pdepths = (dReal *)ALLOCA(contactcount * sizeof(dReal));
+    unsigned marginindex = 0;
+    unsigned highindex = marginindex;
+
+    dReal firstdepth = srccontacts.RetrieveDepthByIndex(0);
+    dReal mindepth = firstdepth, maxdepth = firstdepth;
+    dIASSERT(contactcount > 1);
+
+    for (unsigned i = 1; i < contactcount; i++)
+    {
+        dReal depth = srccontacts.RetrieveDepthByIndex(i);
+
+        if (depth < firstdepth)
+        {
+            dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth;
+            if (depth < mindepth) { mindepth = depth; }
+        }
+        else if (depth > firstdepth)
+        {
+            pdepths[highindex++] = depth;
+            if (maxdepth < depth) { maxdepth = depth; }
+        }
+    }
+
+    unsigned countabove = highindex - marginindex;
+    if (maxcontacts < countabove)
+    {
+        result = FindContactsMarginalDepth(pdepths + marginindex, countabove, maxcontacts, firstdepth, maxdepth);
+    }
+    else if (maxcontacts == countabove)
+    {
+        result = dNextAfter(firstdepth, dInfinity);
+    }
+    else
+    {
+        unsigned countbelow = marginindex;
+        if (maxcontacts <= contactcount - countbelow)
+        {
+            result = firstdepth;
+        }
+        else
+        {
+            result = FindContactsMarginalDepth(pdepths, countbelow, maxcontacts - (contactcount - countbelow), mindepth, firstdepth);
+        }
+    }
+
+    return result;
+}
+
+
+#endif	//_ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_

thirdparty/ode-0.16.5/ode/src/gimpact_gim_contact_accessor.h

@@ -0,0 +1,62 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+#ifndef _ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_
+#define _ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_
+
+
+struct dxGIMCContactAccessor
+{
+    dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(false), m_side2ovr() {}
+    dxGIMCContactAccessor(GIM_CONTACT *ptrimeshcontacts, dGeomID g1, dGeomID g2, int side2ovr) : m_ptrimeshcontacts(ptrimeshcontacts), m_g1(g1), m_g2(g2), m_gotside2ovr(true), m_side2ovr(side2ovr) {}
+
+    dReal RetrieveDepthByIndex(unsigned index) const { return m_ptrimeshcontacts[index].m_depth; }
+
+    void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const
+    {
+        const GIM_CONTACT *ptrimeshcontact = m_ptrimeshcontacts + index;
+        pcontact->pos[0] = ptrimeshcontact->m_point[0];
+        pcontact->pos[1] = ptrimeshcontact->m_point[1];
+        pcontact->pos[2] = ptrimeshcontact->m_point[2];
+        pcontact->pos[3] = REAL(1.0);
+
+        pcontact->normal[0] = ptrimeshcontact->m_normal[0];
+        pcontact->normal[1] = ptrimeshcontact->m_normal[1];
+        pcontact->normal[2] = ptrimeshcontact->m_normal[2];
+        pcontact->normal[3] = 0;
+
+        pcontact->depth = ptrimeshcontact->m_depth;
+        pcontact->g1 = m_g1;
+        pcontact->g2 = m_g2;
+        pcontact->side1 = ptrimeshcontact->m_feature1;
+        pcontact->side2 = !m_gotside2ovr ? ptrimeshcontact->m_feature2 : m_side2ovr;
+    }
+
+    const GIM_CONTACT *m_ptrimeshcontacts;
+    dGeomID         m_g1, m_g2;
+    bool            m_gotside2ovr;
+    int             m_side2ovr;
+};
+
+
+#endif	//_ODE_GIMPACT_GIM_CONTACT_ACCESSOR_H_

thirdparty/ode-0.16.5/ode/src/gimpact_plane_contact_accessor.h

@@ -0,0 +1,62 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+#ifndef _ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_
+#define _ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_
+
+
+struct dxPlaneContactAccessor
+{
+    dxPlaneContactAccessor(const vec4f *planecontact_results, const dReal *plane, dGeomID g1, dGeomID g2) : m_planecontact_results(planecontact_results), m_plane(plane), m_g1(g1), m_g2(g2) {}
+
+    dReal RetrieveDepthByIndex(unsigned index) const { return m_planecontact_results[index][3]; }
+
+    void ExportContactGeomByIndex(dContactGeom *pcontact, unsigned index) const
+    {
+        const vec4f *planecontact = m_planecontact_results + index;
+
+        pcontact->pos[0] = (*planecontact)[0];
+        pcontact->pos[1] = (*planecontact)[1];
+        pcontact->pos[2] = (*planecontact)[2];
+        pcontact->pos[3] = REAL(1.0);
+
+        const dReal *plane = m_plane;
+        pcontact->normal[0] = plane[0];
+        pcontact->normal[1] = plane[1];
+        pcontact->normal[2] = plane[2];
+        pcontact->normal[3] = 0;
+
+        pcontact->depth = (*planecontact)[3];
+        pcontact->g1 = m_g1; // trimesh geom
+        pcontact->g2 = m_g2; // plane geom
+        pcontact->side1 = -1; // note: don't have the triangle index, but OPCODE *does* do this properly
+        pcontact->side2 = -1;
+    }
+
+    const vec4f     *m_planecontact_results;
+    const dReal     *m_plane;
+    dGeomID         m_g1, m_g2;
+};
+
+
+#endif	//_ODE_GIMPACT_PLANE_CONTACT_ACCESSOR_H_

thirdparty/ode-0.16.5/ode/src/heightfield.h

@@ -0,0 +1,245 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// dHeightfield Collider
+//  Martijn Buijs 2006 http://home.planet.nl/~buijs512/
+// Based on Terrain & Cone contrib by:
+//  Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
+
+#ifndef _DHEIGHTFIELD_H_
+#define _DHEIGHTFIELD_H_
+//------------------------------------------------------------------------------
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+
+#define HEIGHTFIELDMAXCONTACTPERCELL 10
+
+
+class HeightFieldVertex;
+class HeightFieldEdge;
+class HeightFieldTriangle;
+
+//
+// dxHeightfieldData
+//
+// Heightfield Data structure
+//
+struct dxHeightfieldData
+{
+    dReal m_fWidth;				// World space heightfield dimension on X axis
+    dReal m_fDepth;				// World space heightfield dimension on Z axis
+    dReal m_fSampleWidth;		// Vertex spacing on X axis edge (== m_vWidth / (m_nWidthSamples-1))
+    dReal m_fSampleDepth;		// Vertex spacing on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
+    dReal m_fSampleZXAspect;    // Relation of Z axis spacing to X axis spacing (== m_fSampleDepth / m_fSampleWidth)
+    dReal m_fInvSampleWidth;		// Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
+    dReal m_fInvSampleDepth;		// Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
+
+    dReal m_fHalfWidth;			// Cache of half of m_fWidth
+    dReal m_fHalfDepth;			// Cache of half of m_fDepth
+
+    dReal m_fMinHeight;        // Min sample height value (scaled and offset)
+    dReal m_fMaxHeight;        // Max sample height value (scaled and offset)
+    dReal m_fThickness;        // Surface thickness (added to bottom AABB)
+    dReal m_fScale;            // Sample value multiplier
+    dReal m_fOffset;           // Vertical sample offset
+
+    int	m_nWidthSamples;       // Vertex count on X axis edge (number of samples)
+    int	m_nDepthSamples;       // Vertex count on Z axis edge (number of samples)
+    int m_bCopyHeightData;     // Do we own the sample data?
+    int	m_bWrapMode;           // Heightfield wrapping mode (0=finite, 1=infinite)
+    int m_nGetHeightMode;      // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float )
+
+    const void* m_pHeightData; // Sample data array
+    void* m_pUserData;         // Callback user data
+
+    dContactGeom            m_contacts[HEIGHTFIELDMAXCONTACTPERCELL];
+
+    dHeightfieldGetHeight* m_pGetHeightCallback;		// Callback pointer.
+
+    dxHeightfieldData();
+    ~dxHeightfieldData();
+
+    void SetData( int nWidthSamples, int nDepthSamples,
+        dReal fWidth, dReal fDepth,
+        dReal fScale, dReal fOffset,
+        dReal fThickness, int bWrapMode );
+
+    void ComputeHeightBounds();
+
+    bool IsOnHeightfield2  ( const HeightFieldVertex * const CellCorner, 
+        const dReal * const pos,  const bool isABC) const;
+
+    dReal GetHeight(int x, int z);
+    dReal GetHeight(dReal x, dReal z);
+
+};
+
+typedef int HeightFieldVertexCoords[2];
+
+class HeightFieldVertex
+{
+public:
+    HeightFieldVertex(){};
+
+    dVector3 vertex;
+    HeightFieldVertexCoords coords;
+    bool state;
+};
+
+class HeightFieldEdge
+{
+public:
+    HeightFieldEdge(){};
+
+    HeightFieldVertex   *vertices[2];
+};
+
+class HeightFieldTriangle
+{
+public:
+    HeightFieldTriangle(){};
+
+    inline void setMinMax()
+    {
+        maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1];
+        maxAAAB = vertices[2]->vertex[1] > maxAAAB  ? vertices[2]->vertex[1] : maxAAAB;
+    };
+
+    HeightFieldVertex   *vertices[3];
+    dReal               planeDef[4];
+    dReal               maxAAAB;
+
+    bool                isUp;
+    bool                state;
+};
+
+class HeightFieldPlane
+{
+public:
+    HeightFieldPlane():
+        trianglelist(0),
+        trianglelistReservedSize(0),
+        trianglelistCurrentSize(0)
+    {
+    }
+
+    ~HeightFieldPlane()
+    {
+        delete [] trianglelist;
+    }
+
+    inline void setMinMax()
+    {
+        const sizeint asize = trianglelistCurrentSize;
+        if (asize > 0)
+        {  
+            maxAAAB = trianglelist[0]->maxAAAB;
+            for (sizeint k = 1; asize > k; k++)
+            {   
+                if (trianglelist[k]->maxAAAB >  maxAAAB)
+                    maxAAAB = trianglelist[k]->maxAAAB;
+            }
+        }
+    };
+
+    void resetTriangleListSize(const sizeint newSize)
+    {
+        if (trianglelistReservedSize < newSize)
+        {
+            delete [] trianglelist;
+            trianglelistReservedSize = newSize;
+            trianglelist = new HeightFieldTriangle *[newSize];
+        }
+        trianglelistCurrentSize = 0;
+    }
+
+    void addTriangle(HeightFieldTriangle *tri)
+    {
+        dIASSERT(trianglelistCurrentSize < trianglelistReservedSize);
+
+        trianglelist[trianglelistCurrentSize++] = tri;
+    }
+
+    HeightFieldTriangle **trianglelist;
+    sizeint             trianglelistReservedSize;
+    sizeint             trianglelistCurrentSize;
+
+    dReal   maxAAAB;
+    dReal   planeDef[4];
+};
+
+//
+// dxHeightfield
+//
+// Heightfield geom structure
+//
+struct dxHeightfield : public dxGeom
+{
+    dxHeightfieldData* m_p_data;
+
+    dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable );
+    ~dxHeightfield();
+
+    void computeAABB();
+
+    int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,  
+        dxGeom *o2, const int numMaxContacts,
+        int flags, dContactGeom *contact, int skip );
+
+    enum
+    {
+        TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 4,
+        TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X = 4,
+        TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z = 4,
+        TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 1 // Triangles are easy to reallocate and hard to predict
+    };
+
+    static inline sizeint AlignBufferSize(sizeint value, sizeint alignment) { dIASSERT((alignment & (alignment - 1)) == 0); return (value + (alignment - 1)) & ~(alignment - 1); }
+
+    void  allocateTriangleBuffer(sizeint numTri);
+    void  resetTriangleBuffer();
+    void  allocatePlaneBuffer(sizeint numTri);
+    void  resetPlaneBuffer();
+    void  allocateHeightBuffer(sizeint numX, sizeint numZ);
+    void  resetHeightBuffer();
+
+    void  sortPlanes(const sizeint numPlanes);
+
+    HeightFieldPlane    **tempPlaneBuffer;
+    HeightFieldPlane    *tempPlaneInstances;
+    sizeint             tempPlaneBufferSize;
+
+    HeightFieldTriangle *tempTriangleBuffer;
+    sizeint             tempTriangleBufferSize;
+
+    HeightFieldVertex   **tempHeightBuffer;
+    HeightFieldVertex   *tempHeightInstances;
+    sizeint             tempHeightBufferSizeX;
+    sizeint             tempHeightBufferSizeZ;
+
+};
+
+
+//------------------------------------------------------------------------------
+#endif //_DHEIGHTFIELD_H_

thirdparty/ode-0.16.5/ode/src/joints/Makefile.am

@@ -0,0 +1,37 @@
+AM_CPPFLAGS = -I$(top_srcdir)/include \
+        -I$(top_builddir)/include \
+        -I$(top_srcdir)/ode/src \
+        -D__ODE__
+
+
+if ENABLE_OU
+
+AM_CPPFLAGS += -I$(top_srcdir)/ou/include
+
+
+endif
+
+
+noinst_LTLIBRARIES = libjoints.la
+
+libjoints_la_SOURCES =  joints.h \
+                        joint.h joint.cpp \
+                        joint_internal.h \
+                        ball.h ball.cpp \
+                        dball.h dball.cpp \
+                        dhinge.h dhinge.cpp \
+                        transmission.h transmission.cpp \
+                        hinge.h hinge.cpp \
+                        slider.h slider.cpp \
+                        contact.h contact.cpp \
+                        universal.h universal.cpp \
+                        hinge2.h hinge2.cpp \
+                        fixed.h fixed.cpp \
+                        null.h null.cpp \
+                        amotor.h amotor.cpp \
+                        lmotor.h lmotor.cpp \
+                        plane2d.h plane2d.cpp \
+                        pu.h pu.cpp \
+                        pr.h pr.cpp \
+                        piston.h piston.cpp
+

thirdparty/ode-0.16.5/ode/src/joints/Makefile.in

@@ -0,0 +1,668 @@
+# Makefile.in generated by automake 1.15 from Makefile.am.
+# @configure_input@
+
+# Copyright (C) 1994-2014 Free Software Foundation, Inc.
+
+# This Makefile.in is free software; the Free Software Foundation
+# gives unlimited permission to copy and/or distribute it,
+# with or without modifications, as long as this notice is preserved.
+
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY, to the extent permitted by law; without
+# even the implied warranty of MERCHANTABILITY or FITNESS FOR A
+# PARTICULAR PURPOSE.
+
+@SET_MAKE@
+
+VPATH = @srcdir@
+am__is_gnu_make = { \
+  if test -z '$(MAKELEVEL)'; then \
+    false; \
+  elif test -n '$(MAKE_HOST)'; then \
+    true; \
+  elif test -n '$(MAKE_VERSION)' && test -n '$(CURDIR)'; then \
+    true; \
+  else \
+    false; \
+  fi; \
+}
+am__make_running_with_option = \
+  case $${target_option-} in \
+      ?) ;; \
+      *) echo "am__make_running_with_option: internal error: invalid" \
+              "target option '$${target_option-}' specified" >&2; \
+         exit 1;; \
+  esac; \
+  has_opt=no; \
+  sane_makeflags=$$MAKEFLAGS; \
+  if $(am__is_gnu_make); then \
+    sane_makeflags=$$MFLAGS; \
+  else \
+    case $$MAKEFLAGS in \
+      *\\[\ \	]*) \
+        bs=\\; \
+        sane_makeflags=`printf '%s\n' "$$MAKEFLAGS" \
+          | sed "s/$$bs$$bs[$$bs $$bs	]*//g"`;; \
+    esac; \
+  fi; \
+  skip_next=no; \
+  strip_trailopt () \
+  { \
+    flg=`printf '%s\n' "$$flg" | sed "s/$$1.*$$//"`; \
+  }; \
+  for flg in $$sane_makeflags; do \
+    test $$skip_next = yes && { skip_next=no; continue; }; \
+    case $$flg in \
+      *=*|--*) continue;; \
+        -*I) strip_trailopt 'I'; skip_next=yes;; \
+      -*I?*) strip_trailopt 'I';; \
+        -*O) strip_trailopt 'O'; skip_next=yes;; \
+      -*O?*) strip_trailopt 'O';; \
+        -*l) strip_trailopt 'l'; skip_next=yes;; \
+      -*l?*) strip_trailopt 'l';; \
+      -[dEDm]) skip_next=yes;; \
+      -[JT]) skip_next=yes;; \
+    esac; \
+    case $$flg in \
+      *$$target_option*) has_opt=yes; break;; \
+    esac; \
+  done; \
+  test $$has_opt = yes
+am__make_dryrun = (target_option=n; $(am__make_running_with_option))
+am__make_keepgoing = (target_option=k; $(am__make_running_with_option))
+pkgdatadir = $(datadir)/@PACKAGE@
+pkgincludedir = $(includedir)/@PACKAGE@
+pkglibdir = $(libdir)/@PACKAGE@
+pkglibexecdir = $(libexecdir)/@PACKAGE@
+am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd
+install_sh_DATA = $(install_sh) -c -m 644
+install_sh_PROGRAM = $(install_sh) -c
+install_sh_SCRIPT = $(install_sh) -c
+INSTALL_HEADER = $(INSTALL_DATA)
+transform = $(program_transform_name)
+NORMAL_INSTALL = :
+PRE_INSTALL = :
+POST_INSTALL = :
+NORMAL_UNINSTALL = :
+PRE_UNINSTALL = :
+POST_UNINSTALL = :
+build_triplet = @build@
+host_triplet = @host@
+@ENABLE_OU_TRUE@am__append_1 = -I$(top_srcdir)/ou/include
+subdir = ode/src/joints
+ACLOCAL_M4 = $(top_srcdir)/aclocal.m4
+am__aclocal_m4_deps = $(top_srcdir)/m4/libtool.m4 \
+	$(top_srcdir)/m4/ltoptions.m4 $(top_srcdir)/m4/ltsugar.m4 \
+	$(top_srcdir)/m4/ltversion.m4 $(top_srcdir)/m4/lt~obsolete.m4 \
+	$(top_srcdir)/m4/pkg.m4 $(top_srcdir)/configure.ac
+am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \
+	$(ACLOCAL_M4)
+DIST_COMMON = $(srcdir)/Makefile.am $(am__DIST_COMMON)
+mkinstalldirs = $(install_sh) -d
+CONFIG_HEADER = $(top_builddir)/ode/src/config.h
+CONFIG_CLEAN_FILES =
+CONFIG_CLEAN_VPATH_FILES =
+LTLIBRARIES = $(noinst_LTLIBRARIES)
+libjoints_la_LIBADD =
+am_libjoints_la_OBJECTS = joint.lo ball.lo dball.lo dhinge.lo \
+	transmission.lo hinge.lo slider.lo contact.lo universal.lo \
+	hinge2.lo fixed.lo null.lo amotor.lo lmotor.lo plane2d.lo \
+	pu.lo pr.lo piston.lo
+libjoints_la_OBJECTS = $(am_libjoints_la_OBJECTS)
+AM_V_lt = $(am__v_lt_@AM_V@)
+am__v_lt_ = $(am__v_lt_@AM_DEFAULT_V@)
+am__v_lt_0 = --silent
+am__v_lt_1 = 
+AM_V_P = $(am__v_P_@AM_V@)
+am__v_P_ = $(am__v_P_@AM_DEFAULT_V@)
+am__v_P_0 = false
+am__v_P_1 = :
+AM_V_GEN = $(am__v_GEN_@AM_V@)
+am__v_GEN_ = $(am__v_GEN_@AM_DEFAULT_V@)
+am__v_GEN_0 = @echo "  GEN     " $@;
+am__v_GEN_1 = 
+AM_V_at = $(am__v_at_@AM_V@)
+am__v_at_ = $(am__v_at_@AM_DEFAULT_V@)
+am__v_at_0 = @
+am__v_at_1 = 
+DEFAULT_INCLUDES = -I.@am__isrc@ -I$(top_builddir)/ode/src
+depcomp = $(SHELL) $(top_srcdir)/depcomp
+am__depfiles_maybe = depfiles
+am__mv = mv -f
+CXXCOMPILE = $(CXX) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) \
+	$(AM_CPPFLAGS) $(CPPFLAGS) $(AM_CXXFLAGS) $(CXXFLAGS)
+LTCXXCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \
+	$(LIBTOOLFLAGS) --mode=compile $(CXX) $(DEFS) \
+	$(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \
+	$(AM_CXXFLAGS) $(CXXFLAGS)
+AM_V_CXX = $(am__v_CXX_@AM_V@)
+am__v_CXX_ = $(am__v_CXX_@AM_DEFAULT_V@)
+am__v_CXX_0 = @echo "  CXX     " $@;
+am__v_CXX_1 = 
+CXXLD = $(CXX)
+CXXLINK = $(LIBTOOL) $(AM_V_lt) --tag=CXX $(AM_LIBTOOLFLAGS) \
+	$(LIBTOOLFLAGS) --mode=link $(CXXLD) $(AM_CXXFLAGS) \
+	$(CXXFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@
+AM_V_CXXLD = $(am__v_CXXLD_@AM_V@)
+am__v_CXXLD_ = $(am__v_CXXLD_@AM_DEFAULT_V@)
+am__v_CXXLD_0 = @echo "  CXXLD   " $@;
+am__v_CXXLD_1 = 
+COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \
+	$(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS)
+LTCOMPILE = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \
+	$(LIBTOOLFLAGS) --mode=compile $(CC) $(DEFS) \
+	$(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) $(CPPFLAGS) \
+	$(AM_CFLAGS) $(CFLAGS)
+AM_V_CC = $(am__v_CC_@AM_V@)
+am__v_CC_ = $(am__v_CC_@AM_DEFAULT_V@)
+am__v_CC_0 = @echo "  CC      " $@;
+am__v_CC_1 = 
+CCLD = $(CC)
+LINK = $(LIBTOOL) $(AM_V_lt) --tag=CC $(AM_LIBTOOLFLAGS) \
+	$(LIBTOOLFLAGS) --mode=link $(CCLD) $(AM_CFLAGS) $(CFLAGS) \
+	$(AM_LDFLAGS) $(LDFLAGS) -o $@
+AM_V_CCLD = $(am__v_CCLD_@AM_V@)
+am__v_CCLD_ = $(am__v_CCLD_@AM_DEFAULT_V@)
+am__v_CCLD_0 = @echo "  CCLD    " $@;
+am__v_CCLD_1 = 
+SOURCES = $(libjoints_la_SOURCES)
+DIST_SOURCES = $(libjoints_la_SOURCES)
+am__can_run_installinfo = \
+  case $$AM_UPDATE_INFO_DIR in \
+    n|no|NO) false;; \
+    *) (install-info --version) >/dev/null 2>&1;; \
+  esac
+am__tagged_files = $(HEADERS) $(SOURCES) $(TAGS_FILES) $(LISP)
+# Read a list of newline-separated strings from the standard input,
+# and print each of them once, without duplicates.  Input order is
+# *not* preserved.
+am__uniquify_input = $(AWK) '\
+  BEGIN { nonempty = 0; } \
+  { items[$$0] = 1; nonempty = 1; } \
+  END { if (nonempty) { for (i in items) print i; }; } \
+'
+# Make sure the list of sources is unique.  This is necessary because,
+# e.g., the same source file might be shared among _SOURCES variables
+# for different programs/libraries.
+am__define_uniq_tagged_files = \
+  list='$(am__tagged_files)'; \
+  unique=`for i in $$list; do \
+    if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \
+  done | $(am__uniquify_input)`
+ETAGS = etags
+CTAGS = ctags
+am__DIST_COMMON = $(srcdir)/Makefile.in $(top_srcdir)/depcomp
+DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST)
+ACLOCAL = @ACLOCAL@
+ALLOCA = @ALLOCA@
+AMTAR = @AMTAR@
+AM_DEFAULT_VERBOSITY = @AM_DEFAULT_VERBOSITY@
+AR = @AR@
+AS = @AS@
+AUTOCONF = @AUTOCONF@
+AUTOHEADER = @AUTOHEADER@
+AUTOMAKE = @AUTOMAKE@
+AWK = @AWK@
+CC = @CC@
+CCDEPMODE = @CCDEPMODE@
+CCD_CFLAGS = @CCD_CFLAGS@
+CCD_LIBS = @CCD_LIBS@
+CFLAGS = @CFLAGS@
+CPP = @CPP@
+CPPFLAGS = @CPPFLAGS@
+CXX = @CXX@
+CXXCPP = @CXXCPP@
+CXXDEPMODE = @CXXDEPMODE@
+CXXFLAGS = @CXXFLAGS@
+CYGPATH_W = @CYGPATH_W@
+DEFS = @DEFS@
+DEPDIR = @DEPDIR@
+DLLTOOL = @DLLTOOL@
+DOXYGEN = @DOXYGEN@
+DSYMUTIL = @DSYMUTIL@
+DUMPBIN = @DUMPBIN@
+ECHO_C = @ECHO_C@
+ECHO_N = @ECHO_N@
+ECHO_T = @ECHO_T@
+EGREP = @EGREP@
+EXEEXT = @EXEEXT@
+EXTRA_LIBTOOL_LDFLAGS = @EXTRA_LIBTOOL_LDFLAGS@
+FGREP = @FGREP@
+GL_LIBS = @GL_LIBS@
+GREP = @GREP@
+INSTALL = @INSTALL@
+INSTALL_DATA = @INSTALL_DATA@
+INSTALL_PROGRAM = @INSTALL_PROGRAM@
+INSTALL_SCRIPT = @INSTALL_SCRIPT@
+INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@
+LD = @LD@
+LDFLAGS = @LDFLAGS@
+LIBOBJS = @LIBOBJS@
+LIBS = @LIBS@
+LIBSTDCXX = @LIBSTDCXX@
+LIBTOOL = @LIBTOOL@
+LIPO = @LIPO@
+LN_S = @LN_S@
+LTLIBOBJS = @LTLIBOBJS@
+LT_SYS_LIBRARY_PATH = @LT_SYS_LIBRARY_PATH@
+MAKEINFO = @MAKEINFO@
+MANIFEST_TOOL = @MANIFEST_TOOL@
+MKDIR_P = @MKDIR_P@
+NM = @NM@
+NMEDIT = @NMEDIT@
+OBJDUMP = @OBJDUMP@
+OBJEXT = @OBJEXT@
+ODE_PRECISION = @ODE_PRECISION@
+ODE_VERSION = @ODE_VERSION@
+ODE_VERSION_INFO = @ODE_VERSION_INFO@
+OTOOL = @OTOOL@
+OTOOL64 = @OTOOL64@
+PACKAGE = @PACKAGE@
+PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@
+PACKAGE_NAME = @PACKAGE_NAME@
+PACKAGE_STRING = @PACKAGE_STRING@
+PACKAGE_TARNAME = @PACKAGE_TARNAME@
+PACKAGE_URL = @PACKAGE_URL@
+PACKAGE_VERSION = @PACKAGE_VERSION@
+PATH_SEPARATOR = @PATH_SEPARATOR@
+PKG_CONFIG = @PKG_CONFIG@
+PKG_CONFIG_LIBDIR = @PKG_CONFIG_LIBDIR@
+PKG_CONFIG_PATH = @PKG_CONFIG_PATH@
+RANLIB = @RANLIB@
+SED = @SED@
+SET_MAKE = @SET_MAKE@
+SHELL = @SHELL@
+STRIP = @STRIP@
+VERSION = @VERSION@
+WINDRES = @WINDRES@
+X11_CFLAGS = @X11_CFLAGS@
+X11_LIBS = @X11_LIBS@
+abs_builddir = @abs_builddir@
+abs_srcdir = @abs_srcdir@
+abs_top_builddir = @abs_top_builddir@
+abs_top_srcdir = @abs_top_srcdir@
+ac_ct_AR = @ac_ct_AR@
+ac_ct_CC = @ac_ct_CC@
+ac_ct_CXX = @ac_ct_CXX@
+ac_ct_DUMPBIN = @ac_ct_DUMPBIN@
+ac_ct_WINDRES = @ac_ct_WINDRES@
+am__include = @am__include@
+am__leading_dot = @am__leading_dot@
+am__quote = @am__quote@
+am__tar = @am__tar@
+am__untar = @am__untar@
+bindir = @bindir@
+build = @build@
+build_alias = @build_alias@
+build_cpu = @build_cpu@
+build_os = @build_os@
+build_vendor = @build_vendor@
+builddir = @builddir@
+datadir = @datadir@
+datarootdir = @datarootdir@
+docdir = @docdir@
+dvidir = @dvidir@
+exec_prefix = @exec_prefix@
+host = @host@
+host_alias = @host_alias@
+host_cpu = @host_cpu@
+host_os = @host_os@
+host_vendor = @host_vendor@
+htmldir = @htmldir@
+includedir = @includedir@
+infodir = @infodir@
+install_sh = @install_sh@
+libdir = @libdir@
+libexecdir = @libexecdir@
+localedir = @localedir@
+localstatedir = @localstatedir@
+mandir = @mandir@
+mkdir_p = @mkdir_p@
+oldincludedir = @oldincludedir@
+pdfdir = @pdfdir@
+prefix = @prefix@
+program_transform_name = @program_transform_name@
+psdir = @psdir@
+runstatedir = @runstatedir@
+sbindir = @sbindir@
+sharedstatedir = @sharedstatedir@
+srcdir = @srcdir@
+subdirs = @subdirs@
+sysconfdir = @sysconfdir@
+target_alias = @target_alias@
+top_build_prefix = @top_build_prefix@
+top_builddir = @top_builddir@
+top_srcdir = @top_srcdir@
+AM_CPPFLAGS = -I$(top_srcdir)/include -I$(top_builddir)/include \
+	-I$(top_srcdir)/ode/src -D__ODE__ $(am__append_1)
+noinst_LTLIBRARIES = libjoints.la
+libjoints_la_SOURCES = joints.h \
+                        joint.h joint.cpp \
+                        joint_internal.h \
+                        ball.h ball.cpp \
+                        dball.h dball.cpp \
+                        dhinge.h dhinge.cpp \
+                        transmission.h transmission.cpp \
+                        hinge.h hinge.cpp \
+                        slider.h slider.cpp \
+                        contact.h contact.cpp \
+                        universal.h universal.cpp \
+                        hinge2.h hinge2.cpp \
+                        fixed.h fixed.cpp \
+                        null.h null.cpp \
+                        amotor.h amotor.cpp \
+                        lmotor.h lmotor.cpp \
+                        plane2d.h plane2d.cpp \
+                        pu.h pu.cpp \
+                        pr.h pr.cpp \
+                        piston.h piston.cpp
+
+all: all-am
+
+.SUFFIXES:
+.SUFFIXES: .cpp .lo .o .obj
+$(srcdir)/Makefile.in:  $(srcdir)/Makefile.am  $(am__configure_deps)
+	@for dep in $?; do \
+	  case '$(am__configure_deps)' in \
+	    *$$dep*) \
+	      ( cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh ) \
+	        && { if test -f $@; then exit 0; else break; fi; }; \
+	      exit 1;; \
+	  esac; \
+	done; \
+	echo ' cd $(top_srcdir) && $(AUTOMAKE) --foreign ode/src/joints/Makefile'; \
+	$(am__cd) $(top_srcdir) && \
+	  $(AUTOMAKE) --foreign ode/src/joints/Makefile
+Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status
+	@case '$?' in \
+	  *config.status*) \
+	    cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \
+	  *) \
+	    echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \
+	    cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \
+	esac;
+
+$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES)
+	cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
+
+$(top_srcdir)/configure:  $(am__configure_deps)
+	cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
+$(ACLOCAL_M4):  $(am__aclocal_m4_deps)
+	cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh
+$(am__aclocal_m4_deps):
+
+clean-noinstLTLIBRARIES:
+	-test -z "$(noinst_LTLIBRARIES)" || rm -f $(noinst_LTLIBRARIES)
+	@list='$(noinst_LTLIBRARIES)'; \
+	locs=`for p in $$list; do echo $$p; done | \
+	      sed 's|^[^/]*$$|.|; s|/[^/]*$$||; s|$$|/so_locations|' | \
+	      sort -u`; \
+	test -z "$$locs" || { \
+	  echo rm -f $${locs}; \
+	  rm -f $${locs}; \
+	}
+
+libjoints.la: $(libjoints_la_OBJECTS) $(libjoints_la_DEPENDENCIES) $(EXTRA_libjoints_la_DEPENDENCIES) 
+	$(AM_V_CXXLD)$(CXXLINK)  $(libjoints_la_OBJECTS) $(libjoints_la_LIBADD) $(LIBS)
+
+mostlyclean-compile:
+	-rm -f *.$(OBJEXT)
+
+distclean-compile:
+	-rm -f *.tab.c
+
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/amotor.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/ball.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/contact.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/dball.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/dhinge.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/fixed.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/hinge.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/hinge2.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/joint.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/lmotor.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/null.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/piston.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/plane2d.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/pr.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/pu.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/slider.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/transmission.Plo@am__quote@
+@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/universal.Plo@am__quote@
+
+.cpp.o:
+@am__fastdepCXX_TRUE@	$(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
+@am__fastdepCXX_TRUE@	$(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	$(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
+@am__fastdepCXX_FALSE@	$(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ $<
+
+.cpp.obj:
+@am__fastdepCXX_TRUE@	$(AM_V_CXX)$(CXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'`
+@am__fastdepCXX_TRUE@	$(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	$(AM_V_CXX)source='$<' object='$@' libtool=no @AMDEPBACKSLASH@
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
+@am__fastdepCXX_FALSE@	$(AM_V_CXX@am__nodep@)$(CXXCOMPILE) -c -o $@ `$(CYGPATH_W) '$<'`
+
+.cpp.lo:
+@am__fastdepCXX_TRUE@	$(AM_V_CXX)$(LTCXXCOMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $<
+@am__fastdepCXX_TRUE@	$(AM_V_at)$(am__mv) $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Plo
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	$(AM_V_CXX)source='$<' object='$@' libtool=yes @AMDEPBACKSLASH@
+@AMDEP_TRUE@@am__fastdepCXX_FALSE@	DEPDIR=$(DEPDIR) $(CXXDEPMODE) $(depcomp) @AMDEPBACKSLASH@
+@am__fastdepCXX_FALSE@	$(AM_V_CXX@am__nodep@)$(LTCXXCOMPILE) -c -o $@ $<
+
+mostlyclean-libtool:
+	-rm -f *.lo
+
+clean-libtool:
+	-rm -rf .libs _libs
+
+ID: $(am__tagged_files)
+	$(am__define_uniq_tagged_files); mkid -fID $$unique
+tags: tags-am
+TAGS: tags
+
+tags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files)
+	set x; \
+	here=`pwd`; \
+	$(am__define_uniq_tagged_files); \
+	shift; \
+	if test -z "$(ETAGS_ARGS)$$*$$unique"; then :; else \
+	  test -n "$$unique" || unique=$$empty_fix; \
+	  if test $$# -gt 0; then \
+	    $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \
+	      "$$@" $$unique; \
+	  else \
+	    $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \
+	      $$unique; \
+	  fi; \
+	fi
+ctags: ctags-am
+
+CTAGS: ctags
+ctags-am: $(TAGS_DEPENDENCIES) $(am__tagged_files)
+	$(am__define_uniq_tagged_files); \
+	test -z "$(CTAGS_ARGS)$$unique" \
+	  || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \
+	     $$unique
+
+GTAGS:
+	here=`$(am__cd) $(top_builddir) && pwd` \
+	  && $(am__cd) $(top_srcdir) \
+	  && gtags -i $(GTAGS_ARGS) "$$here"
+cscopelist: cscopelist-am
+
+cscopelist-am: $(am__tagged_files)
+	list='$(am__tagged_files)'; \
+	case "$(srcdir)" in \
+	  [\\/]* | ?:[\\/]*) sdir="$(srcdir)" ;; \
+	  *) sdir=$(subdir)/$(srcdir) ;; \
+	esac; \
+	for i in $$list; do \
+	  if test -f "$$i"; then \
+	    echo "$(subdir)/$$i"; \
+	  else \
+	    echo "$$sdir/$$i"; \
+	  fi; \
+	done >> $(top_builddir)/cscope.files
+
+distclean-tags:
+	-rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags
+
+distdir: $(DISTFILES)
+	@srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
+	topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \
+	list='$(DISTFILES)'; \
+	  dist_files=`for file in $$list; do echo $$file; done | \
+	  sed -e "s|^$$srcdirstrip/||;t" \
+	      -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \
+	case $$dist_files in \
+	  */*) $(MKDIR_P) `echo "$$dist_files" | \
+			   sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \
+			   sort -u` ;; \
+	esac; \
+	for file in $$dist_files; do \
+	  if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \
+	  if test -d $$d/$$file; then \
+	    dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \
+	    if test -d "$(distdir)/$$file"; then \
+	      find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \
+	    fi; \
+	    if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \
+	      cp -fpR $(srcdir)/$$file "$(distdir)$$dir" || exit 1; \
+	      find "$(distdir)/$$file" -type d ! -perm -700 -exec chmod u+rwx {} \;; \
+	    fi; \
+	    cp -fpR $$d/$$file "$(distdir)$$dir" || exit 1; \
+	  else \
+	    test -f "$(distdir)/$$file" \
+	    || cp -p $$d/$$file "$(distdir)/$$file" \
+	    || exit 1; \
+	  fi; \
+	done
+check-am: all-am
+check: check-am
+all-am: Makefile $(LTLIBRARIES)
+installdirs:
+install: install-am
+install-exec: install-exec-am
+install-data: install-data-am
+uninstall: uninstall-am
+
+install-am: all-am
+	@$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am
+
+installcheck: installcheck-am
+install-strip:
+	if test -z '$(STRIP)'; then \
+	  $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \
+	    install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \
+	      install; \
+	else \
+	  $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \
+	    install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \
+	    "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'" install; \
+	fi
+mostlyclean-generic:
+
+clean-generic:
+
+distclean-generic:
+	-test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES)
+	-test . = "$(srcdir)" || test -z "$(CONFIG_CLEAN_VPATH_FILES)" || rm -f $(CONFIG_CLEAN_VPATH_FILES)
+
+maintainer-clean-generic:
+	@echo "This command is intended for maintainers to use"
+	@echo "it deletes files that may require special tools to rebuild."
+clean: clean-am
+
+clean-am: clean-generic clean-libtool clean-noinstLTLIBRARIES \
+	mostlyclean-am
+
+distclean: distclean-am
+	-rm -rf ./$(DEPDIR)
+	-rm -f Makefile
+distclean-am: clean-am distclean-compile distclean-generic \
+	distclean-tags
+
+dvi: dvi-am
+
+dvi-am:
+
+html: html-am
+
+html-am:
+
+info: info-am
+
+info-am:
+
+install-data-am:
+
+install-dvi: install-dvi-am
+
+install-dvi-am:
+
+install-exec-am:
+
+install-html: install-html-am
+
+install-html-am:
+
+install-info: install-info-am
+
+install-info-am:
+
+install-man:
+
+install-pdf: install-pdf-am
+
+install-pdf-am:
+
+install-ps: install-ps-am
+
+install-ps-am:
+
+installcheck-am:
+
+maintainer-clean: maintainer-clean-am
+	-rm -rf ./$(DEPDIR)
+	-rm -f Makefile
+maintainer-clean-am: distclean-am maintainer-clean-generic
+
+mostlyclean: mostlyclean-am
+
+mostlyclean-am: mostlyclean-compile mostlyclean-generic \
+	mostlyclean-libtool
+
+pdf: pdf-am
+
+pdf-am:
+
+ps: ps-am
+
+ps-am:
+
+uninstall-am:
+
+.MAKE: install-am install-strip
+
+.PHONY: CTAGS GTAGS TAGS all all-am check check-am clean clean-generic \
+	clean-libtool clean-noinstLTLIBRARIES cscopelist-am ctags \
+	ctags-am distclean distclean-compile distclean-generic \
+	distclean-libtool distclean-tags distdir dvi dvi-am html \
+	html-am info info-am install install-am install-data \
+	install-data-am install-dvi install-dvi-am install-exec \
+	install-exec-am install-html install-html-am install-info \
+	install-info-am install-man install-pdf install-pdf-am \
+	install-ps install-ps-am install-strip installcheck \
+	installcheck-am installdirs maintainer-clean \
+	maintainer-clean-generic mostlyclean mostlyclean-compile \
+	mostlyclean-generic mostlyclean-libtool pdf pdf-am ps ps-am \
+	tags tags-am uninstall uninstall-am
+
+.PRECIOUS: Makefile
+
+
+# Tell versions [3.59,3.63) of GNU make to not export all variables.
+# Otherwise a system limit (for SysV at least) may be exceeded.
+.NOEXPORT:

thirdparty/ode-0.16.5/ode/src/joints/amotor.cpp

@@ -0,0 +1,810 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "common.h"
+#include "amotor.h"
+#include "joint_internal.h"
+#include "odeou.h"
+
+
+/*extern */
+void dJointSetAMotorNumAxes(dJointID j, int num)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1));
+    checktype(joint, AMotor);
+
+    num = dCLAMP(num, dSA__MIN, dSA__MAX);
+
+    joint->setNumAxes(num);
+}
+
+/*extern */
+void dJointSetAMotorAxis(dJointID j, int anum, int rel/*=dJointBodyRelativity*/, 
+    dReal x, dReal y, dReal z)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    dAASSERT(dIN_RANGE(rel, dJBR__MIN, dJBR__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    joint->setAxisValue(anum, (dJointBodyRelativity)rel, x, y, z);
+}
+
+/*extern */
+void dJointSetAMotorAngle(dJointID j, int anum, dReal angle)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    joint->setAngleValue(anum, angle);
+}
+
+/*extern */
+void dJointSetAMotorParam(dJointID j, int parameter, dReal value)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    int anum = parameter >> 8;
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    int limotParam = parameter & 0xff;
+    joint->setLimotParameter(anum, limotParam, value);
+}
+
+/*extern */
+void dJointSetAMotorMode(dJointID j, int mode)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    joint->setOperationMode(mode);
+}
+
+/*extern */
+int dJointGetAMotorNumAxes(dJointID j)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    return joint->getNumAxes();
+}
+
+/*extern */
+void dJointGetAMotorAxis(dJointID j, int anum, dVector3 result)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    joint->getAxisValue(result, anum);
+}
+
+/*extern */
+int dJointGetAMotorAxisRel(dJointID j, int anum)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    int result = joint->getAxisBodyRelativity(anum);
+    return result;
+}
+
+/*extern */
+dReal dJointGetAMotorAngle(dJointID j, int anum)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    dReal result = joint->getAngleValue(anum);
+    return result;
+}
+
+/*extern */
+dReal dJointGetAMotorAngleRate(dJointID j, int anum)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    checktype(joint, AMotor);
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    dReal result = joint->calculateAngleRate(anum);
+    return result;
+}
+
+/*extern */
+dReal dJointGetAMotorParam(dJointID j, int parameter)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    int anum = parameter >> 8;
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+    anum = dCLAMP(anum, dSA__MIN, dSA__MAX - 1);
+
+    int limotParam = parameter & 0xff;
+    dReal result = joint->getLimotParameter(anum, limotParam);
+    return result;
+}
+
+/*extern */
+int dJointGetAMotorMode(dJointID j)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    int result = joint->getOperationMode();
+    return result;
+}
+
+/*extern */
+void dJointAddAMotorTorques(dJointID j, dReal torque1, dReal torque2, dReal torque3)
+{
+    dxJointAMotor* joint = (dxJointAMotor*)j;
+    dAASSERT(joint != NULL);
+    checktype(joint, AMotor);
+
+    joint->addTorques(torque1, torque2, torque3);
+}
+
+
+//****************************************************************************
+
+BEGIN_NAMESPACE_OU();
+template<>
+const dJointBodyRelativity CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5>::m_aetElementArray[] =
+{
+    dJBR_BODY1, // dSA_X,
+    dJBR_GLOBAL, // dSA_Y,
+    dJBR_BODY2, // dSA_Z,
+};
+END_NAMESPACE_OU();
+static const CEnumUnsortedElementArray<dSpaceAxis, dSA__MAX, dJointBodyRelativity, 0x160703D5> g_abrEulerAxisAllowedBodyRelativities;
+
+static inline 
+dSpaceAxis EncodeJointConnectedBodyEulerAxis(dJointConnectedBody cbBodyIndex)
+{
+    dSASSERT(dJCB__MAX == 2); 
+    
+    return cbBodyIndex == dJCB_FIRST_BODY ? dSA_X : dSA_Z;
+}
+
+static inline 
+dSpaceAxis EncodeOtherEulerAxis(dSpaceAxis saOneAxis)
+{
+    dIASSERT(saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_FIRST_BODY) || saOneAxis == EncodeJointConnectedBodyEulerAxis(dJCB_SECOND_BODY)); 
+    dSASSERT(dJCB__MAX == 2); 
+    
+    return (dSpaceAxis)(dSA_X + dSA_Z - saOneAxis);
+}
+
+
+//****************************************************************************
+// angular motor
+
+dxJointAMotor::dxJointAMotor(dxWorld *w) :
+    dxJointAMotor_Parent(w),
+    m_mode(dAMotorUser),
+    m_num(0)
+{
+    std::fill(m_rel, m_rel + dARRAY_SIZE(m_rel), dJBR__DEFAULT);
+    { for (int i = 0; i != dARRAY_SIZE(m_axis); ++i) { dZeroVector3(m_axis[i]); } }
+    { for (int i = 0; i != dARRAY_SIZE(m_references); ++i) { dZeroVector3(m_references[i]); } }
+    std::fill(m_angle, m_angle + dARRAY_SIZE(m_angle), REAL(0.0));
+    { for (int i = 0; i != dARRAY_SIZE(m_limot); ++i) { m_limot[i].init(w); } }
+}
+
+
+/*virtual */
+dxJointAMotor::~dxJointAMotor()
+{
+    // The virtual destructor
+}
+
+
+/*virtual */
+void dxJointAMotor::getSureMaxInfo(SureMaxInfo* info)
+{
+    info->max_m = m_num;
+}
+
+/*virtual */
+void dxJointAMotor::getInfo1(dxJoint::Info1 *info)
+{
+    info->m = 0;
+    info->nub = 0;
+
+    // compute the axes and angles, if in Euler mode
+    if (m_mode == dAMotorEuler)
+    {
+        dVector3 ax[dSA__MAX];
+        computeGlobalAxes(ax);
+        computeEulerAngles(ax);
+    }
+
+    // see if we're powered or at a joint limit for each axis
+    const unsigned num = m_num;
+    for (unsigned i = 0; i != num; ++i)
+    {
+        if (m_limot[i].testRotationalLimit(m_angle[i]) 
+            || m_limot[i].fmax > 0)
+        {
+            info->m++;
+        }
+    }
+}
+
+/*virtual */
+void dxJointAMotor::getInfo2(dReal worldFPS, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex)
+{
+    // compute the axes (if not global)
+    dVector3 ax[dSA__MAX];
+    computeGlobalAxes(ax);
+
+    // in Euler angle mode we do not actually constrain the angular velocity
+    // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+    //
+    //    to get   constrain w2-w1 along  ...not
+    //    ------   ---------------------  ------
+    //    d(angle[0])/dt = 0 ax[1] x ax[2]   ax[0]
+    //    d(angle[1])/dt = 0 ax[1]
+    //    d(angle[2])/dt = 0 ax[0] x ax[1]   ax[2]
+    //
+    // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+    // to prove the result for angle[0], write the expression for angle[0] from
+    // GetInfo1 then take the derivative. to prove this for angle[2] it is
+    // easier to take the Euler rate expression for d(angle[2])/dt with respect
+    // to the components of w and set that to 0.
+
+    dVector3 *axptr[dSA__MAX];
+    for (int j = dSA__MIN; j != dSA__MAX; ++j) { axptr[j] = &ax[j]; }
+
+    dVector3 ax0_cross_ax1;
+    dVector3 ax1_cross_ax2;
+    
+    if (m_mode == dAMotorEuler) 
+    {
+        dCalcVectorCross3(ax0_cross_ax1, ax[dSA_X], ax[dSA_Y]);
+        axptr[dSA_Z] = &ax0_cross_ax1;
+        dCalcVectorCross3(ax1_cross_ax2, ax[dSA_Y], ax[dSA_Z]);
+        axptr[dSA_X] = &ax1_cross_ax2;
+    }
+
+    sizeint rowTotalSkip = 0, pairTotalSkip = 0;
+    
+    const unsigned num = m_num;
+    for (unsigned i = 0; i != num; ++i) 
+    {
+        if (m_limot[i].addLimot(this, worldFPS, J1 + rowTotalSkip, J2 + rowTotalSkip, pairRhsCfm + pairTotalSkip, pairLoHi + pairTotalSkip, *(axptr[i]), 1)) 
+        {
+            rowTotalSkip += rowskip;
+            pairTotalSkip += pairskip;
+        }
+    }
+}
+
+/*virtual */
+dJointType dxJointAMotor::type() const
+{
+    return dJointTypeAMotor;
+}
+
+/*virtual */
+sizeint dxJointAMotor::size() const
+{
+    return sizeof(*this);
+}
+
+
+void dxJointAMotor::setOperationMode(int mode)
+{
+    m_mode = mode;
+
+    if (mode == dAMotorEuler)
+    {
+        m_num = dSA__MAX;
+        setEulerReferenceVectors();
+    }
+}
+
+
+void dxJointAMotor::setNumAxes(unsigned num)
+{
+    if (m_mode == dAMotorEuler)
+    {
+        m_num = dSA__MAX;
+    }
+    else
+    {
+        m_num = num;
+    }
+}
+
+
+dJointBodyRelativity dxJointAMotor::getAxisBodyRelativity(unsigned anum) const
+{
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+    dJointBodyRelativity rel = m_rel[anum];
+    if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse())
+    {
+        rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2 into 1
+    }
+
+    return rel;
+}
+
+
+void dxJointAMotor::setAxisValue(unsigned anum, dJointBodyRelativity rel, 
+    dReal x, dReal y, dReal z)
+{
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    dAASSERT(m_mode != dAMotorEuler || !dJBREncodeBodyRelativityStatus(rel) || rel == g_abrEulerAxisAllowedBodyRelativities.Encode((dSpaceAxis)anum));
+
+    // x,y,z is always in global coordinates regardless of rel, so we may have
+    // to convert it to be relative to a body
+    dVector3 r;
+    dAssignVector3(r, x, y, z);
+
+    // adjust rel to match the internal body order
+    if (dJBREncodeBodyRelativityStatus(rel) && GetIsJointReverse())
+    {
+        rel = dJBRSwapBodyRelativity(rel); // turns 1 into 2, 2, into 1
+    }
+
+    m_rel[anum] = rel;
+
+    bool assigned = false;
+
+    if (dJBREncodeBodyRelativityStatus(rel))
+    {
+        if (rel == dJBR_BODY1)
+        {
+            dMultiply1_331(m_axis[anum], this->node[0].body->posr.R, r);
+            assigned = true;
+        }
+        // rel == 2
+        else if (this->node[1].body != NULL)
+        {
+            dIASSERT(rel == dJBR_BODY2);
+
+            dMultiply1_331(m_axis[anum], this->node[1].body->posr.R, r);
+            assigned = true;
+        }
+    }
+    
+    if (!assigned)
+    {
+        dCopyVector3(m_axis[anum], r); 
+    }
+    
+    dNormalize3(m_axis[anum]);
+    
+    if (m_mode == dAMotorEuler) 
+    {
+        setEulerReferenceVectors();
+    }
+}
+
+void dxJointAMotor::getAxisValue(dVector3 result, unsigned anum) const
+{
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+
+    switch (m_mode)
+    {
+        case dAMotorUser:
+        {
+            doGetUserAxis(result, anum);
+            break;
+        }
+
+        case dAMotorEuler:
+        {
+            doGetEulerAxis(result, anum);
+            break;
+        }
+
+        default:
+        {
+            dIASSERT(false);
+            break;
+        }
+    } 
+}
+
+
+void dxJointAMotor::doGetUserAxis(dVector3 result, unsigned anum) const
+{
+    bool retrieved = false;
+
+    if (dJBREncodeBodyRelativityStatus(m_rel[anum])) 
+    {
+        if (m_rel[anum] == dJBR_BODY1)
+        {
+            dMultiply0_331(result, this->node[0].body->posr.R, m_axis[anum]);
+            retrieved = true;
+        }
+        else if (this->node[1].body != NULL)
+        {
+            dMultiply0_331(result, this->node[1].body->posr.R, m_axis[anum]);
+            retrieved = true;
+        }
+    }
+
+    if (!retrieved)
+    {
+        dCopyVector3(result, m_axis[anum]);
+    }
+}
+
+void dxJointAMotor::doGetEulerAxis(dVector3 result, unsigned anum) const
+{
+    // If we're in Euler mode, joint->axis[1] doesn't
+    // have anything sensible in it.  So don't just return
+    // that, find the actual effective axis.
+    // Likewise, the actual axis of rotation for the
+    // the other axes is different from what's stored.
+    dVector3 axes[dSA__MAX];
+    computeGlobalAxes(axes);
+
+    if (anum == dSA_Y) 
+    {
+        dCopyVector3(result, axes[dSA_Y]);
+    } 
+    else if (anum < dSA_Y) // Comparing against the same constant lets compiler reuse EFLAGS register for another conditional jump
+    {
+        dSASSERT(dSA_X < dSA_Y); // Otherwise the condition above is incorrect
+        dIASSERT(anum == dSA_X);
+
+        // This won't be unit length in general,
+        // but it's what's used in getInfo2
+        // This may be why things freak out as
+        // the body-relative axes get close to each other.
+        dCalcVectorCross3(result, axes[dSA_Y], axes[dSA_Z]);
+    } 
+    else 
+    {
+        dSASSERT(dSA_Z > dSA_Y); // Otherwise the condition above is incorrect
+        dIASSERT(anum == dSA_Z);
+
+        // Same problem as above.
+        dCalcVectorCross3(result, axes[dSA_X], axes[dSA_Y]);
+    }
+}
+
+
+void dxJointAMotor::setAngleValue(unsigned anum, dReal angle)
+{
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    dAASSERT(m_mode == dAMotorUser); // This only works for the dAMotorUser
+
+    if (m_mode == dAMotorUser)
+    {
+        m_angle[anum] = angle;
+    }
+}
+
+
+dReal dxJointAMotor::calculateAngleRate(unsigned anum) const
+{
+    dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX));
+    dAASSERT(this->node[0].body != NULL); // Don't call for angle rate before the joint is set up
+
+    dVector3 axis;
+    getAxisValue(axis, anum);
+
+    // NOTE!
+    // For reverse joints, the rate is negated at the function exit to create swapped bodies effect
+    dReal rate = dDOT(axis, this->node[0].body->avel);
+
+    if (this->node[1].body != NULL) 
+    {
+        rate -= dDOT(axis, this->node[1].body->avel);
+    }
+
+    // Negating the rate for reverse joints creates an effect of body swapping
+    dReal result = !GetIsJointReverse() ? rate : -rate;
+    return result;
+}
+
+
+void dxJointAMotor::addTorques(dReal torque1, dReal torque2, dReal torque3)
+{
+    unsigned num = getNumAxes();
+    dAASSERT(dIN_RANGE(num, dSA__MIN, dSA__MAX + 1));
+
+    dVector3 sum;
+    dVector3 torqueVector;
+    dVector3 axes[dSA__MAX];
+
+
+    if (num != dSA__MIN)
+    {
+        computeGlobalAxes(axes);
+
+        if (!GetIsJointReverse())
+        {
+            dAssignVector3(torqueVector, torque1, torque2, torque3);
+        }
+        else
+        {
+            // Negating torques creates an effect of swapped bodies later
+            dAssignVector3(torqueVector, -torque1, -torque2, -torque3);
+        }
+    }
+
+    switch (num)
+    {
+        case dSA_Z + 1:
+        {
+            dAddThreeScaledVectors3(sum, axes[dSA_Z], axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Z], torqueVector[dSA_Y], torqueVector[dSA_X]);
+            break;
+        }
+
+        case dSA_Y + 1:
+        {
+            dAddScaledVectors3(sum, axes[dSA_Y], axes[dSA_X], torqueVector[dSA_Y], torqueVector[dSA_X]);
+            break;
+        }
+
+        case dSA_X + 1:
+        {
+            dCopyScaledVector3(sum, axes[dSA_X], torqueVector[dSA_X]);
+            break;
+        }
+        
+        default:
+        {
+            dSASSERT(dSA_Z > dSA_Y); // Otherwise the addends order needs to be switched
+            dSASSERT(dSA_Y > dSA_X);
+            
+            // Do nothing
+            break;
+        }
+    }
+
+    if (num != dSA__MIN)
+    {
+        dAASSERT(this->node[0].body != NULL); // Don't add torques unless you set the joint up first!
+
+        // NOTE!
+        // For reverse joints, the torqueVector negated at function entry produces the effect of swapped bodies
+        dBodyAddTorque(this->node[0].body, sum[dV3E_X], sum[dV3E_Y], sum[dV3E_Z]);
+        
+        if (this->node[1].body != NULL)
+        {
+            dBodyAddTorque(this->node[1].body, -sum[dV3E_X], -sum[dV3E_Y], -sum[dV3E_Z]);
+        }
+    }
+}
+
+
+// compute the 3 axes in global coordinates
+void dxJointAMotor::computeGlobalAxes(dVector3 ax[dSA__MAX]) const
+{
+    switch (m_mode)
+    {
+        case dAMotorUser:
+        {
+            doComputeGlobalUserAxes(ax);
+            break;
+        }
+
+        case dAMotorEuler:
+        {
+            doComputeGlobalEulerAxes(ax);
+            break;
+        }
+
+        default:
+        {
+            dIASSERT(false);
+            break;
+        }
+    } 
+}
+
+void dxJointAMotor::doComputeGlobalUserAxes(dVector3 ax[dSA__MAX]) const
+{
+    unsigned num = m_num;
+    for (unsigned i = 0; i != num; ++i)
+    {
+        bool assigned = false;
+
+        if (m_rel[i] == dJBR_BODY1)
+        {
+            // relative to b1
+            dMultiply0_331(ax[i], this->node[0].body->posr.R, m_axis[i]);
+            assigned = true;
+        }
+        else if (m_rel[i] == dJBR_BODY2)
+        {
+            // relative to b2
+            if (this->node[1].body != NULL)
+            {
+                dMultiply0_331(ax[i], this->node[1].body->posr.R, m_axis[i]);
+                assigned = true;
+            }
+        }
+
+        if (!assigned)
+        {
+            // global - just copy it
+            dCopyVector3(ax[i], m_axis[i]);
+        }
+    }
+}
+
+void dxJointAMotor::doComputeGlobalEulerAxes(dVector3 ax[dSA__MAX]) const
+{
+    // special handling for Euler mode
+    
+    dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+    dMultiply0_331(ax[firstBodyAxis], this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+
+    dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+
+    if (this->node[1].body != NULL)
+    {
+        dMultiply0_331(ax[secondBodyAxis], this->node[1].body->posr.R, m_axis[secondBodyAxis]);
+    }
+    else
+    {
+        dCopyVector3(ax[secondBodyAxis], m_axis[secondBodyAxis]);
+    }
+
+    dCalcVectorCross3(ax[dSA_Y], ax[dSA_Z], ax[dSA_X]);
+    dNormalize3(ax[dSA_Y]);
+}
+
+
+void dxJointAMotor::computeEulerAngles(dVector3 ax[dSA__MAX])
+{
+    // assumptions:
+    //   global axes already calculated --> ax
+    //   axis[0] is relative to body 1 --> global ax[0]
+    //   axis[2] is relative to body 2 --> global ax[2]
+    //   ax[1] = ax[2] x ax[0]
+    //   original ax[0] and ax[2] are perpendicular
+    //   reference1 is perpendicular to ax[0] (in body 1 frame)
+    //   reference2 is perpendicular to ax[2] (in body 2 frame)
+    //   all ax[] and reference vectors are unit length
+
+    // calculate references in global frame
+    dVector3 refs[dJCB__MAX];
+    dMultiply0_331(refs[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_references[dJCB_FIRST_BODY]);
+
+    if (this->node[1].body != NULL)
+    {
+        dMultiply0_331(refs[dJCB_SECOND_BODY], this->node[1].body->posr.R, m_references[dJCB_SECOND_BODY]);
+    }
+    else
+    {
+        dCopyVector3(refs[dJCB_SECOND_BODY], m_references[dJCB_SECOND_BODY]);
+    }
+
+
+    // get q perpendicular to both ax[0] and ref1, get first euler angle
+    dVector3 q;
+    dJointConnectedBody firstAxisBody = BuildFirstEulerAxisBody();
+
+    dCalcVectorCross3(q, ax[dSA_X], refs[firstAxisBody]);
+    m_angle[dSA_X] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], q), dCalcVectorDot3(ax[dSA_Z], refs[firstAxisBody]));
+
+    // get q perpendicular to both ax[0] and ax[1], get second euler angle
+    dCalcVectorCross3(q, ax[dSA_X], ax[dSA_Y]);
+    m_angle[dSA_Y] = -dAtan2(dCalcVectorDot3(ax[dSA_Z], ax[dSA_X]), dCalcVectorDot3(ax[dSA_Z], q));
+
+    dJointConnectedBody secondAxisBody = EncodeJointOtherConnectedBody(firstAxisBody);
+
+    // get q perpendicular to both ax[1] and ax[2], get third euler angle
+    dCalcVectorCross3(q, ax[dSA_Y], ax[dSA_Z]);
+    m_angle[dSA_Z] = -dAtan2(dCalcVectorDot3(refs[secondAxisBody], ax[dSA_Y]), dCalcVectorDot3(refs[secondAxisBody], q));
+}
+
+
+// set the reference vectors as follows:
+//   * reference1 = current axis[2] relative to body 1
+//   * reference2 = current axis[0] relative to body 2
+// this assumes that:
+//    * axis[0] is relative to body 1
+//    * axis[2] is relative to body 2
+
+void dxJointAMotor::setEulerReferenceVectors()
+{
+    if (/*this->node[0].body != NULL && */this->node[1].body != NULL)
+    {
+        dIASSERT(this->node[0].body != NULL);
+
+        dVector3 r;  // axis[2] and axis[0] in global coordinates
+
+        dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+        dMultiply0_331(r, this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+        dMultiply1_331(m_references[dJCB_SECOND_BODY], this->node[1].body->posr.R, r);
+
+        dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+        dMultiply0_331(r, this->node[1].body->posr.R, m_axis[secondBodyAxis]);
+        dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, r);
+    } 
+    else 
+    {
+        // We want to handle angular motors attached to passive geoms
+        // Replace missing node.R with identity
+        if (this->node[0].body != NULL) 
+        {
+            dSpaceAxis firstBodyAxis = BuildFirstBodyEulerAxis();
+            dMultiply0_331(m_references[dJCB_SECOND_BODY], this->node[0].body->posr.R, m_axis[firstBodyAxis]);
+
+            dSpaceAxis secondBodyAxis = EncodeOtherEulerAxis(firstBodyAxis);
+            dMultiply1_331(m_references[dJCB_FIRST_BODY], this->node[0].body->posr.R, m_axis[secondBodyAxis]);
+        } 
+    }
+}
+
+/*inline */
+dSpaceAxis dxJointAMotor::BuildFirstBodyEulerAxis() const
+{
+    return EncodeJointConnectedBodyEulerAxis(BuildFirstEulerAxisBody());
+}
+
+/*inline */
+dJointConnectedBody dxJointAMotor::BuildFirstEulerAxisBody() const
+{
+    return !GetIsJointReverse() ? dJCB_FIRST_BODY : dJCB_SECOND_BODY;
+}
+

thirdparty/ode-0.16.5/ode/src/joints/amotor.h

@@ -0,0 +1,105 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_AMOTOR_H_
+#define _ODE_JOINT_AMOTOR_H_
+
+#include "joint.h"
+
+
+// angular motor
+
+typedef dxJoint dxJointAMotor_Parent;
+class dxJointAMotor:
+    public dxJointAMotor_Parent
+{
+public:
+    dxJointAMotor(dxWorld *w);
+    virtual ~dxJointAMotor();
+
+public:
+    virtual void getSureMaxInfo(SureMaxInfo* info);
+    virtual void getInfo1(Info1* info);
+    virtual void getInfo2(dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex);
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+public:
+    void setOperationMode(int mode);
+    int getOperationMode() const { return m_mode; }
+
+    void setNumAxes(unsigned num);
+    int getNumAxes() const { return m_num; }
+
+    dJointBodyRelativity getAxisBodyRelativity(unsigned anum) const;
+
+    void setAxisValue(unsigned anum, dJointBodyRelativity rel, dReal x, dReal y, dReal z);
+    void getAxisValue(dVector3 result, unsigned anum) const;
+
+private:
+    void doGetUserAxis(dVector3 result, unsigned anum) const;
+    void doGetEulerAxis(dVector3 result, unsigned anum) const;
+
+public:
+    void setAngleValue(unsigned anum, dReal angle);
+    dReal getAngleValue(unsigned anum) const { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); return m_angle[anum]; }
+
+    dReal calculateAngleRate(unsigned anum) const;
+
+    void setLimotParameter(unsigned anum, int limotParam, dReal value) { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); m_limot[anum].set(limotParam, value); }
+    dReal getLimotParameter(unsigned anum, int limotParam) const { dAASSERT(dIN_RANGE(anum, dSA__MIN, dSA__MAX)); return m_limot[anum].get(limotParam); }
+
+public:
+    void addTorques(dReal torque1, dReal torque2, dReal torque3);
+
+private:
+    void computeGlobalAxes(dVector3 ax[dSA__MAX]) const;
+    void doComputeGlobalUserAxes(dVector3 ax[dSA__MAX]) const;
+    void doComputeGlobalEulerAxes(dVector3 ax[dSA__MAX]) const;
+
+    void computeEulerAngles(dVector3 ax[dSA__MAX]);
+    void setEulerReferenceVectors();
+
+private:
+    inline dSpaceAxis BuildFirstBodyEulerAxis() const;
+    inline dJointConnectedBody BuildFirstEulerAxisBody() const;
+
+private:
+    friend struct dxAMotorJointPrinter;
+
+private:
+    int m_mode;                                   // a dAMotorXXX constant
+    unsigned m_num;                               // number of axes (0..3)
+    dJointBodyRelativity m_rel[dSA__MAX];         // what the axes are relative to (global,b1,b2)
+    dVector3 m_axis[dSA__MAX];                    // three axes
+    // these vectors are used for calculating Euler angles
+    dVector3 m_references[dJCB__MAX];             // original axis[2], relative to body 1; original axis[0], relative to body 2
+    dReal m_angle[dSA__MAX];                      // user-supplied angles for axes
+    dxJointLimitMotor m_limot[dJBR__MAX];         // limit+motor info for axes
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/ball.cpp

@@ -0,0 +1,186 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "ball.h"
+#include "joint_internal.h"
+
+//****************************************************************************
+// ball and socket
+
+dxJointBall::dxJointBall( dxWorld *w ) :
+    dxJoint( w )
+{
+    dSetZero( anchor1, 4 );
+    dSetZero( anchor2, 4 );
+    erp = world->global_erp;
+    cfm = world->global_cfm;
+}
+
+
+void 
+dxJointBall::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 3;
+}
+
+
+void
+dxJointBall::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 3;
+    info->nub = 3;
+}
+
+
+void
+dxJointBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    pairRhsCfm[GI2_CFM] = cfm;
+    pairRhsCfm[pairskip + GI2_CFM] = cfm;
+    pairRhsCfm[2 * pairskip + GI2_CFM] = cfm;
+    setBall( this, worldFPS, this->erp, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
+}
+
+
+
+
+
+void dJointSetBallAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Ball );
+    setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+}
+
+
+void dJointSetBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Ball );
+    joint->anchor2[0] = x;
+    joint->anchor2[1] = y;
+    joint->anchor2[2] = z;
+    joint->anchor2[3] = 0;
+}
+
+void dJointGetBallAnchor( dJointID j, dVector3 result )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Ball );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+        getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetBallAnchor2( dJointID j, dVector3 result )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Ball );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor( joint, result, joint->anchor1 );
+    else
+        getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dxJointBall::set( int num, dReal value )
+{
+    switch ( num )
+    {
+    case dParamCFM:
+        cfm = value;
+        break;
+    case dParamERP:
+        erp = value;
+        break;
+    }
+}
+
+
+dReal dxJointBall::get( int num )
+{
+    switch ( num )
+    {
+    case dParamCFM:
+        return cfm;
+    case dParamERP:
+        return erp;
+    default:
+        return 0;
+    }
+}
+
+
+void dJointSetBallParam( dJointID j, int parameter, dReal value )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Ball );
+    joint->set( parameter, value );
+}
+
+
+dReal dJointGetBallParam( dJointID j, int parameter )
+{
+    dxJointBall* joint = ( dxJointBall* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Ball );
+    return joint->get( parameter );
+}
+
+
+dJointType
+dxJointBall::type() const
+{
+    return dJointTypeBall;
+}
+
+sizeint
+dxJointBall::size() const
+{
+    return sizeof( *this );
+}
+
+void
+dxJointBall::setRelativeValues()
+{
+    dVector3 anchor;
+    dJointGetBallAnchor(this, anchor);
+    setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
+}
+
+
+

thirdparty/ode-0.16.5/ode/src/joints/ball.h

@@ -0,0 +1,54 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_BALL_H_
+#define _ODE_JOINT_BALL_H_
+
+#include "joint.h"
+
+// ball and socket
+
+struct dxJointBall : public dxJoint
+{
+    dVector3 anchor1;   // anchor w.r.t first body
+    dVector3 anchor2;   // anchor w.r.t second body
+    dReal erp;          // error reduction
+    dReal cfm;          // constraint force mix in
+    void set( int num, dReal value );
+    dReal get( int num );
+
+    dxJointBall( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    virtual void setRelativeValues();
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/contact.cpp

@@ -0,0 +1,361 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "contact.h"
+#include "joint_internal.h"
+
+
+
+ //****************************************************************************
+ // contact
+
+dxJointContact::dxJointContact(dxWorld *w) :
+    dxJoint(w)
+{
+}
+
+
+void
+dxJointContact::getSureMaxInfo(SureMaxInfo* info)
+{
+    // ...as the actual m is very likely to hit the maximum
+    info->max_m = (contact.surface.mode&dContactRolling) ? 6 : 3;
+}
+
+
+void
+dxJointContact::getInfo1(dxJoint::Info1 *info)
+{
+    // make sure mu's >= 0, then calculate number of constraint rows and number
+    // of unbounded rows.
+    int m = 1, nub = 0;
+
+    // Anisotropic sliding and rolling and spinning friction 
+    if (contact.surface.mode & dContactAxisDep) {
+        if (contact.surface.mu < 0) {
+            contact.surface.mu = 0;
+        }
+        else if (contact.surface.mu > 0) {
+            if (contact.surface.mu == dInfinity) { nub++; }
+            m++;
+        }
+
+        if (contact.surface.mu2 < 0) {
+            contact.surface.mu2 = 0;
+        }
+        else if (contact.surface.mu2 > 0) {
+            if (contact.surface.mu2 == dInfinity) { nub++; }
+            m++;
+        }
+
+        if ((contact.surface.mode & dContactRolling) != 0) {
+            if (contact.surface.rho < 0) {
+                contact.surface.rho = 0;
+            }
+            else {
+                if (contact.surface.rho == dInfinity) { nub++; }
+                m++;
+            }
+
+            if (contact.surface.rho2 < 0) {
+                contact.surface.rho2 = 0;
+            }
+            else {
+                if (contact.surface.rho2 == dInfinity) { nub++; }
+                m++;
+            }
+
+            if (contact.surface.rhoN < 0) {
+                contact.surface.rhoN = 0;
+            }
+            else {
+                if (contact.surface.rhoN == dInfinity) { nub++; }
+                m++;
+            }
+        }
+    }
+    else {
+        if (contact.surface.mu < 0) {
+            contact.surface.mu = 0;
+        }
+        else if (contact.surface.mu > 0) {
+            if (contact.surface.mu == dInfinity) { nub += 2; }
+            m += 2;
+        }
+
+        if ((contact.surface.mode & dContactRolling) != 0) {
+            if (contact.surface.rho < 0) {
+                contact.surface.rho = 0;
+            }
+            else {
+                if (contact.surface.rho == dInfinity) { nub += 3; }
+                m += 3;
+            }
+        }
+    }
+
+    the_m = m;
+    info->m = m;
+    info->nub = nub;
+}
+
+
+void
+dxJointContact::getInfo2(dReal worldFPS, dReal worldERP,
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+    int *findex)
+{
+    enum 
+    {
+        ROW_NORMAL,
+
+        ROW__OPTIONAL_MIN,
+    };
+
+    const int surface_mode = contact.surface.mode;
+
+    // set right hand side and cfm value for normal
+    dReal erp = (surface_mode & dContactSoftERP) != 0 ? contact.surface.soft_erp : worldERP;
+    dReal k = worldFPS * erp;
+
+    dReal depth = contact.geom.depth - world->contactp.min_depth;
+    if (depth < 0) depth = 0;
+
+    dReal motionN = (surface_mode & dContactMotionN) != 0 ? contact.surface.motionN : REAL(0.0);
+    const dReal pushout = k * depth + motionN;
+
+    bool apply_bounce = (surface_mode & dContactBounce) != 0 && contact.surface.bounce_vel >= 0;
+    dReal outgoing = 0;
+
+    // note: this cap should not limit bounce velocity
+    const dReal maxvel = world->contactp.max_vel;
+    dReal c = pushout > maxvel ? maxvel : pushout;
+
+    // c1,c2 = contact points with respect to body PORs
+    dVector3 c1, c2 = { 0, };
+
+    // get normal, with sign adjusted for body1/body2 polarity
+    dVector3 normal;
+    if ((flags & dJOINT_REVERSE) != 0) {
+        dCopyNegatedVector3(normal, contact.geom.normal);
+    }
+    else {
+        dCopyVector3(normal, contact.geom.normal);
+    }
+
+    dxBody *b1 = node[1].body;
+    if (b1) {
+        dSubtractVectors3(c2, contact.geom.pos, b1->posr.pos);
+        // set Jacobian for b1 normal
+        dCopyNegatedVector3(J2 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal);
+        dCalcVectorCross3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, normal, c2); //== dCalcVectorCross3( J2 + GI2__JA_MIN, c2, normal ); dNegateVector3( J2 + GI2__JA_MIN );
+        if (apply_bounce) {
+            outgoing /*+*/= dCalcVectorDot3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[1].body->avel)
+                - dCalcVectorDot3(normal, node[1].body->lvel);
+        }
+    }
+
+    dxBody *b0 = node[0].body;
+    dSubtractVectors3(c1, contact.geom.pos, b0->posr.pos);
+    // set Jacobian for b0 normal
+    dCopyVector3(J1 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal);
+    dCalcVectorCross3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, c1, normal);
+    if (apply_bounce) {
+        // calculate outgoing velocity (-ve for incoming contact)
+        outgoing += dCalcVectorDot3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[0].body->avel)
+            + dCalcVectorDot3(normal, node[0].body->lvel);
+    }
+
+    // deal with bounce
+    if (apply_bounce) {
+        dReal negated_outgoing = motionN - outgoing;
+        // only apply bounce if the outgoing velocity is greater than the
+        // threshold, and if the resulting c[rowNormal] exceeds what we already have.
+        dIASSERT(contact.surface.bounce_vel >= 0);
+        if (/*contact.surface.bounce_vel >= 0 &&*/
+            negated_outgoing > contact.surface.bounce_vel) {
+            const dReal newc = contact.surface.bounce * negated_outgoing + motionN;
+            if (newc > c) { c = newc; }
+        }
+    }
+
+    pairRhsCfm[ROW_NORMAL * pairskip + GI2_RHS] = c;
+
+    if ((surface_mode & dContactSoftCFM) != 0) {
+        pairRhsCfm[ROW_NORMAL * pairskip + GI2_CFM] = contact.surface.soft_cfm;
+    }
+
+    // set LCP limits for normal
+    pairLoHi[ROW_NORMAL * pairskip + GI2_LO] = 0;
+    pairLoHi[ROW_NORMAL * pairskip + GI2_HI] = dInfinity;
+
+
+    if (the_m > 1) { // if no friction, there is nothing else to do
+        // now do jacobian for tangential forces
+        dVector3 t1, t2; // two vectors tangential to normal
+
+        if ((surface_mode & dContactFDir1) != 0) {   // use fdir1 ?
+            dCopyVector3(t1, contact.fdir1);
+            dCalcVectorCross3(t2, normal, t1);
+        }
+        else {
+            dPlaneSpace(normal, t1, t2);
+        }
+
+        int row = ROW__OPTIONAL_MIN;
+        int currRowSkip = row * rowskip, currPairSkip = row * pairskip;
+
+        // first friction direction
+        const dReal mu = contact.surface.mu;
+
+        if (mu > 0) {
+            dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t1);
+            dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t1);
+
+            if (node[1].body) {
+                dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t1);
+                dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t1, c2); //== dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, c2, t1 ); dNegateVector3( J2 + rowskip + GI2__JA_MIN );
+            }
+
+            // set right hand side
+            if ((surface_mode & dContactMotion1) != 0) {
+                pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion1;
+            }
+            // set slip (constraint force mixing)
+            if ((surface_mode & dContactSlip1) != 0) {
+                pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip1;
+            }
+
+            // set LCP bounds and friction index. this depends on the approximation
+            // mode
+            pairLoHi[currPairSkip + GI2_LO] = -mu;
+            pairLoHi[currPairSkip + GI2_HI] = mu;
+
+            if ((surface_mode & dContactApprox1_1) != 0) {
+                findex[row] = 0;
+            }
+
+            ++row;
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+
+        // second friction direction
+        const dReal mu2 = (surface_mode & dContactMu2) != 0 ? contact.surface.mu2 : mu;
+
+        if (mu2 > 0) {
+            dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t2);
+            dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t2);
+
+            if (node[1].body) {
+                dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t2);
+                dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t2, c2); //== dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, c2, t2 ); dNegateVector3( J2 + currRowSkip + GI2__JA_MIN );
+            }
+
+            // set right hand side
+            if ((surface_mode & dContactMotion2) != 0) {
+                pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion2;
+            }
+            // set slip (constraint force mixing)
+            if ((surface_mode & dContactSlip2) != 0) {
+                pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip2;
+            }
+
+            // set LCP bounds and friction index. this depends on the approximation
+            // mode
+            pairLoHi[currPairSkip + GI2_LO] = -mu2;
+            pairLoHi[currPairSkip + GI2_HI] = mu2;
+
+            if ((surface_mode & dContactApprox1_2) != 0) {
+                findex[row] = 0;
+            }
+
+            ++row;
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+
+        // Handle rolling/spinning friction
+        if ((surface_mode & dContactRolling) != 0) {
+
+            const dReal *const ax[3] = {
+                t1, // Rolling around t1 creates movement parallel to t2
+                t2,
+                normal // Spinning axis
+            };
+
+            const int approx_bits[3] = { dContactApprox1_1, dContactApprox1_2, dContactApprox1_N };
+
+            // Get the coefficients
+            dReal rho[3];
+            rho[0] = contact.surface.rho;
+            if ((surface_mode & dContactAxisDep) != 0) {
+                rho[1] = contact.surface.rho2;
+                rho[2] = contact.surface.rhoN;
+            }
+            else {
+                rho[1] = rho[0];
+                rho[2] = rho[0];
+            }
+
+            for (int i = 0; i != 3; ++i) {
+                if (rho[i] > 0) {
+                    // Set the angular axis
+                    dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, ax[i]);
+
+                    if (b1) {
+                        dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, ax[i]);
+                    }
+
+                    // Set the lcp limits
+                    pairLoHi[currPairSkip + GI2_LO] = -rho[i];
+                    pairLoHi[currPairSkip + GI2_HI] = rho[i];
+
+                    // Should we use proportional force?
+                    if ((surface_mode & approx_bits[i]) != 0) {
+                        // Make limits proportional to normal force
+                        findex[row] = 0;
+                    }
+
+                    ++row;
+                    currRowSkip += rowskip; currPairSkip += pairskip;
+                }
+            }
+        }
+    }
+}
+
+dJointType
+dxJointContact::type() const
+{
+    return dJointTypeContact;
+}
+
+
+sizeint
+dxJointContact::size() const
+{
+    return sizeof(*this);
+}
+

thirdparty/ode-0.16.5/ode/src/joints/contact.h

@@ -0,0 +1,48 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_CONTACT_H_
+#define _ODE_JOINT_CONTACT_H_
+
+#include "joint.h"
+
+// contact
+
+struct dxJointContact : public dxJoint
+{
+    int the_m;   // number of rows computed by getInfo1
+    dContact contact;
+
+    dxJointContact( dxWorld* w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex);
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/dball.cpp

@@ -0,0 +1,314 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "dball.h"
+#include "joint_internal.h"
+
+/*
+ * Double Ball joint: tries to maintain a fixed distance between two anchor
+ * points.
+ */
+
+dxJointDBall::dxJointDBall(dxWorld *w) :
+    dxJoint(w)
+{
+    dSetZero(anchor1, 3);
+    dSetZero(anchor2, 3);
+    targetDistance = 0;
+    erp = world->global_erp;
+    cfm = world->global_cfm;
+}
+
+void
+dxJointDBall::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 1;
+}
+void
+dxJointDBall::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 1;
+    info->nub = 1;
+}
+
+void
+dxJointDBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dVector3 globalA1, globalA2;
+    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
+    
+    if (node[1].body) {
+        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
+    } else {
+        dCopyVector3(globalA2, anchor2);
+    }
+
+    dVector3 q;
+    dSubtractVectors3(q, globalA1, globalA2);
+
+#ifdef dSINGLE
+    const dReal MIN_LENGTH = REAL(1e-7);
+#else
+    const dReal MIN_LENGTH = REAL(1e-12);
+#endif
+
+    if (dCalcVectorLength3(q) < MIN_LENGTH) {
+        // too small, let's choose an arbitrary direction
+        // heuristic: difference in velocities at anchors
+        dVector3 v1, v2;
+        dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
+    
+        if (node[1].body) {
+            dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
+        } else {
+            dZeroVector3(v2);
+        }
+
+        dSubtractVectors3(q, v1, v2);
+
+        if (dCalcVectorLength3(q) < MIN_LENGTH) {
+            // this direction is as good as any
+            dAssignVector3(q, 1, 0, 0);
+        }
+    }
+    dNormalize3(q);
+
+    dCopyVector3(J1 + GI2__JL_MIN, q);
+
+    dVector3 relA1;
+    dBodyVectorToWorld(node[0].body,
+                       anchor1[0], anchor1[1], anchor1[2],
+                       relA1);
+
+    dMatrix3 a1m;
+    dZeroMatrix3(a1m);
+    dSetCrossMatrixMinus(a1m, relA1, 4);
+
+    dMultiply1_331(J1 + GI2__JA_MIN, a1m, q);
+
+    if (node[1].body) {
+        dCopyNegatedVector3(J2 + GI2__JL_MIN, q);
+
+        dVector3 relA2;
+        dBodyVectorToWorld(node[1].body,
+                           anchor2[0], anchor2[1], anchor2[2],
+                           relA2);
+        dMatrix3 a2m;
+        dZeroMatrix3(a2m);
+        dSetCrossMatrixPlus(a2m, relA2, 4);
+        dMultiply1_331(J2 + GI2__JA_MIN, a2m, q);
+    }
+    
+    const dReal k = worldFPS * this->erp;
+    pairRhsCfm[GI2_RHS] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
+    pairRhsCfm[GI2_CFM] = this->cfm;
+}
+
+
+void
+dxJointDBall::updateTargetDistance()
+{
+    dVector3 p1, p2;
+
+    if (node[0].body)
+        dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1);
+    else
+        dCopyVector3(p1, anchor1);
+    if (node[1].body)
+        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2);
+    else
+        dCopyVector3(p2, anchor2);
+
+    targetDistance = dCalcPointsDistance3(p1, p2);
+}
+
+
+void dJointSetDBallAnchor1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[1].body)
+            dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2);
+        else {
+            joint->anchor2[0] = x;
+            joint->anchor2[1] = y;
+            joint->anchor2[2] = z;
+        }
+    } else {
+        if (joint->node[0].body)
+            dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1);
+        else {
+            joint->anchor1[0] = x;
+            joint->anchor1[1] = y;
+            joint->anchor1[2] = z;
+        }
+    }
+
+    joint->updateTargetDistance();
+}
+
+
+void dJointSetDBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[0].body)
+            dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1);
+        else {
+            joint->anchor1[0] = x;
+            joint->anchor1[1] = y;
+            joint->anchor1[2] = z;
+        }
+    } else {
+        if (joint->node[1].body)
+            dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2);
+        else {
+            joint->anchor2[0] = x;
+            joint->anchor2[1] = y;
+            joint->anchor2[2] = z;
+        }
+    }
+
+    joint->updateTargetDistance();
+}
+
+dReal dJointGetDBallDistance(dJointID j)
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    return joint->targetDistance;
+}
+
+void dJointSetDBallDistance(dJointID j, dReal dist)
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( dist>=0, "target distance must be non-negative" );
+
+    joint->targetDistance = dist;
+}
+
+
+void dJointGetDBallAnchor1( dJointID j, dVector3 result )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[1].body)
+            dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result);
+        else
+            dCopyVector3(result, joint->anchor2);
+    } else {
+        if (joint->node[0].body)
+            dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result);
+        else
+            dCopyVector3(result, joint->anchor1);
+    }
+}
+
+
+void dJointGetDBallAnchor2( dJointID j, dVector3 result )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[0].body)
+            dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result);
+        else
+            dCopyVector3(result, joint->anchor1);
+    } else {
+        if (joint->node[1].body)
+            dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result);
+        else
+            dCopyVector3(result, joint->anchor2);
+    }
+}
+
+
+void dJointSetDBallParam( dJointID j, int parameter, dReal value )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    switch ( parameter ) {
+        case dParamCFM:
+            joint->cfm = value;
+            break;
+        case dParamERP:
+            joint->erp = value;
+            break;
+    }
+}
+
+
+dReal dJointGetDBallParam( dJointID j, int parameter )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    switch ( parameter ) {
+        case dParamCFM:
+            return joint->cfm;
+        case dParamERP:
+            return joint->erp;
+        default:
+            return 0;
+    }
+}
+
+
+dJointType
+dxJointDBall::type() const
+{
+    return dJointTypeDBall;
+}
+
+sizeint
+dxJointDBall::size() const
+{
+    return sizeof( *this );
+}
+
+void
+dxJointDBall::setRelativeValues()
+{
+    updateTargetDistance();
+}
+
+
+

thirdparty/ode-0.16.5/ode/src/joints/dball.h

@@ -0,0 +1,58 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_DBALL_H_
+#define _ODE_JOINT_DBALL_H_
+
+#include "joint.h"
+
+// ball and socket
+
+struct dxJointDBall : public dxJoint
+{
+    dVector3 anchor1;   // anchor w.r.t first body
+    dVector3 anchor2;   // anchor w.r.t second body
+    dReal erp;          // error reduction
+    dReal cfm;          // constraint force mix in
+    dReal targetDistance;
+
+    void set( int num, dReal value );
+    dReal get( int num );
+
+    void updateTargetDistance();
+
+    dxJointDBall( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    virtual void setRelativeValues();
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/dhinge.cpp

@@ -0,0 +1,220 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "dhinge.h"
+#include "joint_internal.h"
+
+/*
+ * Double Hinge joint
+ */
+
+dxJointDHinge::dxJointDHinge(dxWorld* w) :
+    dxJointDBall(w)
+{
+    dSetZero(axis1, 3);
+    dSetZero(axis2, 3);
+}
+
+
+void
+dxJointDHinge::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 4;
+}
+
+
+void
+dxJointDHinge::getInfo1( dxJoint::Info1* info )
+{
+    info->m = 4;
+    info->nub = 4;
+}
+
+
+void
+dxJointDHinge::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dxJointDBall::getInfo2( worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, pairLoHi, findex ); // sets row0
+    
+    dVector3 globalAxis1;
+    dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
+
+    dxBody *body1 = node[1].body;
+
+    // angular constraints, perpendicular to axis
+    dVector3 p, q;
+    dPlaneSpace(globalAxis1, p, q);
+
+    dCopyVector3(J1 + rowskip + GI2__JA_MIN, p);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, p);
+    }
+
+    dCopyVector3(J1 + 2 * rowskip + GI2__JA_MIN, q);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + 2 * rowskip + GI2__JA_MIN, q);
+    }
+
+    dVector3 globalAxis2;
+    if ( body1 ) {
+        dBodyVectorToWorld(body1, axis2[0], axis2[1], axis2[2], globalAxis2);
+    } else {
+        dCopyVector3(globalAxis2, axis2);
+    }
+    
+    // similar to the hinge joint
+    dVector3 u;
+    dCalcVectorCross3(u, globalAxis1, globalAxis2);
+
+    const dReal k = worldFPS * this->erp;
+    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( u, p );
+    pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( u, q );
+
+
+
+
+    /*
+     * Constraint along the axis: translation along it should couple angular movement.
+     * This is just the ball-and-socket derivation, projected onto the hinge axis,
+     * producing a single constraint at the end.
+     *
+     * The choice of "ball" position can be arbitrary; we could place it at the center
+     * of one of the bodies, canceling out its rotational jacobian; or we could make
+     * everything symmetrical by just placing at the midpoint between the centers.
+     *
+     * I like symmetry, so I'll use the second approach here. I'll call the midpoint h.
+     *
+     * Of course, if the second body is NULL, the first body is pretty much locked
+     * along this axis, and the linear constraint is enough.
+     */
+
+    int rowskip_mul_3 = 3 * rowskip;
+    dCopyVector3(J1 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);
+
+    if ( body1 ) {
+        dVector3 h;
+        dAddScaledVectors3(h, node[0].body->posr.pos, body1->posr.pos, -0.5, 0.5);
+
+        dCalcVectorCross3(J1 + rowskip_mul_3 + GI2__JA_MIN, h, globalAxis1);
+
+        dCopyNegatedVector3(J2 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);
+        dCopyVector3(J2 + rowskip_mul_3 + GI2__JA_MIN, J1 + rowskip_mul_3 + GI2__JA_MIN);
+    }
+
+    // error correction: both anchors should lie on the same plane perpendicular to the axis
+    dVector3 globalA1, globalA2;
+    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
+
+    if ( body1 ) {
+        dBodyGetRelPointPos(body1, anchor2[0], anchor2[1], anchor2[2], globalA2);
+    } else {
+        dCopyVector3(globalA2, anchor2);
+    }
+
+    dVector3 d;
+    dSubtractVectors3(d, globalA1, globalA2); // displacement error
+    pairRhsCfm[3 * pairskip + GI2_RHS] = -k * dCalcVectorDot3(globalAxis1, d);
+}
+
+void dJointSetDHingeAxis( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axis1);
+    if (joint->node[1].body)
+        dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
+    else {
+        joint->axis2[0] = x;
+        joint->axis2[1] = y;
+        joint->axis2[2] = z;
+    }
+    dNormalize3(joint->axis1);
+    dNormalize3(joint->axis2);
+}
+
+void dJointGetDHingeAxis( dJointID j, dVector3 result )
+{
+    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    dBodyVectorToWorld(joint->node[0].body, joint->axis1[0], joint->axis1[1], joint->axis1[2], result);
+}
+
+
+void dJointSetDHingeAnchor1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dJointSetDBallAnchor1(j, x, y, z);
+}
+
+
+void dJointSetDHingeAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dJointSetDBallAnchor2(j, x, y, z);
+}
+
+dReal dJointGetDHingeDistance(dJointID j)
+{
+    return dJointGetDBallDistance(j);
+}
+
+
+void dJointGetDHingeAnchor1( dJointID j, dVector3 result )
+{
+    dJointGetDBallAnchor1(j, result);
+}
+
+
+void dJointGetDHingeAnchor2( dJointID j, dVector3 result )
+{
+    dJointGetDBallAnchor2(j, result);
+}
+
+
+void dJointSetDHingeParam( dJointID j, int parameter, dReal value )
+{
+    dJointSetDBallParam(j, parameter, value);
+}
+
+
+dReal dJointGetDHingeParam( dJointID j, int parameter )
+{
+    return dJointGetDBallParam(j, parameter);
+}
+
+dJointType
+dxJointDHinge::type() const
+{
+    return dJointTypeDHinge;
+}
+
+sizeint
+dxJointDHinge::size() const
+{
+    return sizeof( *this );
+}

thirdparty/ode-0.16.5/ode/src/joints/dhinge.h

@@ -0,0 +1,46 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_DHINGE_
+#define _ODE_JOINT_DHINGE_
+
+#include "dball.h"
+
+struct dxJointDHinge : public dxJointDBall 
+{
+    dVector3 axis1, axis2;
+    
+    dxJointDHinge(dxWorld *w);
+
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+};
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/joints/fixed.cpp

@@ -0,0 +1,216 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "fixed.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// fixed joint
+
+dxJointFixed::dxJointFixed ( dxWorld *w ) :
+    dxJoint ( w )
+{
+    dSetZero ( offset, 4 );
+    dSetZero ( qrel, 4 );
+    erp = world->global_erp;
+    cfm = world->global_cfm;
+}
+
+
+void 
+dxJointFixed::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointFixed::getInfo1 ( dxJoint::Info1 *info )
+{
+    info->m = 6;
+    info->nub = 6;
+}
+
+
+void
+dxJointFixed::getInfo2 ( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // Three rows for orientation
+    setFixedOrientation ( this, worldFPS, worldERP, 
+        rowskip, J1 + dSA__MAX * rowskip, J2 + dSA__MAX * rowskip,
+        pairskip, pairRhsCfm + dSA__MAX * pairskip, qrel );
+
+    // Three rows for position.
+    // set Jacobian
+    J1[GI2_JLX] = 1;
+    J1[rowskip + GI2_JLY] = 1;
+    J1[2 * rowskip + GI2_JLZ] = 1;
+
+    dReal k = worldFPS * this->erp;
+    dxBody *b0 = node[0].body, *b1 = node[1].body;
+
+    dVector3 ofs;
+    dMultiply0_331 ( ofs, b0->posr.R, offset );
+
+    if ( b1 ) {
+        dSetCrossMatrixPlus( J1 + GI2__JA_MIN, ofs, rowskip );
+
+        J2[GI2_JLX] = -1;
+        J2[rowskip + GI2_JLY] = -1;
+        J2[2 * rowskip + GI2_JLZ] = -1;
+    }
+
+    // set right hand side for the first three rows (linear)
+    if ( b1 ) {
+        for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) {
+            pairRhsCfm[currPairSkip + GI2_RHS] = k * ( b1->posr.pos[j] - b0->posr.pos[j] + ofs[j] );
+        }
+    } else {
+        for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) {
+            pairRhsCfm[currPairSkip + GI2_RHS] = k * ( offset[j] - b0->posr.pos[j] );
+        }
+    }
+
+    dReal cfm = this->cfm;
+    pairRhsCfm[GI2_CFM] = cfm;
+    pairRhsCfm[pairskip + GI2_CFM] = cfm;
+    pairRhsCfm[2 * pairskip + GI2_CFM] = cfm;
+}
+
+
+void dJointSetFixed ( dJointID j )
+{
+    dxJointFixed* joint = ( dxJointFixed* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Fixed );
+    int i;
+
+    // This code is taken from dJointSetSliderAxis(), we should really put the
+    // common code in its own function.
+    // compute the offset between the bodies
+    if ( joint->node[0].body )
+    {
+        if ( joint->node[1].body )
+        {
+            dReal ofs[4];
+            for ( i = 0; i < 4; i++ )
+                ofs[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
+            dMultiply1_331 ( joint->offset, joint->node[0].body->posr.R, ofs );
+        }
+        else
+        {
+            joint->offset[0] = joint->node[0].body->posr.pos[0];
+            joint->offset[1] = joint->node[0].body->posr.pos[1];
+            joint->offset[2] = joint->node[0].body->posr.pos[2];
+        }
+    }
+
+    joint->computeInitialRelativeRotation();
+}
+
+void dxJointFixed::set ( int num, dReal value )
+{
+    switch ( num )
+    {
+    case dParamCFM:
+        cfm = value;
+        break;
+    case dParamERP:
+        erp = value;
+        break;
+    }
+}
+
+
+dReal dxJointFixed::get ( int num )
+{
+    switch ( num )
+    {
+    case dParamCFM:
+        return cfm;
+    case dParamERP:
+        return erp;
+    default:
+        return 0;
+    }
+}
+
+
+void dJointSetFixedParam ( dJointID j, int parameter, dReal value )
+{
+    dxJointFixed* joint = ( dxJointFixed* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Fixed );
+    joint->set ( parameter, value );
+}
+
+
+dReal dJointGetFixedParam ( dJointID j, int parameter )
+{
+    dxJointFixed* joint = ( dxJointFixed* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Fixed );
+    return joint->get ( parameter );
+}
+
+
+dJointType
+dxJointFixed::type() const
+{
+    return dJointTypeFixed;
+}
+
+
+sizeint
+dxJointFixed::size() const
+{
+    return sizeof ( *this );
+}
+
+void
+dxJointFixed::computeInitialRelativeRotation()
+{
+    if (node[0].body )
+    {
+        if (node[1].body )
+        {
+            dQMultiply1 (qrel, node[0].body->q, node[1].body->q );
+        }
+        else
+        {
+            // set qrel to the transpose of the first body q
+            qrel[0] =  node[0].body->q[0];
+            qrel[1] = -node[0].body->q[1];
+            qrel[2] = -node[0].body->q[2];
+            qrel[3] = -node[0].body->q[3];
+        }
+    }
+}
+

thirdparty/ode-0.16.5/ode/src/joints/fixed.h

@@ -0,0 +1,54 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_FIXED_H_
+#define _ODE_JOINT_FIXED_H_
+
+#include "joint.h"
+
+
+// fixed
+
+struct dxJointFixed : public dxJoint
+{
+    dQuaternion qrel;   // initial relative rotation body1 -> body2
+    dVector3 offset;    // relative offset between the bodies
+    dReal erp;          // error reduction parameter
+    dReal cfm;          // constraint force mix-in
+    void  set ( int num, dReal value );
+    dReal get ( int num );
+
+    dxJointFixed ( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1 ( Info1* info );
+    virtual void getInfo2 ( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    void computeInitialRelativeRotation();
+};
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/joints/hinge.cpp

@@ -0,0 +1,394 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "hinge.h"
+#include "joint_internal.h"
+
+
+//****************************************************************************
+// hinge
+
+dxJointHinge::dxJointHinge( dxWorld *w ) :
+    dxJoint( w )
+{
+    dSetZero( anchor1, 4 );
+    dSetZero( anchor2, 4 );
+    dSetZero( axis1, 4 );
+    axis1[0] = 1;
+    dSetZero( axis2, 4 );
+    axis2[0] = 1;
+    dSetZero( qrel, 4 );
+    limot.init( world );
+}
+
+
+void 
+dxJointHinge::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointHinge::getInfo1( dxJoint::Info1 *info )
+{
+    info->nub = 5;
+
+    // see if joint is powered
+    if ( limot.fmax > 0 )
+        info->m = 6; // powered hinge needs an extra constraint row
+    else info->m = 5;
+
+    // see if we're at a joint limit.
+    if (( limot.lostop >= -M_PI || limot.histop <= M_PI ) &&
+        limot.lostop <= limot.histop )
+    {
+        dReal angle = getHingeAngle( node[0].body,
+            node[1].body,
+            axis1, qrel );
+        if ( limot.testRotationalLimit( angle ) )
+            info->m = 6;
+    }
+}
+
+
+void dxJointHinge::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // set the three ball-and-socket rows
+    setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
+
+    // set the two hinge rows. the hinge axis should be the only unconstrained
+    // rotational axis, the angular velocity of the two bodies perpendicular to
+    // the hinge axis should be equal. thus the constraint equations are
+    //    p*w1 - p*w2 = 0
+    //    q*w1 - q*w2 = 0
+    // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+    // are the angular velocity vectors of the two bodies.
+
+    dVector3 ax1;  // length 1 joint axis in global coordinates, from 1st body
+    dVector3 p, q; // plane space vectors for ax1
+    dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+    dPlaneSpace( ax1, p, q );
+
+    dxBody *body1 = node[1].body;
+    
+    int currRowSkip = 3 * rowskip;
+    dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, p);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, p);
+    }
+
+    currRowSkip += rowskip;
+    dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q);
+    }
+
+    // compute the right hand side of the constraint equation. set relative
+    // body velocities along p and q to bring the hinge back into alignment.
+    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+    // ...as ax1 and ax2 are unit length. if theta is smallish,
+    // theta ~= sin(theta), so
+    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+    // ax1 x ax2 is in the plane space of ax1, so we project the angular
+    // velocity to p and q to find the right hand side.
+
+    dVector3 b;
+    if ( body1 ) {
+        dVector3 ax2;
+        dMultiply0_331( ax2, body1->posr.R, axis2 );
+        dCalcVectorCross3( b, ax1, ax2 );
+    } else {
+        dCalcVectorCross3( b, ax1, axis2 );
+    }
+
+    dReal k = worldFPS * worldERP;
+    int currPairSkip = 3 * pairskip;
+    pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, p );
+    currPairSkip += pairskip;
+    pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( b, q );
+
+    // if the hinge is powered, or has joint limits, add in the stuff
+    currRowSkip += rowskip;
+    currPairSkip += pairskip;
+    limot.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+
+
+void dJointSetHingeAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+    setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetHingeAnchorDelta( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+
+    if ( joint->node[0].body )
+    {
+        dReal q[4];
+        q[0] = x - joint->node[0].body->posr.pos[0];
+        q[1] = y - joint->node[0].body->posr.pos[1];
+        q[2] = z - joint->node[0].body->posr.pos[2];
+        q[3] = 0;
+        dMultiply1_331( joint->anchor1, joint->node[0].body->posr.R, q );
+
+        if ( joint->node[1].body )
+        {
+            q[0] = x - joint->node[1].body->posr.pos[0];
+            q[1] = y - joint->node[1].body->posr.pos[1];
+            q[2] = z - joint->node[1].body->posr.pos[2];
+            q[3] = 0;
+            dMultiply1_331( joint->anchor2, joint->node[1].body->posr.R, q );
+        }
+        else
+        {
+            // Move the relative displacement between the passive body and the
+            //  anchor in the same direction as the passive body has just moved
+            joint->anchor2[0] = x + dx;
+            joint->anchor2[1] = y + dy;
+            joint->anchor2[2] = z + dz;
+        }
+    }
+    joint->anchor1[3] = 0;
+    joint->anchor2[3] = 0;
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+
+void dJointSetHingeAxis( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+    setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetHingeAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+    setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
+    joint->computeInitialRelativeRotation();
+
+    if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle;
+
+    dQuaternion qAngle, qOffset;
+    dQFromAxisAndAngle(qAngle, x, y, z, dangle);
+    dQMultiply3(qOffset, qAngle, joint->qrel);
+    joint->qrel[0] = qOffset[0];
+    joint->qrel[1] = qOffset[1];
+    joint->qrel[2] = qOffset[2];
+    joint->qrel[3] = qOffset[3];
+}
+
+
+
+void dJointGetHingeAnchor( dJointID j, dVector3 result )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+        getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetHingeAnchor2( dJointID j, dVector3 result )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor( joint, result, joint->anchor1 );
+    else
+        getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dJointGetHingeAxis( dJointID j, dVector3 result )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge );
+    getAxis( joint, result, joint->axis1 );
+}
+
+
+void dJointSetHingeParam( dJointID j, int parameter, dReal value )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+    joint->limot.set( parameter, value );
+}
+
+
+dReal dJointGetHingeParam( dJointID j, int parameter )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge );
+    return joint->limot.get( parameter );
+}
+
+
+dReal dJointGetHingeAngle( dJointID j )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dAASSERT( joint );
+    checktype( joint, Hinge );
+    if ( joint->node[0].body )
+    {
+        dReal ang = getHingeAngle( joint->node[0].body,
+            joint->node[1].body,
+            joint->axis1,
+            joint->qrel );
+        if ( joint->flags & dJOINT_REVERSE )
+            return -ang;
+        else
+            return ang;
+    }
+    else return 0;
+}
+
+
+dReal dJointGetHingeAngleRate( dJointID j )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dAASSERT( joint );
+    checktype( joint, Hinge );
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
+        return rate;
+    }
+    else return 0;
+}
+
+
+void dJointAddHingeTorque( dJointID j, dReal torque )
+{
+    dxJointHinge* joint = ( dxJointHinge* )j;
+    dVector3 axis;
+    dAASSERT( joint );
+    checktype( joint, Hinge );
+
+    if ( joint->flags & dJOINT_REVERSE )
+        torque = -torque;
+
+    getAxis( joint, axis, joint->axis1 );
+    axis[0] *= torque;
+    axis[1] *= torque;
+    axis[2] *= torque;
+
+    if ( joint->node[0].body != 0 )
+        dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
+    if ( joint->node[1].body != 0 )
+        dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+}
+
+
+dJointType
+dxJointHinge::type() const
+{
+    return dJointTypeHinge;
+}
+
+
+
+sizeint
+dxJointHinge::size() const
+{
+    return sizeof( *this );
+}
+
+
+void
+dxJointHinge::setRelativeValues()
+{
+    dVector3 vec;
+    dJointGetHingeAnchor(this, vec);
+    setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
+
+    dJointGetHingeAxis(this, vec);
+    setAxes( this,  vec[0], vec[1], vec[2], axis1, axis2 );
+    computeInitialRelativeRotation();
+}
+
+
+/// Compute initial relative rotation body1 -> body2, or env -> body1
+void
+dxJointHinge::computeInitialRelativeRotation()
+{
+    if ( node[0].body )
+    {
+        if ( node[1].body )
+        {
+            dQMultiply1( qrel, node[0].body->q, node[1].body->q );
+        }
+        else
+        {
+            // set qrel to the transpose of the first body q
+            qrel[0] =  node[0].body->q[0];
+            qrel[1] = -node[0].body->q[1];
+            qrel[2] = -node[0].body->q[2];
+            qrel[3] = -node[0].body->q[3];
+        }
+    }
+}
+

thirdparty/ode-0.16.5/ode/src/joints/hinge.h

@@ -0,0 +1,57 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_HINGE_H_
+#define _ODE_JOINT_HINGE_H_
+
+#include "joint.h"
+
+
+// hinge
+
+struct dxJointHinge : public dxJoint
+{
+    dVector3 anchor1;   // anchor w.r.t first body
+    dVector3 anchor2;   // anchor w.r.t second body
+    dVector3 axis1;     // axis w.r.t first body
+    dVector3 axis2;     // axis w.r.t second body
+    dQuaternion qrel;   // initial relative rotation body1 -> body2
+    dxJointLimitMotor limot; // limit and motor information
+
+    dxJointHinge( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    virtual void setRelativeValues();
+
+    void computeInitialRelativeRotation();
+};
+
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/joints/hinge2.cpp

@@ -0,0 +1,546 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "hinge2.h"
+#include "joint_internal.h"
+
+
+
+
+//****************************************************************************
+// hinge 2. note that this joint must be attached to two bodies for it to work
+
+dReal
+dxJointHinge2::measureAngle1() const
+{
+    // bring axis 2 into first body's reference frame
+    dVector3 p, q;
+    if (node[1].body)
+        dMultiply0_331( p, node[1].body->posr.R, axis2 );
+    else
+        dCopyVector3(p, axis2);
+
+    if (node[0].body)
+        dMultiply1_331( q, node[0].body->posr.R, p );
+    else
+        dCopyVector3(q, p);
+
+    dReal x = dCalcVectorDot3( v1, q );
+    dReal y = dCalcVectorDot3( v2, q );
+    return -dAtan2( y, x );
+}
+
+dReal
+dxJointHinge2::measureAngle2() const
+{
+    // bring axis 1 into second body's reference frame
+    dVector3 p, q;
+    if (node[0].body)
+        dMultiply0_331( p, node[0].body->posr.R, axis1 );
+    else
+        dCopyVector3(p, axis1);
+
+    if (node[1].body)
+        dMultiply1_331( q, node[1].body->posr.R, p );
+    else
+        dCopyVector3(q, p);
+
+    dReal x = dCalcVectorDot3( w1, q );
+    dReal y = dCalcVectorDot3( w2, q );
+    return -dAtan2( y, x );
+}
+
+
+dxJointHinge2::dxJointHinge2( dxWorld *w ) :
+    dxJoint( w )
+{
+    dSetZero( anchor1, 4 );
+    dSetZero( anchor2, 4 );
+    dSetZero( axis1, 4 );
+    axis1[0] = 1;
+    dSetZero( axis2, 4 );
+    axis2[1] = 1;
+    c0 = 0;
+    s0 = 0;
+
+    dSetZero( v1, 4 );
+    v1[0] = 1;
+    dSetZero( v2, 4 );
+    v2[1] = 1;
+
+    limot1.init( world );
+    limot2.init( world );
+
+    susp_erp = world->global_erp;
+    susp_cfm = world->global_cfm;
+
+    flags |= dJOINT_TWOBODIES;
+}
+
+
+void 
+dxJointHinge2::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointHinge2::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 4;
+    info->nub = 4;
+
+    // see if we're powered or at a joint limit for axis 1
+    limot1.limit = 0;
+    if (( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
+        limot1.lostop <= limot1.histop )
+    {
+        dReal angle = measureAngle1();
+        limot1.testRotationalLimit( angle );
+    }
+    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
+
+    // see if we're powering axis 2 (we currently never limit this axis)
+    limot2.limit = 0;
+    if ( limot2.fmax > 0 ) info->m++;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
+/// relative to body 1 and 2 initially) and then computes the constrained
+/// rotational axis as the cross product of ax1 and ax2.
+/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
+/// from dot and cross product rules.
+///
+/// @param ax1 Will contain the joint axis1 in world frame
+/// @param ax2 Will contain the joint axis2 in world frame
+/// @param axis Will contain the cross product of ax1 x ax2
+/// @param sin_angle
+/// @param cos_angle
+////////////////////////////////////////////////////////////////////////////////
+void
+dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
+                           dReal &sin_angle, dReal &cos_angle) const
+{
+    dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
+    dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
+    dCalcVectorCross3(axCross,ax1,ax2);
+    sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
+    cos_angle = dCalcVectorDot3 (ax1,ax2);
+}
+
+
+void
+dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // get information we need to set the hinge row
+    dReal s, c;
+    dVector3 q;
+
+    dVector3 ax1, ax2;
+    getAxisInfo( ax1, ax2, q, s, c );
+    dNormalize3( q );   // @@@ quicker: divide q by s ?
+
+    // set the three ball-and-socket rows (aligned to the suspension axis ax1)
+    setBall2( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2, ax1, susp_erp );
+    // set parameter for the suspension
+    pairRhsCfm[GI2_CFM] = susp_cfm;
+
+    // set the hinge row
+    int currRowSkip = 3 * rowskip;
+    dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q);
+    if ( node[1].body ) {
+        dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q);
+    }
+
+    // compute the right hand side for the constrained rotational DOF.
+    // axis 1 and axis 2 are separated by an angle `theta'. the desired
+    // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
+    // in the joint structure. the correcting angular velocity is:
+    //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
+    //                      = (erp*fps) * (theta0-theta)
+    // (theta0-theta) can be computed using the following small-angle-difference
+    // approximation:
+    //   theta0-theta ~= tan(theta0-theta)
+    //                 = sin(theta0-theta)/cos(theta0-theta)
+    //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
+    //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
+    // where c = cos(theta), s = sin(theta)
+    //       c0 = cos(theta0), s0 = sin(theta0)
+
+    dReal k = worldFPS * worldERP;
+
+    int currPairSkip = 3 * pairskip;
+    pairRhsCfm[currPairSkip + GI2_RHS] = k * ( c0 * s - this->s0 * c );
+
+    currRowSkip += rowskip; currPairSkip += pairskip;
+    // if the axis1 hinge is powered, or has joint limits, add in more stuff
+    if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
+        currRowSkip += rowskip; currPairSkip += pairskip;
+    }
+
+    // if the axis2 hinge is powered, add in more stuff
+    limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
+}
+
+
+// compute vectors v1 and v2 (embedded in body1), used to measure angle
+// between body 1 and body 2
+
+void
+dxJointHinge2::makeV1andV2()
+{
+    if ( node[0].body )
+    {
+        // get axis 1 and 2 in global coords
+        dVector3 ax1, ax2, v;
+        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
+
+        // modify axis 2 so it's perpendicular to axis 1
+        dReal k = dCalcVectorDot3( ax1, ax2 );
+        dAddVectorScaledVector3(ax2, ax2, ax1, -k);
+        
+        if (dxSafeNormalize3( ax2 )) {
+            // make v1 = modified axis2, v2 = axis1 x (modified axis2)
+            dCalcVectorCross3( v, ax1, ax2 );
+            dMultiply1_331( v1, node[0].body->posr.R, ax2 );
+            dMultiply1_331( v2, node[0].body->posr.R, v );
+        }
+        else {
+            dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent");
+        }
+    }
+}
+
+// same as above, but for the second axis
+
+void
+dxJointHinge2::makeW1andW2()
+{
+    if ( node[1].body )
+    {
+        // get axis 1 and 2 in global coords
+        dVector3 ax1, ax2, w;
+        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
+
+        // modify axis 1 so it's perpendicular to axis 2
+        dReal k = dCalcVectorDot3( ax2, ax1 );
+        dAddVectorScaledVector3(ax1, ax1, ax2, -k);
+        
+        if (dxSafeNormalize3( ax1 )) {
+            // make w1 = modified axis1, w2 = axis2 x (modified axis1)
+            dCalcVectorCross3( w, ax2, ax1 );
+            dMultiply1_331( w1, node[1].body->posr.R, ax1 );
+            dMultiply1_331( w2, node[1].body->posr.R, w );
+        }
+        else {
+            dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent");
+        }
+    }
+}
+
+
+/*ODE_API */
+void dJointSetHinge2Anchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+    
+    joint->makeV1andV2();
+    joint->makeW1andW2();
+}
+
+
+/*ODE_API */
+void dJointSetHinge2Axes (dJointID j, const dReal *axis1/*=[dSA__MAX],=NULL*/, const dReal *axis2/*=[dSA__MAX],=NULL*/)
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    dAASSERT(axis1 != NULL || axis2 != NULL);
+    dAASSERT(joint->node[0].body != NULL || axis1 == NULL);
+    dAASSERT(joint->node[1].body != NULL || axis2 == NULL);
+
+    if ( axis1 != NULL )
+    {
+        setAxes(joint, axis1[dSA_X], axis1[dSA_Y], axis1[dSA_Z], joint->axis1, NULL);
+    }
+    
+    if ( axis2 != NULL )
+    {
+        setAxes(joint, axis2[dSA_X], axis2[dSA_Y], axis2[dSA_Z], NULL, joint->axis2);
+    }
+
+    // compute the sin and cos of the angle between axis 1 and axis 2
+    dVector3 ax1, ax2, ax;
+    joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 );
+
+    joint->makeV1andV2();
+    joint->makeW1andW2();
+}
+
+
+/*ODE_API_DEPRECATED ODE_API */
+void dJointSetHinge2Axis1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dVector3 axis1;
+    axis1[dSA_X] = x; axis1[dSA_Y] = y; axis1[dSA_Z] = z;
+    dJointSetHinge2Axes(j, axis1, NULL);
+}
+
+/*ODE_API_DEPRECATED ODE_API */
+void dJointSetHinge2Axis2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dVector3 axis2;
+    axis2[dSA_X] = x; axis2[dSA_Y] = y; axis2[dSA_Z] = z;
+    dJointSetHinge2Axes(j, NULL, axis2);
+}
+
+
+void dJointSetHinge2Param( dJointID j, int parameter, dReal value )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        joint->limot2.set( parameter & 0xff, value );
+    }
+    else
+    {
+        if ( parameter == dParamSuspensionERP ) joint->susp_erp = value;
+        else if ( parameter == dParamSuspensionCFM ) joint->susp_cfm = value;
+        else joint->limot1.set( parameter, value );
+    }
+}
+
+
+void dJointGetHinge2Anchor( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+        getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetHinge2Anchor2( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor( joint, result, joint->anchor1 );
+    else
+        getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dJointGetHinge2Axis1( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body )
+    {
+        dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis1 );
+    }
+    else
+    {
+        dZeroVector3(result);
+        dUASSERT( false, "the joint does not have first body attached" );
+    }
+}
+
+
+void dJointGetHinge2Axis2( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[1].body )
+    {
+        dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
+    }
+    else
+    {
+        dZeroVector3(result);
+        dUASSERT( false, "the joint does not have second body attached" );
+    }
+}
+
+
+dReal dJointGetHinge2Param( dJointID j, int parameter )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        return joint->limot2.get( parameter & 0xff );
+    }
+    else
+    {
+        if ( parameter == dParamSuspensionERP ) return joint->susp_erp;
+        else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm;
+        else return joint->limot1.get( parameter );
+    }
+}
+
+
+dReal dJointGetHinge2Angle1( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    return joint->measureAngle1();
+}
+
+
+dReal dJointGetHinge2Angle2( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    return joint->measureAngle2();
+}
+
+
+
+dReal dJointGetHinge2Angle1Rate( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body )
+            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    else return 0;
+}
+
+
+dReal dJointGetHinge2Angle2Rate( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body && joint->node[1].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body )
+            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    else return 0;
+}
+
+
+void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dVector3 axis1, axis2;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    if ( joint->node[0].body && joint->node[1].body )
+    {
+        dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
+        dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
+        axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
+        axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
+        axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
+        dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
+        dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
+    }
+}
+
+
+dJointType
+dxJointHinge2::type() const
+{
+    return dJointTypeHinge2;
+}
+
+
+sizeint
+dxJointHinge2::size() const
+{
+    return sizeof( *this );
+}
+
+
+void
+dxJointHinge2::setRelativeValues()
+{
+    dVector3 anchor;
+    dJointGetHinge2Anchor(this, anchor);
+    setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
+
+    dVector3 axis;
+
+    if ( node[0].body )
+    {
+        dJointGetHinge2Axis1(this, axis);
+        setAxes( this, axis[0],axis[1],axis[2], axis1, NULL );
+    }
+
+    if ( node[0].body )
+    {
+        dJointGetHinge2Axis2(this, axis);
+        setAxes( this, axis[0],axis[1],axis[2], NULL, axis2 );
+    }
+
+    dVector3 ax1, ax2;
+    getAxisInfo( ax1, ax2, axis, s0, c0 );
+
+    makeV1andV2();
+    makeW1andW2();
+}

thirdparty/ode-0.16.5/ode/src/joints/hinge2.h

@@ -0,0 +1,71 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_HINGE2_H_
+#define _ODE_JOINT_HINGE2_H_
+
+#include "joint.h"
+
+
+// hinge 2
+
+struct dxJointHinge2 : public dxJoint
+{
+    dVector3 anchor1;   // anchor w.r.t first body
+    dVector3 anchor2;   // anchor w.r.t second body
+    dVector3 axis1;     // axis 1 w.r.t first body
+    dVector3 axis2;     // axis 2 w.r.t second body
+    dReal c0, s0;       // cos,sin of desired angle between axis 1,2
+    dVector3 v1, v2;    // angle ref vectors embedded in first body
+    dVector3 w1, w2;    // angle ref vectors embedded in second body
+    dxJointLimitMotor limot1; // limit+motor info for axis 1
+    dxJointLimitMotor limot2; // limit+motor info for axis 2
+    dReal susp_erp, susp_cfm; // suspension parameters (erp,cfm)
+
+
+    dReal measureAngle1() const;
+    dReal measureAngle2() const;
+    void makeV1andV2();
+    void makeW1andW2();
+
+    void getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axis,
+        dReal &sin_angle, dReal &cos_Angle) const;
+
+
+
+    dxJointHinge2( dxWorld *w );
+
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    virtual void setRelativeValues();
+};
+
+
+
+#endif

thirdparty/ode-0.16.5/ode/src/joints/joint.cpp

@@ -0,0 +1,931 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+design note: the general principle for giving a joint the option of connecting
+to the static environment (i.e. the absolute frame) is to check the second
+body (joint->node[1].body), and if it is zero then behave as if its body
+transform is the identity.
+
+*/
+
+#include <ode/ode.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+#include "joint.h"
+#include "joint_internal.h"
+#include "util.h"
+
+extern void addObjectToList( dObject *obj, dObject **first );
+
+dxJoint::dxJoint( dxWorld *w ) :
+    dObject( w )
+{
+    //printf("constructing %p\n", this);
+    dIASSERT( w );
+    flags = 0;
+    node[0].joint = this;
+    node[0].body = 0;
+    node[0].next = 0;
+    node[1].joint = this;
+    node[1].body = 0;
+    node[1].next = 0;
+    dSetZero( lambda, 6 );
+
+    addObjectToList( this, ( dObject ** ) &w->firstjoint );
+
+    w->nj++;
+    feedback = 0;
+}
+
+dxJoint::~dxJoint()
+{ }
+
+
+/*virtual */
+void dxJoint::setRelativeValues()
+{
+    // Do nothing
+}
+
+bool dxJoint::isEnabled() const
+{
+    return ( (flags & dJOINT_DISABLED) == 0 &&
+        (node[0].body->invMass > 0 ||
+        (node[1].body && node[1].body->invMass > 0)) );
+}
+
+
+sizeint dxJointGroup::exportJoints(dxJoint **jlist)
+{
+    sizeint i=0;
+    dxJoint *j = (dxJoint*) m_stack.rewind();
+    while (j != NULL) {
+        jlist[i++] = j;
+        j = (dxJoint*) (m_stack.next (j->size()));
+    }
+    return i;
+}
+
+void dxJointGroup::freeAll()
+{
+    m_num = 0;
+    m_stack.freeAll();
+}
+
+
+//****************************************************************************
+// externs
+
+// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
+// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
+
+//****************************************************************************
+// utility
+
+// set three "ball-and-socket" rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setBall( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dVector3 anchor1, dVector3 anchor2 )
+{
+    // anchor points in global coordinates with respect to body PORs.
+    dVector3 a1, a2;
+
+    // set Jacobian
+    J1[dxJoint::GI2_JLX] = 1;
+    J1[rowskip + dxJoint::GI2_JLY] = 1;
+    J1[2 * rowskip + dxJoint::GI2_JLZ] = 1;
+    dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+    dSetCrossMatrixMinus( J1 + dxJoint::GI2__JA_MIN, a1, rowskip );
+
+    dxBody *b1 = joint->node[1].body;
+    if ( b1 )
+    {
+        J2[dxJoint::GI2_JLX] = -1;
+        J2[rowskip + dxJoint::GI2_JLY] = -1;
+        J2[2 * rowskip + dxJoint::GI2_JLZ] = -1;
+        dMultiply0_331( a2, b1->posr.R, anchor2 );
+        dSetCrossMatrixPlus( J2 + dxJoint::GI2__JA_MIN, a2, rowskip );
+    }
+
+    // set right hand side
+    dReal k = fps * erp;
+    dxBody *b0 = joint->node[0].body;
+    if ( b1 )
+    {
+        dReal *currRhsCfm = pairRhsCfm;
+        for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+        {
+            currRhsCfm[dxJoint::GI2_RHS] = k * ( a2[j] + b1->posr.pos[j] - a1[j] - b0->posr.pos[j] );
+            currRhsCfm += pairskip;
+        }
+    }
+    else
+    {
+        dReal *currRhsCfm = pairRhsCfm;
+        for ( int j = dSA__MIN; j != dSA__MAX; j++ )
+        {
+            currRhsCfm[dxJoint::GI2_RHS] = k * ( anchor2[j] - a1[j] - b0->posr.pos[j] );
+            currRhsCfm += pairskip;
+        }
+    }
+}
+
+
+// this is like setBall(), except that `axis' is a unit length vector
+// (in global coordinates) that should be used for the first jacobian
+// position row (the other two row vectors will be derived from this).
+// `erp1' is the erp value to use along the axis.
+
+void setBall2( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dVector3 anchor1, dVector3 anchor2,
+    dVector3 axis, dReal erp1 )
+{
+    // anchor points in global coordinates with respect to body PORs.
+    dVector3 a1, a2;
+
+    // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
+    // [0 1 0] and [0 0 1], which makes everything much easier.
+    dVector3 q1, q2;
+    dPlaneSpace( axis, q1, q2 );
+
+    // set Jacobian
+    dCopyVector3(J1 + dxJoint::GI2__JL_MIN, axis);
+    dCopyVector3(J1 + rowskip + dxJoint::GI2__JL_MIN, q1);
+    dCopyVector3(J1 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+    dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
+    dCalcVectorCross3( J1 + dxJoint::GI2__JA_MIN, a1, axis );
+    dCalcVectorCross3( J1 + rowskip + dxJoint::GI2__JA_MIN, a1, q1 );
+    dCalcVectorCross3( J1 + 2 * rowskip + dxJoint::GI2__JA_MIN, a1, q2 );
+
+    dxBody *b0 = joint->node[0].body;
+    dAddVectors3(a1, a1, b0->posr.pos);
+
+    // set right hand side - measure error along (axis,q1,q2)
+    dReal k1 = fps * erp1;
+    dReal k = fps * erp;
+
+    dxBody *b1 = joint->node[1].body;
+    if ( b1 )
+    {
+        dCopyNegatedVector3(J2 + dxJoint::GI2__JL_MIN, axis);
+        dCopyNegatedVector3(J2 + rowskip + dxJoint::GI2__JL_MIN, q1);
+        dCopyNegatedVector3(J2 + 2 * rowskip + dxJoint::GI2__JL_MIN, q2);
+        dMultiply0_331( a2, b1->posr.R, anchor2 );
+        dCalcVectorCross3( J2 + dxJoint::GI2__JA_MIN, axis, a2 ); //== dCalcVectorCross3( J2 + dxJoint::GI2__J2A_MIN, a2, axis ); dNegateVector3( J2 + dxJoint::GI2__J2A_MIN );
+        dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__JA_MIN, q1, a2 ); //== dCalcVectorCross3( J2 + rowskip + dxJoint::GI2__J2A_MIN, a2, q1 ); dNegateVector3( J2 + rowskip + dxJoint::GI2__J2A_MIN );
+        dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__JA_MIN, q2, a2 ); //== dCalcVectorCross3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN, a2, q2 ); dNegateVector3( J2 + 2 * rowskip + dxJoint::GI2__J2A_MIN );
+
+        dAddVectors3(a2, a2, b1->posr.pos);
+
+        dVector3 a2_minus_a1;
+        dSubtractVectors3(a2_minus_a1, a2, a1);
+        pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
+        pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, a2_minus_a1 );
+        pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, a2_minus_a1 );
+    }
+    else
+    {
+        dVector3 anchor2_minus_a1;
+        dSubtractVectors3(anchor2_minus_a1, anchor2, a1);
+        pairRhsCfm[dxJoint::GI2_RHS] = k1 * dCalcVectorDot3( axis, anchor2_minus_a1 );
+        pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q1, anchor2_minus_a1 );
+        pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k * dCalcVectorDot3( q2, anchor2_minus_a1 );
+    }
+}
+
+
+// set three orientation rows in the constraint equation, and the
+// corresponding right hand side.
+
+void setFixedOrientation( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dQuaternion qrel )
+{
+    // 3 rows to make body rotations equal
+    J1[dxJoint::GI2_JAX] = 1;
+    J1[rowskip + dxJoint::GI2_JAY] = 1;
+    J1[2 * rowskip + dxJoint::GI2_JAZ] = 1;
+
+    dxBody *b1 = joint->node[1].body;
+    if ( b1 )
+    {
+        J2[dxJoint::GI2_JAX] = -1;
+        J2[rowskip + dxJoint::GI2_JAY] = -1;
+        J2[2 * rowskip + dxJoint::GI2_JAZ] = -1;
+    }
+
+    // compute the right hand side. the first three elements will result in
+    // relative angular velocity of the two bodies - this is set to bring them
+    // back into alignment. the correcting angular velocity is
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * u
+    //                      = (erp*fps) * theta * u
+    // where rotation along unit length axis u by theta brings body 2's frame
+    // to qrel with respect to body 1's frame. using a small angle approximation
+    // for sin(), this gives
+    //    angular_velocity  = (erp*fps) * 2 * v
+    // where the quaternion of the relative rotation between the two bodies is
+    //    q = [cos(theta/2) sin(theta/2)*u] = [s v]
+
+    // get qerr = relative rotation (rotation error) between two bodies
+    dQuaternion qerr, e;
+    dxBody *b0 = joint->node[0].body;
+    if ( b1 )
+    {
+        dQuaternion qq;
+        dQMultiply1( qq, b0->q, b1->q );
+        dQMultiply2( qerr, qq, qrel );
+    }
+    else
+    {
+        dQMultiply3( qerr, b0->q, qrel );
+    }
+    if ( qerr[0] < 0 )
+    {
+        qerr[1] = -qerr[1];  // adjust sign of qerr to make theta small
+        qerr[2] = -qerr[2];
+        qerr[3] = -qerr[3];
+    }
+    dMultiply0_331( e, b0->posr.R, qerr + 1 );  // @@@ bad SIMD padding!
+    dReal k_mul_2 = fps * erp * REAL(2.0);
+    pairRhsCfm[dxJoint::GI2_RHS] = k_mul_2 * e[dSA_X];
+    pairRhsCfm[pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Y];
+    pairRhsCfm[2 * pairskip + dxJoint::GI2_RHS] = k_mul_2 * e[dSA_Z];
+}
+
+
+// compute anchor points relative to bodies
+
+void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
+                dVector3 anchor1, dVector3 anchor2 )
+{
+    dxBody *b0 = j->node[0].body;
+    if ( b0 )
+    {
+        dReal q[4];
+        q[0] = x - b0->posr.pos[0];
+        q[1] = y - b0->posr.pos[1];
+        q[2] = z - b0->posr.pos[2];
+        q[3] = 0;
+        dMultiply1_331( anchor1, b0->posr.R, q );
+
+        dxBody *b1 = j->node[1].body;
+        if ( b1 )
+        {
+            q[0] = x - b1->posr.pos[0];
+            q[1] = y - b1->posr.pos[1];
+            q[2] = z - b1->posr.pos[2];
+            q[3] = 0;
+            dMultiply1_331( anchor2, b1->posr.R, q );
+        }
+        else
+        {
+            anchor2[0] = x;
+            anchor2[1] = y;
+            anchor2[2] = z;
+        }
+    }
+    anchor1[3] = 0;
+    anchor2[3] = 0;
+}
+
+
+// compute axes relative to bodies. either axis1 or axis2 can be 0.
+
+void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
+             dVector3 axis1, dVector3 axis2 )
+{
+    dxBody *b0 = j->node[0].body;
+    if ( b0 )
+    {
+        dReal q[4];
+        q[0] = x;
+        q[1] = y;
+        q[2] = z;
+        q[3] = 0;
+        dNormalize3( q );
+
+        if ( axis1 )
+        {
+            dMultiply1_331( axis1, b0->posr.R, q );
+            axis1[3] = 0;
+        }
+
+        if ( axis2 )
+        {
+            dxBody *b1 = j->node[1].body;
+            if ( b1 )
+            {
+                dMultiply1_331( axis2, b1->posr.R, q );
+            }
+            else
+            {
+                axis2[0] = x;
+                axis2[1] = y;
+                axis2[2] = z;
+            }
+            axis2[3] = 0;
+        }
+    }
+}
+
+
+void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
+{
+    dxBody *b0 = j->node[0].body;
+    if ( b0 )
+    {
+        dMultiply0_331( result, b0->posr.R, anchor1 );
+        result[0] += b0->posr.pos[0];
+        result[1] += b0->posr.pos[1];
+        result[2] += b0->posr.pos[2];
+    }
+}
+
+
+void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )
+{
+    dxBody *b1 = j->node[1].body;
+    if ( b1 )
+    {
+        dMultiply0_331( result, b1->posr.R, anchor2 );
+        result[0] += b1->posr.pos[0];
+        result[1] += b1->posr.pos[1];
+        result[2] += b1->posr.pos[2];
+    }
+    else
+    {
+        result[0] = anchor2[0];
+        result[1] = anchor2[1];
+        result[2] = anchor2[2];
+    }
+}
+
+
+void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
+{
+    dxBody *b0 = j->node[0].body;
+    if ( b0 )
+    {
+        dMultiply0_331( result, b0->posr.R, axis1 );
+    }
+}
+
+
+void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
+{
+    dxBody *b1 = j->node[1].body;
+    if ( b1 )
+    {
+        dMultiply0_331( result, b1->posr.R, axis2 );
+    }
+    else
+    {
+        result[0] = axis2[0];
+        result[1] = axis2[1];
+        result[2] = axis2[2];
+    }
+}
+
+
+dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
+{
+    // the angle between the two bodies is extracted from the quaternion that
+    // represents the relative rotation between them. recall that a quaternion
+    // q is:
+    //    [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
+    // where s is a scalar and v is a 3-vector. u is a unit length axis and
+    // theta is a rotation along that axis. we can get theta/2 by:
+    //    theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
+    // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
+    //    |v| = |sin(theta/2)| * |u|
+    //        = |sin(theta/2)|
+    // using this value will have a strange effect. recall that there are two
+    // quaternion representations of a given rotation, q and -q. typically as
+    // a body rotates along the axis it will go through a complete cycle using
+    // one representation and then the next cycle will use the other
+    // representation. this corresponds to u pointing in the direction of the
+    // hinge axis and then in the opposite direction. the result is that theta
+    // will appear to go "backwards" every other cycle. here is a fix: if u
+    // points "away" from the direction of the hinge (motor) axis (i.e. more
+    // than 90 degrees) then use -q instead of q. this represents the same
+    // rotation, but results in the cos(theta/2) value being sign inverted.
+
+    // extract the angle from the quaternion. cost2 = cos(theta/2),
+    // sint2 = |sin(theta/2)|
+    dReal cost2 = qrel[0];
+    dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
+    dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
+        ( 2 * dAtan2( sint2, cost2 ) ) :  // if u points in direction of axis
+        ( 2 * dAtan2( sint2, -cost2 ) );  // if u points in opposite direction
+
+    // the angle we get will be between 0..2*pi, but we want to return angles
+    // between -pi..pi
+    if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
+
+    // the angle we've just extracted has the wrong sign
+    theta = -theta;
+
+    return theta;
+}
+
+
+// given two bodies (body1,body2), the hinge axis that they are connected by
+// w.r.t. body1 (axis), and the initial relative orientation between them
+// (q_initial), return the relative rotation angle. the initial relative
+// orientation corresponds to an angle of zero. if body2 is 0 then measure the
+// angle between body1 and the static frame.
+//
+// this will not return the correct angle if the bodies rotate along any axis
+// other than the given hinge axis.
+
+dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis,
+                    dQuaternion q_initial )
+{
+    // get qrel = relative rotation between the two bodies
+    dQuaternion qrel;
+    if ( body2 )
+    {
+        dQuaternion qq;
+        dQMultiply1( qq, body1->q, body2->q );
+        dQMultiply2( qrel, qq, q_initial );
+    }
+    else
+    {
+        // pretend body2->q is the identity
+        dQMultiply3( qrel, body1->q, q_initial );
+    }
+
+    return getHingeAngleFromRelativeQuat( qrel, axis );
+}
+
+//****************************************************************************
+// dxJointLimitMotor
+
+void dxJointLimitMotor::init( dxWorld *world )
+{
+    vel = 0;
+    fmax = 0;
+    lostop = -dInfinity;
+    histop = dInfinity;
+    fudge_factor = 1;
+    normal_cfm = world->global_cfm;
+    stop_erp = world->global_erp;
+    stop_cfm = world->global_cfm;
+    bounce = 0;
+    limit = 0;
+    limit_err = 0;
+}
+
+
+void dxJointLimitMotor::set( int num, dReal value )
+{
+    switch ( num )
+    {
+    case dParamLoStop:
+        lostop = value;
+        break;
+    case dParamHiStop:
+        histop = value;
+        break;
+    case dParamVel:
+        vel = value;
+        break;
+    case dParamFMax:
+        if ( value >= 0 ) fmax = value;
+        break;
+    case dParamFudgeFactor:
+        if ( value >= 0 && value <= 1 ) fudge_factor = value;
+        break;
+    case dParamBounce:
+        bounce = value;
+        break;
+    case dParamCFM:
+        normal_cfm = value;
+        break;
+    case dParamStopERP:
+        stop_erp = value;
+        break;
+    case dParamStopCFM:
+        stop_cfm = value;
+        break;
+    }
+}
+
+
+dReal dxJointLimitMotor::get( int num ) const
+{
+    switch ( num )
+    {
+    case dParamLoStop:
+        return lostop;
+    case dParamHiStop:
+        return histop;
+    case dParamVel:
+        return vel;
+    case dParamFMax:
+        return fmax;
+    case dParamFudgeFactor:
+        return fudge_factor;
+    case dParamBounce:
+        return bounce;
+    case dParamCFM:
+        return normal_cfm;
+    case dParamStopERP:
+        return stop_erp;
+    case dParamStopCFM:
+        return stop_cfm;
+    default:
+        return 0;
+    }
+}
+
+
+bool dxJointLimitMotor::testRotationalLimit( dReal angle )
+{
+    if ( angle <= lostop )
+    {
+        limit = 1;
+        limit_err = angle - lostop;
+        return true;
+    }
+    else if ( angle >= histop )
+    {
+        limit = 2;
+        limit_err = angle - histop;
+        return true;
+    }
+    else
+    {
+        limit = 0;
+        return false;
+    }
+}
+
+
+bool dxJointLimitMotor::addLimot( dxJoint *joint,
+    dReal fps, dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+    const dVector3 ax1, int rotational )
+{
+    // if the joint is powered, or has joint limits, add in the extra row
+    int powered = fmax > 0;
+    if ( powered || limit )
+    {
+        dReal *J1Used = rotational ? J1 + GI2__JA_MIN : J1 + GI2__JL_MIN;
+        dReal *J2Used = rotational ? J2 + GI2__JA_MIN : J2 + GI2__JL_MIN;
+
+        dCopyVector3(J1Used, ax1);
+
+        dxBody *b1 = joint->node[1].body;
+        if ( b1 )
+        {
+            dCopyNegatedVector3(J2Used, ax1);
+        }
+
+        // linear limot torque decoupling step:
+        //
+        // if this is a linear limot (e.g. from a slider), we have to be careful
+        // that the linear constraint forces (+/- ax1) applied to the two bodies
+        // do not create a torque couple. in other words, the points that the
+        // constraint force is applied at must lie along the same ax1 axis.
+        // a torque couple will result in powered or limited slider-jointed free
+        // bodies from gaining angular momentum.
+        // the solution used here is to apply the constraint forces at the point
+        // halfway between the body centers. there is no penalty (other than an
+        // extra tiny bit of computation) in doing this adjustment. note that we
+        // only need to do this if the constraint connects two bodies.
+
+        dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
+        if ( !rotational && b1 )
+        {
+            dxBody *b0 = joint->node[0].body;
+            dVector3 c;
+            c[0] = REAL( 0.5 ) * ( b1->posr.pos[0] - b0->posr.pos[0] );
+            c[1] = REAL( 0.5 ) * ( b1->posr.pos[1] - b0->posr.pos[1] );
+            c[2] = REAL( 0.5 ) * ( b1->posr.pos[2] - b0->posr.pos[2] );
+            dCalcVectorCross3( ltd, c, ax1 );
+            dCopyVector3(J1 + dxJoint::GI2__JA_MIN, ltd);
+            dCopyVector3(J2 + dxJoint::GI2__JA_MIN, ltd);
+        }
+
+        // if we're limited low and high simultaneously, the joint motor is
+        // ineffective
+        if ( limit && ( lostop == histop ) ) powered = 0;
+
+        if ( powered )
+        {
+            pairRhsCfm[GI2_CFM] = normal_cfm;
+            if ( ! limit )
+            {
+                pairRhsCfm[GI2_RHS] = vel;
+                pairLoHi[GI2_LO] = -fmax;
+                pairLoHi[GI2_HI] = fmax;
+            }
+            else
+            {
+                // the joint is at a limit, AND is being powered. if the joint is
+                // being powered into the limit then we apply the maximum motor force
+                // in that direction, because the motor is working against the
+                // immovable limit. if the joint is being powered away from the limit
+                // then we have problems because actually we need *two* lcp
+                // constraints to handle this case. so we fake it and apply some
+                // fraction of the maximum force. the fraction to use can be set as
+                // a fudge factor.
+
+                dReal fm = fmax;
+                if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+                // if we're powering away from the limit, apply the fudge factor
+                if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+                
+                dReal fm_ax1_0 = fm*ax1[0], fm_ax1_1 = fm*ax1[1], fm_ax1_2 = fm*ax1[2];
+                
+                dxBody *b0 = joint->node[0].body;
+                dxWorldProcessContext *world_process_context = b0->world->unsafeGetWorldProcessingContext(); 
+
+                world_process_context->LockForAddLimotSerialization();
+
+                if ( rotational )
+                {
+                    dxBody *b1 = joint->node[1].body;
+                    if ( b1 != NULL ) 
+                    {
+                        dBodyAddTorque( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+                    }
+
+                    dBodyAddTorque( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+                }
+                else
+                {
+                    dxBody *b1 = joint->node[1].body;
+                    if ( b1 != NULL )
+                    {
+                        // linear limot torque decoupling step: refer to above discussion
+                        dReal neg_fm_ltd_0 = -fm*ltd[0], neg_fm_ltd_1 = -fm*ltd[1], neg_fm_ltd_2 = -fm*ltd[2];
+                        dBodyAddTorque( b0, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+                        dBodyAddTorque( b1, neg_fm_ltd_0, neg_fm_ltd_1, neg_fm_ltd_2 );
+
+                        dBodyAddForce( b1, fm_ax1_0, fm_ax1_1, fm_ax1_2 );
+                    }
+
+                    dBodyAddForce( b0, -fm_ax1_0, -fm_ax1_1, -fm_ax1_2 );
+                }
+
+                world_process_context->UnlockForAddLimotSerialization();
+            }
+        }
+
+        if ( limit )
+        {
+            dReal k = fps * stop_erp;
+            pairRhsCfm[GI2_RHS] = -k * limit_err;
+            pairRhsCfm[GI2_CFM] = stop_cfm;
+
+            if ( lostop == histop )
+            {
+                // limited low and high simultaneously
+                pairLoHi[GI2_LO] = -dInfinity;
+                pairLoHi[GI2_HI] = dInfinity;
+            }
+            else
+            {
+                if ( limit == 1 )
+                {
+                    // low limit
+                    pairLoHi[GI2_LO] = 0;
+                    pairLoHi[GI2_HI] = dInfinity;
+                }
+                else
+                {
+                    // high limit
+                    pairLoHi[GI2_LO] = -dInfinity;
+                    pairLoHi[GI2_HI] = 0;
+                }
+
+                // deal with bounce
+                if ( bounce > 0 )
+                {
+                    // calculate joint velocity
+                    dReal vel;
+                    if ( rotational )
+                    {
+                        vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
+                        if ( joint->node[1].body )
+                            vel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
+                    }
+                    else
+                    {
+                        vel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
+                        if ( joint->node[1].body )
+                            vel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
+                    }
+
+                    // only apply bounce if the velocity is incoming, and if the
+                    // resulting c[] exceeds what we already have.
+                    if ( limit == 1 )
+                    {
+                        // low limit
+                        if ( vel < 0 )
+                        {
+                            dReal newc = -bounce * vel;
+                            if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+                        }
+                    }
+                    else
+                    {
+                        // high limit - all those computations are reversed
+                        if ( vel > 0 )
+                        {
+                            dReal newc = -bounce * vel;
+                            if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return true;
+    }
+    return false;
+}
+
+/**
+    This function generalizes the "linear limot torque decoupling"
+    in addLimot to use anchor points provided by the caller.
+
+    This makes it so that the appropriate torques are applied to
+    a body when it's being linearly motored or limited using anchor points
+    that aren't at the center of mass.
+
+    pt1 and pt2 are centered in body coordinates but use global directions.
+    I.e., they are conveniently found within joint code with:
+      getAxis(joint,pt1,anchor1);
+      getAxis2(joint,pt2,anchor2);
+*/
+bool dxJointLimitMotor::addTwoPointLimot( dxJoint *joint, dReal fps,
+    dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+    const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 )
+{
+    // if the joint is powered, or has joint limits, add in the extra row
+    int powered = fmax > 0;
+    if ( powered || limit )
+    {
+        // Set the linear portion
+        dCopyVector3(J1 + GI2__JL_MIN, ax1);
+        // Set the angular portion (to move the linear constraint 
+        // away from the center of mass).  
+        dCalcVectorCross3(J1 + GI2__JA_MIN, pt1, ax1);
+        // Set the constraints for the second body
+        if ( joint->node[1].body ) {
+            dCopyNegatedVector3(J2 + GI2__JL_MIN, ax1);
+            dCalcVectorCross3(J2 + GI2__JA_MIN, pt2, J2 + GI2__JL_MIN);
+        }
+
+        // if we're limited low and high simultaneously, the joint motor is
+        // ineffective
+        if ( limit && ( lostop == histop ) ) powered = 0;
+
+        if ( powered )
+        {
+            pairRhsCfm[GI2_CFM] = normal_cfm;
+            if ( ! limit )
+            {
+                pairRhsCfm[GI2_RHS] = vel;
+                pairLoHi[GI2_LO] = -fmax;
+                pairLoHi[GI2_HI] = fmax;
+            }
+            else
+            {
+                // the joint is at a limit, AND is being powered. if the joint is
+                // being powered into the limit then we apply the maximum motor force
+                // in that direction, because the motor is working against the
+                // immovable limit. if the joint is being powered away from the limit
+                // then we have problems because actually we need *two* lcp
+                // constraints to handle this case. so we fake it and apply some
+                // fraction of the maximum force. the fraction to use can be set as
+                // a fudge factor.
+
+                dReal fm = fmax;
+                if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
+
+                // if we're powering away from the limit, apply the fudge factor
+                if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
+
+               
+                const dReal* tAx1 = J1 + GI2__JA_MIN;
+                dBodyAddForce( joint->node[0].body, -fm*ax1[dSA_X], -fm*ax1[dSA_Y], -fm*ax1[dSA_Z] );
+                dBodyAddTorque( joint->node[0].body, -fm*tAx1[dSA_X], -fm*tAx1[dSA_Y], -fm*tAx1[dSA_Z] );
+
+                if ( joint->node[1].body )
+                {
+                    const dReal* tAx2 = J2 + GI2__JA_MIN;
+                    dBodyAddForce( joint->node[1].body, fm*ax1[dSA_X], fm*ax1[dSA_Y], fm*ax1[dSA_Z] );
+                    dBodyAddTorque( joint->node[1].body, -fm*tAx2[dSA_X], -fm*tAx2[dSA_Y], -fm*tAx2[dSA_Z] );
+                }
+                
+            }
+        }
+
+        if ( limit )
+        {
+            dReal k = fps * stop_erp;
+            pairRhsCfm[GI2_RHS] = -k * limit_err;
+            pairRhsCfm[GI2_CFM] = stop_cfm;
+
+            if ( lostop == histop )
+            {
+                // limited low and high simultaneously
+                pairLoHi[GI2_LO] = -dInfinity;
+                pairLoHi[GI2_HI] = dInfinity;
+            }
+            else
+            {
+                if ( limit == 1 )
+                {
+                    // low limit
+                    pairLoHi[GI2_LO] = 0;
+                    pairLoHi[GI2_HI] = dInfinity;
+                }
+                else
+                {
+                    // high limit
+                    pairLoHi[GI2_LO] = -dInfinity;
+                    pairLoHi[GI2_HI] = 0;
+                }
+
+                // deal with bounce
+                if ( bounce > 0 )
+                {
+                    // calculate relative velocity of the two anchor points
+                    dReal vel = 
+  	                    dCalcVectorDot3( joint->node[0].body->lvel, J1 + GI2__JL_MIN ) +
+  	                    dCalcVectorDot3( joint->node[0].body->avel, J1 + GI2__JA_MIN );
+  	                if (joint->node[1].body) {
+  	                    vel +=
+  	                        dCalcVectorDot3( joint->node[1].body->lvel, J2 + GI2__JL_MIN ) +
+  	                        dCalcVectorDot3( joint->node[1].body->avel, J2 + GI2__JA_MIN );
+  	                }
+
+                    // only apply bounce if the velocity is incoming, and if the
+                    // resulting c[] exceeds what we already have.
+                    if ( limit == 1 )
+                    {
+                        // low limit
+                        if ( vel < 0 )
+                        {
+                            dReal newc = -bounce * vel;
+                            if ( newc > pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+                        }
+                    }
+                    else
+                    {
+                        // high limit - all those computations are reversed
+                        if ( vel > 0 )
+                        {
+                            dReal newc = -bounce * vel;
+                            if ( newc < pairRhsCfm[GI2_RHS] ) pairRhsCfm[GI2_RHS] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return true;
+    }
+    return false;
+}
+
+
+// Local Variables:
+// mode:c++
+// c-basic-offset:4
+// End:

thirdparty/ode-0.16.5/ode/src/joints/joint.h

@@ -0,0 +1,326 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_H_
+#define _ODE_JOINT_H_
+
+
+#include <ode/contact.h>
+#include "../common.h"
+#include "../objects.h"
+#include "../obstack.h"
+
+
+// joint flags
+enum
+{
+    // if this flag is set, the joint was allocated in a joint group
+    dJOINT_INGROUP = 1,
+
+    // if this flag is set, the joint was attached with arguments (0,body).
+    // our convention is to treat all attaches as (body,0), i.e. so node[0].body
+    // is always nonzero, so this flag records the fact that the arguments were
+    // swapped.
+    dJOINT_REVERSE = 2,
+
+    // if this flag is set, the joint can not have just one body attached to it,
+    // it must have either zero or two bodies attached.
+    dJOINT_TWOBODIES = 4,
+
+    dJOINT_DISABLED = 8
+};
+
+
+enum dJointConnectedBody
+{
+    dJCB__MIN,
+
+    dJCB_FIRST_BODY = dJCB__MIN,
+    dJCB_SECOND_BODY,
+
+    dJCB__MAX,
+
+};
+
+static inline 
+dJointConnectedBody EncodeJointOtherConnectedBody(dJointConnectedBody cbBodyKind)
+{
+    dIASSERT(dIN_RANGE(cbBodyKind, dJCB__MIN, dJCB__MAX));
+    dSASSERT(dJCB__MAX == 2);
+
+    return (dJointConnectedBody)(dJCB_FIRST_BODY + dJCB_SECOND_BODY - cbBodyKind);
+}
+
+/* joint body relativity enumeration */
+enum dJointBodyRelativity 
+{
+    dJBR__MIN,
+
+    dJBR_GLOBAL = dJBR__MIN,
+
+    dJBR__BODIES_MIN,
+
+    dJBR_BODY1 = dJBR__BODIES_MIN + dJCB_FIRST_BODY,
+    dJBR_BODY2 = dJBR__BODIES_MIN + dJCB_SECOND_BODY,
+
+    dJBR__BODIES_MAX = dJBR__BODIES_MIN + dJCB__MAX,
+
+    dJBR__MAX,
+
+    dJBR__DEFAULT = dJBR_GLOBAL,
+    dJBR__BODIES_COUNT = dJBR__BODIES_MAX - dJBR__BODIES_MIN,
+
+};
+
+ODE_PURE_INLINE int dJBREncodeBodyRelativityStatus(int relativity)
+{
+    return dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX);
+}
+
+ODE_PURE_INLINE dJointBodyRelativity dJBRSwapBodyRelativity(int relativity)
+{
+    dIASSERT(dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX));
+    return (dJointBodyRelativity)(dJBR_BODY1 + dJBR_BODY2 - relativity);
+}
+
+
+
+
+// there are two of these nodes in the joint, one for each connection to a
+// body. these are node of a linked list kept by each body of it's connecting
+// joints. but note that the body pointer in each node points to the body that
+// makes use of the *other* node, not this node. this trick makes it a bit
+// easier to traverse the body/joint graph.
+
+struct dxJointNode
+{
+    dxJoint *joint;     // pointer to enclosing dxJoint object
+    dxBody *body;       // *other* body this joint is connected to
+    dxJointNode *next;  // next node in body's list of connected joints
+};
+
+
+struct dxJoint : public dObject
+{
+    // naming convention: the "first" body this is connected to is node[0].body,
+    // and the "second" body is node[1].body. if this joint is only connected
+    // to one body then the second body is 0.
+
+    // info returned by getInfo1 function. the constraint dimension is m (<=6).
+    // i.e. that is the total number of rows in the jacobian. `nub' is the
+    // number of unbounded variables (which have lo,hi = -/+ infinity).
+
+    struct Info1
+    {
+        // Structure size should not exceed sizeof(pointer) bytes to have 
+        // to have good memory pattern in dxQuickStepper()
+        uint8 m, nub;
+    };
+
+    // info returned by getInfo2 function
+
+    enum
+    {
+        GI2__J_MIN,
+        GI2__JL_MIN = GI2__J_MIN + dDA__L_MIN,
+
+        GI2_JLX = GI2__J_MIN + dDA_LX,
+        GI2_JLY = GI2__J_MIN + dDA_LY,
+        GI2_JLZ = GI2__J_MIN + dDA_LZ,
+
+        GI2__JL_MAX = GI2__J_MIN + dDA__L_MAX,
+
+        GI2__JA_MIN = GI2__J_MIN + dDA__A_MIN,
+
+        GI2_JAX = GI2__J_MIN + dDA_AX,
+        GI2_JAY = GI2__J_MIN + dDA_AY,
+        GI2_JAZ = GI2__J_MIN + dDA_AZ,
+
+        GI2__JA_MAX = GI2__J_MIN + dDA__A_MAX,
+        GI2__J_MAX = GI2__J_MIN + dDA__MAX,
+    };
+
+    enum
+    {
+        GI2_RHS,
+        GI2_CFM,
+        GI2__RHS_CFM_MAX,
+    };
+
+    enum
+    {
+        GI2_LO,
+        GI2_HI,
+        GI2__LO_HI_MAX,
+    };
+
+    // info returned by getSureMaxInfo function. 
+    // The information is used for memory reservation in calculations.
+
+    struct SureMaxInfo
+    {
+        // The value of `max_m' must ALWAYS be not less than the value of `m'
+        // the getInfo1 call can generate in current joint state. Another 
+        // requirement is that the value should be provided very quickly, 
+        // without the excessive calculations.
+        // If it is hard/impossible to quickly predict the maximal value of `m'
+        // (which is the case for most joint types) the maximum for current 
+        // joint type in general should be returned. If it can be known the `m'
+        // will be smaller, it can save a bit of memory from being reserved 
+        // for calculations if that smaller value is returned.
+
+        uint8 max_m; // Estimate of maximal `m' in Info1
+    };
+
+
+    unsigned flags;             // dJOINT_xxx flags
+    dxJointNode node[2];        // connections to bodies. node[1].body can be 0
+    dJointFeedback *feedback;   // optional feedback structure
+    dReal lambda[6];            // lambda generated by last step
+
+
+    dxJoint( dxWorld *w );
+    virtual ~dxJoint();
+
+    bool GetIsJointReverse() const { return (this->flags & dJOINT_REVERSE) != 0; }
+
+    virtual void getInfo1( Info1* info ) = 0;
+
+    // integrator parameters
+    virtual void getInfo2( 
+        // fps=frames per second (1/stepsize), erp=default error reduction parameter (0..1)
+        dReal worldFPS, dReal worldERP, 
+        // elements to jump from one row to the next in J's
+        int rowskip,
+        // for the first and second body, pointers to two (linear and angular)
+        // n*3 jacobian sub matrices, stored by rows. these matrices will have
+        // been initialized to 0 on entry. if the second body is zero then the
+        // J2xx pointers may be 0.
+        dReal *J1, dReal *J2,
+        // elements to jump from one pair of scalars to the next
+        int pairskip,
+        // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+        // "constraint force mixing" vector. c is set to zero on entry, cfm is
+        // set to a constant value (typically very small or zero) value on entry.
+        dReal *pairRhsCfm,
+        // lo and hi limits for variables (set to -/+ infinity on entry).
+        dReal *pairLoHi,
+        // findex vector for variables. see the LCP solver interface for a
+        // description of what this does. this is set to -1 on entry.
+        // note that the returned indexes are relative to the first index of
+        // the constraint.
+        int *findex) = 0;
+    // This call quickly!!! estimates maximum value of "m" that could be returned by getInfo1()
+    // See comments at definition of SureMaxInfo for details.
+    virtual void getSureMaxInfo( SureMaxInfo* info ) = 0;
+    virtual dJointType type() const = 0;
+    virtual sizeint size() const = 0;
+
+    /// Set values which are relative with respect to bodies.
+    /// Each dxJoint should redefine it if needed.
+    virtual void setRelativeValues();
+
+    // Test if this joint should be used in the simulation step
+    // (has the enabled flag set, and is attached to at least one dynamic body)
+    bool isEnabled() const;
+};
+
+
+// joint group. NOTE: any joints in the group that have their world destroyed
+// will have their world pointer set to 0.
+
+struct dxJointGroup : public dBase
+{
+    dxJointGroup(): m_num(0), m_stack() {}
+
+    template<class T>
+    T *alloc(dWorldID w)
+    {
+        T *j = (T *)m_stack.alloc(sizeof(T));
+        if (j != NULL) {
+            ++m_num;
+            new(j) T(w);
+            j->flags |= dJOINT_INGROUP;
+        }
+        return j;
+    }
+
+    sizeint getJointCount() const { return m_num; }
+    sizeint exportJoints(dxJoint **jlist);
+
+    void *beginEnum() { return m_stack.rewind(); }
+    void *continueEnum(sizeint num_bytes) { return m_stack.next(num_bytes); }
+
+    void freeAll();
+
+private:
+    sizeint m_num;        // number of joints on the stack
+    dObStack m_stack; // a stack of (possibly differently sized) dxJoint objects.
+};
+
+// common limit and motor information for a single joint axis of movement
+struct dxJointLimitMotor
+{
+    dReal vel, fmax;        // powered joint: velocity, max force
+    dReal lostop, histop;   // joint limits, relative to initial position
+    dReal fudge_factor;     // when powering away from joint limits
+    dReal normal_cfm;       // cfm to use when not at a stop
+    dReal stop_erp, stop_cfm; // erp and cfm for when at joint limit
+    dReal bounce;           // restitution factor
+    // variables used between getInfo1() and getInfo2()
+    int limit;          // 0=free, 1=at lo limit, 2=at hi limit
+    dReal limit_err;    // if at limit, amount over limit
+
+    void init( dxWorld * );
+    void set( int num, dReal value );
+    dReal get( int num ) const;
+    bool testRotationalLimit( dReal angle );
+
+    enum
+    {
+        GI2__JL_MIN = dxJoint::GI2__JL_MIN,
+        GI2__JA_MIN = dxJoint::GI2__JA_MIN,
+        GI2_JAX = dxJoint::GI2_JAX,
+        GI2_JAY = dxJoint::GI2_JAY,
+        GI2_JAZ = dxJoint::GI2_JAZ,
+        GI2_RHS = dxJoint::GI2_RHS,
+        GI2_CFM = dxJoint::GI2_CFM,
+        GI2_LO = dxJoint::GI2_LO,
+        GI2_HI = dxJoint::GI2_HI,
+    };
+
+    bool addLimot( dxJoint *joint, dReal fps, 
+        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+        const dVector3 ax1, int rotational );
+    bool addTwoPointLimot( dxJoint *joint, dReal fps,
+        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+        const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 );
+};
+
+
+#endif
+
+
+// Local Variables:
+// mode:c++
+// c-basic-offset:4
+// End:

thirdparty/ode-0.16.5/ode/src/joints/joint_internal.h

@@ -0,0 +1,70 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+
+#ifndef _ODE_JOINT_INTERNAL_H_
+#define _ODE_JOINT_INTERNAL_H_
+
+
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include "matrix.h"
+#include "odemath.h"
+
+
+#define checktype(j,t) dUASSERT(j->type() == dJointType##t, \
+    "joint type is not " #t)
+
+
+void setBall( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dVector3 anchor1, dVector3 anchor2 );
+void setBall2( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dVector3 anchor1, dVector3 anchor2,
+    dVector3 axis, dReal erp1 );
+
+void setFixedOrientation( dxJoint *joint, dReal fps, dReal erp, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, 
+    dQuaternion qrel );
+
+
+void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
+    dVector3 anchor1, dVector3 anchor2 );
+
+void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 );
+void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 );
+
+void setAxes( dxJoint *j, dReal x, dReal y, dReal z,
+             dVector3 axis1, dVector3 axis2 );
+void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 );
+void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 );
+
+
+dReal getHingeAngle( dxBody *body1, dxBody *body2, dVector3 axis, dQuaternion q_initial );
+dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis );
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/joints.h

@@ -0,0 +1,48 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINTS_H_
+#define _ODE_JOINTS_H_
+
+#include <ode/common.h>
+
+#include "joint.h"
+
+#include "ball.h"
+#include "dball.h"
+#include "dhinge.h"
+#include "transmission.h"
+#include "hinge.h"
+#include "slider.h"
+#include "contact.h"
+#include "universal.h"
+#include "hinge2.h"
+#include "fixed.h"
+#include "null.h"
+#include "amotor.h"
+#include "lmotor.h"
+#include "plane2d.h"
+#include "pu.h"
+#include "pr.h"
+#include "piston.h"
+
+#endif

thirdparty/ode-0.16.5/ode/src/joints/lmotor.cpp

@@ -0,0 +1,214 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "lmotor.h"
+#include "joint_internal.h"
+
+
+//****************************************************************************
+// lmotor joint
+dxJointLMotor::dxJointLMotor( dxWorld *w ) :
+    dxJoint( w )
+{
+    int i;
+    num = 0;
+    for ( i = 0;i < 3;i++ )
+    {
+        dSetZero( axis[i], 4 );
+        limot[i].init( world );
+    }
+}
+
+void
+dxJointLMotor::computeGlobalAxes( dVector3 ax[3] )
+{
+    for ( int i = 0; i < num; i++ )
+    {
+        if ( rel[i] == 1 )
+        {
+            dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
+        }
+        else if ( rel[i] == 2 )
+        {
+            if ( node[1].body )   // jds: don't assert, just ignore
+            {
+                dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
+            }
+        }
+        else
+        {
+            ax[i][0] = axis[i][0];
+            ax[i][1] = axis[i][1];
+            ax[i][2] = axis[i][2];
+        }
+    }
+}
+
+void 
+dxJointLMotor::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = num;
+}
+
+void
+dxJointLMotor::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 0;
+    info->nub = 0;
+    for ( int i = 0; i < num; i++ )
+    {
+        if ( limot[i].fmax > 0 )
+        {
+            info->m++;
+        }
+    }
+}
+
+void
+dxJointLMotor::getInfo2( dReal worldFPS, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dVector3 ax[3];
+    computeGlobalAxes( ax );
+
+    int currRowSkip = 0, currPairSkip = 0;
+    for ( int i = 0; i < num; ++i ) {
+        if (limot[i].addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax[i], 0 )) {
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+    }
+}
+
+void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    //for now we are ignoring rel!
+    dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
+    checktype( joint, LMotor );
+
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+
+    if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1
+
+    joint->rel[anum] = rel;
+
+    dVector3 r;
+    r[0] = x;
+    r[1] = y;
+    r[2] = z;
+    r[3] = 0;
+    if ( rel > 0 )
+    {
+        if ( rel == 1 )
+        {
+            dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
+        }
+        else
+        {
+            //second body has to exists thanks to ref 1 line
+            dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
+        }
+    }
+    else
+    {
+        joint->axis[anum][0] = r[0];
+        joint->axis[anum][1] = r[1];
+        joint->axis[anum][2] = r[2];
+    }
+
+    dNormalize3( joint->axis[anum] );
+}
+
+void dJointSetLMotorNumAxes( dJointID j, int num )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    dAASSERT( joint && num >= 0 && num <= 3 );
+    checktype( joint, LMotor );
+    if ( num < 0 ) num = 0;
+    if ( num > 3 ) num = 3;
+    joint->num = num;
+}
+
+void dJointSetLMotorParam( dJointID j, int parameter, dReal value )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    dAASSERT( joint );
+    checktype( joint, LMotor );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    joint->limot[anum].set( parameter, value );
+}
+
+int dJointGetLMotorNumAxes( dJointID j )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    dAASSERT( joint );
+    checktype( joint, LMotor );
+    return joint->num;
+}
+
+
+void dJointGetLMotorAxis( dJointID j, int anum, dVector3 result )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    dAASSERT( joint && anum >= 0 && anum < 3 );
+    checktype( joint, LMotor );
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    result[0] = joint->axis[anum][0];
+    result[1] = joint->axis[anum][1];
+    result[2] = joint->axis[anum][2];
+}
+
+dReal dJointGetLMotorParam( dJointID j, int parameter )
+{
+    dxJointLMotor* joint = ( dxJointLMotor* )j;
+    dAASSERT( joint );
+    checktype( joint, LMotor );
+    int anum = parameter >> 8;
+    if ( anum < 0 ) anum = 0;
+    if ( anum > 2 ) anum = 2;
+    parameter &= 0xff;
+    return joint->limot[anum].get( parameter );
+}
+
+dJointType
+dxJointLMotor::type() const
+{
+    return dJointTypeLMotor;
+}
+
+
+sizeint
+dxJointLMotor::size() const
+{
+    return sizeof( *this );
+}
+

thirdparty/ode-0.16.5/ode/src/joints/lmotor.h

@@ -0,0 +1,51 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_LMOTOR_H_
+#define _ODE_JOINT_LMOTOR_H_
+
+#include "joint.h"
+
+struct dxJointLMotor : public dxJoint
+{
+    int num;
+    int rel[3];
+    dVector3 axis[3];
+    dxJointLimitMotor limot[3];
+
+    void computeGlobalAxes( dVector3 ax[3] );
+
+
+    dxJointLMotor( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/null.cpp

@@ -0,0 +1,74 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "null.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// null joint
+dxJointNull::dxJointNull( dxWorld *w ) :
+    dxJoint( w )
+{
+}
+
+void 
+dxJointNull::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 0;
+}
+
+
+void
+dxJointNull::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 0;
+    info->nub = 0;
+}
+
+
+void
+dxJointNull::getInfo2( dReal /*worldFPS*/, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dDebug( 0, "this should never get called" );
+}
+
+dJointType
+dxJointNull::type() const
+{
+    return dJointTypeNull;
+}
+
+sizeint
+dxJointNull::size() const
+{
+    return sizeof( *this );
+}
+
+

thirdparty/ode-0.16.5/ode/src/joints/null.h

@@ -0,0 +1,46 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_NULL_H_
+#define _ODE_JOINT_NULL_H_
+
+#include "joint.h"
+
+
+
+// null joint, for testing only
+
+struct dxJointNull : public dxJoint
+{
+    dxJointNull( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+};
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/piston.cpp

@@ -0,0 +1,729 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "piston.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Piston
+//
+
+dxJointPiston::dxJointPiston ( dxWorld *w ) :
+    dxJoint ( w )
+{
+    dSetZero ( axis1, 4 );
+    dSetZero ( axis2, 4 );
+
+    axis1[0] = 1;
+    axis2[0] = 1;
+
+    dSetZero ( qrel, 4 );
+
+    dSetZero ( anchor1, 4 );
+    dSetZero ( anchor2, 4 );
+
+    limotP.init ( world );
+
+    limotR.init ( world );
+}
+
+
+dReal dJointGetPistonPosition ( dJointID j )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    if ( joint->node[0].body )
+    {
+        dVector3 q;
+        // get the anchor (or offset) in global coordinates
+        dMultiply0_331 ( q, joint->node[0].body->posr.R, joint->anchor1 );
+
+        if ( joint->node[1].body )
+        {
+            dVector3 anchor2;
+            // get the anchor2 in global coordinates
+            dMultiply0_331 ( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
+
+            q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+                ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
+            q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+                ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
+            q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+                ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
+        }
+        else
+        {
+            // N.B. When there is no body 2 the joint->anchor2 is already in
+            //      global coordinates
+            q[0] = ( ( joint->node[0].body->posr.pos[0] + q[0] ) -
+                ( joint->anchor2[0] ) );
+            q[1] = ( ( joint->node[0].body->posr.pos[1] + q[1] ) -
+                ( joint->anchor2[1] ) );
+            q[2] = ( ( joint->node[0].body->posr.pos[2] + q[2] ) -
+                ( joint->anchor2[2] ) );
+
+            if ( joint->flags & dJOINT_REVERSE )
+            {
+                q[0] = -q[0];
+                q[1] = -q[1];
+                q[2] = -q[2];
+            }
+        }
+
+        // get axis in global coordinates
+        dVector3 ax;
+        dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+        return dCalcVectorDot3 ( ax, q );
+    }
+
+    dDEBUGMSG ( "The function always return 0 since no body are attached" );
+    return 0;
+}
+
+
+dReal dJointGetPistonPositionRate ( dJointID j )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    // get axis in global coordinates
+    dVector3 ax;
+    dMultiply0_331 ( ax, joint->node[0].body->posr.R, joint->axis1 );
+
+    // The linear velocity created by the rotation can be discarded since
+    // the rotation is along the prismatic axis and this rotation don't create
+    // linear velocity in the direction of the prismatic axis.
+    if ( joint->node[1].body )
+    {
+        return ( dCalcVectorDot3 ( ax, joint->node[0].body->lvel ) -
+            dCalcVectorDot3 ( ax, joint->node[1].body->lvel ) );
+    }
+    else
+    {
+        dReal rate = dCalcVectorDot3 ( ax, joint->node[0].body->lvel );
+        return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
+    }
+}
+
+
+dReal dJointGetPistonAngle ( dJointID j )
+{
+    dxJointPiston* joint = ( dxJointPiston * ) j;
+    dAASSERT ( joint );
+    checktype ( joint, Piston );
+
+    if ( joint->node[0].body )
+    {
+        dReal ang = getHingeAngle ( joint->node[0].body, joint->node[1].body, joint->axis1,
+            joint->qrel );
+        if ( joint->flags & dJOINT_REVERSE )
+            return -ang;
+        else
+            return ang;
+    }
+    else return 0;
+}
+
+
+dReal dJointGetPistonAngleRate ( dJointID j )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dAASSERT ( joint );
+    checktype ( joint, Piston );
+
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+        dMultiply0_331 ( axis, joint->node[0].body->posr.R, joint->axis1 );
+        dReal rate = dCalcVectorDot3 ( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body ) rate -= dCalcVectorDot3 ( axis, joint->node[1].body->avel );
+        if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
+        return rate;
+    }
+    else return 0;
+}
+
+
+void 
+dxJointPiston::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointPiston::getInfo1 ( dxJoint::Info1 *info )
+{
+    info->nub = 4; // Number of unbound variables
+    // The only bound variable is one linear displacement
+
+    info->m = 4; // Default number of constraint row
+
+    // see if we're at a joint limit.
+    limotP.limit = 0;
+    if ( ( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
+        limotP.lostop <= limotP.histop )
+    {
+        // measure joint position
+        dReal pos = dJointGetPistonPosition ( this );
+        limotP.testRotationalLimit ( pos );     // N.B. The fucntion is ill named
+    }
+
+    // powered Piston or at limits needs an extra constraint row
+    if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+
+
+    // see if we're at a joint limit.
+    limotR.limit = 0;
+    if ( ( limotR.lostop > -dInfinity || limotR.histop < dInfinity ) &&
+        limotR.lostop <= limotR.histop )
+    {
+        // measure joint position
+        dReal angle = getHingeAngle ( node[0].body, node[1].body, axis1,
+            qrel );
+        limotR.testRotationalLimit ( angle );
+    }
+
+    // powered Piston or at limits needs an extra constraint row
+    if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+
+}
+
+
+void
+dxJointPiston::getInfo2 ( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    const dReal k = worldFPS * worldERP;
+
+
+    // Pull out pos and R for both bodies. also get the `connection'
+    // vector pos2-pos1.
+
+    dVector3 dist; // Current position of body_1  w.r.t "anchor"
+    // 2 bodies anchor is center of body 2
+    // 1 bodies anchor is origin
+    dVector3 lanchor2 = { 0,0,0 };
+
+    dReal *pos1 = node[0].body->posr.pos;
+    dReal *R1   = node[0].body->posr.R;
+    dReal *R2 = NULL;
+
+    dxBody *body1 = node[1].body;
+
+    if ( body1 ) 
+    {
+        dReal *pos2 = body1->posr.pos;
+        R2   = body1->posr.R;
+
+        dMultiply0_331 ( lanchor2, R2, anchor2 );
+        dist[0] = lanchor2[0] + pos2[0] - pos1[0];
+        dist[1] = lanchor2[1] + pos2[1] - pos1[1];
+        dist[2] = lanchor2[2] + pos2[2] - pos1[2];
+    } 
+    else 
+    {
+        // pos2 = 0; // N.B. We can do that to be safe but it is no necessary
+        // R2 = 0;   // N.B. We can do that to be safe but it is no necessary
+        if ( (flags & dJOINT_REVERSE) != 0 )
+        {
+            dSubtractVectors3(dist, pos1, anchor2); // Invert the value
+        }
+        else
+        {
+            dSubtractVectors3(dist, anchor2, pos1);
+        }
+    }
+
+    // ======================================================================
+    // Work on the angular part (i.e. row 0, 1)
+    // Set the two orientation rows. The rotoide axis should be the only
+    // unconstrained rotational axis, the angular velocity of the two bodies
+    // perpendicular to the rotoide axis should be equal.
+    // Thus the constraint equations are:
+    //    p*w1 - p*w2 = 0
+    //    q*w1 - q*w2 = 0
+    // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
+    // are the angular velocity vectors of the two bodies.
+    // Since the rotoide axis is the same as the prismatic axis.
+    //
+    //
+    // Also, compute the right hand side (RHS) of the rotation constraint equation set.
+    // The first 2 element will result in the relative angular velocity of the two
+    // bodies along axis p and q. This is set to bring the rotoide back into alignment.
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * u
+    //                      = (erp*fps) * theta * u
+    // where rotation along unit length axis u by theta brings body 2's frame
+    //
+    // if theta is smallish, sin(theta) ~= theta and cos(theta) ~= 1
+    // where the quaternion of the relative rotation between the two bodies is
+    //    quat = [cos(theta/2) sin(theta/2)*u]
+    //    quat = [1 theta/2*u]
+    //         => q[0] ~= 1
+    //            2 * q[1+i] = theta * u[i]
+    //
+    // Since there is no constraint along the rotoide axis
+    // only along p and q that we want the same angular velocity and need to reduce
+    // the error
+    dVector3 b, ax1, p, q;
+    dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
+
+    // Find the 2 axis perpendicular to the rotoide axis.
+    dPlaneSpace ( ax1, p, q );
+
+    // LHS
+    dCopyVector3 ( J1 + GI2__JA_MIN, p );
+
+    if ( body1 )
+    {
+        dCopyNegatedVector3 ( J2 + GI2__JA_MIN, p );
+    }
+
+    dCopyVector3 ( J1 + rowskip + GI2__JA_MIN, q );
+
+    if ( body1 ) 
+    {
+        dCopyNegatedVector3 ( J2 + rowskip + GI2__JA_MIN, q );
+
+        // Some math for the RHS
+        dVector3 ax2;
+        dMultiply0_331 ( ax2, R2, axis2 );
+        dCalcVectorCross3( b, ax1, ax2 );
+    }
+    else
+    {
+        // Some math for the RHS
+        dCalcVectorCross3( b, ax1, axis2 );
+    }
+
+    // RHS
+    pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3 ( p, b );
+    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3 ( q, b );
+
+
+    // ======================================================================
+    // Work on the linear part (i.e row 2,3)
+    // p2 + R2 anchor2' = p1 + R1 dist'
+    // v2 + w2 R2 anchor2' + R2 d(anchor2')/dt  = v1 + w1 R1 dist' + R1 d(dist')/dt
+    // v2 + w2 x anchor2 = v1 + w1 x dist + v_p
+    // v_p is speed of prismatic joint (i.e. elongation rate)
+    // Since the constraints are perpendicular to v_p we have:
+    // p . v_p = 0 and q . v_p = 0
+    // Along p and q we have (since sliding along the prismatic axis is disregarded):
+    // u . ( v2 + w2 x anchor2 = v1 + w1 x dist + v_p) ( where u is p or q )
+    // Simplify
+    // u . v2 + u. w2 x anchor2 = u . v1 + u . w1 x dist
+    // or
+    // u . v1 - u . v2 + u . w1 x dist - u2 . w2 x anchor2 = 0
+    // using the fact that (a x b = - b x a)
+    // u . v1 - u . v2 - u . dist x w1  + u . anchor2 x w2 = 0
+    // With the help of the triple product:
+    //   i.e.  a . b x c = b . c x a = c . a x b  or  a . b x c = a x b . c
+    //   Ref: http://mathworld.wolfram.com/ScalarTripleProduct.html
+    // u . v1 - u . v2 - u x dist . w1 + u x anchor2 . w2 = 0
+    // u . v1 - u . v2 + dist x u . w1 - u x anchor2 . w2 = 0
+    //
+    // Coeff for 1er line of: J1l => p, J2l => -p
+    // Coeff for 2er line of: J1l => q, J2l => -q
+    // Coeff for 1er line of: J1a => dist x p, J2a => p x anchor2
+    // Coeff for 2er line of: J1a => dist x q, J2a => q x anchor2
+
+    int currRowSkip = 2 * rowskip;
+    {
+        dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, p );
+        dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, p );
+
+        if ( body1 )
+        {
+            // info->J2l[s2+i] = -p[i];
+            dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, p );
+            // q x anchor2 instead of anchor2 x q since we want the negative value
+            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, p, lanchor2 );
+        }
+    }
+
+    currRowSkip += rowskip;
+    {
+        dCopyVector3 ( J1 + currRowSkip + GI2__JL_MIN, q );
+        dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, q );
+
+        if ( body1 )
+        {
+            // info->J2l[s3+i] = -q[i];
+            dCopyNegatedVector3 ( J2 + currRowSkip + GI2__JL_MIN, q );
+            // The cross product is in reverse order since we want the negative value
+            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, lanchor2 );
+        }
+    }
+
+    // We want to make correction for motion not in the line of the axis
+    // We calculate the displacement w.r.t. the "anchor" pt.
+    // i.e. Find the difference between the current position and the initial
+    //      position along the constrained axies (i.e. axis p and q).
+    // The bodies can move w.r.t each other only along the prismatic axis
+    //
+    // Compute the RHS of rows 2 and 3
+    dVector3 err;
+    dMultiply0_331 ( err, R1, anchor1 );
+    dSubtractVectors3( err, dist, err );
+
+    int currPairSkip = 2 * pairskip;
+    {
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, err );
+    }
+
+    currPairSkip += pairskip;
+    {
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, err );
+    }
+
+    currRowSkip += rowskip; currPairSkip += pairskip;
+    
+    if ( body1 || (flags & dJOINT_REVERSE) == 0 )
+    {
+        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 0 ))
+        {
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+    }
+    else
+    {
+        dVector3 rAx1;
+        dCopyNegatedVector3(rAx1, ax1);
+
+        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAx1, 0 ))
+        {
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+    }
+
+    limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+void dJointSetPistonAnchor ( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+    setAnchors ( joint, x, y, z, joint->anchor1, joint->anchor2 );
+    joint->computeInitialRelativeRotation();
+
+}
+
+void dJointSetPistonAnchorOffset (dJointID j, dReal x, dReal y, dReal z,
+                                  dReal dx, dReal dy, dReal dz)
+{
+    dxJointPiston* joint = (dxJointPiston*) j;
+    dUASSERT (joint,"bad joint argument");
+    checktype ( joint, Piston );
+
+    if (joint->flags & dJOINT_REVERSE)
+    {
+        dx = -dx;
+        dy = -dy;
+        dz = -dz;
+    }
+
+    if (joint->node[0].body)
+    {
+        joint->node[0].body->posr.pos[0] -= dx;
+        joint->node[0].body->posr.pos[1] -= dy;
+        joint->node[0].body->posr.pos[2] -= dz;
+    }
+
+    setAnchors (joint,x ,y, z, joint->anchor1, joint->anchor2);
+
+    if (joint->node[0].body)
+    {
+        joint->node[0].body->posr.pos[0] += dx;
+        joint->node[0].body->posr.pos[1] += dy;
+        joint->node[0].body->posr.pos[2] += dz;
+    }
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+
+void dJointGetPistonAnchor ( dJointID j, dVector3 result )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    dUASSERT ( result, "bad result argument" );
+    checktype ( joint, Piston );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2 ( joint, result, joint->anchor2 );
+    else
+        getAnchor ( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetPistonAnchor2 ( dJointID j, dVector3 result )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    dUASSERT ( result, "bad result argument" );
+    checktype ( joint, Piston );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor ( joint, result, joint->anchor1 );
+    else
+        getAnchor2 ( joint, result, joint->anchor2 );
+}
+
+
+
+void dJointSetPistonAxis ( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPistonAxisDelta ( dJointID j, dReal x, dReal y, dReal z,
+                               dReal dx, dReal dy, dReal dz )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    setAxes ( joint, x, y, z, joint->axis1, joint->axis2 );
+
+    joint->computeInitialRelativeRotation();
+
+    dVector3 c = {0,0,0};
+    if ( joint->node[1].body )
+    {
+        c[0] = ( joint->node[0].body->posr.pos[0] -
+            joint->node[1].body->posr.pos[0] - dx );
+        c[1] = ( joint->node[0].body->posr.pos[1] -
+            joint->node[1].body->posr.pos[1] - dy );
+        c[2] = ( joint->node[0].body->posr.pos[2] -
+            joint->node[1].body->posr.pos[2] - dz );
+    }
+    else /*if ( joint->node[0].body )*/ // -- body[0] should always be present -- there is a matrix multiplication below
+    {
+        c[0] = joint->node[0].body->posr.pos[0] - dx;
+        c[1] = joint->node[0].body->posr.pos[1] - dy;
+        c[2] = joint->node[0].body->posr.pos[2] - dz;
+    }
+
+    // Convert into frame of body 1
+    dMultiply1_331 ( joint->anchor1, joint->node[0].body->posr.R, c );
+}
+
+
+
+void dJointGetPistonAxis ( dJointID j, dVector3 result )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    dUASSERT ( result, "bad result argument" );
+    checktype ( joint, Piston );
+
+    getAxis ( joint, result, joint->axis1 );
+}
+
+void dJointSetPistonParam ( dJointID j, int parameter, dReal value )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    if ( ( parameter & 0xff00 ) == 0x100 )
+    {
+        joint->limotR.set ( parameter & 0xff, value );
+    }
+    else
+    {
+        joint->limotP.set ( parameter, value );
+    }
+}
+
+
+dReal dJointGetPistonParam ( dJointID j, int parameter )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    if ( ( parameter & 0xff00 ) == 0x100 )
+    {
+        return joint->limotR.get ( parameter & 0xff );
+    }
+    else
+    {
+        return joint->limotP.get ( parameter );
+    }
+}
+
+
+void dJointAddPistonForce ( dJointID j, dReal force )
+{
+    dxJointPiston* joint = ( dxJointPiston* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Piston );
+
+    if ( joint->flags & dJOINT_REVERSE )
+        force -= force;
+
+    dVector3 axis;
+    getAxis ( joint, axis, joint->axis1 );
+    // axis[i] *= force
+    dScaleVector3( axis, force );
+
+
+    if ( joint->node[0].body != 0 )
+        dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
+    if ( joint->node[1].body != 0 )
+        dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+
+    if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
+    {
+        // Case where we don't need ltd since center of mass of both bodies
+        // pass by the anchor point '*' when travelling along the prismatic axis.
+        //                                     Body_2
+        //   Body_1                             -----
+        //    ---                |--           |     |
+        //   |   |---------------*-------------|     |     ---> prismatic axis
+        //    ---                |--           |     |
+        //                                      -----
+        //                                      Body_2
+        // Case where we need ltd
+        //   Body_1
+        //    ---
+        //   |   |---------
+        //    ---          |
+        //                 |     |--
+        //                  -----*-----                    ---> prismatic axis
+        //                       |--   |
+        //                             |
+        //                             |
+        //                             |        -----
+        //                             |       |     |
+        //                              -------|     |
+        //                                     |     |
+        //                                      -----
+        //                                      Body_2
+        //
+        // In real life force apply at the '*' point
+        // But in ODE the force are applied on the center of mass of Body_1 and Body_2
+        // So we have to add torques on both bodies to compensate for that when there
+        // is an offset between the anchor point and the center of mass of both bodies.
+        //
+        // We need to add to each body T = r x F
+        // Where r is the distance between the cm and '*'
+
+        dVector3 ltd; // Linear Torque Decoupling vector (a torque)
+        dVector3 c;   // Distance of the body w.r.t the anchor
+        // N.B. The distance along the prismatic axis might not
+        //      not be included in this variable since it won't add
+        //      anything to the ltd.
+
+        // Calculate the distance of the body w.r.t the anchor
+
+        // The anchor1 of body1 can be used since:
+        // Real anchor = Position of body 1 + anchor + d* axis1 = anchor in world frame
+        // d is the position of the prismatic joint (i.e. elongation)
+        // Since axis1 x axis1 == 0
+        // We can do the following.
+        dMultiply0_331 ( c, joint->node[0].body->posr.R, joint->anchor1 );
+        dCalcVectorCross3( ltd, c, axis );
+        dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
+
+
+        dMultiply0_331 ( c, joint->node[1].body->posr.R, joint->anchor2 );
+        dCalcVectorCross3( ltd, c, axis );
+        dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
+    }
+}
+
+
+dJointType
+dxJointPiston::type() const
+{
+    return dJointTypePiston;
+}
+
+
+sizeint
+dxJointPiston::size() const
+{
+    return sizeof ( *this );
+}
+
+
+
+void
+dxJointPiston::setRelativeValues()
+{
+    dVector3 vec;
+    dJointGetPistonAnchor(this, vec);
+    setAnchors( this, vec[0], vec[1], vec[2], anchor1, anchor2 );
+
+    dJointGetPistonAxis(this, vec);
+    setAxes( this,  vec[0], vec[1], vec[2], axis1, axis2 );
+
+    computeInitialRelativeRotation();
+}
+
+
+
+
+void
+dxJointPiston::computeInitialRelativeRotation()
+{
+    if ( node[0].body )
+    {
+        if ( node[1].body )
+        {
+            dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
+        }
+        else
+        {
+            // set joint->qrel to the transpose of the first body q
+            qrel[0] = node[0].body->q[0];
+            for ( int i = 1; i < 4; i++ )
+                qrel[i] = -node[0].body->q[i];
+            // WARNING do we need the - in -joint->node[0].body->q[i]; or not
+        }
+    }
+}

thirdparty/ode-0.16.5/ode/src/joints/piston.h

@@ -0,0 +1,112 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_PISTON_H_
+#define _ODE_JOINT_PISTON_H_
+
+#include "joint.h"
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Component of a Piston joint
+/// <PRE>
+///                              |- Anchor point
+///      Body_1                  |                       Body_2
+///      +---------------+       V                       +------------------+
+///     /               /|                             /                  /|
+///    /               / +       |--      ______      /                  / +
+///   /      x        /./........x.......(_____()..../         x        /.......> axis
+///  +---------------+ /         |--                +------------------+ /
+///  |               |/                             |                  |/
+///  +---------------+                              +------------------+
+///          |                                                 |
+///          |                                                 |
+///          |------------------> <----------------------------|
+///              anchor1                  anchor2
+///
+///
+/// </PRE>
+///
+/// When the prismatic joint as been elongated (i.e. dJointGetPistonPosition)
+/// return a value >  0
+/// <PRE>
+///                                   |- Anchor point
+///      Body_1                       |                       Body_2
+///      +---------------+            V                       +------------------+
+///     /               /|                                  /                  /|
+///    /               / +            |--      ______      /                  / +
+///   /      x        /./........_____x.......(_____()..../         x        /.......> axis
+///  +---------------+ /              |--                +------------------+ /
+///  |               |/                                  |                  |/
+///  +---------------+                                   +------------------+
+///          |                                                      |
+///          |                                                      |
+///          |------------------>      <----------------------------|
+///              anchor1         |----|         anchor2
+///                                ^
+///                                |-- This is what dJointGetPistonPosition will
+///                                    return
+/// </PRE>
+////////////////////////////////////////////////////////////////////////////////
+struct dxJointPiston : public dxJoint
+{
+    dVector3 axis1;          ///< Axis of the prismatic and rotoide w.r.t first body
+    dVector3 axis2;          ///< Axis of the prismatic and rotoide w.r.t second body
+
+
+    dQuaternion qrel;        ///< Initial relative rotation body1 -> body2
+
+    /// Anchor w.r.t first body.
+    /// This is the same as the offset for the Slider joint
+    /// @note To find the position of the anchor when the body 1 has moved
+    ///       you must add the position of the prismatic joint
+    ///       i.e anchor = R1 * anchor1 + dJointGetPistonPosition() * (R1 * axis1)
+    dVector3 anchor1;
+    dVector3 anchor2;        //< anchor w.r.t second body
+
+    /// limit and motor information for the prismatic
+    /// part of the joint
+    dxJointLimitMotor limotP;
+
+    /// limit and motor information for the rotoide
+    /// part of the joint
+    dxJointLimitMotor limotR;
+
+    dxJointPiston( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+
+    virtual void setRelativeValues();
+
+    void computeInitialRelativeRotation();
+};
+
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/plane2d.cpp

@@ -0,0 +1,195 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "plane2d.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Plane2D
+/*
+This code is part of the Plane2D ODE joint
+by psero@gmx.de
+Wed Apr 23 18:53:43 CEST 2003
+*/
+
+
+static const dReal   Midentity[3][3] =
+{
+    {   1,  0,  0   },
+    {   0,  1,  0   },
+    {   0,  0,  1,  }
+};
+
+
+dxJointPlane2D::dxJointPlane2D( dxWorld *w ) :
+    dxJoint( w )
+{
+    motor_x.init( world );
+    motor_y.init( world );
+    motor_angle.init( world );
+}
+
+
+void 
+dxJointPlane2D::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointPlane2D::getInfo1( dxJoint::Info1 *info )
+{
+    info->nub = 3;
+    info->m = 3;
+
+    if ( motor_x.fmax > 0 )
+        row_motor_x = info->m++;
+    else
+        row_motor_x = 0;
+
+    if ( motor_y.fmax > 0 )
+        row_motor_y = info->m++;
+    else
+        row_motor_y = 0;
+
+    if ( motor_angle.fmax > 0 )
+        row_motor_angle = info->m++;
+    else
+        row_motor_angle = 0;
+}
+
+
+
+void
+dxJointPlane2D::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dReal eps = worldFPS * worldERP;
+
+    /*
+        v = v1, w = omega1
+        (v2, omega2 not important (== static environment))
+
+        constraint equations:
+            vz = 0
+            wx = 0
+            wy = 0
+
+        <=> ( 0 0 1 ) (vx)   ( 0 0 0 ) (wx)   ( 0 )
+            ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
+            ( 0 0 0 ) (vz)   ( 0 1 0 ) (wz)   ( 0 )
+            J1/J1l           Omega1/J1a
+    */
+
+    // fill in linear and angular coeff. for left hand side:
+
+    J1[GI2_JLZ] = 1;
+    J1[rowskip + GI2_JAX] = 1;
+    J1[2 * rowskip + GI2_JAY] = 1;
+
+    // error correction (against drift):
+
+    // a) linear vz, so that z (== pos[2]) == 0
+    pairRhsCfm[GI2_RHS] = eps * -node[0].body->posr.pos[2];
+
+# if 0
+    // b) angular correction? -> left to application !!!
+    dReal       *body_z_axis = &node[0].body->R[8];
+    pairRhsCfm[pairskip + GI2_RHS] = eps * + atan2( body_z_axis[1], body_z_axis[2] );  // wx error
+    pairRhsCfm[2 * pairskip + GI2_RHS] = eps * -atan2( body_z_axis[0], body_z_axis[2] );  // wy error
+# endif
+
+    // if the slider is powered, or has joint limits, add in the extra row:
+
+    if ( row_motor_x > 0 )
+    {
+        int currRowSkip = row_motor_x * rowskip, currPairSkip = row_motor_x * pairskip;
+        motor_x.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[0], 0 );
+    }
+
+    if ( row_motor_y > 0 )
+    {
+        int currRowSkip = row_motor_y * rowskip, currPairSkip = row_motor_y * pairskip;
+        motor_y.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[1], 0 );
+    }
+
+    if ( row_motor_angle > 0 )
+    {
+        int currRowSkip = row_motor_angle * rowskip, currPairSkip = row_motor_angle * pairskip;
+        motor_angle.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[2], 1 );
+    }
+}
+
+
+dJointType
+dxJointPlane2D::type() const
+{
+    return dJointTypePlane2D;
+}
+
+
+sizeint
+dxJointPlane2D::size() const
+{
+    return sizeof( *this );
+}
+
+
+
+void dJointSetPlane2DXParam( dxJoint *joint,
+                            int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_x.set( parameter, value );
+}
+
+
+void dJointSetPlane2DYParam( dxJoint *joint,
+                            int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_y.set( parameter, value );
+}
+
+
+
+void dJointSetPlane2DAngleParam( dxJoint *joint,
+                                int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_angle.set( parameter, value );
+}
+

thirdparty/ode-0.16.5/ode/src/joints/plane2d.h

@@ -0,0 +1,54 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_PLANE2D_H_
+#define _ODE_JOINT_PLANE2D_H_
+
+#include "joint.h"
+
+
+// 2d joint, constrains to z == 0
+
+struct dxJointPlane2D : public dxJoint
+{
+    int                 row_motor_x;
+    int                 row_motor_y;
+    int                 row_motor_angle;
+    dxJointLimitMotor   motor_x;
+    dxJointLimitMotor   motor_y;
+    dxJointLimitMotor   motor_angle;
+
+
+    dxJointPlane2D( dxWorld *w );
+    virtual void getSureMaxInfo( SureMaxInfo* info );
+    virtual void getInfo1( Info1* info );
+    virtual void getInfo2( dReal worldFPS, dReal worldERP, 
+        int rowskip, dReal *J1, dReal *J2,
+        int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+        int *findex );
+    virtual dJointType type() const;
+    virtual sizeint size() const;
+};
+
+
+#endif
+

thirdparty/ode-0.16.5/ode/src/joints/pr.cpp

@@ -0,0 +1,613 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 <ode/odeconfig.h>
+#include "config.h"
+#include "pr.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Prismatic and Rotoide
+
+dxJointPR::dxJointPR( dxWorld *w ) :
+    dxJoint( w )
+{
+    // Default Position
+    // Z^
+    //  | Body 1       P      R          Body2
+    //  |+---------+   _      _         +-----------+
+    //  ||         |----|----(_)--------+           |
+    //  |+---------+   -                +-----------+
+    //  |
+    // X.-----------------------------------------> Y
+    // N.B. X is comming out of the page
+    dSetZero( anchor2, 4 );
+
+    dSetZero( axisR1, 4 );
+    axisR1[0] = 1;
+    dSetZero( axisR2, 4 );
+    axisR2[0] = 1;
+
+    dSetZero( axisP1, 4 );
+    axisP1[1] = 1;
+    dSetZero( qrel, 4 );
+    dSetZero( offset, 4 );
+
+    limotR.init( world );
+    limotP.init( world );
+}
+
+
+dReal dJointGetPRPosition( dJointID j )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+
+    dVector3 q;
+    // get the offset in global coordinates
+    dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );
+
+    if ( joint->node[1].body )
+    {
+        dVector3 anchor2;
+
+        // get the anchor2 in global coordinates
+        dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
+
+        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
+            ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
+        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
+            ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
+        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
+            ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
+
+    }
+    else
+    {
+        //N.B. When there is no body 2 the joint->anchor2 is already in
+        //     global coordinates
+
+        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
+            ( joint->anchor2[0] ) );
+        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
+            ( joint->anchor2[1] ) );
+        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
+            ( joint->anchor2[2] ) );
+
+        if ( joint->flags & dJOINT_REVERSE )
+        {
+            q[0] = -q[0];
+            q[1] = -q[1];
+            q[2] = -q[2];
+        }
+    }
+
+    dVector3 axP;
+    // get prismatic axis in global coordinates
+    dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
+
+    return dCalcVectorDot3( axP, q );
+}
+
+dReal dJointGetPRPositionRate( dJointID j )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+    // get axis1 in global coordinates
+    dVector3 ax1;
+    dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );
+
+    if ( joint->node[1].body )
+    {
+        dVector3 lv2;
+        dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
+        return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
+    }
+    else
+    {
+        dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
+        return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
+    }
+}
+
+
+
+dReal dJointGetPRAngle( dJointID j )
+{
+    dxJointPR* joint = ( dxJointPR* )j;
+    dAASSERT( joint );
+    checktype( joint, PR );
+    if ( joint->node[0].body )
+    {
+        dReal ang = getHingeAngle( joint->node[0].body,
+            joint->node[1].body,
+            joint->axisR1,
+            joint->qrel );
+        if ( joint->flags & dJOINT_REVERSE )
+            return -ang;
+        else
+            return ang;
+    }
+    else return 0;
+}
+
+
+
+dReal dJointGetPRAngleRate( dJointID j )
+{
+    dxJointPR* joint = ( dxJointPR* )j;
+    dAASSERT( joint );
+    checktype( joint, PR );
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axisR1 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        if ( joint->flags & dJOINT_REVERSE ) rate = -rate;
+        return rate;
+    }
+    else return 0;
+}
+
+
+
+
+void 
+dxJointPR::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+
+void
+dxJointPR::getInfo1( dxJoint::Info1 *info )
+{
+    info->nub = 4;
+    info->m = 4;
+
+
+    // see if we're at a joint limit.
+    limotP.limit = 0;
+    if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) &&
+        limotP.lostop <= limotP.histop )
+    {
+        // measure joint position
+        dReal pos = dJointGetPRPosition( this );
+        limotP.testRotationalLimit( pos );  // N.B. The function is ill named
+    }
+
+    // powered needs an extra constraint row
+    if ( limotP.limit || limotP.fmax > 0 ) info->m++;
+
+
+    // see if we're at a joint limit.
+    limotR.limit = 0;
+    if (( limotR.lostop >= -M_PI || limotR.histop <= M_PI ) &&
+        limotR.lostop <= limotR.histop )
+    {
+        dReal angle = getHingeAngle( node[0].body,
+            node[1].body,
+            axisR1, qrel );
+        limotR.testRotationalLimit( angle );
+    }
+
+    // powered morit or at limits needs an extra constraint row
+    if ( limotR.limit || limotR.fmax > 0 ) info->m++;
+
+}
+
+
+
+void
+dxJointPR::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dReal k = worldFPS * worldERP;
+
+
+    dVector3 q;  // plane space of axP and after that axR
+
+    // pull out pos and R for both bodies. also get the `connection'
+    // vector pos2-pos1.
+
+    dReal *pos2 = NULL, *R2 = NULL;
+    
+    dReal *pos1 = node[0].body->posr.pos;
+    dReal *R1 = node[0].body->posr.R;
+
+    dxBody *body1 = node[1].body;
+
+    if ( body1 )
+    {
+        pos2 = body1->posr.pos;
+        R2 = body1->posr.R;
+    }
+
+
+    dVector3 axP; // Axis of the prismatic joint in global frame
+    dMultiply0_331( axP, R1, axisP1 );
+
+    // distance between the body1 and the anchor2 in global frame
+    // Calculated in the same way as the offset
+    dVector3 wanchor2 = {0, 0, 0}, dist;
+
+    if ( body1 )
+    {
+        // Calculate anchor2 in world coordinate
+        dMultiply0_331( wanchor2, R2, anchor2 );
+        dist[0] = wanchor2[0] + pos2[0] - pos1[0];
+        dist[1] = wanchor2[1] + pos2[1] - pos1[1];
+        dist[2] = wanchor2[2] + pos2[2] - pos1[2];
+    }
+    else
+    {
+        if ( (flags & dJOINT_REVERSE) != 0 )
+        {
+            dSubtractVectors3(dist, pos1, anchor2); // Invert the value
+        }
+        else
+        {
+            dSubtractVectors3(dist, anchor2, pos1); // Invert the value
+        }
+    }
+
+
+    // ======================================================================
+    // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered
+
+    // Set the two rotoide rows. The rotoide axis should be the only unconstrained
+    // rotational axis, the angular velocity of the two bodies perpendicular to
+    // the rotoide axis should be equal. Thus the constraint equations are
+    //    p*w1 - p*w2 = 0
+    //    q*w1 - q*w2 = 0
+    // where p and q are unit vectors normal to the rotoide axis, and w1 and w2
+    // are the angular velocity vectors of the two bodies.
+    dVector3 ax2;
+    dVector3 ax1;
+    dMultiply0_331( ax1, R1, axisR1 );
+    dCalcVectorCross3( q , ax1, axP );
+
+    dCopyVector3(J1 + GI2__JA_MIN, axP);
+
+    if ( body1 )
+    {
+        dCopyNegatedVector3(J2 + GI2__JA_MIN, axP);
+    }
+
+    dCopyVector3(J1 + rowskip + GI2__JA_MIN, q);
+
+    if ( body1 )
+    {
+        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, q);
+    }
+
+    // Compute the right hand side of the constraint equation set. Relative
+    // body velocities along p and q to bring the rotoide back into alignment.
+    // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
+    // We need to rotate both bodies along the axis u = (ax1 x ax2).
+    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+    // along u to cover angle erp*theta in one step :
+    //   |angular_velocity| = angle/time = erp*theta / stepsize
+    //                      = (erp*fps) * theta
+    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+    // ...as ax1 and ax2 are unit length. if theta is smallish,
+    // theta ~= sin(theta), so
+    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+    // ax1 x ax2 is in the plane space of ax1, so we project the angular
+    // velocity to p and q to find the right hand side.
+
+    if ( body1 )
+    {
+        dMultiply0_331( ax2, R2, axisR2 );
+    }
+    else
+    {
+        dCopyVector3(ax2, axisR2);
+    }
+
+    dVector3 b;
+    dCalcVectorCross3( b, ax1, ax2 );
+    pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3( b, axP );
+    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( b, q );
+
+
+
+    // ==========================
+    // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
+    // or 5 if rotoide and prismatic powered
+
+    // two rows. we want: vel2 = vel1 + w1 x c ... but this would
+    // result in three equations, so we project along the planespace vectors
+    // so that sliding along the prismatic axis is disregarded. for symmetry we
+    // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
+
+    // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
+    // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD  v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p
+    // v_p is speed of prismatic joint (i.e. elongation rate)
+    // Since the constraints are perpendicular to v_p we have:
+    // p dot v_p = 0 and q dot v_p = 0
+    // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
+    // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
+    // ==
+    // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist
+    // since a . (b x c) = - b . (a x c) = - (a x c) . b
+    // and a x b = - b x a
+    // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
+    // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
+    // Coeff for 1er line of: J1l => ax1, J2l => -ax1
+    // Coeff for 2er line of: J1l => q, J2l => -q
+    // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
+    // Coeff for 2er line of: J1a => dist x q,   J2a => - anchor2 x q
+
+    int currRowSkip = 2 * rowskip;
+    {
+        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, ax1 );
+        dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, ax1 );
+
+        if ( body1 )
+        {
+            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, ax1 );
+            // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
+            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, ax2, wanchor2 );   // since ax1 == ax2
+        }
+    }
+
+    currRowSkip += rowskip;
+    {
+        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
+        dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, dist, q );
+
+        if ( body1 )
+        {
+            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, q);
+            // The cross product is in reverse order since we want the negative value
+            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, wanchor2 );
+        }
+    }
+
+    // We want to make correction for motion not in the line of the axisP
+    // We calculate the displacement w.r.t. the anchor pt.
+    //
+    // compute the elements 2 and 3 of right hand side.
+    // we want to align the offset point (in body 2's frame) with the center of body 1.
+    // The position should be the same when we are not along the prismatic axis
+    dVector3 err;
+    dMultiply0_331( err, R1, offset );
+    dSubtractVectors3(err, dist, err);
+
+    int currPairSkip = 2 * pairskip;
+    {
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( ax1, err );
+    }
+
+    currPairSkip += pairskip;
+    {
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( q, err );
+    }
+
+    currRowSkip += rowskip; currPairSkip += pairskip;
+
+    if (  body1 || (flags & dJOINT_REVERSE) == 0 )
+    {
+        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, 0 ))
+        {
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+    }
+    else
+    {
+        dVector3 rAxP;
+        dCopyNegatedVector3(rAxP, axP);
+
+        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAxP, 0 ))
+        {
+            currRowSkip += rowskip; currPairSkip += pairskip;
+        }
+    }
+
+    limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
+}
+
+
+// compute initial relative rotation body1 -> body2, or env -> body1
+void
+dxJointPR::computeInitialRelativeRotation()
+{
+    if ( node[0].body )
+    {
+        if ( node[1].body )
+        {
+            dQMultiply1( qrel, node[0].body->q, node[1].body->q );
+        }
+        else
+        {
+            // set joint->qrel to the transpose of the first body q
+            qrel[0] = node[0].body->q[0];
+            for ( int i = 1; i < 4; i++ )
+                qrel[i] = -node[0].body->q[i];
+            // WARNING do we need the - in -joint->node[0].body->q[i]; or not
+        }
+    }
+}
+
+void dJointSetPRAnchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+    setAnchors( joint, x, y, z, joint->offset, joint->anchor2 );
+}
+
+
+void dJointSetPRAxis1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+
+    setAxes( joint, x, y, z, joint->axisP1, 0 );
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPRAxis2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+    setAxes( joint, x, y, z, joint->axisR1, joint->axisR2 );
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetPRParam( dJointID j, int parameter, dReal value )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        joint->limotR.set( parameter & 0xff, value );  // Take only lower part of the
+    }                                              // parameter value
+    else
+    {
+        joint->limotP.set( parameter, value );
+    }
+}
+
+void dJointGetPRAnchor( dJointID j, dVector3 result )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, PR );
+
+    if ( joint->node[1].body )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+    {
+        result[0] = joint->anchor2[0];
+        result[1] = joint->anchor2[1];
+        result[2] = joint->anchor2[2];
+    }
+}
+
+void dJointGetPRAxis1( dJointID j, dVector3 result )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, PR );
+    getAxis( joint, result, joint->axisP1 );
+}
+
+void dJointGetPRAxis2( dJointID j, dVector3 result )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, PR );
+    getAxis( joint, result, joint->axisR1 );
+}
+
+dReal dJointGetPRParam( dJointID j, int parameter )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, PR );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        return joint->limotR.get( parameter & 0xff );
+    }
+    else
+    {
+        return joint->limotP.get( parameter );
+    }
+}
+
+void dJointAddPRTorque( dJointID j, dReal torque )
+{
+    dxJointPR* joint = ( dxJointPR* ) j;
+    dVector3 axis;
+    dAASSERT( joint );
+    checktype( joint, PR );
+
+    if ( joint->flags & dJOINT_REVERSE )
+        torque = -torque;
+
+    getAxis( joint, axis, joint->axisR1 );
+    axis[0] *= torque;
+    axis[1] *= torque;
+    axis[2] *= torque;
+
+    if ( joint->node[0].body != 0 )
+        dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
+    if ( joint->node[1].body != 0 )
+        dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+}
+
+
+dJointType
+dxJointPR::type() const
+{
+    return dJointTypePR;
+}
+
+sizeint
+dxJointPR::size() const
+{
+    return sizeof( *this );
+}
+
+
+void
+dxJointPR::setRelativeValues()
+{
+    dVector3 anchor;
+    dJointGetPRAnchor(this, anchor);
+    setAnchors( this, anchor[0], anchor[1], anchor[2], offset, anchor2 );
+
+    dVector3 axis;
+    dJointGetPRAxis1(this, axis);
+    setAxes( this, axis[0], axis[1], axis[2], axisP1, 0 );
+
+    dJointGetPRAxis2(this, axis);
+    setAxes( this, axis[0], axis[1], axis[2], axisR1, axisR2 );
+
+    computeInitialRelativeRotation();
+}
+
+
+
+
+