diff -r 000000000000 -r 2f259fa3e83a ode/src/collision_cylinder_sphere.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ode/src/collision_cylinder_sphere.cpp Tue Feb 02 01:00:49 2010 +0200 @@ -0,0 +1,251 @@ +/************************************************************************* +* * +* 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 Christoph Beyer (boernerb@web.de) * + * * + * In Cylinder/Sphere-collisions, there are three possibilies: * + * 1. collision with the cylinder's nappe * + * 2. collision with one of the cylinder's disc * + * 3. collision with one of the disc's border * + * * + * This collider computes two distances (s, t) and based on them, * + * it decides, which collision we have. * + * This collider always generates 1 (or 0, if we have no collison) * + * contacts. * + * It is able to "separate" cylinder and sphere in all * + * configurations, but it never pays attention to velocity. * + * So, in extrem situations, "tunneling-effect" is possible. * + * * + *******************************************************************/ + +#include +#include +#include +#include +#include +#include "collision_kernel.h" // for dxGeom + +int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere, + int /*flags*/, dContactGeom *contact, int /*skip*/) +{ + int GeomCount = 0; // count of used contacts + + const dReal toleranz = REAL(0.0001f); + + // get the data from the geoms + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + const dReal* pfRot1 = dGeomGetRotation(Cylinder); + + dReal radius2; + radius2 = dGeomSphereGetRadius(Sphere); + const dReal* SpherePos = dGeomGetPosition(Sphere); + + // G1Pos1 is the middle of the first disc + // G1Pos2 is the middle of the second disc + // vDir1 is the unit direction of the cylinderaxis + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = dMUL(length,REAL(0.5)); // just a precomputed factor + G1Pos2[0] = dMUL(vDir1[0],s) + cylpos[0]; + G1Pos2[1] = dMUL(vDir1[1],s) + cylpos[1]; + G1Pos2[2] = dMUL(vDir1[2],s) + cylpos[2]; + + G1Pos1[0] = dMUL(vDir1[0],-s) + cylpos[0]; + G1Pos1[1] = dMUL(vDir1[1],-s) + cylpos[1]; + G1Pos1[2] = dMUL(vDir1[2],-s) + cylpos[2]; + + dVector3 C; + dReal t; + // Step 1: compute the two distances 's' and 't' + // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle + s = dMUL((SpherePos[0] - G1Pos1[0]),vDir1[0]) - dMUL((G1Pos1[1] - SpherePos[1]),vDir1[1]) - dMUL((G1Pos1[2] - SpherePos[2]),vDir1[2]); + if(s < (-radius2) || s > (length + radius2) ) + { + // Sphere is too far away from the discs + // no collision + return 0; + } + + // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis + C[0] = dMUL(s,vDir1[0]) + G1Pos1[0] - SpherePos[0]; + C[1] = dMUL(s,vDir1[1]) + G1Pos1[1] - SpherePos[1]; + C[2] = dMUL(s,vDir1[2]) + G1Pos1[2] - SpherePos[2]; + // t is the distance from the Sphere-middle to the cylinder-axis! + t = dSqrt(dMUL(C[0],C[0]) + dMUL(C[1],C[1]) + dMUL(C[2],C[2]) ); + if(t > (radius + radius2) ) + { + // Sphere is too far away from the cylinder axis! + // no collision + return 0; + } + + // decide which kind of collision we have: + if(t > radius && (s < 0 || s > length) ) + { + // 3. collision + if(s <= 0) + { + contact->depth = radius2 - dSqrt( dMUL(s,s) + dMUL((t - radius),(t - radius)) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos1[0]; + contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos1[1]; + contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos1[2]; + contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth)); + contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth)); + contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth)); + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + else + { + // now s is bigger than length here! + contact->depth = radius2 - dSqrt( dMUL((s - length),(s - length)) + dMUL((t - radius),(t - radius)) ); + if(contact->depth < 0) + { + // no collision! + return 0; + } + contact->pos[0] = dMUL(dDIV(C[0],t),-radius) + G1Pos2[0]; + contact->pos[1] = dMUL(dDIV(C[1],t),-radius) + G1Pos2[1]; + contact->pos[2] = dMUL(dDIV(C[2],t),-radius) + G1Pos2[2]; + contact->normal[0] = dDIV((contact->pos[0] - SpherePos[0]),(radius2 - contact->depth)); + contact->normal[1] = dDIV((contact->pos[1] - SpherePos[1]),(radius2 - contact->depth)); + contact->normal[2] = dDIV((contact->pos[2] - SpherePos[2]),(radius2 - contact->depth)); + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + else if( (radius - t) <= s && (radius - t) <= (length - s) ) + { + // 1. collsision + if(t > (radius2 + toleranz)) + { + // cylinder-axis is outside the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + C[0] = dDIV(C[0],t); + C[1] = dDIV(C[1],t); + C[2] = dDIV(C[2],t); + contact->pos[0] = dMUL(C[0],radius2) + SpherePos[0]; + contact->pos[1] = dMUL(C[1],radius2) + SpherePos[1]; + contact->pos[2] = dMUL(C[2],radius2) + SpherePos[2]; + contact->normal[0] = C[0]; + contact->normal[1] = C[1]; + contact->normal[2] = C[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + else + { + // cylinder-axis is outside of the sphere + contact->depth = (radius2 + radius) - t; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + else + { + contact->pos[0] = C[0] + SpherePos[0]; + contact->pos[1] = C[1] + SpherePos[1]; + contact->pos[2] = C[2] + SpherePos[2]; + contact->normal[0] = dDIV(C[0],t); + contact->normal[1] = dDIV(C[1],t); + contact->normal[2] = dDIV(C[2],t); + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } + } + else + { + // 2. collision + if(s <= dMUL(length,REAL(0.5)) ) + { + // collsision with the first disc + contact->depth = s + radius2; + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = dMUL(radius2,vDir1[0]) + SpherePos[0]; + contact->pos[1] = dMUL(radius2,vDir1[1]) + SpherePos[1]; + contact->pos[2] = dMUL(radius2,vDir1[2]) + SpherePos[2]; + contact->normal[0] = vDir1[0]; + contact->normal[1] = vDir1[1]; + contact->normal[2] = vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + else + { + // collsision with the second disc + contact->depth = (radius2 + length - s); + if(contact->depth < 0) + { + // should never happen, but just for safeness + return 0; + } + contact->pos[0] = dMUL(radius2,-vDir1[0]) + SpherePos[0]; + contact->pos[1] = dMUL(radius2,-vDir1[1]) + SpherePos[1]; + contact->pos[2] = dMUL(radius2,-vDir1[2]) + SpherePos[2]; + contact->normal[0] = -vDir1[0]; + contact->normal[1] = -vDir1[1]; + contact->normal[2] = -vDir1[2]; + contact->g1 = Cylinder; + contact->g2 = Sphere; + GeomCount++; + return GeomCount; + } + } +}