Find the closest point inside a rotated BoxShape towards another point outside.

:information_source: Attention Topic was automatically imported from the old Question2Answer platform.
:bust_in_silhouette: Asked By Kazuren

I have a rotated BoxShape and given a point outside the shape, I would like to find the closest point that lies on the shape towards that point.

I have developed a method that can do this for non rotated boxes but I am struggling with the math required to do this for a rotated box.

public static Vector3 GetClosestPoint(this BoxShape box, Transform boxTransform, Vector3 p)
{
  Vector3 worldPoint = Vector3.Zero;

  float boxX1 = boxTransform.origin.x - box.Extents.x;
  float boxX2 = boxTransform.origin.x + box.Extents.x;

  float boxY1 = boxTransform.origin.y - box.Extents.y;
  float boxY2 = boxTransform.origin.y + box.Extents.y;

  float boxZ1 = boxTransform.origin.z - box.Extents.z;
  float boxZ2 = boxTransform.origin.z + box.Extents.z;

  worldPoint.x = Mathf.Clamp(p.x, boxX1, boxX2);
  worldPoint.y = Mathf.Clamp(p.y, boxY1, boxY2);
  worldPoint.z = Mathf.Clamp(p.z, boxZ1, boxZ2);

  return worldPoint;
}

Solutions I found online say to make the box axis-aligned first but I am unsure how to go about doing that in Godot.

Here are some stack exchange links discussing this:

I am using C# but solutions in GDScript are welcome as I can easily translate between the two.

Just as an off-the-wall suggestion, you can probably also do this with a temporary Area2D and a RayCast2D

jgodfrey | 2022-11-23 17:45

Where would I cast the ray towards though? If I cast it towards the box origin then that wouldn’t necessarily get me the closest point and the longer the box the more inaccurate it’s going to be.

Kazuren | 2022-11-23 17:56

Re-reading the above, I think I misunderstood what you’re trying to do. And, you’re right. I don’t think my suggestion is helpful here… :frowning:

Here’s another SO hit for your trouble though… :slight_smile:

algorithm - How to calculate the distance from a point to the nearest point of a rectange? - Stack Overflow

jgodfrey | 2022-11-23 19:59

I realize I’m continuing to be unhelpful here as I was assuming 2D, but now I realize you’re working with 3D data…

One point of clarification… I assume you’re looking for the closest point that lies anywhere on the surface of your BoxShape, right? That is, you’re not just looking for the closest existing point from the BoxShape data, right?

jgodfrey | 2022-11-23 20:34

Yes, the closest point could be anywhere on the corner, edge or face of the box or even inside the box itself if the point we’re inputting is inside the box itself.

Kazuren | 2022-11-23 20:45

:bust_in_silhouette: Reply From: Kazuren

I managed to solve this thanks to this Stack Overflow question/answer algorithm - How to find the closest point on a right rectangular prism ( 3d rectangle ) - Stack Overflow

Solution as depicted in the link above: “Project the point onto each independent axis of the 3D rectangle to find the scalar parameters of the projection. Then saturate the scalar parameters at the limit of the faces. Then sum the components to get the answer”

Here’s the working code.

public static Vector3 GetClosestPoint(this BoxShape box, Transform boxTransform, Vector3 p)
{
  Vector3 origin = boxTransform.Xform(new Vector3(-box.Extents.x, -box.Extents.y, -box.Extents.z));

  Vector3 px = boxTransform.Xform(new Vector3(box.Extents.x, -box.Extents.y, -box.Extents.z));
  Vector3 py = boxTransform.Xform(new Vector3(-box.Extents.x, box.Extents.y, -box.Extents.z));
  Vector3 pz = boxTransform.Xform(new Vector3(-box.Extents.x, -box.Extents.y, box.Extents.z));

  Vector3 vx = (px - origin);
  Vector3 vy = (py - origin);
  Vector3 vz = (pz - origin);

  var tx = vx.Dot(p - origin) / vx.LengthSquared();
  var ty = vy.Dot(p - origin) / vy.LengthSquared();
  var tz = vz.Dot(p - origin) / vz.LengthSquared();

  tx = tx < 0 ? 0 : tx > 1 ? 1 : tx;
  ty = ty < 0 ? 0 : ty > 1 ? 1 : ty;
  tz = tz < 0 ? 0 : tz > 1 ? 1 : tz;

  Vector3 worldPoint = tx * vx + ty * vy + tz * vz + origin;

  return worldPoint;
}