/*************************************************************************
* *
* 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-sphere collider by Alen Ladavac (Croteam)
* Ported to ODE by Nguyen Binh ( www.coderfarm.com )
*/
// NOTES :
// I only try to solve some obvious problems on cylinder-sphere
// If you like to solve all problem when very large sphere drop over
// very small cylinder or vice versa, you will need to re-organize the code.
// I would eventually, try to do it later, when I have more time.
// On this version, I only try to solve the problem when a very large cylinder
// drop over very small sphere.
#include <ode/collision.h>
#include <ode/matrix.h>
#include <ode/rotation.h>
#include <ode/odemath.h>
#include "collision_util.h"
#include <ode/objects.h>
// Axis of Cylinder - ODE use axis Z.
static const int nCYLINDER_AXIS = 2;
// Method to cure very deep penetration
// When two objects are in very deep penetration, we should exaggerate
// the depth value between them or numerical errors will make one object go
// through the other.
// 0 : do nothing - keep the calculated depth
// 1 : exaggerate calculated depth by multiple of fDepthRecoverRatio times.
// 2 : exaggerate calculated depth by multiple of log of ration between
// heavy object and light object. This method take a little more computing
// power but can be used to solved almost all "drop very large object over
// small object" problems.
#define _DEPTH_RECOVER_METHOD_ 0
#if (_DEPTH_RECOVER_METHOD_ == 1)
static const dReal fDepthRecoverRatio = REAL(2.0);
#endif
int dCollideCylinderSphere(dxGeom *gCylinder, dxGeom *gSphere, int flags, dContactGeom *contact, int skip)
{
// get source hull position and orientation
// Number of contacts
int nContacts = 0;
dQuaternion mQuatCylinder;
dGeomGetQuaternion(gCylinder,mQuatCylinder);
dVector3 vCylinderPos;
dMatrix3 mCylinderRot;
const dReal* pRotCyc = dGeomGetRotation(gCylinder);
dMatrix3Copy(pRotCyc,mCylinderRot);
const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(gCylinder);
dVector3Copy(*pPosCyc,vCylinderPos);
// get capsule radius and size
dReal fCylinderRadius;
dReal fCylinderSize;
dGeomCylinderGetParams(gCylinder,&fCylinderRadius,&fCylinderSize);
// get destination hull position and radius
dMatrix3 mSphereRot;
dVector3 vSpherePos;
dReal fSphereRadius = dGeomSphereGetRadius(gSphere);
const dReal* pSphereRot = dGeomGetRotation(gSphere);
dMatrix3Copy(pSphereRot,mSphereRot);
const dVector3* pSpherePos = (const dVector3*)dGeomGetPosition(gSphere);
dVector3Copy(*pSpherePos,vSpherePos);
// transform sphere position in cylinder frame
dVector3 vSpherePosInCylinderFrame;
// temporary variables
dVector3 vTemp;
dVector3 vTemp2;
// Sphere position relative to Cylinder
dVector3Subtract(vSpherePos,vCylinderPos,vTemp);
dQuaternion mInvQuatCylinder;
dQuatInv(mQuatCylinder,mInvQuatCylinder);
dQuatTransform(mInvQuatCylinder,vTemp,vSpherePosInCylinderFrame);
// cylinder boundaries along cylinder axis
dReal fHighCylinderBase = fCylinderSize*0.5f;
dReal fLowCylinderBase = -fCylinderSize*0.5f;
dReal fDeltaHigh = (vSpherePosInCylinderFrame[nCYLINDER_AXIS] - fHighCylinderBase );
dReal fDeltaLow = (fLowCylinderBase - vSpherePosInCylinderFrame[nCYLINDER_AXIS] );
// check if sphere intersecting with cylindrical part - side part
if( fDeltaHigh <= REAL(0.0) && fDeltaLow <= REAL(0.0))
{
// This mean the center of sphere lies between high and low base along cylinder axis
// of the cylinder
// calculate center of sphere on cylindrical axis which is referent for collision
// This circle of cylinder is in the same level with the center of sphere
dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)};
vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS];
// calculate distance between two spheres
dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp );
dReal fDistance = dVector3Length( vTemp );
if ( fDistance <= (fCylinderRadius + fSphereRadius))
{
dReal fTemp;
dReal fDepth;
dVector3 vPoint;
// Axis dependent - Should change when you don't use cylinder along z axis
if (dFabs(vSpherePosInCylinderFrame[0]) <= fCylinderRadius
&& dFabs(vSpherePosInCylinderFrame[1]) <= fCylinderRadius)
{
// Actually, not side penetrate but very deep top (or) bottom penetrate
// We have to use some trick to solve it.
// First try to find top or bottom penetrate
dVector3 vCylinderLinearVel = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
dVector3 vSphereLinearVel = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
dBodyID CylinderBody = dGeomGetBody(gCylinder);
dBodyID SphereBody = dGeomGetBody(gSphere);
// Get linear velocity
if (CylinderBody)
{
const dReal* pCylinderVel = dBodyGetLinearVel(CylinderBody);
vCylinderLinearVel[0] = pCylinderVel[0];
vCylinderLinearVel[1] = pCylinderVel[1];
vCylinderLinearVel[2] = pCylinderVel[2];
}
if (SphereBody)
{
const dReal* pSphereVel = dBodyGetLinearVel(SphereBody);
vSphereLinearVel[0] = pSphereVel[0];
vSphereLinearVel[1] = pSphereVel[1];
vSphereLinearVel[2] = pSphereVel[2];
}
dVector3 vSphereVelInCylinderFrame;
dVector3Subtract(vSphereLinearVel,vCylinderLinearVel,vSphereVelInCylinderFrame);
#if (_DEPTH_RECOVER_METHOD_ == 2)
dReal fRelativeVel = dVector3Length(vSphereVelInCylinderFrame);
#endif
dNormalize3(vSphereVelInCylinderFrame);
dVector3 vCylinderAxis;
dMat3GetCol(mCylinderRot,nCYLINDER_AXIS,vCylinderAxis);
dNormalize3(vCylinderAxis);
dReal fAngle = dVector3Dot(vSphereVelInCylinderFrame,vCylinderAxis);
// Solve problem when drop very large cylinder over very small sphere
if (fAngle < 0 )
{
// Top penetrate
// collision normal showing up from top cylinder plane
dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)};
vNormal[nCYLINDER_AXIS] = REAL(-1.0);
// Transform to cylinder space
dQuatTransform(mQuatCylinder,vNormal,vTemp2);
dNormalize3(vTemp2);
// set collision point in cylinder frame
dVector3Copy(vSpherePosInCylinderFrame,vPoint);
vPoint[nCYLINDER_AXIS] = fHighCylinderBase;
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision depth
dReal fDepth = fSphereRadius - fDeltaHigh;
// Experiment show that we need to exaggerate depth ratio to
// keep the small object from being stuck in other
#if (_DEPTH_RECOVER_METHOD_ == 1)
// Constant ratio
fDepth *= fDepthRecoverRatio;
#endif // (_DEPTH_RECOVER_METHOD_ == 1)
// use log of ratio between large object and small object masses
#if (_DEPTH_RECOVER_METHOD_ == 2)
if (CylinderBody && SphereBody)
{
// No static geom -> need to exaggerate
dMass sphereMass;
dBodyGetMass(SphereBody,&sphereMass);
dMass cylinderMass;
dBodyGetMass(CylinderBody,&cylinderMass);
dReal fRatio1 = cylinderMass.mass/sphereMass.mass;
if (fRatio1 > REAL(1.0))
{
fDepth *= fRelativeVel *dSqrt(fRatio1);
}
else
{
fDepth *= fRelativeVel * dSqrt( REAL(1.0) / fRatio1);
}
}
#endif // (_DEPTH_RECOVER_METHOD_ == 2)
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vTemp2,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
else
{
// Near Bottom
dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)};
vNormal[nCYLINDER_AXIS] = REAL(1.0);
// Transform to cylinder space
dQuatTransform(mQuatCylinder,vNormal,vTemp2);
dNormalize3(vTemp2);
// set collision point in cylinder frame
dVector3Copy(vSpherePosInCylinderFrame,vPoint);
vPoint[nCYLINDER_AXIS] = fLowCylinderBase;
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision depth
dReal fDepth = fSphereRadius - fDeltaLow;
// Experiment show that we need to exaggerate depth ratio to
// keep the small sphere from being stuck
#if (_DEPTH_RECOVER_METHOD_ == 1)
// Constant ratio
fDepth *= fDepthRecoverRatio;
#endif // (_DEPTH_RECOVER_METHOD_ == 1)
// use log of ratio between large object and small object masses
#if (_DEPTH_RECOVER_METHOD_ == 2)
if (CylinderBody && SphereBody)
{
// No static geom -> need to exaggerate
dMass sphereMass;
dBodyGetMass(SphereBody,&sphereMass);
dMass cylinderMass;
dBodyGetMass(CylinderBody,&cylinderMass);
dReal fRatio1 = cylinderMass.mass/sphereMass.mass;
if (fRatio1 > REAL(1.0))
{
fDepth *= fRelativeVel *dSqrt(fRatio1);
}
else
{
fDepth *= fRelativeVel * dSqrt( REAL(1.0) / fRatio1);
}
}
#endif // (_DEPTH_RECOVER_METHOD_ == 2)
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vTemp2,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
}
// calculate collision normal
dVector3 vNormal;
dQuatTransform(mQuatCylinder,vBodyPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vTemp2);
dVector3Subtract(vSpherePos,vTemp2,vNormal);
dNormalize3(vNormal);
// calculate collision point
fTemp = fCylinderRadius-fSphereRadius-fDistance;
vPoint[0] = vSpherePos[0] + vNormal[0]* fTemp *REAL(0.5);
vPoint[1] = vSpherePos[1] + vNormal[1]* fTemp *REAL(0.5);
vPoint[2] = vSpherePos[2] + vNormal[2]* fTemp *REAL(0.5);
// calculate penetration depth
fDepth = fCylinderRadius + fSphereRadius-fDistance;
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vNormal,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
dVector3Inv(Contact->normal);
}
return nContacts;
}
// check if sphere is intersecting with top or bottom circle of cylinder
}
else
{
// test sphere on top circle of cylinder
if ( fDeltaHigh > REAL(0.0))
{
// check if sphere is intersecting top plane
if( fDeltaHigh < fSphereRadius )
{
// calculate center of sphere on cylindrical axis which is referent for collision
dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)};
vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS];
// distance between sphere and cylinder axis
dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp);
dReal fDistance = dVector3Length(vTemp);
// see if our intersection point is inside top circle
if( fDistance < fCylinderRadius)
{
// collision normal showing up from top cylinder plane
dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)};
vNormal[nCYLINDER_AXIS] = REAL(-1.0);
// Transform to cylinder space
dQuatTransform(mQuatCylinder,vNormal,vTemp2);
dNormalize3(vTemp2);
// set collision point in cylinder frame
dVector3 vPoint;
dVector3Copy(vSpherePosInCylinderFrame,vPoint);
vPoint[nCYLINDER_AXIS] = fHighCylinderBase;
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision depth
dReal fDepth = fSphereRadius - fDeltaHigh;
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vTemp2,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
// if we got here then we are potentially intersecting the top ring
// of the cylinder
// define top circle center point on cylinder axis
dVector3 vE0 = {REAL(0.0),REAL(0.0),REAL(0.0)};
vE0[nCYLINDER_AXIS] = fHighCylinderBase;
// set direction vector from center to circle edge
dVector3 vDirVector;
dVector3Subtract(vSpherePosInCylinderFrame,vE0,vDirVector);
// project it onto top plane
vDirVector[nCYLINDER_AXIS] = REAL(0.0);
// and make it unit vector
dNormalize3(vDirVector);
// define point on the top circle edge
dVector3 vPoint;
vPoint[0] = vE0[0] + vDirVector[0] * fCylinderRadius;
vPoint[1] = vE0[1] + vDirVector[1] * fCylinderRadius;
vPoint[2] = vE0[2] + vDirVector[2] * fCylinderRadius;
// calculate distance from edge to sphere
dVector3Subtract(vPoint,vSpherePosInCylinderFrame,vTemp);
dReal fDistEdgeToSphere = dVector3Length(vTemp);
// if edge/sphere are intersecting
if (fDistEdgeToSphere < fSphereRadius )
{
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision normal
dVector3 vNormal;
dVector3Subtract(vPoint,vSpherePos,vNormal);
// and make it unit vector
dNormalize3(vNormal);
// calculate penetration depth
dReal fDepth = fSphereRadius - fDistEdgeToSphere;
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vNormal,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
}
// test sphere on bottom circle of cylinder
}
else
if (vSpherePosInCylinderFrame[nCYLINDER_AXIS] < fLowCylinderBase)
{
if( fDeltaLow < fSphereRadius )
{
// calculate center of sphere on cylindrical axis which is referent for collision
dVector3 vBodyPoint = {REAL(0.0),REAL(0.0),REAL(0.0)};
vBodyPoint[nCYLINDER_AXIS] = vSpherePosInCylinderFrame[nCYLINDER_AXIS];
// distance between sphere and cylinder axis
dVector3Subtract(vSpherePosInCylinderFrame,vBodyPoint,vTemp);
dReal fDistance = dVector3Length(vTemp);
// see if our intersection point is inside bottom circle
if( fDistance < fCylinderRadius )
{
// collision normal showing up from top cylinder plane
dVector3 vNormal = {REAL(0.0),REAL(0.0),REAL(0.0)};//(0,-1,0);
vNormal[nCYLINDER_AXIS] = REAL(1.0);
dQuatTransform(mQuatCylinder,vNormal,vTemp2);
dNormalize3(vTemp2);
// set collision point in cylinder frame
dVector3 vPoint;
dVector3Copy(vSpherePosInCylinderFrame,vPoint);
vPoint[nCYLINDER_AXIS] = fLowCylinderBase;
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision depth
dReal fDepth = fSphereRadius - fDeltaLow;
// generate contact
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vTemp2,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
// if we got here then we are potentially intersecting the bottom ring
// of the cylinder
// define top circle center point on cylinder axis
dVector3 vE0 = {REAL(0.0),REAL(0.0),REAL(0.0)};//(0,fLowCylinderBase,0);
vE0[nCYLINDER_AXIS] = fLowCylinderBase;
// set direction vector from center to circle edge
dVector3 vDirVector;
dVector3Subtract(vSpherePosInCylinderFrame,vE0,vDirVector);
// project it onto top plane
vDirVector[nCYLINDER_AXIS] = REAL(0.0);
// and make it unit vector
dNormalize3(vDirVector);
// define point on the top circle edge
dVector3 vPoint;
vPoint[0] = vE0[0] + vDirVector[0] * fCylinderRadius;
vPoint[1] = vE0[1] + vDirVector[1] * fCylinderRadius;
vPoint[2] = vE0[2] + vDirVector[2] * fCylinderRadius;
dVector3Subtract(vPoint,vSpherePosInCylinderFrame,vTemp);
dReal fDistEdgeToSphere = dVector3Length(vTemp);
// if edge/sphere are intersecting
if (fDistEdgeToSphere < fSphereRadius )
{
// transform in absolute space
dQuatTransform(mQuatCylinder,vPoint,vTemp);
dVector3Add(vTemp,vCylinderPos,vPoint);
// calculate collision normal
dVector3 vNormal;// = dVector3(vPoint - vSpherePos);
dVector3Subtract(vPoint,vSpherePos,vNormal);
// and make it unit vector
dNormalize3(vNormal);
// calculate penetration depth
dReal fDepth = fSphereRadius - fDistEdgeToSphere;
{
dContactGeom* Contact = SAFECONTACT(flags, contact, nContacts, skip );
Contact->depth = fDepth;
dVector3Copy(vNormal,Contact->normal);
dVector3Copy(vPoint,Contact->pos);
Contact->g1 = gCylinder;
Contact->g2 = gSphere;
nContacts++;
}
return nContacts;
}
}
}
}
return nContacts;
}
syntax highlighted by Code2HTML, v. 0.9.1