private void ResolveCollision(XContact contact)
{
var a = contact.src;
var b = contact.dst;
var normal = contact.normal;
float sv = Vector3.Dot((b.rigidbody.velocity - a.rigidbody.velocity), normal);
if (sv > 0)
{
return;
}
float e = (a.restitution + b.restitution) * 0.5f;
var tm = a.rigidbody.InverseMass + b.rigidbody.InverseMass;
if (tm == 0)
{
return;
}
float impulse = -(1 + e) * sv / tm;
a.rigidbody.velocity -= impulse * a.rigidbody.InverseMass * normal;
b.rigidbody.velocity += impulse * b.rigidbody.InverseMass * normal;
PositionalCorrection(contact);
}
private void PositionalCorrection(XContact contact)
{
var a = contact.src;
var b = contact.dst;
var tm = a.rigidbody.InverseMass + b.rigidbody.InverseMass;
if (tm == 0)
{
return;
}
float percent = 0.2f;
float slop = 0.01f;
Vector3 correction = Mathf.Max(contact.penetration - slop, 0) / tm * percent * contact.normal;
a.Position = a.rigidbody.position -= a.rigidbody.InverseMass * correction;
b.Position = b.rigidbody.position += b.rigidbody.InverseMass * correction;
}
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, SphereXCollider dst, out XContact?contact)
{
var dir = dst.Position - src.Position;
var sqrDist = dir.sqrMagnitude;
var space = src.Radius + dst.Radius;
var sqrSpace = space * space;
if (sqrDist < sqrSpace)
{
var dist = Mathf.Sqrt(sqrDist);
if (dist != 0)
{
contact = new XContact(src, dst, dir.normalized, space - dist);
}
else
{
contact = new XContact(src, dst, Vector3.up, src.Radius);
}
return(true);
}
contact = null;
return(false);
}
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(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(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);
}
