--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/collision_cylinder_box.cpp Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,985 @@
+/*************************************************************************
+* *
+* 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-box collider by Alen Ladavac
+ * Ported to ODE by Nguyen Binh
+ */
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+#include <ode/lookup_tables.h>
+
+static const int MAX_CYLBOX_CLIP_POINTS = 16;
+static const int nCYLINDER_AXIS = 2;
+// Number of segment of cylinder base circle.
+// Must be divisible by 4.
+static const int nCYLINDER_SEGMENT = 8;
+
+#define MAX_FLOAT dInfinity
+
+// Data that passed through the collider's functions
+typedef struct _sCylinderBoxData
+{
+ // cylinder parameters
+ dMatrix3 mCylinderRot;
+ dVector3 vCylinderPos;
+ dVector3 vCylinderAxis;
+ dReal fCylinderRadius;
+ dReal fCylinderSize;
+ dVector3 avCylinderNormals[nCYLINDER_SEGMENT];
+
+ // box parameters
+
+ dMatrix3 mBoxRot;
+ dVector3 vBoxPos;
+ dVector3 vBoxHalfSize;
+ // box vertices array : 8 vertices
+ dVector3 avBoxVertices[8];
+
+ // global collider data
+ dVector3 vDiff;
+ dVector3 vNormal;
+ dReal fBestDepth;
+ dReal fBestrb;
+ dReal fBestrc;
+ int iBestAxis;
+
+ // contact data
+ dVector3 vEp0, vEp1;
+ dReal fDepth0, fDepth1;
+
+ // ODE stuff
+ dGeomID gBox;
+ dGeomID gCylinder;
+ dContactGeom* gContact;
+ int iFlags;
+ int iSkip;
+ int nContacts;
+
+} sCylinderBoxData;
+
+
+// initialize collision data
+void _cldInitCylinderBox(sCylinderBoxData& cData)
+{
+ // get cylinder position, orientation
+ const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder);
+ dMatrix3Copy(pRotCyc,cData.mCylinderRot);
+
+ const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder);
+ dVector3Copy(*pPosCyc,cData.vCylinderPos);
+
+ dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis);
+
+ // get cylinder radius and size
+ dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize);
+
+ // get box position, orientation, size
+ const dReal* pRotBox = dGeomGetRotation(cData.gBox);
+ dMatrix3Copy(pRotBox,cData.mBoxRot);
+ const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox);
+ dVector3Copy(*pPosBox,cData.vBoxPos);
+
+ dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize);
+ cData.vBoxHalfSize[0] = dMUL(cData.vBoxHalfSize[0],REAL(0.5));
+ cData.vBoxHalfSize[1] = dMUL(cData.vBoxHalfSize[1],REAL(0.5));
+ cData.vBoxHalfSize[2] = dMUL(cData.vBoxHalfSize[2],REAL(0.5));
+
+ // vertex 0
+ cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0];
+ cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1];
+ cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2];
+
+ // vertex 1
+ cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0];
+ cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1];
+ cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2];
+
+ // vertex 2
+ cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0];
+ cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1];
+ cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2];
+
+ // vertex 3
+ cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0];
+ cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1];
+ cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2];
+
+ // vertex 4
+ cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0];
+ cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1];
+ cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2];
+
+ // vertex 5
+ cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0];
+ cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1];
+ cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2];
+
+ // vertex 6
+ cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0];
+ cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1];
+ cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2];
+
+ // vertex 7
+ cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0];
+ cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1];
+ cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2];
+
+ // temp index
+ int i = 0;
+ dVector3 vTempBoxVertices[8];
+ // transform vertices in absolute space
+ for(i=0; i < 8; i++)
+ {
+ dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]);
+ dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]);
+ }
+
+ // find relative position
+ dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff);
+ cData.fBestDepth = MAX_FLOAT;
+ cData.vNormal[0] = REAL(0.0);
+ cData.vNormal[1] = REAL(0.0);
+ cData.vNormal[2] = REAL(0.0);
+
+ // calculate basic angle for nCYLINDER_SEGMENT-gon
+ dReal fAngle = dPI/nCYLINDER_SEGMENT;
+
+ // calculate angle increment
+ dReal fAngleIncrement = dMUL(fAngle,REAL(2.0));
+
+ // calculate nCYLINDER_SEGMENT-gon points
+ for(i = 0; i < nCYLINDER_SEGMENT; i++)
+ {
+ cData.avCylinderNormals[i][0] = -dCos(fAngle);
+ cData.avCylinderNormals[i][1] = -dSin(fAngle);
+ cData.avCylinderNormals[i][2] = 0;
+
+ fAngle += fAngleIncrement;
+ }
+
+ cData.fBestrb = 0;
+ cData.fBestrc = 0;
+ cData.iBestAxis = 0;
+ cData.nContacts = 0;
+
+}
+
+// test for given separating axis
+int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis )
+{
+ // check length of input normal
+ dReal fL = dVector3Length(vInputNormal);
+ // if not long enough
+ if ( fL < REAL(2e-5f) )
+ {
+ // do nothing
+ return 1;
+ }
+
+ // otherwise make it unit for sure
+ dNormalize3(vInputNormal);
+
+ // project box and Cylinder on mAxis
+ dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal);
+
+ dReal frc;
+
+ if (fdot1 > REAL(1.0))
+ {
+ fdot1 = REAL(1.0);
+ frc = dFabs(dMUL(cData.fCylinderSize,REAL(0.5)));
+ }
+
+ // project box and capsule on iAxis
+ frc = dFabs( dMUL(fdot1,(cData.fCylinderSize*REAL(0.5)))) + dMUL(cData.fCylinderRadius,dSqrt(REAL(1.0)-(dMUL(fdot1,fdot1))));
+
+ dVector3 vTemp1;
+ dReal frb = REAL(0.0);
+
+ dMat3GetCol(cData.mBoxRot,0,vTemp1);
+ frb = dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[0]);
+
+ dMat3GetCol(cData.mBoxRot,1,vTemp1);
+ frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[1]);
+
+ dMat3GetCol(cData.mBoxRot,2,vTemp1);
+ frb += dMUL(dFabs(dVector3Dot(vTemp1,vInputNormal)),cData.vBoxHalfSize[2]);
+
+ // project their distance on separating axis
+ dReal fd = dVector3Dot(cData.vDiff,vInputNormal);
+
+ // if they do not overlap exit, we have no intersection
+ if ( dFabs(fd) > frc+frb )
+ {
+ return 0;
+ }
+
+ // get depth
+ dReal fDepth = - dFabs(fd) + (frc+frb);
+
+ // get maximum depth
+ if ( fDepth < cData.fBestDepth )
+ {
+ cData.fBestDepth = fDepth;
+ dVector3Copy(vInputNormal,cData.vNormal);
+ cData.iBestAxis = iAxis;
+ cData.fBestrb = frb;
+ cData.fBestrc = frc;
+
+ // flip normal if interval is wrong faced
+ if (fd > 0)
+ {
+ dVector3Inv(cData.vNormal);
+ }
+ }
+
+ return 1;
+}
+
+
+// check for separation between box edge and cylinder circle edge
+int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
+ const dVector3 &vCenterPoint,
+ const dVector3 &vVx0, const dVector3 &vVx1,
+ int iAxis )
+{
+ // calculate direction of edge
+ dVector3 vDirEdge;
+ dVector3Subtract(vVx1,vVx0,vDirEdge);
+ dNormalize3(vDirEdge);
+ // starting point of edge
+ dVector3 vEStart;
+ dVector3Copy(vVx0,vEStart);
+
+ // calculate angle cosine between cylinder axis and edge
+ dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);
+
+ // if edge is perpendicular to cylinder axis
+ if(dFabs(fdot2) < REAL(2e-5f))
+ {
+ // this can't be separating axis, because edge is parallel to circle plane
+ return 1;
+ }
+
+ // find point of intersection between edge line and circle plane
+ dVector3 vTemp1;
+ dVector3Subtract(vCenterPoint,vEStart,vTemp1);
+ dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
+ dVector3 vpnt;
+ vpnt[0]= vEStart[0] + dMUL(vDirEdge[0],dDIV(fdot1,fdot2));
+ vpnt[1]= vEStart[1] + dMUL(vDirEdge[1],dDIV(fdot1,fdot2));
+ vpnt[2]= vEStart[2] + dMUL(vDirEdge[2],dDIV(fdot1,fdot2));
+
+ // find tangent vector on circle with same center (vCenterPoint) that
+ // touches point of intersection (vpnt)
+ dVector3 vTangent;
+ dVector3Subtract(vCenterPoint,vpnt,vTemp1);
+ dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
+
+ // find vector orthogonal both to tangent and edge direction
+ dVector3 vAxis;
+ dVector3Cross(vTangent,vDirEdge,vAxis);
+
+ // use that vector as separating axis
+ return _cldTestAxis( cData, vAxis, iAxis );
+}
+
+// Test separating axis for collision
+int _cldTestSeparatingAxes(sCylinderBoxData& cData)
+{
+ // reset best axis
+ cData.fBestDepth = MAX_FLOAT;
+ cData.iBestAxis = 0;
+ cData.fBestrb = 0;
+ cData.fBestrc = 0;
+ cData.nContacts = 0;
+
+ dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
+
+ // Epsilon value for checking axis vector length
+ const dReal fEpsilon = REAL(2e-5f);
+
+ // axis A0
+ dMat3GetCol(cData.mBoxRot, 0 , vAxis);
+ if (!_cldTestAxis( cData, vAxis, 1 ))
+ {
+ return 0;
+ }
+
+ // axis A1
+ dMat3GetCol(cData.mBoxRot, 1 , vAxis);
+ if (!_cldTestAxis( cData, vAxis, 2 ))
+ {
+ return 0;
+ }
+
+ // axis A2
+ dMat3GetCol(cData.mBoxRot, 2 , vAxis);
+ if (!_cldTestAxis( cData, vAxis, 3 ))
+ {
+ return 0;
+ }
+
+ // axis C - Cylinder Axis
+ //vAxis = vCylinderAxis;
+ dVector3Copy(cData.vCylinderAxis , vAxis);
+ if (!_cldTestAxis( cData, vAxis, 4 ))
+ {
+ return 0;
+ }
+
+ // axis CxA0
+ //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 ));
+ dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis);
+ if(dVector3Length2( vAxis ) > fEpsilon )
+ {
+ if (!_cldTestAxis( cData, vAxis, 5 ))
+ {
+ return 0;
+ }
+ }
+
+ // axis CxA1
+ //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 ));
+ dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis);
+ if(dVector3Length2( vAxis ) > fEpsilon )
+ {
+ if (!_cldTestAxis( cData, vAxis, 6 ))
+ {
+ return 0;
+ }
+ }
+
+ // axis CxA2
+ //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 ));
+ dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis);
+ if(dVector3Length2( vAxis ) > fEpsilon )
+ {
+ if (!_cldTestAxis( cData, vAxis, 7 ))
+ {
+ return 0;
+ }
+ }
+
+ int i = 0;
+ dVector3 vTemp1;
+ dVector3 vTemp2;
+ // here we check box's vertices axis
+ for(i=0; i< 8; i++)
+ {
+ //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos));
+ dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1);
+ dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2);
+ //vAxis = ( vCylinderAxis cross vAxis );
+ dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis);
+ if(dVector3Length2( vAxis ) > fEpsilon )
+ {
+ if (!_cldTestAxis( cData, vAxis, 8 + i ))
+ {
+ return 0;
+ }
+ }
+ }
+
+ // ************************************
+ // this is defined for first 12 axes
+ // normal of plane that contains top circle of cylinder
+ // center of top circle of cylinder
+ dVector3 vcc;
+ vcc[0] = (cData.vCylinderPos)[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vcc[1] = (cData.vCylinderPos)[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vcc[2] = (cData.vCylinderPos)[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+ // ************************************
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19))
+ {
+ return 0;
+ }
+
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27))
+ {
+ return 0;
+ }
+
+ // ************************************
+ // this is defined for second 12 axes
+ // normal of plane that contains bottom circle of cylinder
+ // center of bottom circle of cylinder
+ // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5));
+ vcc[0] = (cData.vCylinderPos)[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vcc[1] = (cData.vCylinderPos)[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vcc[2] = (cData.vCylinderPos)[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+ // ************************************
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38))
+ {
+ return 0;
+ }
+
+ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39))
+ {
+ return 0;
+ }
+
+ return 1;
+}
+
+int _cldClipCylinderToBox(sCylinderBoxData& cData)
+{
+
+ // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal
+ dVector3 vN;
+ dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal);
+ vN[0] = cData.vNormal[0] - dMUL(cData.vCylinderAxis[0],fTemp1);
+ vN[1] = cData.vNormal[1] - dMUL(cData.vCylinderAxis[1],fTemp1);
+ vN[2] = cData.vNormal[2] - dMUL(cData.vCylinderAxis[2],fTemp1);
+
+ // normalize that vector
+ dNormalize3(vN);
+
+ // translate cylinder end points by the vector
+ dVector3 vCposTrans;
+ vCposTrans[0] = cData.vCylinderPos[0] + dMUL(vN[0],cData.fCylinderRadius);
+ vCposTrans[1] = cData.vCylinderPos[1] + dMUL(vN[1],cData.fCylinderRadius);
+ vCposTrans[2] = cData.vCylinderPos[2] + dMUL(vN[2],cData.fCylinderRadius);
+
+ cData.vEp0[0] = vCposTrans[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ cData.vEp0[1] = vCposTrans[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ cData.vEp0[2] = vCposTrans[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+
+ cData.vEp1[0] = vCposTrans[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ cData.vEp1[1] = vCposTrans[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ cData.vEp1[2] = vCposTrans[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+
+ // transform edge points in box space
+ cData.vEp0[0] -= cData.vBoxPos[0];
+ cData.vEp0[1] -= cData.vBoxPos[1];
+ cData.vEp0[2] -= cData.vBoxPos[2];
+
+ cData.vEp1[0] -= cData.vBoxPos[0];
+ cData.vEp1[1] -= cData.vBoxPos[1];
+ cData.vEp1[2] -= cData.vBoxPos[2];
+
+ dVector3 vTemp1;
+ // clip the edge to box
+ dVector4 plPlane;
+ // plane 0 +x
+ dMat3GetCol(cData.mBoxRot,0,vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // plane 1 +y
+ dMat3GetCol(cData.mBoxRot,1,vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // plane 2 +z
+ dMat3GetCol(cData.mBoxRot,2,vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // plane 3 -x
+ dMat3GetCol(cData.mBoxRot,0,vTemp1);
+ dVector3Inv(vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // plane 4 -y
+ dMat3GetCol(cData.mBoxRot,1,vTemp1);
+ dVector3Inv(vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // plane 5 -z
+ dMat3GetCol(cData.mBoxRot,2,vTemp1);
+ dVector3Inv(vTemp1);
+ dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane);
+ if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane ))
+ {
+ return 0;
+ }
+
+ // calculate depths for both contact points
+ cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal);
+ cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal);
+
+ // clamp depths to 0
+ if(cData.fDepth0<0)
+ {
+ cData.fDepth0 = REAL(0.0);
+ }
+
+ if(cData.fDepth1<0)
+ {
+ cData.fDepth1 = REAL(0.0);
+ }
+
+ // back transform edge points from box to absolute space
+ cData.vEp0[0] += cData.vBoxPos[0];
+ cData.vEp0[1] += cData.vBoxPos[1];
+ cData.vEp0[2] += cData.vBoxPos[2];
+
+ cData.vEp1[0] += cData.vBoxPos[0];
+ cData.vEp1[1] += cData.vBoxPos[1];
+ cData.vEp1[2] += cData.vBoxPos[2];
+
+ dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
+ Contact0->depth = cData.fDepth0;
+ dVector3Copy(cData.vNormal,Contact0->normal);
+ dVector3Copy(cData.vEp0,Contact0->pos);
+ Contact0->g1 = cData.gCylinder;
+ Contact0->g2 = cData.gBox;
+ dVector3Inv(Contact0->normal);
+ cData.nContacts++;
+
+ dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
+ Contact1->depth = cData.fDepth1;
+ dVector3Copy(cData.vNormal,Contact1->normal);
+ dVector3Copy(cData.vEp1,Contact1->pos);
+ Contact1->g1 = cData.gCylinder;
+ Contact1->g2 = cData.gBox;
+ dVector3Inv(Contact1->normal);
+ cData.nContacts++;
+
+ return 1;
+}
+
+
+void _cldClipBoxToCylinder(sCylinderBoxData& cData )
+{
+ dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel;
+ // check which circle from cylinder we take for clipping
+ if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) )
+ {
+ // get top circle
+ vCylinderCirclePos[0] = cData.vCylinderPos[0] + dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vCylinderCirclePos[1] = cData.vCylinderPos[1] + dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vCylinderCirclePos[2] = cData.vCylinderPos[2] + dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+
+ vCylinderCircleNormal_Rel[0] = REAL(0.0);
+ vCylinderCircleNormal_Rel[1] = REAL(0.0);
+ vCylinderCircleNormal_Rel[2] = REAL(0.0);
+ vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0);
+ }
+ else
+ {
+ // get bottom circle
+ vCylinderCirclePos[0] = cData.vCylinderPos[0] - dMUL(cData.vCylinderAxis[0],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vCylinderCirclePos[1] = cData.vCylinderPos[1] - dMUL(cData.vCylinderAxis[1],dMUL(cData.fCylinderSize,REAL(0.5)));
+ vCylinderCirclePos[2] = cData.vCylinderPos[2] - dMUL(cData.vCylinderAxis[2],dMUL(cData.fCylinderSize,REAL(0.5)));
+
+ vCylinderCircleNormal_Rel[0] = REAL(0.0);
+ vCylinderCircleNormal_Rel[1] = REAL(0.0);
+ vCylinderCircleNormal_Rel[2] = REAL(0.0);
+ vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0);
+ }
+
+ // vNr is normal in Box frame, pointing from Cylinder to Box
+ dVector3 vNr;
+ dMatrix3 mBoxInv;
+
+ // Find a way to use quaternion
+ dMatrix3Inv(cData.mBoxRot,mBoxInv);
+ dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr);
+
+ dVector3 vAbsNormal;
+
+ vAbsNormal[0] = dFabs( vNr[0] );
+ vAbsNormal[1] = dFabs( vNr[1] );
+ vAbsNormal[2] = dFabs( vNr[2] );
+
+ // find which face in box is closest to cylinder
+ int iB0, iB1, iB2;
+
+ // Different from Croteam's code
+ if (vAbsNormal[1] > vAbsNormal[0])
+ {
+ // 1 > 0
+ if (vAbsNormal[0]> vAbsNormal[2])
+ {
+ // 0 > 2 -> 1 > 0 >2
+ iB0 = 1; iB1 = 0; iB2 = 2;
+ }
+ else
+ {
+ // 2 > 0-> Must compare 1 and 2
+ if (vAbsNormal[1] > vAbsNormal[2])
+ {
+ // 1 > 2 -> 1 > 2 > 0
+ iB0 = 1; iB1 = 2; iB2 = 0;
+ }
+ else
+ {
+ // 2 > 1 -> 2 > 1 > 0;
+ iB0 = 2; iB1 = 1; iB2 = 0;
+ }
+ }
+ }
+ else
+ {
+ // 0 > 1
+ if (vAbsNormal[1] > vAbsNormal[2])
+ {
+ // 1 > 2 -> 0 > 1 > 2
+ iB0 = 0; iB1 = 1; iB2 = 2;
+ }
+ else
+ {
+ // 2 > 1 -> Must compare 0 and 2
+ if (vAbsNormal[0] > vAbsNormal[2])
+ {
+ // 0 > 2 -> 0 > 2 > 1;
+ iB0 = 0; iB1 = 2; iB2 = 1;
+ }
+ else
+ {
+ // 2 > 0 -> 2 > 0 > 1;
+ iB0 = 2; iB1 = 0; iB2 = 1;
+ }
+ }
+ }
+
+ dVector3 vCenter;
+ // find center of box polygon
+ dVector3 vTemp;
+ if (vNr[iB0] > 0)
+ {
+ dMat3GetCol(cData.mBoxRot,iB0,vTemp);
+ vCenter[0] = cData.vBoxPos[0] - dMUL(cData.vBoxHalfSize[iB0],vTemp[0]);
+ vCenter[1] = cData.vBoxPos[1] - dMUL(cData.vBoxHalfSize[iB0],vTemp[1]);
+ vCenter[2] = cData.vBoxPos[2] - dMUL(cData.vBoxHalfSize[iB0],vTemp[2]);
+ }
+ else
+ {
+ dMat3GetCol(cData.mBoxRot,iB0,vTemp);
+ vCenter[0] = cData.vBoxPos[0] + dMUL(cData.vBoxHalfSize[iB0],vTemp[0]);
+ vCenter[1] = cData.vBoxPos[1] + dMUL(cData.vBoxHalfSize[iB0],vTemp[1]);
+ vCenter[2] = cData.vBoxPos[2] + dMUL(cData.vBoxHalfSize[iB0],vTemp[2]);
+ }
+
+ // find the vertices of box polygon
+ dVector3 avPoints[4];
+ dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS];
+ dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS];
+
+ int i=0;
+ for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++)
+ {
+ avTempArray1[i][0] = REAL(0.0);
+ avTempArray1[i][1] = REAL(0.0);
+ avTempArray1[i][2] = REAL(0.0);
+
+ avTempArray2[i][0] = REAL(0.0);
+ avTempArray2[i][1] = REAL(0.0);
+ avTempArray2[i][2] = REAL(0.0);
+ }
+
+ dVector3 vAxis1, vAxis2;
+
+ dMat3GetCol(cData.mBoxRot,iB1,vAxis1);
+ dMat3GetCol(cData.mBoxRot,iB2,vAxis2);
+
+ avPoints[0][0] = vCenter[0] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]);
+ avPoints[0][1] = vCenter[1] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]);
+ avPoints[0][2] = vCenter[2] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]);
+
+ avPoints[1][0] = vCenter[0] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]);
+ avPoints[1][1] = vCenter[1] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]);
+ avPoints[1][2] = vCenter[2] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) - dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]);
+
+ avPoints[2][0] = vCenter[0] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]);
+ avPoints[2][1] = vCenter[1] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]);
+ avPoints[2][2] = vCenter[2] - dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]);
+
+ avPoints[3][0] = vCenter[0] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[0]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[0]);
+ avPoints[3][1] = vCenter[1] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[1]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[1]);
+ avPoints[3][2] = vCenter[2] + dMUL(cData.vBoxHalfSize[iB1],vAxis1[2]) + dMUL(cData.vBoxHalfSize[iB2],vAxis2[2]);
+
+ // transform box points to space of cylinder circle
+ dMatrix3 mCylinderInv;
+ dMatrix3Inv(cData.mCylinderRot,mCylinderInv);
+
+ for(i=0; i<4; i++)
+ {
+ dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp);
+ dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]);
+ }
+
+ int iTmpCounter1 = 0;
+ int iTmpCounter2 = 0;
+ dVector4 plPlane;
+
+ // plane of cylinder that contains circle for intersection
+ dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane);
+ dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane);
+
+
+ // Body of base circle of Cylinder
+ int nCircleSegment = 0;
+ for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++)
+ {
+ dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane);
+
+ if (0 == (nCircleSegment % 2))
+ {
+ dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane);
+ }
+ else
+ {
+ dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane );
+ }
+
+ }
+
+ // back transform clipped points to absolute space
+ dReal ftmpdot;
+ dReal fTempDepth;
+ dVector3 vPoint;
+
+ if (nCircleSegment %2)
+ {
+ for( i=0; i<iTmpCounter2; i++)
+ {
+ dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]);
+ vPoint[0] += vCylinderCirclePos[0];
+ vPoint[1] += vCylinderCirclePos[1];
+ vPoint[2] += vCylinderCirclePos[2];
+
+ dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
+ ftmpdot = dVector3Dot(vTemp, cData.vNormal);
+ fTempDepth = cData.fBestrc - ftmpdot;
+ // Depth must be positive
+ if (fTempDepth > REAL(0.0))
+ {
+ // generate contacts
+ dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
+ Contact0->depth = fTempDepth;
+ dVector3Copy(cData.vNormal,Contact0->normal);
+ dVector3Copy(vPoint,Contact0->pos);
+ Contact0->g1 = cData.gCylinder;
+ Contact0->g2 = cData.gBox;
+ dVector3Inv(Contact0->normal);
+ cData.nContacts++;
+ }
+ }
+ }
+ else
+ {
+ for( i=0; i<iTmpCounter1; i++)
+ {
+ dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]);
+ vPoint[0] += vCylinderCirclePos[0];
+ vPoint[1] += vCylinderCirclePos[1];
+ vPoint[2] += vCylinderCirclePos[2];
+
+ dVector3Subtract(vPoint,cData.vCylinderPos,vTemp);
+ ftmpdot = dVector3Dot(vTemp, cData.vNormal);
+ fTempDepth = cData.fBestrc - ftmpdot;
+ // Depth must be positive
+ if (fTempDepth > REAL(0.0))
+ {
+ // generate contacts
+ dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip);
+ Contact0->depth = fTempDepth;
+ dVector3Copy(cData.vNormal,Contact0->normal);
+ dVector3Copy(vPoint,Contact0->pos);
+ Contact0->g1 = cData.gCylinder;
+ Contact0->g2 = cData.gBox;
+ dVector3Inv(Contact0->normal);
+ cData.nContacts++;
+ }
+ }
+ }
+}
+
+
+// Cylinder - Box by CroTeam
+// Ported by Nguyen Binh
+int dCollideCylinderBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+ sCylinderBoxData cData;
+
+ // Assign ODE stuff
+ cData.gCylinder = o1;
+ cData.gBox = o2;
+ cData.iFlags = flags;
+ cData.iSkip = skip;
+ cData.gContact = contact;
+
+ // initialize collider
+ _cldInitCylinderBox( cData );
+
+ // do intersection test and find best separating axis
+ if(!_cldTestSeparatingAxes( cData ) )
+ {
+ // if not found do nothing
+ return 0;
+ }
+
+ // if best separation axis is not found
+ if ( cData.iBestAxis == 0 )
+ {
+ // this should not happen (we should already exit in that case)
+
+ // do nothing
+ return 0;
+ }
+
+ dReal fdot = dVector3Dot(cData.vNormal,cData.vCylinderAxis);
+ // choose which clipping method are we going to apply
+ if (dFabs(fdot) < REAL(0.9) )
+ {
+ // clip cylinder over box
+ if(!_cldClipCylinderToBox(cData))
+ {
+ return 0;
+ }
+ }
+ else
+ {
+ _cldClipBoxToCylinder(cData);
+ }
+
+ return cData.nContacts;
+}
+