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);
}
public virtual bool GetConcatInfo(float3 fromPosition, float3 predictPosition, out ConcatInfo concatInfo)
{
if (this.GetClosestSurfacePoint(fromPosition, out concatInfo))
{
concatInfo.position += concatInfo.normal * concatOffset;
return(true);
}
else if (this.GetClosestSurfacePoint(predictPosition, out concatInfo))
{
concatInfo.position += concatInfo.normal * concatOffset;
return(true);
}
concatInfo = default;
return(false);
}
public virtual bool GetClosestSurfacePoint(float3 p, out ConcatInfo closestPoint)
{
closestPoint = default(ConcatInfo);
return(false);
}
public override bool GetClosestSurfacePoint(float3 p, out ConcatInfo concatInfo)
{
return(IntersectUtil.GetClosestSurfacePoint(p, desc, out concatInfo));
}