public void AddBox(BoxDesc box, RigidbodyDesc rigidbodyDesc, int entityId)
{
_boxes.Add(new RigidbodyColliderDesc <BoxDesc>()
{
entityId = entityId,
collider = box,
rigidbody = rigidbodyDesc,
});
}
/// <summary>
/// 检查点是否在Box内
/// </summary>
public static bool PointInside(float3 p, BoxDesc box)
{
var o = box.min; //min作为原点
p = p - o;
float3 projOnAxis = new float3(
math.dot(p, box.ax.xyz),
math.dot(p, box.ay.xyz),
math.dot(p, box.az.xyz)
);
float3 axisLength = new float3(box.ax.w, box.ay.w, box.az.w);
return(math.all(projOnAxis > 0) && math.all(projOnAxis < axisLength));
}
/// <summary>
/// 获取box表面离p最近的点。
/// 返回值表示p是否在box内部
/// </summary>
public static bool GetClosestSurfacePoint(float3 p, BoxDesc box, out ConcatInfo concatInfo)
{
var o = box.min; //min作为原点
p = p - o;
float3 projOnAxis = new float3(
math.dot(p, box.ax.xyz),
math.dot(p, box.ay.xyz),
math.dot(p, box.az.xyz)
);
float3 axisLength = new float3(box.ax.w, box.ay.w, box.az.w);
float3 side = math.step(axisLength * 0.5f, projOnAxis); // >0.5 => 1 | <0.5 => 0
float3 signedDist = (1 - side * 2) * (projOnAxis - side * axisLength);
bool inside = math.all(signedDist > 0);
if (inside)
{
var dst = signedDist.x;
var axis = box.ax;
var sideFlag = side.x;
var axisIndex = 0;
if (signedDist.y < dst)
{
dst = signedDist.y;
axisIndex = 1;
sideFlag = side.y;
axis = box.ay;
}
if (signedDist.z < dst)
{
dst = signedDist.z;
sideFlag = side.z;
axisIndex = 2;
axis = box.az;
}
concatInfo = new ConcatInfo();
concatInfo.normal = sideFlag == 1 ? axis.xyz : -axis.xyz;
var offset = (projOnAxis[axisIndex] - sideFlag * axis.w);
concatInfo.position = o + p - axis.xyz * offset;
return(true);
}
else
{
concatInfo = default;
return(false);
}
}