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