29 #ifndef _ODE_COLLISION_UTIL_H_
30 #define _ODE_COLLISION_UTIL_H_
32 #include <ode/common.h>
33 #include <ode/contact.h>
34 #include <ode/rotation.h>
40 #define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
43 #include "collision_kernel.h"
46 dIASSERT(Index >= 0 && Index < (Flags & NUMC_MASK));
47 return ((
dContactGeom*)(((
char*)Contacts) + (Index * Stride)));
55 int dCollideSpheres (dVector3 p1, dReal r1,
66 void dLineClosestApproach (
const dVector3 pa,
const dVector3 ua,
67 const dVector3 pb,
const dVector3 ub,
68 dReal *alpha, dReal *beta);
78 void dClosestLineBoxPoints (
const dVector3 p1,
const dVector3 p2,
79 const dVector3 c,
const dMatrix3 R,
81 dVector3 lret, dVector3 bret);
85 int dClipEdgeToPlane(dVector3 &vEpnt0, dVector3 &vEpnt1,
const dVector4& plPlane);
87 void dClipPolyToPlane(
const dVector3 avArrayIn[],
const int ctIn, dVector3 avArrayOut[],
int &ctOut,
const dVector4 &plPlane );
89 void dClipPolyToCircle(
const dVector3 avArrayIn[],
const int ctIn, dVector3 avArrayOut[],
int &ctOut,
const dVector4 &plPlane ,dReal fRadius);
92 static inline void dVector3Subtract(
const dVector3& a,
const dVector3& b,dVector3& c)
94 dSubtractVectors3(c, a, b);
97 static inline void dVector3Scale(dVector3& a,dReal nScale)
99 dScaleVector3(a, nScale);
102 static inline void dVector3Add(
const dVector3& a,
const dVector3& b,dVector3& c)
104 dAddVectors3(c, a, b);
107 static inline void dVector3Copy(
const dVector3& a,dVector3& c)
112 static inline void dVector4Copy(
const dVector4& a,dVector4& c)
117 static inline void dVector3Cross(
const dVector3& a,
const dVector3& b,dVector3& c)
119 dCalcVectorCross3(c, a, b);
122 static inline dReal dVector3Length(
const dVector3& a)
124 return dCalcVectorLength3(a);
127 static inline dReal dVector3LengthSquare(
const dVector3& a)
129 return dCalcVectorLengthSquare3(a);
132 static inline dReal dVector3Dot(
const dVector3& a,
const dVector3& b)
134 return dCalcVectorDot3(a, b);
137 static inline void dVector3Inv(dVector3& a)
142 static inline void dMat3GetCol(
const dMatrix3& m,
const int col, dVector3& v)
144 dGetMatrixColumn3(v, m, col);
147 static inline void dVector3CrossMat3Col(
const dMatrix3& m,
const int col,
const dVector3& v,dVector3& r)
149 dCalcVectorCross3_114(r, v, m + col);
152 static inline void dMat3ColCrossVector3(
const dMatrix3& m,
const int col,
const dVector3& v,dVector3& r)
154 dCalcVectorCross3_141(r, m + col, v);
157 static inline void dMultiplyMat3Vec3(
const dMatrix3& m,
const dVector3& v, dVector3& r)
159 dMultiply0_331(r, m, v);
162 static inline dReal dPointPlaneDistance(
const dVector3& point,
const dVector4& plane)
164 return (plane[0]*point[0] + plane[1]*point[1] + plane[2]*point[2] + plane[3]);
167 static inline void dConstructPlane(
const dVector3& normal,
const dReal& distance, dVector4& plane)
169 plane[0] = normal[0];
170 plane[1] = normal[1];
171 plane[2] = normal[2];
175 static inline void dMatrix3Copy(
const dReal* source,dMatrix3& dest)
177 dCopyMatrix4x3(dest, source);
180 static inline dReal dMatrix3Det(
const dMatrix3& mat )
184 det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] )
185 - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] )
186 + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] );
192 inline void dMatrix3Inv(
const dMatrix3& ma, dMatrix3& dst )
194 dReal det = dMatrix3Det( ma );
196 if ( dFabs( det ) < REAL(0.0005) )
198 dRSetIdentity( dst );
202 double detRecip = REAL(1.0) / det;
204 dst[0] = ( ma[5]*ma[10] - ma[6]*ma[9] ) * detRecip;
205 dst[1] = ( ma[9]*ma[2] - ma[1]*ma[10] ) * detRecip;
206 dst[2] = ( ma[1]*ma[6] - ma[5]*ma[2] ) * detRecip;
208 dst[4] = ( ma[6]*ma[8] - ma[4]*ma[10] ) * detRecip;
209 dst[5] = ( ma[0]*ma[10] - ma[8]*ma[2] ) * detRecip;
210 dst[6] = ( ma[4]*ma[2] - ma[0]*ma[6] ) * detRecip;
212 dst[8] = ( ma[4]*ma[9] - ma[8]*ma[5] ) * detRecip;
213 dst[9] = ( ma[8]*ma[1] - ma[0]*ma[9] ) * detRecip;
214 dst[10] = ( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip;
217 inline void dQuatTransform(
const dQuaternion& quat,
const dVector3& source,dVector3& dest)
221 dReal x0 = source[0] * quat[0] + source[2] * quat[2] - source[1] * quat[3];
222 dReal x1 = source[1] * quat[0] + source[0] * quat[3] - source[2] * quat[1];
223 dReal x2 = source[2] * quat[0] + source[1] * quat[1] - source[0] * quat[2];
224 dReal x3 = source[0] * quat[1] + source[1] * quat[2] + source[2] * quat[3];
226 dest[0] = quat[0] * x0 + quat[1] * x3 + quat[2] * x2 - quat[3] * x1;
227 dest[1] = quat[0] * x1 + quat[2] * x3 + quat[3] * x0 - quat[1] * x2;
228 dest[2] = quat[0] * x2 + quat[3] * x3 + quat[1] * x1 - quat[2] * x0;
250 inline void dQuatInvTransform(
const dQuaternion& quat,
const dVector3& source,dVector3& dest)
253 dReal norm = quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3];
255 if (norm > REAL(0.0))
258 invQuat[0] = quat[0] / norm;
259 invQuat[1] = -quat[1] / norm;
260 invQuat[2] = -quat[2] / norm;
261 invQuat[3] = -quat[3] / norm;
263 dQuatTransform(invQuat,source,dest);
269 dVector3Copy(source,dest);
273 inline void dGetEulerAngleFromRot(
const dMatrix3& mRot,dReal& rX,dReal& rY,dReal& rZ)
275 rY = asin(mRot[0 * 4 + 2]);
280 rX = atan2(-mRot[1*4 + 2], mRot[2*4 + 2]);
281 rZ = atan2(-mRot[0*4 + 1], mRot[0*4 + 0]);
286 rX = -atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
293 rX = atan2(mRot[1*4 + 0], mRot[1*4 + 1]);
298 inline void dQuatInv(
const dQuaternion& source, dQuaternion& dest)
300 dReal norm = source[0]*source[0] + source[1]*source[1] + source[2]*source[2] + source[3]*source[3];
304 dest[0] = source[0] / norm;
305 dest[1] = -source[1] / norm;
306 dest[2] = -source[2] / norm;
307 dest[3] = -source[3] / norm;