public Vector3 ClosestCornerTo(Vector3 worldPos)
{
var lp = LocalFromWorldPoint(worldPos);
var corner = PoseUtils.ScaleVector3(PoseUtils.Sign(lp), Primitive.vectorRadii);
return(WorldFromLocalPoint(corner));
}
public Vector3 LocalFromUnitPoint(Vector3 pnt)
{
return(PoseUtils.ScaleVector3(pnt, vectorRadii));
}
internal DistanceSample SampleDistanceVectorFromPointLocal(Vector3 localPos, ShapeSample shape)
{
if (verticesInXZ != null)
{
var polygon = verticesInXZ;
var nearestPoint = localPos;
if ((shape == ShapeSample.Fill) && GeometryUtils.PointInPolygon(localPos, polygon))
{
nearestPoint.y = 0.0f;
}
else
{
nearestPoint = ClosetPointOnPolygonEdge(localPos, polygon);
}
var toSurf = (nearestPoint - localPos);
return(new DistanceSample(toSurf.magnitude, toSurf));
}
if (primitiveType == FieldPrimitiveType.Box)
{
// Note: C0 not C1 continuous
if (shape == ShapeSample.Fill)
{
var nearest = PoseUtils.ScaleVector3(PoseUtils.Min(PoseUtils.Abs(localPos), vectorRadii), PoseUtils.Sign(localPos));
var b = PoseUtils.Abs(localPos) - (vectorRadii);
var dist = Mathf.Max(b.x, Mathf.Max(b.y, b.z));
if (dist < 0.0f)
{
b = vectorRadii - PoseUtils.Abs(localPos);
nearest = localPos;
var nearestTip = PoseUtils.ScaleVector3(vectorRadii, PoseUtils.Sign(localPos));
if (PoseUtils.IsXTheMin(b))
{
nearest.x = nearestTip.x;
}
else if (PoseUtils.IsYTheMin(b))
{
nearest.y = nearestTip.y;
}
else
{
nearest.z = nearestTip.z;
}
}
else
{
dist = (nearest - localPos).magnitude;
}
var toSurf = (nearest - localPos);
return(new DistanceSample(dist, toSurf));
}
else if (shape == ShapeSample.Edge)
{
var absLocal = PoseUtils.Abs(localPos);
var absUnit = PoseUtils.Abs(PoseUtils.InvScaleVector3(localPos, vectorRadii) - Vector3.one);
var absNearest = vectorRadii;
if (PoseUtils.IsXTheMin(absUnit))
{
absNearest = new Vector3(absLocal.x, vectorRadii.y, vectorRadii.z);
}
else if (PoseUtils.IsYTheMin(absUnit))
{
absNearest = new Vector3(vectorRadii.x, absLocal.y, vectorRadii.z);
}
else
{
absNearest = new Vector3(vectorRadii.x, vectorRadii.y, absLocal.z);
}
var nearest = PoseUtils.ScaleVector3(absNearest, PoseUtils.Sign(localPos));
var toSurf = (nearest - localPos);
var dist = toSurf.magnitude;
return(new DistanceSample(dist, toSurf));
}
else
{
throw new System.NotImplementedException("TODO:" + shape);
}
}
else if (primitiveType == FieldPrimitiveType.Sphere)
{
var nearest = PoseUtils.ScaleVector3(localPos.normalized, vectorRadii);
var dist = (localPos.magnitude - nearest.magnitude);
return(new DistanceSample(dist, nearest - localPos));
}
else if (primitiveType == FieldPrimitiveType.PlaneForward)
{
if (shape == ShapeSample.Edge)
{
throw new System.NotImplementedException("TODO: Plane+Edge");
}
Vector3 nearest;
var flipDistance = false;
if ((shape == ShapeSample.Fill) && (localPos.z >= 0))
{
nearest = new Vector3(
Mathf.Clamp(localPos.x, -vectorRadii.x, vectorRadii.x),
Mathf.Clamp(localPos.y, -vectorRadii.y, vectorRadii.y),
Mathf.Clamp(localPos.z, -vectorRadii.z, vectorRadii.z)
);
nearest.z = 0.0f;
}
else // if edge or if behind the plane
{
nearest = PoseUtils.ScaleVector3(vectorRadii, PoseUtils.Sign(localPos));
nearest.z = 0.0f;
}
var dist = (localPos.magnitude - nearest.magnitude) * (flipDistance ? -1.0f : 1.0f);
return(new DistanceSample(dist, nearest - localPos));
}
else if (primitiveType == FieldPrimitiveType.LineOnX)
{
var radius = vectorRadii.x;
var fx = Mathf.Clamp(localPos.x, -radius, radius);
var nearest = new Vector3(fx, 0, 0);
var dist = nearest.magnitude;
return(new DistanceSample(dist, nearest));
}
else
{
throw new System.NotImplementedException("TODO.");
}
}