added some get methods and stuff
[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         
30
31         const SimdVector3 &vert0=triangle[0];
32         const SimdVector3 &vert1=triangle[1];
33         const SimdVector3 &vert2=triangle[2];
34
35         SimdVector3 v10; v10 = vert1 - vert0 ;
36         SimdVector3 v20; v20 = vert2 - vert0 ;
37
38         SimdVector3 triangleNormal; triangleNormal = v10.cross( v20 );
39         
40         const float dist = vert0.dot(triangleNormal);
41         float dist_a = triangleNormal.dot(m_from) ;
42         dist_a-= dist;
43         float dist_b = triangleNormal.dot(m_to);
44         dist_b -= dist;
45
46         if ( dist_a * dist_b >= 0.0f)
47         {
48                 return ; // same sign
49         }
50         
51         const float proj_length=dist_a-dist_b;
52         const float distance = (dist_a)/(proj_length);
53         // Now we have the intersection point on the plane, we'll see if it's inside the triangle
54         // Add an epsilon as a tolerance for the raycast,
55         // in case the ray hits exacly on the edge of the triangle.
56         // It must be scaled for the triangle size.
57         
58         if(distance < m_hitFraction)
59         {
60                 
61
62                 float edge_tolerance =triangleNormal.length2();         
63                 edge_tolerance *= -0.0001f;
64                 SimdVector3 point; point.setInterpolate3( m_from, m_to, distance);
65                 {
66                         SimdVector3 v0p; v0p = vert0 - point;
67                         SimdVector3 v1p; v1p = vert1 - point;
68                         SimdVector3 cp0; cp0 = v0p.cross( v1p );
69
70                         if ( (float)(cp0.dot(triangleNormal)) >=edge_tolerance) 
71                         {
72                                                 
73
74                                 SimdVector3 v2p; v2p = vert2 -  point;
75                                 SimdVector3 cp1;
76                                 cp1 = v1p.cross( v2p);
77                                 if ( (float)(cp1.dot(triangleNormal)) >=edge_tolerance) 
78                                 {
79                                         SimdVector3 cp2;
80                                         cp2 = v2p.cross(v0p);
81                                         
82                                         if ( (float)(cp2.dot(triangleNormal)) >=edge_tolerance) 
83                                         {
84                                                 m_hitFraction = distance;
85                                                 if ( dist_a > 0 )
86                                                 {
87                                                         m_hitNormalLocal = triangleNormal;
88                                                 }
89                                                 else
90                                                 {
91                                                         m_hitNormalLocal = -triangleNormal;
92                                                 }
93                                                 
94                                                 m_hitFound= true;
95                                         }
96                                 }
97                         }
98                 }
99         }
100 }