d4e29882d376d2c59bfca3fc0d3494426d88068e
[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
25
26
27 btPersistentManifold::btPersistentManifold()
28 :m_body0(0),
29 m_body1(0),
30 m_cachedPoints (0),
31 m_index1a(0)
32 {
33 }
34
35
36
37
38 #ifdef DEBUG_PERSISTENCY
39 #include <stdio.h>
40 void    btPersistentManifold::DebugPersistency()
41 {
42         int i;
43         printf("DebugPersistency : numPoints %d\n",m_cachedPoints);
44         for (i=0;i<m_cachedPoints;i++)
45         {
46                 printf("m_pointCache[%d].m_userPersistentData = %x\n",i,m_pointCache[i].m_userPersistentData);
47         }
48 }
49 #endif //DEBUG_PERSISTENCY
50
51 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
52 {
53
54         void* oldPtr = pt.m_userPersistentData;
55         if (oldPtr)
56         {
57 #ifdef DEBUG_PERSISTENCY
58                 int i;
59                 int occurance = 0;
60                 for (i=0;i<m_cachedPoints;i++)
61                 {
62                         if (m_pointCache[i].m_userPersistentData == oldPtr)
63                         {
64                                 occurance++;
65                                 if (occurance>1)
66                                         printf("error in clearUserCache\n");
67                         }
68                 }
69                 btAssert(occurance<=0);
70 #endif //DEBUG_PERSISTENCY
71
72                 if (pt.m_userPersistentData && gContactDestroyedCallback)
73                 {
74                         (*gContactDestroyedCallback)(pt.m_userPersistentData);
75                         pt.m_userPersistentData = 0;
76                 }
77                 
78 #ifdef DEBUG_PERSISTENCY
79                 DebugPersistency();
80 #endif
81         }
82
83         
84 }
85
86
87 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) 
88 {
89
90                 //calculate 4 possible cases areas, and take biggest area
91                 //also need to keep 'deepest'
92                 
93                 int maxPenetrationIndex = -1;
94 #define KEEP_DEEPEST_POINT 1
95 #ifdef KEEP_DEEPEST_POINT
96                 btScalar maxPenetration = pt.getDistance();
97                 for (int i=0;i<4;i++)
98                 {
99                         if (m_pointCache[i].getDistance() < maxPenetration)
100                         {
101                                 maxPenetrationIndex = i;
102                                 maxPenetration = m_pointCache[i].getDistance();
103                         }
104                 }
105 #endif //KEEP_DEEPEST_POINT
106                 
107                 btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.));
108                 if (maxPenetrationIndex != 0)
109                 {
110                         btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
111                         btVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
112                         btVector3 cross = a0.cross(b0);
113                         res0 = cross.length2();
114                 }
115                 if (maxPenetrationIndex != 1)
116                 {
117                         btVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
118                         btVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
119                         btVector3 cross = a1.cross(b1);
120                         res1 = cross.length2();
121                 }
122
123                 if (maxPenetrationIndex != 2)
124                 {
125                         btVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
126                         btVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
127                         btVector3 cross = a2.cross(b2);
128                         res2 = cross.length2();
129                 }
130
131                 if (maxPenetrationIndex != 3)
132                 {
133                         btVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
134                         btVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
135                         btVector3 cross = a3.cross(b3);
136                         res3 = cross.length2();
137                 }
138
139                 btVector4 maxvec(res0,res1,res2,res3);
140                 int biggestarea = maxvec.closestAxis4();
141                 return biggestarea;
142 }
143
144
145 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
146 {
147         btScalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
148         int size = getNumContacts();
149         int nearestPoint = -1;
150         for( int i = 0; i < size; i++ )
151         {
152                 const btManifoldPoint &mp = m_pointCache[i];
153
154                 btVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
155                 const btScalar distToManiPoint = diffA.dot(diffA);
156                 if( distToManiPoint < shortestDist )
157                 {
158                         shortestDist = distToManiPoint;
159                         nearestPoint = i;
160                 }
161         }
162         return nearestPoint;
163 }
164
165 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
166 {
167         btAssert(validContactDistance(newPoint));
168
169         int insertIndex = getNumContacts();
170         if (insertIndex == MANIFOLD_CACHE_SIZE)
171         {
172 #if MANIFOLD_CACHE_SIZE >= 4
173                 //sort cache so best points come first, based on area
174                 insertIndex = sortCachedPoints(newPoint);
175 #else
176                 insertIndex = 0;
177 #endif
178                 clearUserCache(m_pointCache[insertIndex]);
179                 
180         } else
181         {
182                 m_cachedPoints++;
183
184                 
185         }
186         if (insertIndex<0)
187                 insertIndex=0;
188
189         btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
190         m_pointCache[insertIndex] = newPoint;
191         return insertIndex;
192 }
193
194 btScalar        btPersistentManifold::getContactBreakingThreshold() const
195 {
196         return m_contactBreakingThreshold;
197 }
198
199
200
201 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
202 {
203         int i;
204 #ifdef DEBUG_PERSISTENCY
205         printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
206                 trA.getOrigin().getX(),
207                 trA.getOrigin().getY(),
208                 trA.getOrigin().getZ(),
209                 trB.getOrigin().getX(),
210                 trB.getOrigin().getY(),
211                 trB.getOrigin().getZ());
212 #endif //DEBUG_PERSISTENCY
213         /// first refresh worldspace positions and distance
214         for (i=getNumContacts()-1;i>=0;i--)
215         {
216                 btManifoldPoint &manifoldPoint = m_pointCache[i];
217                 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
218                 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
219                 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
220                 manifoldPoint.m_lifeTime++;
221         }
222
223         /// then 
224         btScalar distance2d;
225         btVector3 projectedDifference,projectedPoint;
226         for (i=getNumContacts()-1;i>=0;i--)
227         {
228                 
229                 btManifoldPoint &manifoldPoint = m_pointCache[i];
230                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
231                 if (!validContactDistance(manifoldPoint))
232                 {
233                         removeContactPoint(i);
234                 } else
235                 {
236                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
237                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
238                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
239                         distance2d = projectedDifference.dot(projectedDifference);
240                         if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
241                         {
242                                 removeContactPoint(i);
243                         } else
244                         {
245                                 //contact point processed callback
246                                 if (gContactProcessedCallback)
247                                         (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
248                         }
249                 }
250         }
251 #ifdef DEBUG_PERSISTENCY
252         DebugPersistency();
253 #endif //
254 }
255
256
257
258
259