dali-physics/third-party/bullet3/src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp

   1
   2 #if 0
   3 /*
   4 Bullet Continuous Collision Detection and Physics Library
   5 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 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.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17
  18 #include "b3ContactCache.h"
  19 #include "Bullet3Common/b3Transform.h"
  20
  21 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
  22
  23 b3Scalar                                        gContactBreakingThreshold = b3Scalar(0.02);
  24
  25 ///gContactCalcArea3Points will approximate the convex hull area using 3 points
  26 ///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower
  27 bool                                            gContactCalcArea3Points = true;
  28
  29
  30
  31
  32 static inline b3Scalar calcArea4Points(const b3Vector3 &p0,const b3Vector3 &p1,const b3Vector3 &p2,const b3Vector3 &p3)
  33 {
  34         // It calculates possible 3 area constructed from random 4 points and returns the biggest one.
  35
  36         b3Vector3 a[3],b[3];
  37         a[0] = p0 - p1;
  38         a[1] = p0 - p2;
  39         a[2] = p0 - p3;
  40         b[0] = p2 - p3;
  41         b[1] = p1 - p3;
  42         b[2] = p1 - p2;
  43
  44         //todo: Following 3 cross production can be easily optimized by SIMD.
  45         b3Vector3 tmp0 = a[0].cross(b[0]);
  46         b3Vector3 tmp1 = a[1].cross(b[1]);
  47         b3Vector3 tmp2 = a[2].cross(b[2]);
  48
  49         return b3Max(b3Max(tmp0.length2(),tmp1.length2()),tmp2.length2());
  50 }
  51 #if 0
  52
  53 //using localPointA for all points
  54 int b3ContactCache::sortCachedPoints(const b3Vector3& pt)
  55 {
  56                 //calculate 4 possible cases areas, and take biggest area
  57                 //also need to keep 'deepest'
  58
  59                 int maxPenetrationIndex = -1;
  60 #define KEEP_DEEPEST_POINT 1
  61 #ifdef KEEP_DEEPEST_POINT
  62                 b3Scalar maxPenetration = pt.getDistance();
  63                 for (int i=0;i<4;i++)
  64                 {
  65                         if (m_pointCache[i].getDistance() < maxPenetration)
  66                         {
  67                                 maxPenetrationIndex = i;
  68                                 maxPenetration = m_pointCache[i].getDistance();
  69                         }
  70                 }
  71 #endif  //KEEP_DEEPEST_POINT
  72
  73                 b3Scalar res0(b3Scalar(0.)),res1(b3Scalar(0.)),res2(b3Scalar(0.)),res3(b3Scalar(0.));
  74
  75         if (gContactCalcArea3Points)
  76         {
  77                 if (maxPenetrationIndex != 0)
  78                 {
  79                         b3Vector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
  80                         b3Vector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
  81                         b3Vector3 cross = a0.cross(b0);
  82                         res0 = cross.length2();
  83                 }
  84                 if (maxPenetrationIndex != 1)
  85                 {
  86                         b3Vector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
  87                         b3Vector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
  88                         b3Vector3 cross = a1.cross(b1);
  89                         res1 = cross.length2();
  90                 }
  91
  92                 if (maxPenetrationIndex != 2)
  93                 {
  94                         b3Vector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
  95                         b3Vector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
  96                         b3Vector3 cross = a2.cross(b2);
  97                         res2 = cross.length2();
  98                 }
  99
 100                 if (maxPenetrationIndex != 3)
 101                 {
 102                         b3Vector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
 103                         b3Vector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
 104                         b3Vector3 cross = a3.cross(b3);
 105                         res3 = cross.length2();
 106                 }
 107         }
 108         else
 109         {
 110                 if(maxPenetrationIndex != 0) {
 111                         res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
 112                 }
 113
 114                 if(maxPenetrationIndex != 1) {
 115                         res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA);
 116                 }
 117
 118                 if(maxPenetrationIndex != 2) {
 119                         res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA);
 120                 }
 121
 122                 if(maxPenetrationIndex != 3) {
 123                         res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA);
 124                 }
 125         }
 126         b3Vector4 maxvec(res0,res1,res2,res3);
 127         int biggestarea = maxvec.closestAxis4();
 128         return biggestarea;
 129
 130 }
 131
 132
 133 int b3ContactCache::getCacheEntry(const b3Vector3& newPoint) const
 134 {
 135         b3Scalar shortestDist =  getContactBreakingThreshold() * getContactBreakingThreshold();
 136         int size = getNumContacts();
 137         int nearestPoint = -1;
 138         for( int i = 0; i < size; i++ )
 139         {
 140                 const b3Vector3 &mp = m_pointCache[i];
 141
 142                 b3Vector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
 143                 const b3Scalar distToManiPoint = diffA.dot(diffA);
 144                 if( distToManiPoint < shortestDist )
 145                 {
 146                         shortestDist = distToManiPoint;
 147                         nearestPoint = i;
 148                 }
 149         }
 150         return nearestPoint;
 151 }
 152
 153 int b3ContactCache::addManifoldPoint(const b3Vector3& newPoint)
 154 {
 155         b3Assert(validContactDistance(newPoint));
 156
 157         int insertIndex = getNumContacts();
 158         if (insertIndex == MANIFOLD_CACHE_SIZE)
 159         {
 160 #if MANIFOLD_CACHE_SIZE >= 4
 161                 //sort cache so best points come first, based on area
 162                 insertIndex = sortCachedPoints(newPoint);
 163 #else
 164                 insertIndex = 0;
 165 #endif
 166                 clearUserCache(m_pointCache[insertIndex]);
 167
 168         } else
 169         {
 170                 m_cachedPoints++;
 171
 172
 173         }
 174         if (insertIndex<0)
 175                 insertIndex=0;
 176
 177         //b3Assert(m_pointCache[insertIndex].m_userPersistentData==0);
 178         m_pointCache[insertIndex] = newPoint;
 179         return insertIndex;
 180 }
 181
 182 #endif
 183
 184 bool b3ContactCache::validContactDistance(const b3Vector3& pt)
 185 {
 186         return pt.w <= gContactBreakingThreshold;
 187 }
 188
 189 void b3ContactCache::removeContactPoint(struct b3Contact4Data& newContactCache,int i)
 190 {
 191         int numContacts = b3Contact4Data_getNumPoints(&newContactCache);
 192         if (i!=(numContacts-1))
 193         {
 194                 b3Swap(newContactCache.m_localPosA[i],newContactCache.m_localPosA[numContacts-1]);
 195                 b3Swap(newContactCache.m_localPosB[i],newContactCache.m_localPosB[numContacts-1]);
 196                 b3Swap(newContactCache.m_worldPosB[i],newContactCache.m_worldPosB[numContacts-1]);
 197         }
 198         b3Contact4Data_setNumPoints(&newContactCache,numContacts-1);
 199
 200 }
 201
 202
 203 void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transform& trB, struct b3Contact4Data& contacts)
 204 {
 205
 206         int numContacts = b3Contact4Data_getNumPoints(&contacts);
 207
 208
 209         int i;
 210         /// first refresh worldspace positions and distance
 211         for (i=numContacts-1;i>=0;i--)
 212         {
 213                 b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
 214                 b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
 215                 contacts.m_worldPosB[i] = worldPosB;
 216                 float distance = (worldPosA -  worldPosB).dot(contacts.m_worldNormalOnB);
 217                 contacts.m_worldPosB[i].w = distance;
 218         }
 219
 220         /// then
 221         b3Scalar distance2d;
 222         b3Vector3 projectedDifference,projectedPoint;
 223         for (i=numContacts-1;i>=0;i--)
 224         {
 225                 b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
 226                 b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
 227                 b3Vector3&pt = contacts.m_worldPosB[i];
 228                 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
 229                 if (!validContactDistance(pt))
 230                 {
 231                         removeContactPoint(contacts,i);
 232                 } else
 233                 {
 234                         //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
 235                         projectedPoint = worldPosA - contacts.m_worldNormalOnB * contacts.m_worldPosB[i].w;
 236                         projectedDifference = contacts.m_worldPosB[i] - projectedPoint;
 237                         distance2d = projectedDifference.dot(projectedDifference);
 238                         if (distance2d  > gContactBreakingThreshold*gContactBreakingThreshold )
 239                         {
 240                                 removeContactPoint(contacts,i);
 241                         } else
 242                         {
 243                                 ////contact point processed callback
 244                                 //if (gContactProcessedCallback)
 245                                 //      (*gContactProcessedCallback)(manifoldPoint,(void*)m_body0,(void*)m_body1);
 246                         }
 247                 }
 248         }
 249
 250
 251 }
 252
 253 #endif