Back to index

supertuxkart  0.5+dfsg1
SphereTriangleDetector.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "LinearMath/btScalar.h"
00017 #include "SphereTriangleDetector.h"
00018 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00019 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00020 
00021 
00022 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
00023 :m_sphere(sphere),
00024 m_triangle(triangle)
00025 {
00026 
00027 }
00028 
00029 void   SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
00030 {
00031 
00032        (void)debugDraw;
00033        const btTransform& transformA = input.m_transformA;
00034        const btTransform& transformB = input.m_transformB;
00035 
00036        btVector3 point,normal;
00037        btScalar timeOfImpact = btScalar(1.);
00038        btScalar depth = btScalar(0.);
00039 //     output.m_distance = btScalar(1e30);
00040        //move sphere into triangle space
00041        btTransform   sphereInTr = transformB.inverseTimes(transformA);
00042 
00043        if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
00044        {
00045               output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
00046        }
00047 
00048 }
00049 
00050 #define MAX_OVERLAP btScalar(0.)
00051 
00052 
00053 
00054 // See also geometrictools.com
00055 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
00056 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
00057        btVector3 diff = p - from;
00058        btVector3 v = to - from;
00059        btScalar t = v.dot(diff);
00060        
00061        if (t > 0) {
00062               btScalar dotVV = v.dot(v);
00063               if (t < dotVV) {
00064                      t /= dotVV;
00065                      diff -= t*v;
00066               } else {
00067                      t = 1;
00068                      diff -= v;
00069               }
00070        } else
00071               t = 0;
00072 
00073        nearest = from + t*v;
00074        return diff.dot(diff);      
00075 }
00076 
00077 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
00078        btVector3 lp(p);
00079        btVector3 lnormal(normal);
00080        
00081        return pointInTriangle(vertices, lnormal, &lp);
00082 }
00083 
00085 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact)
00086 {
00087 
00088        const btVector3* vertices = &m_triangle->getVertexPtr(0);
00089        const btVector3& c = sphereCenter;
00090        btScalar r = m_sphere->getRadius();
00091 
00092        btVector3 delta (0,0,0);
00093 
00094        btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
00095        normal.normalize();
00096        btVector3 p1ToCentre = c - vertices[0];
00097        btScalar distanceFromPlane = p1ToCentre.dot(normal);
00098 
00099        if (distanceFromPlane < btScalar(0.))
00100        {
00101               //triangle facing the other way
00102        
00103               distanceFromPlane *= btScalar(-1.);
00104               normal *= btScalar(-1.);
00105        }
00106 
00108        extern btScalar gContactBreakingThreshold;
00109 
00110        btScalar contactMargin = gContactBreakingThreshold;
00111        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
00112        bool isInsideShellPlane = distanceFromPlane < r;
00113        
00114        btScalar deltaDotNormal = delta.dot(normal);
00115        if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
00116               return false;
00117 
00118        // Check for contact / intersection
00119        bool hasContact = false;
00120        btVector3 contactPoint;
00121        if (isInsideContactPlane) {
00122               if (facecontains(c,vertices,normal)) {
00123                      // Inside the contact wedge - touches a point on the shell plane
00124                      hasContact = true;
00125                      contactPoint = c - normal*distanceFromPlane;
00126               } else {
00127                      // Could be inside one of the contact capsules
00128                      btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
00129                      btVector3 nearestOnEdge;
00130                      for (int i = 0; i < m_triangle->getNumEdges(); i++) {
00131                             
00132                             btPoint3 pa;
00133                             btPoint3 pb;
00134                             
00135                             m_triangle->getEdge(i,pa,pb);
00136 
00137                             btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
00138                             if (distanceSqr < contactCapsuleRadiusSqr) {
00139                                    // Yep, we're inside a capsule
00140                                    hasContact = true;
00141                                    contactPoint = nearestOnEdge;
00142                             }
00143                             
00144                      }
00145               }
00146        }
00147 
00148        if (hasContact) {
00149               btVector3 contactToCentre = c - contactPoint;
00150               btScalar distanceSqr = contactToCentre.length2();
00151               if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
00152                      btScalar distance = btSqrt(distanceSqr);
00153                      resultNormal = contactToCentre;
00154                      resultNormal.normalize();
00155                      point = contactPoint;
00156                      depth = -(r-distance);
00157                      return true;
00158               }
00159 
00160               if (delta.dot(contactToCentre) >= btScalar(0.0)) 
00161                      return false;
00162               
00163               // Moving towards the contact point -> collision
00164               point = contactPoint;
00165               timeOfImpact = btScalar(0.0);
00166               return true;
00167        }
00168        
00169        return false;
00170 }
00171 
00172 
00173 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
00174 {
00175        const btVector3* p1 = &vertices[0];
00176        const btVector3* p2 = &vertices[1];
00177        const btVector3* p3 = &vertices[2];
00178 
00179        btVector3 edge1( *p2 - *p1 );
00180        btVector3 edge2( *p3 - *p2 );
00181        btVector3 edge3( *p1 - *p3 );
00182 
00183        btVector3 p1_to_p( *p - *p1 );
00184        btVector3 p2_to_p( *p - *p2 );
00185        btVector3 p3_to_p( *p - *p3 );
00186 
00187        btVector3 edge1_normal( edge1.cross(normal));
00188        btVector3 edge2_normal( edge2.cross(normal));
00189        btVector3 edge3_normal( edge3.cross(normal));
00190        
00191        btScalar r1, r2, r3;
00192        r1 = edge1_normal.dot( p1_to_p );
00193        r2 = edge2_normal.dot( p2_to_p );
00194        r3 = edge3_normal.dot( p3_to_p );
00195        if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
00196             ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
00197               return true;
00198        return false;
00199 
00200 }