public static bool getSphereDistance(BoxShape boxShape, VIntTransform m44T, VInt3 sphereCenter, VFixedPoint radius, out VInt3 normal, out VFixedPoint penetrationDepth) { VInt3 boxHalfExtent = boxShape.getHalfExtent(); VFixedPoint boxMargin = boxShape.getMargin(); penetrationDepth = VFixedPoint.Zero; // convert the sphere position to the box's local space VInt3 sphereRelPos = m44T.InverseTransformPoint(sphereCenter); // Determine the closest point to the sphere center in the box VInt3 closestPoint = sphereRelPos; closestPoint.x = (FMath.Min(boxHalfExtent.x, closestPoint.x)); closestPoint.x = (FMath.Max(-boxHalfExtent.x, closestPoint.x)); closestPoint.y = (FMath.Min(boxHalfExtent.y, closestPoint.y)); closestPoint.y = (FMath.Max(-boxHalfExtent.y, closestPoint.y)); closestPoint.z = (FMath.Min(boxHalfExtent.z, closestPoint.z)); closestPoint.z = (FMath.Max(-boxHalfExtent.z, closestPoint.z)); VFixedPoint intersectionDist = radius + boxMargin; VFixedPoint contactDist = intersectionDist; normal = sphereRelPos - closestPoint; //if there is no penetration, we are done VFixedPoint dist2 = normal.sqrMagnitude; if (dist2 > contactDist * contactDist) { return(false); } VFixedPoint distance; //special case if the sphere center is inside the box if (dist2 <= Globals.EPS) { distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, ref closestPoint, out normal); } else //compute the penetration details { distance = normal.magnitude; normal /= distance; } // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; VInt3 tmp = m44T.TransformDirection(normal); normal = tmp; return(true); }
public static bool rayTestBox(VInt3 fromPos, VInt3 toPos, VInt3 boxHalfExtent, VIntTransform boxTransform, ref VFixedPoint t, ref VInt3 normal) { VInt3 rayFromPointLocal = boxTransform.InverseTransformPoint(fromPos); VInt3 rayToPointLocal = boxTransform.InverseTransformPoint(toPos); t = VFixedPoint.One; VInt3 normalInLocal = VInt3.zero; if (AabbUtils.RayAabb(rayFromPointLocal, rayToPointLocal, -boxHalfExtent, boxHalfExtent, ref t, ref normalInLocal)) { normal = -boxTransform.TransformDirection(normalInLocal); return(true); } return(false); }
static void calculateContacts(VFixedPoint extentX, VFixedPoint extentY, VFixedPoint extentZ, VInt3[] pts, VInt3 incidentfaceNormalInNew, VInt3 localNormal, VFixedPoint contactDist, VIntTransform transformNew, PersistentManifold resultOut, bool flip) { localNormal = transformNew.TransformDirection(localNormal); VFixedPoint nExtentX = -extentX; VFixedPoint nExtentY = -extentY; bool[] penetration = new bool[4]; bool[] area = new bool[4]; VInt3 bmin = new VInt3(VFixedPoint.MaxValue, VFixedPoint.MaxValue, VFixedPoint.MaxValue); VInt3 bmax = new VInt3(VFixedPoint.MinValue, VFixedPoint.MinValue, VFixedPoint.MinValue); VInt3 bound = new VInt3(extentX, extentY, VFixedPoint.MaxValue); for (int i = 0; i < 4; i++) { bmin = VInt3.Min(bmin, pts[i]); bmax = VInt3.Max(bmax, pts[i]); VFixedPoint z = -pts[i].z; if (contactDist > z) { penetration[i] = true; VInt3 absPt = pts[i].Abs(); bool con = bound >= absPt; if (con) { area[i] = true; VInt3 localPointA = pts[i]; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = pts[i]; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, z); resultOut.addManifoldPoint(contactPoint); } else { area[i] = false; } } else { penetration[i] = false; area[i] = false; } } if (resultOut.getContactPointsNum() == PersistentManifold.MANIFOLD_CACHE_SIZE) { return; } { { VFixedPoint denom = incidentfaceNormalInNew.z; { VInt3 q0 = new VInt3(extentX, extentY, VFixedPoint.Zero); if (contains(pts, q0, bmin, bmax)) { VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0); VFixedPoint t = nom / denom; VFixedPoint pen = -t; if (contactDist > pen) { VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen); resultOut.addManifoldPoint(contactPoint); } } } { VInt3 q0 = new VInt3(extentX, nExtentY, VFixedPoint.Zero); if (contains(pts, q0, bmin, bmax)) { VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0); VFixedPoint t = nom / denom; VFixedPoint pen = -t; if (contactDist > pen) { VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen); resultOut.addManifoldPoint(contactPoint); } } } { VInt3 q0 = new VInt3(nExtentX, extentY, VFixedPoint.Zero); if (contains(pts, q0, bmin, bmax)) { VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0); VFixedPoint t = nom / denom; VFixedPoint pen = -t; if (contactDist > pen) { VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen); resultOut.addManifoldPoint(contactPoint); } } } { VInt3 q0 = new VInt3(nExtentX, nExtentY, VFixedPoint.Zero); if (contains(pts, q0, bmin, bmax)) { VFixedPoint nom = VInt3.Dot(incidentfaceNormalInNew, pts[0] - q0); VFixedPoint t = nom / denom; VFixedPoint pen = -t; if (contactDist > pen) { VInt3 localPointA = q0; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = q0; localPointB.z = t; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, pen); resultOut.addManifoldPoint(contactPoint); } } } } } VInt3 ext = new VInt3(extentX, extentY, extentZ * VFixedPoint.Two); VInt3 negExt = new VInt3(nExtentX, nExtentY, -(contactDist + Globals.EPS)); for (int start = 0, end = 3; start < 4; end = start++) { VInt3 p0 = pts[start]; VInt3 p1 = pts[end]; if (!penetration[start] && !penetration[end]) { continue; } bool con0 = penetration[start] && area[start]; bool con1 = penetration[end] && area[end]; if (con0 && con1) { continue; } VFixedPoint tmin = VFixedPoint.Zero, tmax = VFixedPoint.Zero; if (AabbUtils.RayAabb2(p0, p1, negExt, ext, ref tmin, ref tmax)) { if (!con0 && tmin > VFixedPoint.Zero) { VInt3 intersectP = p0 * (VFixedPoint.One - tmin) + p1 * tmin; VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z); resultOut.addManifoldPoint(contactPoint); } if (!con1 && tmax < VFixedPoint.One) { VInt3 intersectP = p0 * (VFixedPoint.One - tmax) + p1 * tmax; VInt3 localPointA = intersectP; localPointA.z = VFixedPoint.Zero; localPointA = transformNew.TransformPoint(localPointA); VInt3 localPointB = intersectP; localPointB = transformNew.TransformPoint(localPointB); ManifoldPoint contactPoint = new ManifoldPoint(flip ? localPointB : localPointA, flip ? localPointA : localPointB, localNormal, -intersectP.z); resultOut.addManifoldPoint(contactPoint); } } } }