1 files changed, 162 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
new file mode 100644
index 0000000000..a3fa82287b
--- /dev/null
+++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h
@@ -0,0 +1,162 @@
+
+#ifndef B3_CONTACT_SPHERE_SPHERE_H
+#define B3_CONTACT_SPHERE_SPHERE_H
+
+
+
+
+
+void	computeContactSphereConvex(int pairIndex,
+																int bodyIndexA, int bodyIndexB, 
+																int collidableIndexA, int collidableIndexB, 
+																const b3RigidBodyData* rigidBodies, 
+																const b3Collidable* collidables,
+																const b3ConvexPolyhedronData* convexShapes,
+																const b3Vector3* convexVertices,
+																const int* convexIndices,
+																const b3GpuFace* faces,
+																b3Contact4* globalContactsOut,
+																int& nGlobalContactsOut,
+																int maxContactCapacity)
+{
+
+	float radius = collidables[collidableIndexA].m_radius;
+	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
+
+
+
+	float4 pos = rigidBodies[bodyIndexB].m_pos;
+	
+
+	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
+
+	b3Transform tr;
+	tr.setIdentity();
+	tr.setOrigin(pos);
+	tr.setRotation(quat);
+	b3Transform trInv = tr.inverse();
+
+	float4 spherePos = trInv(spherePos1);
+
+	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+	int numFaces = convexShapes[shapeIndex].m_numFaces;
+	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
+	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
+	float minDist = -1000000.f; // TODO: What is the largest/smallest float?
+	bool bCollide = true;
+	int region = -1;
+	float4 localHitNormal;
+	for ( int f = 0; f < numFaces; f++ )
+	{
+		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f];
+		float4 planeEqn;
+		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
+		float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal);
+		planeEqn = n1;
+		planeEqn[3] = face.m_plane.w;
+
+		float4 pntReturn;
+		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
+
+		if ( dist > radius)
+		{
+			bCollide = false;
+			break;
+		}
+
+		if ( dist > 0 )
+		{
+			//might hit an edge or vertex
+			b3Vector3 out;
+
+			bool isInPoly = IsPointInPolygon(spherePos,
+					&face,
+					&convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+					convexIndices,
+                    &out);
+			if (isInPoly)
+			{
+				if (dist>minDist)
+				{
+					minDist = dist;
+					closestPnt = pntReturn;
+					localHitNormal = planeEqn;
+					region=1;
+				}
+			} else
+			{
+				b3Vector3 tmp = spherePos-out;
+				b3Scalar l2 = tmp.length2();
+				if (l2<radius*radius)
+				{
+					dist  = b3Sqrt(l2);
+					if (dist>minDist)
+					{
+						minDist = dist;
+						closestPnt = out;
+						localHitNormal = tmp/dist;
+						region=2;
+					}
+					
+				} else
+				{
+					bCollide = false;
+					break;
+				}
+			}
+		}
+		else
+		{
+			if ( dist > minDist )
+			{
+				minDist = dist;
+				closestPnt = pntReturn;
+				localHitNormal = planeEqn;
+				region=3;
+			}
+		}
+	}
+	static int numChecks = 0;
+	numChecks++;
+
+	if (bCollide && minDist > -10000)
+	{
+		
+		float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
+		float4 pOnB1 = tr(closestPnt);
+		//printf("dist ,%f,",minDist);
+		float actualDepth = minDist-radius;
+		if (actualDepth<0)
+		{
+		//printf("actualDepth = ,%f,", actualDepth);
+		//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+		//printf("region=,%d,\n", region);
+		pOnB1[3] = actualDepth;
+
+		int dstIdx;
+//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+		
+		if (nGlobalContactsOut < maxContactCapacity)
+		{
+			dstIdx=nGlobalContactsOut;
+			nGlobalContactsOut++;
+
+			b3Contact4* c = &globalContactsOut[dstIdx];
+			c->m_worldNormalOnB = normalOnSurfaceB1;
+			c->setFrictionCoeff(0.7);
+			c->setRestituitionCoeff(0.f);
+
+			c->m_batchIdx = pairIndex;
+			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
+			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
+			c->m_worldPosB[0] = pOnB1;
+			int numPoints = 1;
+			c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+		}//if (dstIdx < numPairs)
+		}
+	}//if (hasCollision)
+	
+}
+#endif //B3_CONTACT_SPHERE_SPHERE_H