Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / Bullet / BroadphaseCollision / SimpleBroadphase.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
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 "SimpleBroadphase.h"
12 #include "BroadphaseCollision/CollisionDispatcher.h"
13 #include "BroadphaseCollision/CollisionAlgorithm.h"
14
15 #include "SimdVector3.h"
16 #include "SimdTransform.h"
17 #include "SimdMatrix3x3.h"
18 #include <vector>
19 #include "CollisionShapes/CollisionMargin.h"
20
21 SimpleBroadphase::SimpleBroadphase()
22 :  m_firstFreeProxy(0),
23   m_numProxies(0),
24  m_blockedForChanges(false),
25  m_NumOverlapBroadphasePair(0)
26 {
27         int i;
28         for (i=0;i<SIMPLE_MAX_PROXIES;i++)
29         {
30                 m_freeProxies[i] = i;
31         }
32 }
33
34 SimpleBroadphase::~SimpleBroadphase()
35 {
36         /*int i;
37         for (i=m_numProxies-1;i>=0;i--)
38         {
39                 BP_Proxy* proxy = m_pProxies[i]; 
40                 destroyProxy(proxy);
41         }
42         */
43 }
44
45
46 BroadphaseProxy*        SimpleBroadphase::CreateProxy( void *object, int type, const SimdVector3& min,  const SimdVector3& max) 
47 {
48         if (m_numProxies >= SIMPLE_MAX_PROXIES)
49         {
50                 assert(0);
51                 return 0; //should never happen, but don't let the game crash ;-)
52         }
53         assert(min[0]<= max[0] && min[1]<= max[1] && min[2]<= max[2]);
54
55         int freeIndex= m_freeProxies[m_firstFreeProxy];
56         BroadphaseProxy* proxy = new (&m_proxies[freeIndex])SimpleBroadphaseProxy(object,type,min,max);
57         m_firstFreeProxy++;
58
59         m_pProxies[m_numProxies] = proxy;
60         m_numProxies++;
61
62         return proxy;
63 }
64 void    SimpleBroadphase::DestroyProxy(BroadphaseProxy* proxy)
65 {
66                 
67                 int i;
68                 
69                 BroadphaseProxy* proxy1 = &m_proxies[0];
70         
71                 int index = proxy - proxy1;
72                 m_freeProxies[--m_firstFreeProxy] = index;
73
74                 for ( i=m_NumOverlapBroadphasePair-1;i>=0;i--)
75                 {
76                         BroadphasePair& pair = m_OverlappingPairs[i];
77                         if (pair.m_pProxy0 == proxy ||
78                                         pair.m_pProxy1 == proxy)
79                         {
80                                 RemoveOverlappingPair(pair);
81                         }
82                 }
83                 
84                 for (i=0;i<m_numProxies;i++)
85                 {
86                         if (m_pProxies[i] == proxy)
87                         {
88                                 m_proxies[i] = m_proxies[m_numProxies-1];
89                                 break;
90                         }
91                 }
92                 m_numProxies--;
93                 
94 }
95
96 SimpleBroadphaseProxy*  SimpleBroadphase::GetSimpleProxyFromProxy(BroadphaseProxy* proxy)
97 {
98         SimpleBroadphaseProxy* proxy0 = static_cast<SimpleBroadphaseProxy*>(proxy);
99
100         int index = proxy0 - &m_proxies[0];
101         //assert(index < m_numProxies);
102
103         SimpleBroadphaseProxy* sbp = &m_proxies[index];
104         return sbp;
105 }
106 void    SimpleBroadphase::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const SimdVector3& aabbMax)
107 {
108         SimpleBroadphaseProxy* sbp = GetSimpleProxyFromProxy(proxy);
109         sbp->m_min = aabbMin;
110         sbp->m_max = aabbMax;
111 }
112
113 void    SimpleBroadphase::CleanOverlappingPair(BroadphasePair& pair)
114 {
115         for (int dispatcherId=0;dispatcherId<SIMPLE_MAX_ALGORITHMS;dispatcherId++)
116         {
117                 if (pair.m_algorithms[dispatcherId])
118                 {
119                         {
120                                 delete pair.m_algorithms[dispatcherId];
121                                 pair.m_algorithms[dispatcherId]=0;
122                         }
123                 }
124         }
125 }
126
127
128 void    SimpleBroadphase::CleanProxyFromPairs(BroadphaseProxy* proxy)
129 {
130         for (int i=0;i<m_NumOverlapBroadphasePair;i++)
131         {
132                 BroadphasePair& pair = m_OverlappingPairs[i];
133                 if (pair.m_pProxy0 == proxy ||
134                                 pair.m_pProxy1 == proxy)
135                 {
136                         CleanOverlappingPair(pair);
137                 }
138         }
139 }
140
141 void    SimpleBroadphase::AddOverlappingPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
142 {
143         
144
145         BroadphasePair pair(*proxy0,*proxy1);
146         m_OverlappingPairs[m_NumOverlapBroadphasePair] = pair;
147
148         int i;
149         for (i=0;i<SIMPLE_MAX_ALGORITHMS;i++)
150         {
151                 assert(!m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i]);
152                 m_OverlappingPairs[m_NumOverlapBroadphasePair].m_algorithms[i] = 0;
153         }
154         m_NumOverlapBroadphasePair++;
155 }
156         
157 bool    SimpleBroadphase::FindPair(BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
158 {
159         bool found = false;
160         int i;
161         for (i=m_NumOverlapBroadphasePair-1;i>=0;i--)
162         {
163                 BroadphasePair& pair = m_OverlappingPairs[i];
164                 if (((pair.m_pProxy0 == proxy0) && (pair.m_pProxy1 == proxy1)) ||
165                         ((pair.m_pProxy0 == proxy1) && (pair.m_pProxy1 == proxy0)))
166                 {
167                         found = true;
168                         break;
169                 }
170         }       
171
172         return found;
173 }
174 void    SimpleBroadphase::RemoveOverlappingPair(BroadphasePair& pair)
175 {
176     CleanOverlappingPair(pair);
177         int     index = &pair - &m_OverlappingPairs[0];
178         //remove efficiently, swap with the last
179         m_OverlappingPairs[index] = m_OverlappingPairs[m_NumOverlapBroadphasePair-1];
180         m_NumOverlapBroadphasePair--;
181 }
182
183 bool    SimpleBroadphase::AabbOverlap(SimpleBroadphaseProxy* proxy0,SimpleBroadphaseProxy* proxy1)
184 {
185         return proxy0->m_min[0] <= proxy1->m_max[0] && proxy1->m_min[0] <= proxy0->m_max[0] && 
186                    proxy0->m_min[1] <= proxy1->m_max[1] && proxy1->m_min[1] <= proxy0->m_max[1] &&
187                    proxy0->m_min[2] <= proxy1->m_max[2] && proxy1->m_min[2] <= proxy0->m_max[2];
188
189 }
190 void    SimpleBroadphase::RefreshOverlappingPairs()
191 {
192         //first check for new overlapping pairs
193         int i,j;
194
195         for (i=0;i<m_numProxies;i++)
196         {
197                 BroadphaseProxy* proxy0 = m_pProxies[i];
198                 for (j=i+1;j<m_numProxies;j++)
199                 {
200                         BroadphaseProxy* proxy1 = m_pProxies[j];
201                         SimpleBroadphaseProxy* p0 = GetSimpleProxyFromProxy(proxy0);
202                         SimpleBroadphaseProxy* p1 = GetSimpleProxyFromProxy(proxy1);
203
204                         if (AabbOverlap(p0,p1))
205                         {
206                                 if ( !FindPair(proxy0,proxy1))
207                                 {
208                                         AddOverlappingPair(proxy0,proxy1);
209                                 }
210                         }
211
212                 }
213         }
214
215         //then remove non-overlapping ones
216         for (i=0;i<m_NumOverlapBroadphasePair;i++)
217         {
218                 BroadphasePair& pair = m_OverlappingPairs[i];
219                 SimpleBroadphaseProxy* proxy0 = GetSimpleProxyFromProxy(pair.m_pProxy0);
220                 SimpleBroadphaseProxy* proxy1 = GetSimpleProxyFromProxy(pair.m_pProxy1);
221                 if (!AabbOverlap(proxy0,proxy1))
222                 {
223             RemoveOverlappingPair(pair);
224                 }
225         }
226
227         //BroadphasePair        m_OverlappingPairs[SIMPLE_MAX_OVERLAP];
228         //int   m_NumOverlapBroadphasePair;
229
230 }
231
232 void    SimpleBroadphase::DispatchAllCollisionPairs(Dispatcher& dispatcher,DispatcherInfo& dispatchInfo)
233 {
234         m_blockedForChanges = true;
235
236         int i;
237         
238         int dispatcherId = dispatcher.GetUniqueId();
239
240         RefreshOverlappingPairs();
241
242         for (i=0;i<m_NumOverlapBroadphasePair;i++)
243         {
244                 
245                 BroadphasePair& pair = m_OverlappingPairs[i];
246                 
247                 if (dispatcherId>= 0)
248                 {
249                         //dispatcher will keep algorithms persistent in the collision pair
250                         if (!pair.m_algorithms[dispatcherId])
251                         {
252                                 pair.m_algorithms[dispatcherId] = dispatcher.FindAlgorithm(
253                                         *pair.m_pProxy0,
254                                         *pair.m_pProxy1);
255                         }
256
257                         if (pair.m_algorithms[dispatcherId])
258                         {
259                                 if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
260                                 {
261                                         pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount,dispatchInfo.m_useContinuous);
262                                 } else
263                                 {
264                                         float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount);
265                                         if (dispatchInfo.m_timeOfImpact > toi)
266                                                 dispatchInfo.m_timeOfImpact = toi;
267
268                                 }
269                         }
270                 } else
271                 {
272                         //non-persistent algorithm dispatcher
273                                 CollisionAlgorithm* algo = dispatcher.FindAlgorithm(
274                                         *pair.m_pProxy0,
275                                         *pair.m_pProxy1);
276
277                                 if (algo)
278                                 {
279                                         if (dispatchInfo.m_dispatchFunc ==              DispatcherInfo::DISPATCH_DISCRETE)
280                                         {
281                                                 algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount,dispatchInfo.m_useContinuous);
282                                         } else
283                                         {
284                                                 float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo.m_timeStep,dispatchInfo.m_stepCount);
285                                                 if (dispatchInfo.m_timeOfImpact > toi)
286                                                         dispatchInfo.m_timeOfImpact = toi;
287                                         }
288                                 }
289                 }
290
291         }
292
293         m_blockedForChanges = false;
294
295 }
296
297