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