add ode

1 files changed, 358 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/collision_util.h b/libs/ode-0.16.1/ode/src/collision_util.h
new file mode 100644
index 0000000..57e116a
--- /dev/null
+++ b/libs/ode-0.16.1/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