From 2f8d7092bc2c9609fa98d6888106b96f38b22828 Mon Sep 17 00:00:00 2001 From: dan miller Date: Sun, 21 Oct 2007 08:36:32 +0000 Subject: libraries moved to opensim-libs, a new repository --- libraries/ode-0.9/ode/src/collision_util.h | 340 ----------------------------- 1 file changed, 340 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_util.h (limited to 'libraries/ode-0.9/ode/src/collision_util.h') diff --git a/libraries/ode-0.9/ode/src/collision_util.h b/libraries/ode-0.9/ode/src/collision_util.h deleted file mode 100644 index 4c479e3..0000000 --- a/libraries/ode-0.9/ode/src/collision_util.h +++ /dev/null @@ -1,340 +0,0 @@ -/************************************************************************* - * * - * 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 -#include -#include -#include - - -// 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 -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 -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] *= nScale ; - a[1] *= nScale ; - 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(a[0]*a[0]+a[1]*a[1]+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 (a[0]*a[0]+a[1]*a[1]+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] = v[1] * m[2*4 + col] - v[2] * m[1*4 + col]; - r[1] = v[2] * m[0*4 + col] - v[0] * m[2*4 + col]; - r[2] = v[0] * m[1*4 + col] - v[1] * m[0*4 + col]; -} - -inline void dMat3ColCrossVector3(const dMatrix3& m,const int col,const dVector3& v,dVector3& r) -{ - r[0] = v[2] * m[1*4 + col] - v[1] * m[2*4 + col]; - r[1] = v[0] * m[2*4 + col] - v[2] * m[0*4 + col]; - r[2] = v[1] * m[0*4 + col] - 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 (plane[0]*point[0] + plane[1]*point[1] + 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 = 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 ); -} - - -inline void dMatrix3Inv( const dMatrix3& ma, dMatrix3& dst ) -{ - dReal det = dMatrix3Det( ma ); - - if ( dFabs( det ) < REAL(0.0005) ) - { - dRSetIdentity( dst ); - return; - } - - dst[0] = ma[5]*ma[10] - ma[6]*ma[9] / det; - dst[1] = -( ma[1]*ma[10] - ma[9]*ma[2] ) / det; - dst[2] = ma[1]*ma[6] - ma[5]*ma[2] / det; - - dst[4] = -( ma[4]*ma[10] - ma[6]*ma[8] ) / det; - dst[5] = ma[0]*ma[10] - ma[8]*ma[2] / det; - dst[6] = -( ma[0]*ma[6] - ma[4]*ma[2] ) / det; - - dst[8] = ma[4]*ma[9] - ma[8]*ma[5] / det; - dst[9] = -( ma[0]*ma[9] - ma[8]*ma[1] ) / det; - dst[10] = ma[0]*ma[5] - 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 = 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]; - */ -} - -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); - } -} - -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); - } -} - -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) - { - dest[0] = source[0] / norm; - dest[1] = -source[1] / norm; - dest[2] = -source[2] / norm; - dest[3] = -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); - } -} - - -#endif -- cgit v1.1