btRaycastCallback.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 <stdio.h>
00017 
00018 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00019 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00020 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00022 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00023 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00024 #include "btRaycastCallback.h"
00025 
00026 btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const btVector3& to, unsigned int flags)
00027         :
00028         m_from(from),
00029         m_to(to),
00030    //@BP Mod
00031    m_flags(flags),
00032         m_hitFraction(btScalar(1.))
00033 {
00034 
00035 }
00036 
00037 
00038 
00039 void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
00040 {
00041         const btVector3 &vert0=triangle[0];
00042         const btVector3 &vert1=triangle[1];
00043         const btVector3 &vert2=triangle[2];
00044 
00045         btVector3 v10; v10 = vert1 - vert0 ;
00046         btVector3 v20; v20 = vert2 - vert0 ;
00047 
00048         btVector3 triangleNormal; triangleNormal = v10.cross( v20 );
00049         
00050         const btScalar dist = vert0.dot(triangleNormal);
00051         btScalar dist_a = triangleNormal.dot(m_from) ;
00052         dist_a-= dist;
00053         btScalar dist_b = triangleNormal.dot(m_to);
00054         dist_b -= dist;
00055 
00056         if ( dist_a * dist_b >= btScalar(0.0) )
00057         {
00058                 return ; // same sign
00059         }
00060 
00061         if (((m_flags & kF_FilterBackfaces) != 0) && (dist_a <= btScalar(0.0)))
00062         {
00063                 // Backface, skip check
00064                 return;
00065         }
00066 
00067         
00068         const btScalar proj_length=dist_a-dist_b;
00069         const btScalar distance = (dist_a)/(proj_length);
00070         // Now we have the intersection point on the plane, we'll see if it's inside the triangle
00071         // Add an epsilon as a tolerance for the raycast,
00072         // in case the ray hits exacly on the edge of the triangle.
00073         // It must be scaled for the triangle size.
00074         
00075         if(distance < m_hitFraction)
00076         {
00077                 
00078 
00079                 btScalar edge_tolerance =triangleNormal.length2();              
00080                 edge_tolerance *= btScalar(-0.0001);
00081                 btVector3 point; point.setInterpolate3( m_from, m_to, distance);
00082                 {
00083                         btVector3 v0p; v0p = vert0 - point;
00084                         btVector3 v1p; v1p = vert1 - point;
00085                         btVector3 cp0; cp0 = v0p.cross( v1p );
00086 
00087                         if ( (btScalar)(cp0.dot(triangleNormal)) >=edge_tolerance) 
00088                         {
00089                                                 
00090 
00091                                 btVector3 v2p; v2p = vert2 -  point;
00092                                 btVector3 cp1;
00093                                 cp1 = v1p.cross( v2p);
00094                                 if ( (btScalar)(cp1.dot(triangleNormal)) >=edge_tolerance) 
00095                                 {
00096                                         btVector3 cp2;
00097                                         cp2 = v2p.cross(v0p);
00098                                         
00099                                         if ( (btScalar)(cp2.dot(triangleNormal)) >=edge_tolerance) 
00100                                         {
00101                                           //@BP Mod
00102                                           // Triangle normal isn't normalized
00103                                       triangleNormal.normalize();
00104 
00105                                          //@BP Mod - Allow for unflipped normal when raycasting against backfaces
00106                                                 if (((m_flags & kF_KeepUnflippedNormal) == 0) && (dist_a <= btScalar(0.0)))
00107                                                 {
00108                                                         m_hitFraction = reportHit(-triangleNormal,distance,partId,triangleIndex);
00109                                                 }
00110                                                 else
00111                                                 {
00112                                                         m_hitFraction = reportHit(triangleNormal,distance,partId,triangleIndex);
00113                                                 }
00114                                         }
00115                                 }
00116                         }
00117                 }
00118         }
00119 }
00120 
00121 
00122 btTriangleConvexcastCallback::btTriangleConvexcastCallback (const btConvexShape* convexShape, const btTransform& convexShapeFrom, const btTransform& convexShapeTo, const btTransform& triangleToWorld, const btScalar triangleCollisionMargin)
00123 {
00124         m_convexShape = convexShape;
00125         m_convexShapeFrom = convexShapeFrom;
00126         m_convexShapeTo = convexShapeTo;
00127         m_triangleToWorld = triangleToWorld;
00128         m_hitFraction = 1.0f;
00129         m_triangleCollisionMargin = triangleCollisionMargin;
00130         m_allowedPenetration = 0.f;
00131 }
00132 
00133 void
00134 btTriangleConvexcastCallback::processTriangle (btVector3* triangle, int partId, int triangleIndex)
00135 {
00136         btTriangleShape triangleShape (triangle[0], triangle[1], triangle[2]);
00137     triangleShape.setMargin(m_triangleCollisionMargin);
00138 
00139         btVoronoiSimplexSolver  simplexSolver;
00140         btGjkEpaPenetrationDepthSolver  gjkEpaPenetrationSolver;
00141 
00142 //#define  USE_SUBSIMPLEX_CONVEX_CAST 1
00143 //if you reenable USE_SUBSIMPLEX_CONVEX_CAST see commented out code below
00144 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
00145         btSubsimplexConvexCast convexCaster(m_convexShape, &triangleShape, &simplexSolver);
00146 #else
00147         //btGjkConvexCast       convexCaster(m_convexShape,&triangleShape,&simplexSolver);
00148         btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,&gjkEpaPenetrationSolver);
00149 #endif //#USE_SUBSIMPLEX_CONVEX_CAST
00150         
00151         btConvexCast::CastResult castResult;
00152         castResult.m_fraction = btScalar(1.);
00153         castResult.m_allowedPenetration = m_allowedPenetration;
00154         if (convexCaster.calcTimeOfImpact(m_convexShapeFrom,m_convexShapeTo,m_triangleToWorld, m_triangleToWorld, castResult))
00155         {
00156                 //add hit
00157                 if (castResult.m_normal.length2() > btScalar(0.0001))
00158                 {                                       
00159                         if (castResult.m_fraction < m_hitFraction)
00160                         {
00161 /* btContinuousConvexCast's normal is already in world space */
00162 /*
00163 #ifdef USE_SUBSIMPLEX_CONVEX_CAST
00164                                 //rotate normal into worldspace
00165                                 castResult.m_normal = m_convexShapeFrom.getBasis() * castResult.m_normal;
00166 #endif //USE_SUBSIMPLEX_CONVEX_CAST
00167 */
00168                                 castResult.m_normal.normalize();
00169 
00170                                 reportHit (castResult.m_normal,
00171                                                         castResult.m_hitPoint,
00172                                                         castResult.m_fraction,
00173                                                         partId,
00174                                                         triangleIndex);
00175                         }
00176                 }
00177         }
00178 }