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