public static bool Intersect(SphereXCollider src, CylinderXCollider dst, out XContact?contact)
{
return(Intersect(dst, src, out contact));
}
// 没有处理在内部的情况
public static bool Intersect(CylinderXCollider src, CylinderXCollider dst, out XContact?contact)
{
var space = src.Radius + dst.Radius;
var sqrSpace = space * space;
Vector3 n = Vector3.zero;
n.x = dst.Position.x - src.Position.x;
n.z = dst.Position.z - src.Position.z;
if (n.sqrMagnitude > sqrSpace)
{
contact = null;
return(false);
}
var halfHa = src.Height * 0.5f;
var topA = src.Position.y + halfHa;
var bottomA = src.Position.y - halfHa;
var halfHb = dst.Height * 0.5f;
var topB = dst.Position.y + halfHb;
var bottomB = dst.Position.y - halfHb;
Vector3 normal;
float penetration;
if (Mathf.Sign(topA - topB) == Mathf.Sign(bottomB - bottomA))
{
// 水平相撞
normal = n.normalized;
penetration = space - n.magnitude;
}
else if (n.sqrMagnitude < Mathf.Pow(dst.Radius - src.Radius, 2f))
{
// 垂直相撞
if (dst.Position.y > src.Position.y)
{
normal = Vector3.up;
penetration = topA - bottomB;
}
else
{
normal = Vector3.down;
penetration = topB - bottomA;
}
}
else
{
var horizontalP = space - n.magnitude;
float verticalP;
// 斜向相撞
if (topB > topA)
{
verticalP = topA - bottomB;
}
else
{
verticalP = topB - bottomA;
}
if (horizontalP < verticalP)
{
normal = n.normalized;
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);
}
public static bool Intersect(CylinderXCollider src, SphereXCollider dst, out XContact?contact)
{
var n = dst.Position - src.Position;
Vector3 closest = n;
var extents = src.bounds.Extents;
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 v = new Vector2(closest.x, closest.z);
if (v.sqrMagnitude > src.Radius * src.Radius)
{
v = v.normalized * src.Radius;
closest.x = v.x;
closest.z = v.y;
}
bool inside = false;
if (n == closest)
{
inside = true;
// 往上下移动的最短距离
var dist1 = extents.y - Mathf.Abs(closest.y);
// 往水平四周移动的最短距离
var dist2 = src.Radius - v.magnitude;
if (dist1 < dist2)
{
closest.y = closest.y > 0 ? extents.y : -extents.y;
}
else
{
v = v.normalized * src.Radius;
closest.x = v.x;
closest.z = v.y;
}
}
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 = 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);
}
