[#17963] NearSensor segmentation fault
[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                 
176                 if (insertIndex<0)
177                         insertIndex=0;
178 #else
179                 insertIndex = 0;
180 #endif
181                 clearUserCache(m_pointCache[insertIndex]);
182                 
183         } else
184         {
185                 m_cachedPoints++;
186         }
187
188         btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
189         m_pointCache[insertIndex] = newPoint;
190         return insertIndex;
191 }
192
193 btScalar        btPersistentManifold::getContactBreakingThreshold() const
194 {
195         return m_contactBreakingThreshold;
196 }
197
198
199
200 void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
201 {
202         int i;
203 #ifdef DEBUG_PERSISTENCY
204         printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
205                 trA.getOrigin().getX(),
206                 trA.getOrigin().getY(),
207                 trA.getOrigin().getZ(),
208                 trB.getOrigin().getX(),
209                 trB.getOrigin().getY(),
210                 trB.getOrigin().getZ());
211 #endif //DEBUG_PERSISTENCY
212         /// first refresh worldspace positions and distance
213         for (i=getNumContacts()-1;i>=0;i--)
214         {
215                 btManifoldPoint &manifoldPoint = m_pointCache[i];
216                 manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
217                 manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
218                 manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
219                 manifoldPoint.m_lifeTime++;
220         }
221
222         /// then 
223         btScalar distance2d;
224         btVector3 projectedDifference,projectedPoint;
225         for (i=getNumContacts()-1;i>=0;i--)
226         {
227                 
228                 btManifoldPoint &manifoldPoint = m_pointCache[i];
229                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
230                 if (!validContactDistance(manifoldPoint))
231                 {
232                         removeContactPoint(i);
233                 } else
234                 {
235                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
236                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
237                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
238                         distance2d = projectedDifference.dot(projectedDifference);
239                         if (distance2d  > getContactBreakingThreshold()*getContactBreakingThreshold() )
240                         {
241                                 removeContactPoint(i);
242                         } else
243                         {
244                                 //contact point processed callback
245                                 if (gContactProcessedCallback)
246                                         (*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
247                         }
248                 }
249         }
250 #ifdef DEBUG_PERSISTENCY
251         DebugPersistency();
252 #endif //
253 }
254
255
256
257
258