a280c031a27d0d179fc11bbf403780b193c950d9
[blender.git] / extern / bullet / Bullet / NarrowPhaseCollision / SubSimplexConvexCast.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 "SubSimplexConvexCast.h"
13 #include "CollisionShapes/ConvexShape.h"
14 #include "CollisionShapes/MinkowskiSumShape.h"
15 #include "NarrowPhaseCollision/SimplexSolverInterface.h"
16
17
18 SubsimplexConvexCast::SubsimplexConvexCast (ConvexShape* convexA,ConvexShape* convexB,SimplexSolverInterface* simplexSolver)
19 :m_simplexSolver(simplexSolver),
20 m_convexA(convexA),m_convexB(convexB)
21 {
22 }
23
24
25 #define MAX_ITERATIONS 1000
26
27 bool    SubsimplexConvexCast::calcTimeOfImpact(
28                 const SimdTransform& fromA,
29                 const SimdTransform& toA,
30                 const SimdTransform& fromB,
31                 const SimdTransform& toB,
32                 CastResult& result)
33 {
34
35                 MinkowskiSumShape combi(m_convexA,m_convexB);
36         MinkowskiSumShape* convex = &combi;
37
38         SimdTransform   rayFromLocalA;
39         SimdTransform   rayToLocalA;
40
41         rayFromLocalA = fromA.inverse()* fromB;
42         rayToLocalA = toA.inverse()* toB;
43
44
45         m_simplexSolver->reset();
46
47         convex->SetTransformB(SimdTransform(rayFromLocalA.getBasis()));
48
49         float radius = 0.01f;
50
51         SimdScalar lambda = 0.f;
52         SimdVector3 s = rayFromLocalA.getOrigin();
53         SimdVector3 r = rayToLocalA.getOrigin()-rayFromLocalA.getOrigin();
54         SimdVector3 x = s;
55         SimdVector3 v;
56         SimdVector3 arbitraryPoint = convex->LocalGetSupportingVertex(r);
57         
58         v = x - arbitraryPoint;
59
60         int maxIter = MAX_ITERATIONS;
61
62         SimdVector3 n;
63         n.setValue(0.f,0.f,0.f);
64         bool hasResult = false;
65         SimdVector3 c;
66
67         float lastLambda = lambda;
68
69
70         float dist2 = v.length2();
71         float epsilon = 0.0001f;
72
73         SimdVector3     w,p;
74         float VdotR;
75         
76         while ( (dist2 > epsilon) && maxIter--)
77         {
78                 p = convex->LocalGetSupportingVertex( v);
79                  w = x - p;
80
81                 float VdotW = v.dot(w);
82
83                 if ( VdotW > 0.f)
84                 {
85                         VdotR = v.dot(r);
86
87                         if (VdotR >= 0.f)
88                                 return false;
89                         else
90                         {
91                                 lambda = lambda - VdotW / VdotR;
92                                 x = s + lambda * r;
93                                 m_simplexSolver->reset();
94                                 //check next line
95                                 w = x-p;
96                                 lastLambda = lambda;
97                                 n = v;
98                                 hasResult = true;
99                         }
100                 } 
101                 m_simplexSolver->addVertex( w, x , p);
102                 if (m_simplexSolver->closest(v))
103                 {
104                         dist2 = v.length2();
105                         hasResult = true;
106                         //printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
107                         //printf("DIST2=%f\n",dist2);
108                         //printf("numverts = %i\n",m_simplexSolver->numVertices());
109                 } else
110                 {
111                         dist2 = 0.f;
112                 } 
113         }
114
115         int numiter = MAX_ITERATIONS - maxIter;
116 //      printf("number of iterations: %d", numiter);
117
118         //printf("lambd%f",lambda);
119         result.m_fraction = lambda;
120         result.m_normal = n;
121
122         return true;
123 }
124
125
126