dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

   1
   2 #ifndef B3_CONTACT_SPHERE_SPHERE_H
   3 #define B3_CONTACT_SPHERE_SPHERE_H
   4
   5 void computeContactSphereConvex(int pairIndex,
   6                                                                 int bodyIndexA, int bodyIndexB,
   7                                                                 int collidableIndexA, int collidableIndexB,
   8                                                                 const b3RigidBodyData* rigidBodies,
   9                                                                 const b3Collidable* collidables,
  10                                                                 const b3ConvexPolyhedronData* convexShapes,
  11                                                                 const b3Vector3* convexVertices,
  12                                                                 const int* convexIndices,
  13                                                                 const b3GpuFace* faces,
  14                                                                 b3Contact4* globalContactsOut,
  15                                                                 int& nGlobalContactsOut,
  16                                                                 int maxContactCapacity)
  17 {
  18         float radius = collidables[collidableIndexA].m_radius;
  19         float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
  20         b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
  21
  22         float4 pos = rigidBodies[bodyIndexB].m_pos;
  23
  24         b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
  25
  26         b3Transform tr;
  27         tr.setIdentity();
  28         tr.setOrigin(pos);
  29         tr.setRotation(quat);
  30         b3Transform trInv = tr.inverse();
  31
  32         float4 spherePos = trInv(spherePos1);
  33
  34         int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
  35         int shapeIndex = collidables[collidableIndex].m_shapeIndex;
  36         int numFaces = convexShapes[shapeIndex].m_numFaces;
  37         float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
  38         float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
  39         float minDist = -1000000.f;  // TODO: What is the largest/smallest float?
  40         bool bCollide = true;
  41         int region = -1;
  42         float4 localHitNormal;
  43         for (int f = 0; f < numFaces; f++)
  44         {
  45                 b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
  46                 float4 planeEqn;
  47                 float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
  48                 float4 n1 = localPlaneNormal;  //quatRotate(quat,localPlaneNormal);
  49                 planeEqn = n1;
  50                 planeEqn[3] = face.m_plane.w;
  51
  52                 float4 pntReturn;
  53                 float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
  54
  55                 if (dist > radius)
  56                 {
  57                         bCollide = false;
  58                         break;
  59                 }
  60
  61                 if (dist > 0)
  62                 {
  63                         //might hit an edge or vertex
  64                         b3Vector3 out;
  65
  66                         bool isInPoly = IsPointInPolygon(spherePos,
  67                                                                                          &face,
  68                                                                                          &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
  69                                                                                          convexIndices,
  70                                                                                          &out);
  71                         if (isInPoly)
  72                         {
  73                                 if (dist > minDist)
  74                                 {
  75                                         minDist = dist;
  76                                         closestPnt = pntReturn;
  77                                         localHitNormal = planeEqn;
  78                                         region = 1;
  79                                 }
  80                         }
  81                         else
  82                         {
  83                                 b3Vector3 tmp = spherePos - out;
  84                                 b3Scalar l2 = tmp.length2();
  85                                 if (l2 < radius * radius)
  86                                 {
  87                                         dist = b3Sqrt(l2);
  88                                         if (dist > minDist)
  89                                         {
  90                                                 minDist = dist;
  91                                                 closestPnt = out;
  92                                                 localHitNormal = tmp / dist;
  93                                                 region = 2;
  94                                         }
  95                                 }
  96                                 else
  97                                 {
  98                                         bCollide = false;
  99                                         break;
 100                                 }
 101                         }
 102                 }
 103                 else
 104                 {
 105                         if (dist > minDist)
 106                         {
 107                                 minDist = dist;
 108                                 closestPnt = pntReturn;
 109                                 localHitNormal = planeEqn;
 110                                 region = 3;
 111                         }
 112                 }
 113         }
 114         static int numChecks = 0;
 115         numChecks++;
 116
 117         if (bCollide && minDist > -10000)
 118         {
 119                 float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal;  //-hitNormalWorld;
 120                 float4 pOnB1 = tr(closestPnt);
 121                 //printf("dist ,%f,",minDist);
 122                 float actualDepth = minDist - radius;
 123                 if (actualDepth < 0)
 124                 {
 125                         //printf("actualDepth = ,%f,", actualDepth);
 126                         //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
 127                         //printf("region=,%d,\n", region);
 128                         pOnB1[3] = actualDepth;
 129
 130                         int dstIdx;
 131                         //    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
 132
 133                         if (nGlobalContactsOut < maxContactCapacity)
 134                         {
 135                                 dstIdx = nGlobalContactsOut;
 136                                 nGlobalContactsOut++;
 137
 138                                 b3Contact4* c = &globalContactsOut[dstIdx];
 139                                 c->m_worldNormalOnB = normalOnSurfaceB1;
 140                                 c->setFrictionCoeff(0.7);
 141                                 c->setRestituitionCoeff(0.f);
 142
 143                                 c->m_batchIdx = pairIndex;
 144                                 c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
 145                                 c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
 146                                 c->m_worldPosB[0] = pOnB1;
 147                                 int numPoints = 1;
 148                                 c->m_worldNormalOnB.w = (b3Scalar)numPoints;
 149                         }  //if (dstIdx < numPairs)
 150                 }
 151         }  //if (hasCollision)
 152 }
 153 #endif  //B3_CONTACT_SPHERE_SPHERE_H