Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / Bullet / NarrowPhaseCollision / MinkowskiPenetrationDepthSolver.cpp
1 #include "MinkowskiPenetrationDepthSolver.h"
2 #include "CollisionShapes/MinkowskiSumShape.h"
3 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
4 #include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
5 #include "NarrowPhaseCollision/GjkPairDetector.h"
6
7
8 struct MyResult : public DiscreteCollisionDetectorInterface::Result
9 {
10
11         MyResult():m_hasResult(false)
12         {
13         }
14         
15         SimdVector3 m_normalOnBInWorld;
16         SimdVector3 m_pointInWorld;
17         float m_depth;
18         bool    m_hasResult;
19
20         void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
21         {
22                 m_normalOnBInWorld = normalOnBInWorld;
23                 m_pointInWorld = pointInWorld;
24                 m_depth = depth;
25                 m_hasResult = true;
26         }
27 };
28
29
30
31 bool MinkowskiPenetrationDepthSolver::CalcPenDepth(SimplexSolverInterface& simplexSolver,
32                                                                                                    ConvexShape* convexA,ConvexShape* convexB,
33                                                                                                    const SimdTransform& transA,const SimdTransform& transB,
34                                                                                                    SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
35 {
36
37
38         //just take fixed number of orientation, and sample the penetration depth in that direction
39
40         int N = 3;
41         float minProj = 1e30f;
42         SimdVector3 minNorm;
43         SimdVector3 minVertex;
44         SimdVector3 minA,minB;
45
46         //not so good, lots of directions overlap, better to use gauss map
47         for (int i=-N;i<N;i++)
48         {
49                 for (int j = -N;j<N;j++)
50                 {
51                         for (int k=-N;k<N;k++)
52                         {
53                                 if (i | j | k)
54                                 {
55                                         SimdVector3 norm(i,j,k);
56                                         norm.normalize();
57
58                                         {
59                                                 SimdVector3 seperatingAxisInA = (-norm)* transA.getBasis();
60                                                 SimdVector3 seperatingAxisInB = norm* transB.getBasis();
61
62                                                 SimdVector3 pInA = convexA->LocalGetSupportingVertex(seperatingAxisInA);
63                                                 SimdVector3 qInB = convexB->LocalGetSupportingVertex(seperatingAxisInB);
64                                                 SimdPoint3  pWorld = transA(pInA);      
65                                                 SimdPoint3  qWorld = transB(qInB);
66
67                                                 SimdVector3 w   = qWorld - pWorld;
68                                                 float delta = norm.dot(w);
69                                                 //find smallest delta
70
71                                                 if (delta < minProj)
72                                                 {
73                                                         minProj = delta;
74                                                         minNorm = norm;
75                                                         minA = pWorld;
76                                                         minB = qWorld;
77                                                 }
78                                         }
79
80                                         {
81                                                 SimdVector3 seperatingAxisInA = (norm)* transA.getBasis();
82                                                 SimdVector3 seperatingAxisInB = -norm* transB.getBasis();
83
84                                                 SimdVector3 pInA = convexA->LocalGetSupportingVertex(seperatingAxisInA);
85                                                 SimdVector3 qInB = convexB->LocalGetSupportingVertex(seperatingAxisInB);
86                                                 SimdPoint3  pWorld = transA(pInA);      
87                                                 SimdPoint3  qWorld = transB(qInB);
88
89                                                 SimdVector3 w   = qWorld - pWorld;
90                                                 float delta = (-norm).dot(w);
91                                                 //find smallest delta
92
93                                                 if (delta < minProj)
94                                                 {
95                                                         minProj = delta ;
96                                                         minNorm = -norm;
97                                                         minA = pWorld;
98                                                         minB = qWorld;
99                                                 }
100                                         }
101
102
103
104                                 }
105                         }
106                 }
107         }
108
109         SimdTransform ident;
110         ident.setIdentity();
111
112         GjkPairDetector gjkdet(convexA,convexB,&simplexSolver,0);
113
114
115         v = minNorm * minProj;
116
117
118         GjkPairDetector::ClosestPointInput input;
119                 
120         SimdVector3 newOrg = transA.getOrigin() + v + v;
121
122         SimdTransform displacedTrans = transA;
123         displacedTrans.setOrigin(newOrg);
124
125         input.m_transformA = displacedTrans;
126         input.m_transformB = transB;
127         input.m_maximumDistanceSquared = 1e30f;
128         
129         MyResult res;
130         gjkdet.GetClosestPoints(input,res);
131
132         if (res.m_hasResult)
133         {
134                 pa = res.m_pointInWorld - res.m_normalOnBInWorld*0.1f*res.m_depth;
135                 pb = res.m_pointInWorld;
136         }
137         return res.m_hasResult;
138 }