98eb1067063eb5b1c6c6bea267d4bd32c51ab453
[blender.git] / extern / bullet / Bullet / NarrowPhaseCollision / RaycastCallback.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10 */
11
12 #include "RaycastCallback.h"
13
14 RaycastCallback::RaycastCallback(const SimdVector3& from,const SimdVector3& to)
15         :
16         m_from(from),
17         m_to(to),
18         m_hitFraction(1.f),
19         m_hitProxy(0),
20         m_hitFound(false)
21 {
22
23 }
24
25
26
27 void RaycastCallback::ProcessTriangle(SimdVector3* triangle)
28 {
29         const SimdVector3 &vert0=triangle[0];
30         const SimdVector3 &vert1=triangle[1];
31         const SimdVector3 &vert2=triangle[2];
32
33         SimdVector3 v10; v10 = vert1 - vert0 ;
34         SimdVector3 v20; v20 = vert2 - vert0 ;
35
36         SimdVector3 triangleNormal; triangleNormal = v10.cross( v20 );
37         
38         const float dist = vert0.dot(triangleNormal);
39         float dist_a = triangleNormal.dot(m_from) ;
40         dist_a-= dist;
41         float dist_b = triangleNormal.dot(m_to);
42         dist_b -= dist;
43
44         if ( dist_a * dist_b >= 0.0f)
45         {
46                 return ; // same sign
47         }
48         
49         const float proj_length=dist_a-dist_b;
50         const float distance = (dist_a)/(proj_length);
51         // Now we have the intersection point on the plane, we'll see if it's inside the triangle
52         // Add an epsilon as a tolerance for the raycast,
53         // in case the ray hits exacly on the edge of the triangle.
54         // It must be scaled for the triangle size.
55         
56         if(distance < m_hitFraction)
57         {
58                 
59
60                 float edge_tolerance =triangleNormal.length2();         
61                 edge_tolerance *= -0.0001f;
62                 SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
63                 {
64                         SimdVector3 v0p; v0p = vert0 - point;
65                         SimdVector3 v1p; v1p = vert1 - point;
66                         SimdVector3 cp0; cp0 = v0p.cross( v1p );
67
68                         if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) 
69                         {
70                                                 
71
72                                 SimdVector3 v2p; v2p = vert2 -  point;
73                                 SimdVector3 cp1;
74                                 cp1 = v1p.cross( v2p);
75                                 if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) 
76                                 {
77                                         SimdVector3 cp2;
78                                         cp2 = v2p.cross(v0p);
79                                         
80                                         if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
81                                         {
82                                                 m_hitFraction = distance;
83                                                 if ( dist_a > 0 )
84                                                 {
85                                                         m_hitNormalLocal = triangleNormal;
86                                                 }
87                                                 else
88                                                 {
89                                                         m_hitNormalLocal = -triangleNormal;
90                                                 }
91                                                 
92                                                 m_hitFound= true;
93                                         }
94                                 }
95                         }
96                 }
97         }
98 }