fixed remaining sqrtf into btSqrt
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / SphereTriangleDetector.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "SphereTriangleDetector.h"
17 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
18 #include "BulletCollision/CollisionShapes/btSphereShape.h"
19
20
21 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
22 :m_sphere(sphere),
23 m_triangle(triangle)
24 {
25
26 }
27
28 void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
29 {
30
31         const btTransform& transformA = input.m_transformA;
32         const btTransform& transformB = input.m_transformB;
33
34         btVector3 point,normal;
35         btScalar timeOfImpact = 1.f;
36         btScalar depth = 0.f;
37 //      output.m_distance = 1e30f;
38         //move sphere into triangle space
39         btTransform     sphereInTr = transformB.inverseTimes(transformA);
40
41         if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
42         {
43                 output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
44         }
45
46 }
47
48 #define MAX_OVERLAP 0.f
49
50
51
52 // See also geometrictools.com
53 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
54 float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
55         btVector3 diff = p - from;
56         btVector3 v = to - from;
57         float t = v.dot(diff);
58         
59         if (t > 0) {
60                 float dotVV = v.dot(v);
61                 if (t < dotVV) {
62                         t /= dotVV;
63                         diff -= t*v;
64                 } else {
65                         t = 1;
66                         diff -= v;
67                 }
68         } else
69                 t = 0;
70
71         nearest = from + t*v;
72         return diff.dot(diff);  
73 }
74
75 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
76         btVector3 lp(p);
77         btVector3 lnormal(normal);
78         
79         return pointInTriangle(vertices, lnormal, &lp);
80 }
81
82 ///combined discrete/continuous sphere-triangle
83 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact)
84 {
85
86         const btVector3* vertices = &m_triangle->getVertexPtr(0);
87         const btVector3& c = sphereCenter;
88         btScalar r = m_sphere->getRadius();
89
90         btVector3 delta (0,0,0);
91
92         btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
93         normal.normalize();
94         btVector3 p1ToCentre = c - vertices[0];
95         float distanceFromPlane = p1ToCentre.dot(normal);
96
97         if (distanceFromPlane < 0.f)
98         {
99                 //triangle facing the other way
100         
101                 distanceFromPlane *= -1.f;
102                 normal *= -1.f;
103         }
104
105         ///todo: move this gContactBreakingThreshold into a proper structure
106         extern float gContactBreakingThreshold;
107
108         float contactMargin = gContactBreakingThreshold;
109         bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
110         bool isInsideShellPlane = distanceFromPlane < r;
111         
112         float deltaDotNormal = delta.dot(normal);
113         if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
114                 return false;
115
116         // Check for contact / intersection
117         bool hasContact = false;
118         btVector3 contactPoint;
119         if (isInsideContactPlane) {
120                 if (facecontains(c,vertices,normal)) {
121                         // Inside the contact wedge - touches a point on the shell plane
122                         hasContact = true;
123                         contactPoint = c - normal*distanceFromPlane;
124                 } else {
125                         // Could be inside one of the contact capsules
126                         float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
127                         btVector3 nearestOnEdge;
128                         for (int i = 0; i < m_triangle->getNumEdges(); i++) {
129                                 
130                                 btPoint3 pa;
131                                 btPoint3 pb;
132                                 
133                                 m_triangle->getEdge(i,pa,pb);
134
135                                 float distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
136                                 if (distanceSqr < contactCapsuleRadiusSqr) {
137                                         // Yep, we're inside a capsule
138                                         hasContact = true;
139                                         contactPoint = nearestOnEdge;
140                                 }
141                                 
142                         }
143                 }
144         }
145
146         if (hasContact) {
147                 btVector3 contactToCentre = c - contactPoint;
148                 float distanceSqr = contactToCentre.length2();
149                 if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
150                         float distance = btSqrt(distanceSqr);
151                         if (1)
152                         {
153                                 resultNormal = contactToCentre;
154                                 resultNormal.normalize();
155                         }
156                         point = contactPoint;
157                         depth = -(r-distance);
158                         return true;
159                 }
160
161                 if (delta.dot(contactToCentre) >= 0.0f) 
162                         return false;
163                 
164                 // Moving towards the contact point -> collision
165                 point = contactPoint;
166                 timeOfImpact = 0.0f;
167                 return true;
168         }
169         
170         return false;
171 }
172
173
174 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
175 {
176         const btVector3* p1 = &vertices[0];
177         const btVector3* p2 = &vertices[1];
178         const btVector3* p3 = &vertices[2];
179
180         btVector3 edge1( *p2 - *p1 );
181         btVector3 edge2( *p3 - *p2 );
182         btVector3 edge3( *p1 - *p3 );
183
184         btVector3 p1_to_p( *p - *p1 );
185         btVector3 p2_to_p( *p - *p2 );
186         btVector3 p3_to_p( *p - *p3 );
187
188         btVector3 edge1_normal( edge1.cross(normal));
189         btVector3 edge2_normal( edge2.cross(normal));
190         btVector3 edge3_normal( edge3.cross(normal));
191         
192         float r1, r2, r3;
193         r1 = edge1_normal.dot( p1_to_p );
194         r2 = edge2_normal.dot( p2_to_p );
195         r3 = edge3_normal.dot( p3_to_p );
196         if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
197              ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
198                 return true;
199         return false;
200
201 }