--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/inc/collision_util.h Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,339 @@
+/*************************************************************************
+ * *
+ * 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/odemath.h>
+#include <ode/rotation.h>
+
+
+// given a pointer `p' to a dContactGeom, return the dContactGeom at
+// p + skip bytes.
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+
+
+// 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
+inline void dVector3Subtract(const dVector3& a,const dVector3& b,dVector3& c)
+{
+ c[0] = a[0] - b[0];
+ c[1] = a[1] - b[1];
+ c[2] = a[2] - b[2];
+}
+
+// Some vector math
+inline void dVector3Scale(dVector3& a,dReal nScale)
+{
+ a[0] = dMUL(a[0],nScale);
+ a[1] = dMUL(a[1],nScale);
+ a[2] = dMUL(a[2],nScale);
+}
+
+inline void dVector3Add(const dVector3& a,const dVector3& b,dVector3& c)
+{
+ c[0] = a[0] + b[0];
+ c[1] = a[1] + b[1];
+ c[2] = a[2] + b[2];
+}
+
+inline void dVector3Copy(const dVector3& a,dVector3& c)
+{
+ c[0] = a[0];
+ c[1] = a[1];
+ c[2] = a[2];
+}
+
+inline void dVector3Cross(const dVector3& a,const dVector3& b,dVector3& c)
+{
+ dCROSS(c,=,a,b);
+}
+
+inline dReal dVector3Length(const dVector3& a)
+{
+ return dSqrt(dMUL(a[0],a[0])+dMUL(a[1],a[1])+dMUL(a[2],a[2]));
+}
+
+inline dReal dVector3Dot(const dVector3& a,const dVector3& b)
+{
+ return dDOT(a,b);
+}
+
+inline void dVector3Inv(dVector3& a)
+{
+ a[0] = -a[0];
+ a[1] = -a[1];
+ a[2] = -a[2];
+}
+
+inline dReal dVector3Length2(const dVector3& a)
+{
+ return (dMUL(a[0],a[0])+dMUL(a[1],a[1])+dMUL(a[2],a[2]));
+}
+
+inline void dMat3GetCol(const dMatrix3& m,const int col, dVector3& v)
+{
+ v[0] = m[col + 0];
+ v[1] = m[col + 4];
+ v[2] = m[col + 8];
+}
+
+inline void dVector3CrossMat3Col(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+{
+ r[0] = dMUL(v[1],m[2*4 + col]) - dMUL(v[2],m[1*4 + col]);
+ r[1] = dMUL(v[2],m[0*4 + col]) - dMUL(v[0],m[2*4 + col]);
+ r[2] = dMUL(v[0],m[1*4 + col]) - dMUL(v[1],m[0*4 + col]);
+}
+
+inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r)
+{
+ r[0] = dMUL(v[2],m[1*4 + col]) - dMUL(v[1],m[2*4 + col]);
+ r[1] = dMUL(v[0],m[2*4 + col]) - dMUL(v[2],m[0*4 + col]);
+ r[2] = dMUL(v[1],m[0*4 + col]) - dMUL(v[0],m[1*4 + col]);
+}
+
+inline void dMultiplyMat3Vec3(const dMatrix3& m,const dVector3& v, dVector3& r)
+{
+ dMULTIPLY0_331(r,m,v);
+}
+
+inline dReal dPointPlaneDistance(const dVector3& point,const dVector4& plane)
+{
+ return (dMUL(plane[0],point[0]) + dMUL(plane[1],point[1]) + dMUL(plane[2],point[2]) + plane[3]);
+}
+
+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;
+}
+
+inline void dMatrix3Copy(const dReal* source,dMatrix3& dest)
+{
+ dest[0] = source[0];
+ dest[1] = source[1];
+ dest[2] = source[2];
+
+ dest[4] = source[4];
+ dest[5] = source[5];
+ dest[6] = source[6];
+
+ dest[8] = source[8];
+ dest[9] = source[9];
+ dest[10]= source[10];
+}
+
+inline dReal dMatrix3Det( const dMatrix3& mat )
+{
+ dReal det;
+
+ det = dMUL(mat[0],( dMUL(mat[5],mat[10]) - dMUL(mat[9],mat[6]) ))
+ - dMUL(mat[1],( dMUL(mat[4],mat[10]) - dMUL(mat[8],mat[6]) ))
+ + dMUL(mat[2],( dMUL(mat[4],mat[9]) - dMUL(mat[8],mat[5]) ));
+
+ return( det );
+}
+
+
+inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst )
+{
+ dReal det = dMatrix3Det( ma );
+
+ if ( dFabs( det ) < REAL(0.0005) )
+ {
+ dRSetIdentity( dst );
+ return;
+ }
+
+ dst[0] = dMUL(ma[5],ma[10]) - dDIV(dMUL(ma[6],ma[9]),det);
+ dst[1] = -dDIV(( dMUL(ma[1],ma[10]) - dMUL(ma[9],ma[2]) ),det);
+ dst[2] = dMUL(ma[1],ma[6]) - dDIV(dMUL(ma[5],ma[2]),det);
+
+ dst[4] = -dDIV(( dMUL(ma[4],ma[10]) - dMUL(ma[6],ma[8]) ),det);
+ dst[5] = dMUL(ma[0],ma[10]) - dDIV(dMUL(ma[8],ma[2]),det);
+ dst[6] = -dDIV(( dMUL(ma[0],ma[6]) - dMUL(ma[4],ma[2]) ),det);
+
+ dst[8] = dMUL(ma[4],ma[9]) - dDIV(dMUL(ma[8],ma[5]),det);
+ dst[9] = -dDIV(( dMUL(ma[0],ma[9]) - dMUL(ma[8],ma[1]) ),det);
+ dst[10] = dMUL(ma[0],ma[5]) - dDIV(dMUL(ma[1],ma[4]),det);
+}
+
+inline void dQuatTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+{
+
+ // Nguyen Binh : this code seem to be the fastest.
+ dReal x0 = dMUL(source[0],quat[0]) + dMUL(source[2],quat[2]) - dMUL(source[1],quat[3]);
+ dReal x1 = dMUL(source[1],quat[0]) + dMUL(source[0],quat[3]) - dMUL(source[2],quat[1]);
+ dReal x2 = dMUL(source[2],quat[0]) + dMUL(source[1],quat[1]) - dMUL(source[0],quat[2]);
+ dReal x3 = dMUL(source[0],quat[1]) + dMUL(source[1],quat[2]) + dMUL(source[2],quat[3]);
+
+ dest[0] = dMUL(quat[0],x0) + dMUL(quat[1],x3) + dMUL(quat[2],x2) - dMUL(quat[3],x1);
+ dest[1] = dMUL(quat[0],x1) + dMUL(quat[2],x3) + dMUL(quat[3],x0) - dMUL(quat[1],x2);
+ dest[2] = dMUL(quat[0],x2) + dMUL(quat[3],x3) + dMUL(quat[1],x1) - dMUL(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];
+ */
+}
+
+inline void dQuatInvTransform(const dQuaternion& quat,const dVector3& source,dVector3& dest)
+{
+
+ dReal norm = dMUL(quat[0],quat[0]) + dMUL(quat[1],quat[1]) + dMUL(quat[2],quat[2]) + dMUL(quat[3],quat[3]);
+
+ if (norm > REAL(0.0))
+ {
+ dQuaternion invQuat;
+ invQuat[0] = dDIV(quat[0],norm);
+ invQuat[1] = -dDIV(quat[1],norm);
+ invQuat[2] = -dDIV(quat[2],norm);
+ invQuat[3] = -dDIV(quat[3],norm);
+
+ dQuatTransform(invQuat,source,dest);
+
+ }
+ else
+ {
+ // Singular -> return identity
+ dVector3Copy(source,dest);
+ }
+}
+
+inline void dGetEulerAngleFromRot(const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
+{
+ rY = asin(mRot[0 * 4 + 2]);
+ if (rY < dPI /2)
+ {
+ if (rY > -dPI /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);
+ }
+}
+
+inline void dQuatInv(const dQuaternion& source, dQuaternion& dest)
+{
+ dReal norm = dMUL(source[0],source[0]) + dMUL(source[1],source[1]) + dMUL(source[2],source[2]) + dMUL(source[3],source[3]);
+
+ if (norm > REAL(0.0f))
+ {
+ dest[0] = dDIV(source[0],norm);
+ dest[1] = -dDIV(source[1],norm);
+ dest[2] = -dDIV(source[2],norm);
+ dest[3] = -dDIV(source[3],norm);
+ }
+ else
+ {
+ // Singular -> return identity
+ dest[0] = REAL(1.0);
+ dest[1] = REAL(0.0);
+ dest[2] = REAL(0.0);
+ dest[3] = REAL(0.0);
+ }
+}
+
+#if 1
+// Fetches a contact
+inline dContactGeom* SAFECONTACT(int /*Flags*/, dContactGeom* Contacts, int Index, int Stride){
+
+ return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
+}
+#endif
+
+
+#endif