7be5ff4d1455309e14b7b20296ebcfce8bc5733f
[blender-staging.git] / extern / bullet / BulletDynamics / CollisionDispatch / ConvexConvexAlgorithm.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 #include "ConvexConvexAlgorithm.h"
12
13 #include <stdio.h>
14 #include "NarrowPhaseCollision/DiscreteCollisionDetectorInterface.h"
15 #include "BroadphaseCollision/BroadphaseInterface.h"
16 #include "Dynamics/RigidBody.h"
17 #include "CollisionShapes/ConvexShape.h"
18 #include "NarrowPhaseCollision/GjkPairDetector.h"
19 #include "BroadphaseCollision/BroadphaseProxy.h"
20 #include "BroadphaseCollision/CollisionDispatcher.h"
21 #include "CollisionShapes/BoxShape.h"
22 #include "CollisionDispatch/ManifoldResult.h"
23
24 #include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
25 #include "NarrowPhaseCollision/ContinuousConvexCollision.h"
26 #include "NarrowPhaseCollision/SubSimplexConvexCast.h"
27 #include "NarrowPhaseCollision/GjkConvexCast.h"
28
29
30
31 #include "CollisionShapes/MinkowskiSumShape.h"
32 #include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
33 #include "CollisionShapes/SphereShape.h"
34
35 #include "NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.h"
36
37
38 #ifdef WIN32
39 void DrawRasterizerLine(const float* from,const float* to,int color);
40 #endif
41
42
43
44
45 //#define PROCESS_SINGLE_CONTACT
46 #ifdef WIN32
47 bool gForceBoxBox = false;//false;//true;
48
49 #else
50 bool gForceBoxBox = false;//false;//true;
51 #endif
52 bool gBoxBoxUseGjk = true;//true;//false;
53 bool gDisableConvexCollision = false;
54
55 bool gUseEpa = false;
56
57
58 ConvexConvexAlgorithm::ConvexConvexAlgorithm(PersistentManifold* mf,const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
59 : CollisionAlgorithm(ci),
60 m_gjkPairDetector(0,0,&m_simplexSolver,0),
61 m_box0(*proxy0),
62 m_box1(*proxy1),
63 m_collisionImpulse(0.f),
64 m_ownManifold (false),
65 m_manifoldPtr(mf),
66 m_lowLevelOfDetail(false),
67 m_useEpa(gUseEpa)
68 {
69         CheckPenetrationDepthSolver();
70
71         RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
72         RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
73
74         if ((body0->getInvMass() != 0.f) || 
75                 (body1->getInvMass() != 0.f))
76         {
77                 if (!m_manifoldPtr)
78                 {
79                         m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
80                         m_ownManifold = true;
81                 }
82         }
83
84 }
85
86
87
88 ConvexConvexAlgorithm::~ConvexConvexAlgorithm()
89 {
90         if (m_ownManifold)
91         {
92                 if (m_manifoldPtr)
93                         m_dispatcher->ReleaseManifold(m_manifoldPtr);
94         }
95 }
96
97 void    ConvexConvexAlgorithm ::SetLowLevelOfDetail(bool useLowLevel)
98 {
99         m_lowLevelOfDetail = useLowLevel;
100 }
101
102 float   ConvexConvexAlgorithm::GetCollisionImpulse() const
103 {
104         if (m_manifoldPtr)
105                 return m_manifoldPtr->GetCollisionImpulse();
106         
107         return 0.f;
108 }
109
110
111 class FlippedContactResult : public DiscreteCollisionDetectorInterface::Result
112 {
113         DiscreteCollisionDetectorInterface::Result* m_org;
114
115 public:
116
117         FlippedContactResult(DiscreteCollisionDetectorInterface::Result* org)
118                 : m_org(org)
119         {
120
121         }
122
123         virtual void AddContactPoint(const SimdVector3& normalOnBInWorld,const SimdVector3& pointInWorld,float depth)
124         {
125                 SimdVector3 flippedNormal = -normalOnBInWorld;
126
127                 m_org->AddContactPoint(flippedNormal,pointInWorld,depth);
128         }
129
130 };
131
132 void    ConvexConvexAlgorithm::CheckPenetrationDepthSolver()
133 {
134 //      if (m_useEpa != gUseEpa)
135         {
136                 m_useEpa  = gUseEpa;
137                 if (m_useEpa)
138                 {
139                         //not distributed
140                         //m_gjkPairDetector.SetPenetrationDepthSolver(new Solid3EpaPenetrationDepth);
141                 } else
142                 {
143                         m_gjkPairDetector.SetPenetrationDepthSolver(new MinkowskiPenetrationDepthSolver);
144                 }
145         }
146         
147 }
148 bool extra = false;
149
150 float gFriction = 0.5f;
151 //
152 // box-box collision algorithm, for simplicity also applies resolution-impulse
153 //
154 void ConvexConvexAlgorithm ::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount, bool useContinuous)
155 {
156         CheckPenetrationDepthSolver();
157
158 //      printf("ConvexConvexAlgorithm::ProcessCollision\n");
159 m_collisionImpulse = 0.f;
160         
161         RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
162         RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
163
164         //todo: move this in the dispatcher
165         if ((body0->GetActivationState() == 2) &&(body1->GetActivationState() == 2))
166                 return;
167
168
169         if (!m_manifoldPtr)
170                 return;
171
172         if ((body0->getInvMass() == 0.f) && 
173                 (body1->getInvMass() == 0.f))
174         {
175                 return;
176         }
177
178         ManifoldResult output(body0,body1,m_manifoldPtr);
179         
180         ConvexShape* min0 = static_cast<ConvexShape*>(body0->GetCollisionShape());
181         ConvexShape* min1 = static_cast<ConvexShape*>(body1->GetCollisionShape());      
182         GjkPairDetector::ClosestPointInput input;
183
184         SphereShape     sphere(0.2f);
185         MinkowskiSumShape       expanded0(min0,&sphere);
186         MinkowskiSumShape       expanded1(min1,&sphere);
187
188         if (useContinuous)
189         {
190                 m_gjkPairDetector.SetMinkowskiA(&expanded0);
191                 m_gjkPairDetector.SetMinkowskiB(&expanded1);
192                 input.m_maximumDistanceSquared = expanded0.GetMargin()+expanded1.GetMargin();
193                 input.m_maximumDistanceSquared *= input.m_maximumDistanceSquared;
194         }
195         else
196         {
197                 m_gjkPairDetector.SetMinkowskiA(min0);
198                 m_gjkPairDetector.SetMinkowskiB(min1);
199                 input.m_maximumDistanceSquared = min0->GetMargin() + min1->GetMargin() + m_manifoldPtr->GetManifoldMargin();
200                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
201         }
202
203         input.m_maximumDistanceSquared = 1e30;//
204         
205         input.m_transformA = body0->getCenterOfMassTransform();
206         input.m_transformB = body1->getCenterOfMassTransform();
207     
208         m_gjkPairDetector.GetClosestPoints(input,output);
209
210 }
211 bool disableCcd = false;
212 float   ConvexConvexAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1,float timeStep,int stepCount)
213 {
214
215         CheckPenetrationDepthSolver();
216
217         m_collisionImpulse = 0.f;
218         
219         RigidBody* body0 = (RigidBody*)m_box0.m_clientObject;
220         RigidBody* body1 = (RigidBody*)m_box1.m_clientObject;
221
222         if (!m_manifoldPtr)
223                 return 1.f;
224
225         if ((body0->getInvMass() == 0.f) && 
226                 (body1->getInvMass() == 0.f))
227         {
228                 return 1.f;
229         }
230
231
232         ConvexShape* min0 = static_cast<ConvexShape*>(body0->GetCollisionShape());
233         ConvexShape* min1 = static_cast<ConvexShape*>(body1->GetCollisionShape());      
234         
235         GjkPairDetector::ClosestPointInput input;
236         input.m_transformA = body0->getCenterOfMassTransform();
237         input.m_transformB = body1->getCenterOfMassTransform();
238         SimdTransform predictA,predictB;
239
240         body0->predictIntegratedTransform(timeStep,predictA);
241         body1->predictIntegratedTransform(timeStep,predictB);
242
243
244         ConvexCast::CastResult result;
245
246
247         VoronoiSimplexSolver voronoiSimplex;
248         //SubsimplexConvexCast ccd(&voronoiSimplex);
249         //GjkConvexCast ccd(&voronoiSimplex);
250         
251         ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,m_penetrationDepthSolver);
252
253         if (disableCcd)
254                 return 1.f;
255
256         if (ccd.calcTimeOfImpact(input.m_transformA,predictA,input.m_transformB,predictB,result))
257         {
258         
259                 //store result.m_fraction in both bodies
260                 int i;
261                 i=0;
262                 
263 //              if (result.m_fraction< 0.1f)
264 //                      result.m_fraction = 0.1f;
265
266                 if (body0->m_hitFraction > result.m_fraction)
267                         body0->m_hitFraction  = result.m_fraction;
268
269                 if (body1->m_hitFraction > result.m_fraction)
270                         body1->m_hitFraction  = result.m_fraction;
271
272                 return result.m_fraction;
273         }
274
275         return 1.f;
276
277
278 }