private static bool SATTest(CubeXCollider src, CubeXCollider dst, out Vector3 normal, out float penetration) { var srcPoints = new Vector3[4]; var dstPoints = new Vector3[4]; var srcExtents = src.Size * 0.5f; var dstExtents = dst.Size * 0.5f; srcPoints[0] = new Vector3(-srcExtents.x, 0, srcExtents.z); srcPoints[1] = new Vector3(srcExtents.x, 0, srcExtents.z); srcPoints[2] = new Vector3(srcExtents.x, 0, -srcExtents.z); srcPoints[3] = new Vector3(-srcExtents.x, 0, -srcExtents.z); srcPoints[0] = src.Quaternion * srcPoints[0] + src.Position; srcPoints[1] = src.Quaternion * srcPoints[1] + src.Position; srcPoints[2] = src.Quaternion * srcPoints[2] + src.Position; srcPoints[3] = src.Quaternion * srcPoints[3] + src.Position; srcPoints[0].y = srcPoints[1].y = srcPoints[2].y = srcPoints[3].y = 0; dstPoints[0] = new Vector3(-dstExtents.x, 0, dstExtents.z); dstPoints[1] = new Vector3(dstExtents.x, 0, dstExtents.z); dstPoints[2] = new Vector3(dstExtents.x, 0, -dstExtents.z); dstPoints[3] = new Vector3(-dstExtents.x, 0, -dstExtents.z); dstPoints[0] = dst.Quaternion * dstPoints[0] + dst.Position; dstPoints[1] = dst.Quaternion * dstPoints[1] + dst.Position; dstPoints[2] = dst.Quaternion * dstPoints[2] + dst.Position; dstPoints[3] = dst.Quaternion * dstPoints[3] + dst.Position; dstPoints[0].y = dstPoints[1].y = dstPoints[2].y = dstPoints[3].y = 0; var axis = new Vector3[4]; axis[0] = src.Quaternion * Vector3.forward; axis[1] = src.Quaternion * Vector3.right; axis[2] = dst.Quaternion * Vector3.forward; axis[3] = dst.Quaternion * Vector3.right; float minOverlap = float.MaxValue; Vector3 minAxis = axis[0]; float overlap; for (int i = 0; i < axis.Length; i++) { if (!ProjectOverlapTest(axis[i], srcPoints, dstPoints, out overlap)) { normal = Vector3.zero; penetration = 0; return(false); } else { if (overlap < minOverlap) { minOverlap = overlap; minAxis = axis[i]; } } } normal = minAxis; penetration = minOverlap; return(true); }
public static bool Intersect(CubeXCollider src, CubeXCollider dst, out XContact?contact) { Vector3 normal; float penetration; if (!SATTest(src, dst, out normal, out penetration)) { contact = null; return(false); } var srcExtents = src.Size * 0.5f; var dstExtents = dst.Size * 0.5f; var srcMinY = src.Position.y - srcExtents.y; var srcMaxY = src.Position.y + srcExtents.y; var dstMinY = dst.Position.y - dstExtents.y; var dstMaxY = dst.Position.y + dstExtents.y; var overlapY = Mathf.Min(srcMaxY, dstMaxY) - Mathf.Max(srcMinY, dstMinY); if ((srcMaxY > dstMaxY && srcMinY < dstMinY) || (srcMaxY < dstMaxY && srcMinY > dstMinY)) { var min = Mathf.Abs(srcMinY - dstMinY); var max = Mathf.Abs(srcMaxY - dstMaxY); overlapY += Mathf.Min(min, max); } if (overlapY < penetration) { penetration = overlapY; normal = Vector3.up; } if (Vector3.Dot(normal, dst.Position - src.Position) < 0) { normal = -normal; } contact = new XContact(src, dst, normal, penetration); return(true); }
public static bool Intersect(SphereXCollider src, CubeXCollider dst, out XContact?contact) { return(Intersect(dst, src, out contact)); }
public static bool Intersect(CubeXCollider src, SphereXCollider dst, out XContact?contact) { // 反向旋转sphere的位置,使得可以在cube的坐标系下进行碰撞判断 var extents = src.Size * 0.5f; var invQ = Quaternion.Inverse(src.Quaternion); var invP = invQ * (dst.Position - src.Position); // 以下所有操作都是在cube的坐标系下,随后的实际方向需要进行坐标系转换 var n = invP; var closest = n; closest.x = Mathf.Clamp(closest.x, -extents.x, extents.x); closest.y = Mathf.Clamp(closest.y, -extents.y, extents.y); closest.z = Mathf.Clamp(closest.z, -extents.z, extents.z); var inside = false; if (n == closest) { inside = true; var disX = extents.x - Mathf.Abs(n.x); var disY = extents.y - Mathf.Abs(n.y); var disZ = extents.z - Mathf.Abs(n.z); //找到最近的一个面 if (disX < disY && disX < disZ) { // 沿X轴 if (n.x > 0) { closest.x = extents.x; } else { closest.x = -extents.x; } } else if (disY < disX && disY < disZ) { // 沿Y轴 if (n.y > 0) { closest.y = extents.y; } else { closest.y = -extents.y; } } else { // 沿Z轴 if (n.z > 0) { closest.z = extents.z; } else { closest.z = -extents.z; } } } var dir = n - closest; var sqrDist = dir.sqrMagnitude; var space = dst.Radius; var sqrSpace = space * space; if (sqrDist < sqrSpace || inside) { var dist = Mathf.Sqrt(sqrDist); var normal = (src.Quaternion * dir).normalized; var penetration = space - dist; if (inside) { normal = -normal; penetration = space + dist; } if (normal == Vector3.zero) { normal = Vector3.up; } contact = new XContact(src, dst, normal, penetration); return(true); } contact = null; return(false); }
public static bool Intersect(CubeXCollider src, CylinderXCollider dst, out XContact?contact) { var invP = Quaternion.Inverse(src.Quaternion) * (dst.Position - src.Position); Vector3 n = Vector3.zero; n.x = invP.x; n.z = invP.z; var extents = src.Size * 0.5f; var halfHa = extents.y; var topA = halfHa; var bottomA = -halfHa; var halfHb = dst.Height * 0.5f; var topB = invP.y + halfHb; var bottomB = invP.y - halfHb; var space = dst.Radius; var sqrSpace = space * space; // 相撞时,俯视图下的圆和矩形必然相交 var closest = n; closest.x = Mathf.Clamp(closest.x, -extents.x, extents.x); closest.z = Mathf.Clamp(closest.z, -extents.z, extents.z); if ((n - closest).sqrMagnitude > sqrSpace) { contact = null; return(false); } // 处理相交的情况 Vector3 normal; float penetration; float verticalP = float.PositiveInfinity; float horizontalP; var inside = false; if (n == closest) { inside = true; var disX = extents.x - Mathf.Abs(n.x); var disZ = extents.z - Mathf.Abs(n.z); //找到最近的一个面 if (disX < disZ) { // 沿X轴 if (n.x > 0) { closest.x = extents.x; } else { closest.x = -extents.x; } } else { // 沿Z轴 if (n.z > 0) { closest.z = extents.z; } else { closest.z = -extents.z; } } horizontalP = space + (n - closest).magnitude; } else { horizontalP = space - (n - closest).magnitude; } if (Mathf.Sign(topA - topB) != Mathf.Sign(bottomB - bottomA)) { // 斜向相撞 if (topB > topA) { verticalP = topA - bottomB; } else { verticalP = topB - bottomA; } } if (horizontalP < verticalP) { normal = (src.Quaternion * (n - closest)).normalized; if (inside) { normal = -normal; } penetration = horizontalP; } else { normal = topB > topA ? Vector3.up : Vector3.down; penetration = verticalP; } if (normal == Vector3.zero) { normal = Vector3.up; } contact = new XContact(src, dst, normal, penetration); return(true); }