public void AddCapsule(CapsuleDesc capsule, RigidbodyDesc rigidbodyDesc, int entityId)
{
_capsules.Add(new RigidbodyColliderDesc <CapsuleDesc>()
{
entityId = entityId,
collider = capsule,
rigidbody = rigidbodyDesc,
});
}
/// <summary>
/// 检查点是否在胶囊体内
/// </summary>
public static bool PointInside(float3 p, CapsuleDesc capsule)
{
var o = capsule.c0; //c0作为原点
var radius = capsule.radius;
var axis = capsule.axis;
p = p - o;
var proj = math.dot(p, axis.xyz); //p点在轴上的投影
if (proj < -radius || proj > axis.w + radius)
{
return(false);
}
var r2 = radius * radius;
if (proj >= 0 && proj <= axis.w)
{
//轴上投影在圆柱体之间
var dist2 = math.dot(p, p) - proj * proj; //计算p到轴的垂直距离平方
return(dist2 < r2);
}
if (proj >= -radius && proj < 0)
{
//轴上投影处于原点附近
return(math.dot(p, p) < r2);
}
if (proj <= axis.w + radius)
{
//轴上投影处于另一头附近
var v = p - (axis.xyz * axis.w);
return(math.dot(v, v) < r2);
}
return(false);
}
public static bool GetClosestSurfacePoint(float3 p, CapsuleDesc capsule, out ConcatInfo concatInfo)
{
concatInfo = default(ConcatInfo);
var o = capsule.c0; //c0作为原点
var radius = capsule.radius;
var axis = capsule.axis;
p = p - o;
var proj = math.dot(p, axis.xyz); //p点在轴上的投影
if (proj < -radius || proj > axis.w + radius)
{
return(false);
}
var r2 = radius * radius;
if (proj >= 0 && proj <= axis.w)
{
//轴上投影在圆柱体之间
var dist2 = math.dot(p, p) - proj * proj; //计算p到轴的垂直距离平方
if (dist2 < r2)
{
var q = axis.xyz * proj;
concatInfo.normal = math.normalize(p - q);
concatInfo.position = o + q + concatInfo.normal * radius;
return(true);
}
else
{
return(false);
}
}
if (proj >= -radius && proj < 0)
{
//轴上投影处于原点附近
var c2p = p;
if (math.dot(c2p, c2p) < r2)
{
concatInfo.normal = math.normalize(c2p);
concatInfo.position = o + radius * concatInfo.normal;
return(true);
}
else
{
return(false);
}
}
if (proj <= axis.w + radius)
{
//轴上投影处于另一头附近
var c = (axis.xyz * axis.w);
var c2p = p - c;
if (math.dot(c2p, c2p) < r2)
{
concatInfo.normal = math.normalize(c2p);
concatInfo.position = o + c + radius * concatInfo.normal;
return(true);
}
else
{
return(false);
}
}
return(false);
}
