[dali_2.3.21] Merge branch 'devel/master'
[platform/core/uifw/dali-toolkit.git] / dali-physics / third-party / bullet3 / src / BulletCollision / NarrowPhaseCollision / btPersistentManifold.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
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 #include "btPersistentManifold.h"
17 #include "LinearMath/btTransform.h"
18 #include "LinearMath/btSerializer.h"
19
20 #ifdef BT_USE_DOUBLE_PRECISION
21 #define btCollisionObjectData btCollisionObjectDoubleData
22 #else
23 #define btCollisionObjectData btCollisionObjectFloatData
24 #endif
25
26 btScalar gContactBreakingThreshold = btScalar(0.02);
27 ContactDestroyedCallback gContactDestroyedCallback = 0;
28 ContactProcessedCallback gContactProcessedCallback = 0;
29 ContactStartedCallback gContactStartedCallback = 0;
30 ContactEndedCallback gContactEndedCallback = 0;
31 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
32 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
33 bool gContactCalcArea3Points = true;
34
35 btPersistentManifold::btPersistentManifold()
36         : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
37           m_body0(0),
38           m_body1(0),
39           m_cachedPoints(0),
40           m_companionIdA(0),
41           m_companionIdB(0),
42           m_index1a(0)
43 {
44 }
45
46 #ifdef DEBUG_PERSISTENCY
47 #include <stdio.h>
48 void btPersistentManifold::DebugPersistency()
49 {
50         int i;
51         printf("DebugPersistency : numPoints %d\n", m_cachedPoints);
52         for (i = 0; i < m_cachedPoints; i++)
53         {
54                 printf("m_pointCache[%d].m_userPersistentData = %x\n", i, m_pointCache[i].m_userPersistentData);
55         }
56 }
57 #endif  //DEBUG_PERSISTENCY
58
59 void btPersistentManifold::clearUserCache(btManifoldPoint& pt)
60 {
61         void* oldPtr = pt.m_userPersistentData;
62         if (oldPtr)
63         {
64 #ifdef DEBUG_PERSISTENCY
65                 int i;
66                 int occurance = 0;
67                 for (i = 0; i < m_cachedPoints; i++)
68                 {
69                         if (m_pointCache[i].m_userPersistentData == oldPtr)
70                         {
71                                 occurance++;
72                                 if (occurance > 1)
73                                         printf("error in clearUserCache\n");
74                         }
75                 }
76                 btAssert(occurance <= 0);
77 #endif  //DEBUG_PERSISTENCY
78
79                 if (pt.m_userPersistentData && gContactDestroyedCallback)
80                 {
81                         (*gContactDestroyedCallback)(pt.m_userPersistentData);
82                         pt.m_userPersistentData = 0;
83                 }
84
85 #ifdef DEBUG_PERSISTENCY
86                 DebugPersistency();
87 #endif
88         }
89 }
90
91 static inline btScalar calcArea4Points(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& p3)
92 {
93         // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
94
95         btVector3 a[3], b[3];
96         a[0] = p0 - p1;
97         a[1] = p0 - p2;
98         a[2] = p0 - p3;
99         b[0] = p2 - p3;
100         b[1] = p1 - p3;
101         b[2] = p1 - p2;
102
103         //todo: Following 3 cross production can be easily optimized by SIMD.
104         btVector3 tmp0 = a[0].cross(b[0]);
105         btVector3 tmp1 = a[1].cross(b[1]);
106         btVector3 tmp2 = a[2].cross(b[2]);
107
108         return btMax(btMax(tmp0.length2(), tmp1.length2()), tmp2.length2());
109 }
110
111 int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt)
112 {
113         //calculate 4 possible cases areas, and take biggest area
114         //also need to keep 'deepest'
115
116         int maxPenetrationIndex = -1;
117 #define KEEP_DEEPEST_POINT 1
118 #ifdef KEEP_DEEPEST_POINT
119         btScalar maxPenetration = pt.getDistance();
120         for (int i = 0; i < 4; i++)
121         {
122                 if (m_pointCache[i].getDistance() < maxPenetration)
123                 {
124                         maxPenetrationIndex = i;
125                         maxPenetration = m_pointCache[i].getDistance();
126                 }
127         }
128 #endif  //KEEP_DEEPEST_POINT
129
130         btScalar res0(btScalar(0.)), res1(btScalar(0.)), res2(btScalar(0.)), res3(btScalar(0.));
131
132         if (gContactCalcArea3Points)
133         {
134                 if (maxPenetrationIndex != 0)
135                 {
136                         btVector3 a0 = pt.m_localPointA - m_pointCache[1].m_localPointA;
137                         btVector3 b0 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA;
138                         btVector3 cross = a0.cross(b0);
139                         res0 = cross.length2();
140                 }
141                 if (maxPenetrationIndex != 1)
142                 {
143                         btVector3 a1 = pt.m_localPointA - m_pointCache[0].m_localPointA;
144                         btVector3 b1 = m_pointCache[3].m_localPointA - m_pointCache[2].m_localPointA;
145                         btVector3 cross = a1.cross(b1);
146                         res1 = cross.length2();
147                 }
148
149                 if (maxPenetrationIndex != 2)
150                 {
151                         btVector3 a2 = pt.m_localPointA - m_pointCache[0].m_localPointA;
152                         btVector3 b2 = m_pointCache[3].m_localPointA - m_pointCache[1].m_localPointA;
153                         btVector3 cross = a2.cross(b2);
154                         res2 = cross.length2();
155                 }
156
157                 if (maxPenetrationIndex != 3)
158                 {
159                         btVector3 a3 = pt.m_localPointA - m_pointCache[0].m_localPointA;
160                         btVector3 b3 = m_pointCache[2].m_localPointA - m_pointCache[1].m_localPointA;
161                         btVector3 cross = a3.cross(b3);
162                         res3 = cross.length2();
163                 }
164         }
165         else
166         {
167                 if (maxPenetrationIndex != 0)
168                 {
169                         res0 = calcArea4Points(pt.m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA);
170                 }
171
172                 if (maxPenetrationIndex != 1)
173                 {
174                         res1 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[2].m_localPointA, m_pointCache[3].m_localPointA);
175                 }
176
177                 if (maxPenetrationIndex != 2)
178                 {
179                         res2 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[3].m_localPointA);
180                 }
181
182                 if (maxPenetrationIndex != 3)
183                 {
184                         res3 = calcArea4Points(pt.m_localPointA, m_pointCache[0].m_localPointA, m_pointCache[1].m_localPointA, m_pointCache[2].m_localPointA);
185                 }
186         }
187         btVector4 maxvec(res0, res1, res2, res3);
188         int biggestarea = maxvec.closestAxis4();
189         return biggestarea;
190 }
191
192 int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
193 {
194         btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
195         int size = getNumContacts();
196         int nearestPoint = -1;
197         for (int i = 0; i < size; i++)
198         {
199                 const btManifoldPoint& mp = m_pointCache[i];
200
201                 btVector3 diffA = mp.m_localPointA - newPoint.m_localPointA;
202                 const btScalar distToManiPoint = diffA.dot(diffA);
203                 if (distToManiPoint < shortestDist)
204                 {
205                         shortestDist = distToManiPoint;
206                         nearestPoint = i;
207                 }
208         }
209         return nearestPoint;
210 }
211
212 int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint, bool isPredictive)
213 {
214         if (!isPredictive)
215         {
216                 btAssert(validContactDistance(newPoint));
217         }
218
219         int insertIndex = getNumContacts();
220         if (insertIndex == MANIFOLD_CACHE_SIZE)
221         {
222 #if MANIFOLD_CACHE_SIZE >= 4
223                 //sort cache so best points come first, based on area
224                 insertIndex = sortCachedPoints(newPoint);
225 #else
226                 insertIndex = 0;
227 #endif
228                 clearUserCache(m_pointCache[insertIndex]);
229         }
230         else
231         {
232                 m_cachedPoints++;
233         }
234         if (insertIndex < 0)
235                 insertIndex = 0;
236
237         btAssert(m_pointCache[insertIndex].m_userPersistentData == 0);
238         m_pointCache[insertIndex] = newPoint;
239         return insertIndex;
240 }
241
242 btScalar btPersistentManifold::getContactBreakingThreshold() const
243 {
244         return m_contactBreakingThreshold;
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                 btManifoldPoint& manifoldPoint = m_pointCache[i];
275                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
276                 if (!validContactDistance(manifoldPoint))
277                 {
278                         removeContactPoint(i);
279                 }
280                 else
281                 {
282                         //todo: friction anchor may require the contact to be around a bit longer
283                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
284                         projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
285                         projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
286                         distance2d = projectedDifference.dot(projectedDifference);
287                         if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold())
288                         {
289                                 removeContactPoint(i);
290                         }
291                         else
292                         {
293                                 //contact point processed callback
294                                 if (gContactProcessedCallback)
295                                         (*gContactProcessedCallback)(manifoldPoint, (void*)m_body0, (void*)m_body1);
296                         }
297                 }
298         }
299 #ifdef DEBUG_PERSISTENCY
300         DebugPersistency();
301 #endif  //
302 }
303
304 int btPersistentManifold::calculateSerializeBufferSize() const
305 {
306         return sizeof(btPersistentManifoldData);
307 }
308
309 const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
310 {
311         btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
312         memset(dataOut, 0, sizeof(btPersistentManifoldData));
313
314         dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
315         dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
316         dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
317         dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
318         dataOut->m_numCachedPoints = manifold->getNumContacts();
319         dataOut->m_companionIdA = manifold->m_companionIdA;
320         dataOut->m_companionIdB = manifold->m_companionIdB;
321         dataOut->m_index1a = manifold->m_index1a;
322         dataOut->m_objectType = manifold->m_objectType;
323
324         for (int i = 0; i < this->getNumContacts(); i++)
325         {
326                 const btManifoldPoint& pt = manifold->getContactPoint(i);
327                 dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
328                 dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
329                 dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
330                 dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
331                 pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
332                 pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
333                 pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
334                 dataOut->m_pointCacheDistance[i] = pt.m_distance1;
335                 dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
336                 dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
337                 dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
338                 dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
339                 dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
340                 dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
341                 dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
342                 dataOut->m_pointCacheIndex0[i] = pt.m_index0;
343                 dataOut->m_pointCacheIndex1[i] = pt.m_index1;
344                 dataOut->m_pointCachePartId0[i] = pt.m_partId0;
345                 dataOut->m_pointCachePartId1[i] = pt.m_partId1;
346                 pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
347                 pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
348                 dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
349                 pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
350                 pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
351                 dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
352                 dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
353                 dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
354                 dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
355                 dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
356         }
357         return btPersistentManifoldDataName;
358 }
359
360 void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
361 {
362         m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
363         m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
364         m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
365         m_companionIdA = manifoldDataPtr->m_companionIdA;
366         m_companionIdB = manifoldDataPtr->m_companionIdB;
367         //m_index1a = manifoldDataPtr->m_index1a;
368         m_objectType = manifoldDataPtr->m_objectType;
369
370         for (int i = 0; i < this->getNumContacts(); i++)
371         {
372                 btManifoldPoint& pt = m_pointCache[i];
373
374                 pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
375                 pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
376                 pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
377                 pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
378                 pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
379                 pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
380                 pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
381                 pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
382                 pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
383                 pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
384                 pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
385                 pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
386                 pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
387                 pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
388                 pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
389                 pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
390                 pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
391                 pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
392                 pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
393                 pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
394                 pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
395                 pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
396                 pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
397                 pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
398                 pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
399                 pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
400                 pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
401                 pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
402                 pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
403         }
404 }
405
406 void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
407 {
408         m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
409         m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
410         m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
411         m_companionIdA = manifoldDataPtr->m_companionIdA;
412         m_companionIdB = manifoldDataPtr->m_companionIdB;
413         //m_index1a = manifoldDataPtr->m_index1a;
414         m_objectType = manifoldDataPtr->m_objectType;
415
416         for (int i = 0; i < this->getNumContacts(); i++)
417         {
418                 btManifoldPoint& pt = m_pointCache[i];
419
420                 pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
421                 pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
422                 pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
423                 pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
424                 pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
425                 pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
426                 pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
427                 pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
428                 pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
429                 pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
430                 pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
431                 pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
432                 pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
433                 pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
434                 pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
435                 pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
436                 pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
437                 pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
438                 pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
439                 pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
440                 pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
441                 pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
442                 pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
443                 pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
444                 pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
445                 pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
446                 pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
447                 pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
448                 pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
449         }
450 }