bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / NarrowPhaseCollision / btPersistentManifold.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btPersistentManifold.h"
18 #include "LinearMath/btTransform.h"
19
20
21 btScalar                                        gContactBreakingThreshold = btScalar(0.02);
22 ContactDestroyedCallback        gContactDestroyedCallback = 0;
23 ContactProcessedCallback        gContactProcessedCallback = 0;
24 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
25 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
26 bool                                            gContactCalcArea3Points = true;
27
28
29 btPersistentManifold::btPersistentManifold()
30 :btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
31 m_body0(0),
32 m_body1(0),
33 m_cachedPoints (0),
34 m_index1a(0)
35 {
36 }
37
38
39
40
41 #ifdef DEBUG_PERSISTENCY
42 #include <stdio.h>
43 void    btPersistentManifold::DebugPersistency()
44 {
45         int i;
46         printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
47         for (i=0;i<m_cachedPoints;i++)
48         {
49                 printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
50         }
51 }
52 #endif //DEBUG_PERSISTENCY
53
54 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
55 {
56
57         void* oldPtr = pt.m_userPersistentData;
58         if (oldPtr)
59         {
60 #ifdef DEBUG_PERSISTENCY
61                 int i;
62                 int occurance = 0;
63                 for (i=0;i<m_cachedPoints;i++)
64                 {
65                         if (m_pointCache[i].m_userPersistentData == oldPtr)
66                         {
67                                 occurance++;
68                                 if (occurance>1)
69                                         printf("error in clearUserCache\n");
70                         }
71                 }
72                 btAssert(occurance<=0);
73 #endif //DEBUG_PERSISTENCY
74
75                 if (pt.m_userPersistentData && gContactDestroyedCallback)
76                 {
77                         (*gContactDestroyedCallback)(pt.m_userPersistentData);
78                         pt.m_userPersistentData = 0;
79                 }
80                 
81 #ifdef DEBUG_PERSISTENCY
82                 DebugPersistency();
83 #endif
84         }
85
86         
87 }
88
89 static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3)
90 {
91         // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
92
93         btVector3 a[3],b[3];
94         a[0] = p0 - p1;
95         a[1] = p0 - p2;
96         a[2] = p0 - p3;
97         b[0] = p2 - p3;
98         b[1] = p1 - p3;
99         b[2] = p1 - p2;
100
101         //todo: Following 3 cross production can be easily optimized by SIMD.
102         btVector3 tmp0 = a[0].cross(b[0]);
103         btVector3 tmp1 = a[1].cross(b[1]);
104         btVector3 tmp2 = a[2].cross(b[2]);
105
106         return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2());
107 }
108
109 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
110 {
111                 //calculate 4 possible cases areas, and take biggest area
112                 //also need to keep 'deepest'
113                 
114                 int maxPenetrationIndex = -1;
115 #define KEEP_DEEPEST_POINT 1
116 #ifdef KEEP_DEEPEST_POINT
117                 btScalar maxPenetration = pt.getDistance();
118                 for (int i=0;i<4;i++)
119                 {
120                         if (m_pointCache[i].getDistance() < maxPenetration)
121                         {
122                                 maxPenetrationIndex = i;
123                                 maxPenetration = m_pointCache[i].getDistance();
124                         }
125                 }
126 #endif //KEEP_DEEPEST_POINT
127                 
128                 btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
129
130         if (gContactCalcArea3Points)
131         {
132                 if (maxPenetrationIndex != 0)
133                 {
134                         btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
135                         btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
136                         btVector3 cross = a0.cross(b0);
137                         res0 = cross.length2();
138                 }
139                 if (maxPenetrationIndex != 1)
140                 {
141                         btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
142                         btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
143                         btVector3 cross = a1.cross(b1);
144                         res1 = cross.length2();
145                 }
146
147                 if (maxPenetrationIndex != 2)
148                 {
149                         btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
150                         btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
151                         btVector3 cross = a2.cross(b2);
152                         res2 = cross.length2();
153                 }
154
155                 if (maxPenetrationIndex != 3)
156                 {
157                         btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
158                         btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
159                         btVector3 cross = a3.cross(b3);
160                         res3 = cross.length2();
161                 }
162         } 
163         else
164         {
165                 if(maxPenetrationIndex != 0) {
166                         res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
167                 }
168
169                 if(maxPenetrationIndex != 1) {
170                         res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
171                 }
172
173                 if(maxPenetrationIndex != 2) {
174                         res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
175                 }
176
177                 if(maxPenetrationIndex != 3) {
178                         res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
179                 }
180         }
181         btVector4 maxvec(res0,res1,res2,res3);
182         int biggestarea = maxvec.closestAxis4();
183         return biggestarea;
184         
185 }
186
187
188 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
189 {
190         btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
191         int size = getNumContacts();
192         int nearestPoint = -1;
193         for( int i = 0; i < size; i++ )
194         {
195                 const btManifoldPoint &mp = m_pointCache[i];
196
197                 btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
198                 const btScalar distToManiPoint = diffA.dot(diffA);
199                 if( distToManiPoint < shortestDist )
200                 {
201                         shortestDist = distToManiPoint;
202                         nearestPoint = i;
203                 }
204         }
205         return nearestPoint;
206 }
207
208 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
209 {
210         if (!isPredictive)
211         {
212                 btAssert(validContactDistance(newPoint));
213         }
214         
215         int insertIndex = getNumContacts();
216         if (insertIndex == MANIFOLD_CACHE_SIZE)
217         {
218 #if MANIFOLD_CACHE_SIZE >= 4
219                 //sort cache so best points come first, based on area
220                 insertIndex = sortCachedPoints(newPoint);
221 #else
222                 insertIndex = 0;
223 #endif
224                 clearUserCache(m_pointCache[insertIndex]);
225                 
226         } else
227         {
228                 m_cachedPoints++;
229
230                 
231         }
232         if (insertIndex<0)
233                 insertIndex=0;
234
235         btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
236         m_pointCache[insertIndex] = newPoint;
237         return insertIndex;
238 }
239
240 btScalar        btPersistentManifold::getContactBreakingThreshold() const
241 {
242         return m_contactBreakingThreshold;
243 }
244
245
246
247 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
248 {
249         int i;
250 #ifdef DEBUG_PERSISTENCY
251         printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
252                 trA.getOrigin().getX(),
253                 trA.getOrigin().getY(),
254                 trA.getOrigin().getZ(),
255                 trB.getOrigin().getX(),
256                 trB.getOrigin().getY(),
257                 trB.getOrigin().getZ());
258 #endif //DEBUG_PERSISTENCY
259         /// first refresh worldspace positions and distance
260         for (i=getNumContacts()-1;i>=0;i--)
261         {
262                 btManifoldPoint &manifoldPoint = m_pointCache[i];
263                 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
264                 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
265                 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
266                 manifoldPoint.m_lifeTime++;
267         }
268
269         /// then 
270         btScalar distance2d;
271         btVector3 projectedDifference,projectedPoint;
272         for (i=getNumContacts()-1;i>=0;i--)
273         {
274                 
275                 btManifoldPoint &manifoldPoint = m_pointCache[i];
276                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
277                 if (!validContactDistance(manifoldPoint))
278                 {
279                         removeContactPoint(i);
280                 } else
281                 {
282                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
283                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
284                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
285                         distance2d = projectedDifference.dot(projectedDifference);
286                         if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
287                         {
288                                 removeContactPoint(i);
289                         } else
290                         {
291                                 //contact point processed callback
292                                 if (gContactProcessedCallback)
293                                         (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
294                         }
295                 }
296         }
297 #ifdef DEBUG_PERSISTENCY
298         DebugPersistency();
299 #endif //
300 }
301
302
303
304
305