Math Lib: Add dist_squared_ray_to_aabb_v3 utility

1 files changed, 223 insertions, 0 deletions

diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c
index a104487f8d1..dcc3e85a982 100644
--- a/source/blender/blenlib/intern/math_geom.c
+++ b/source/blender/blenlib/intern/math_geom.c
@@ -2254,6 +2254,229 @@ bool isect_ray_aabb_v3(
 	return true;
 }
 
+void dist_squared_ray_to_aabb_v3_precalc(
+        struct NearestRayToAABB_Precalc *data,
+        const float ray_origin[3], const float ray_direction[3])
+{
+	float dir_sq[3];
+
+	for (int i = 0; i < 3; i++) {
+		data->ray_origin[i] = ray_origin[i];
+		data->ray_dot_axis[i] = ray_direction[i];
+		data->idot_axis[i] = (data->ray_dot_axis[i] != 0.0f) ? (1.0f / data->ray_dot_axis[i]) : FLT_MAX;
+		/* It has to be a function of `idot_axis`,
+		 * since the division of 1 by 0.0f, can be -inf or +inf */
+		data->sign[i] = (data->idot_axis[i] < 0.0f);
+
+		dir_sq[i] = SQUARE(data->ray_dot_axis[i]);
+	}
+
+	/* `diag_sq` Length square of each face diagonal */
+	float diag_sq[3] = {
+		dir_sq[1] + dir_sq[2],
+		dir_sq[0] + dir_sq[2],
+		dir_sq[0] + dir_sq[1],
+	};
+	data->idiag_sq[0] = (diag_sq[0] > FLT_EPSILON) ? (1.0f / diag_sq[0]) : FLT_MAX;
+	data->idiag_sq[1] = (diag_sq[1] > FLT_EPSILON) ? (1.0f / diag_sq[1]) : FLT_MAX;
+	data->idiag_sq[2] = (diag_sq[2] > FLT_EPSILON) ? (1.0f / diag_sq[2]) : FLT_MAX;
+
+	data->cdot_axis[0] = data->ray_dot_axis[0] * data->idiag_sq[0];
+	data->cdot_axis[1] = data->ray_dot_axis[1] * data->idiag_sq[1];
+	data->cdot_axis[2] = data->ray_dot_axis[2] * data->idiag_sq[2];
+}
+
+/**
+ * Returns the squared distance from a ray to a bound-box `AABB`.
+ * It is based on `fast_ray_nearest_hit` solution to obtain
+ * the coordinates of the nearest edge of Bound Box to the ray
+ */
+float dist_squared_ray_to_aabb_v3(
+        const struct NearestRayToAABB_Precalc *data,
+        const float bb_min[3], const float bb_max[3],
+        bool r_axis_closest[3])
+{
+	/* `tmin` is a vector that has the smaller distances to each of the
+	 * infinite planes of the `AABB` faces (hit in nearest face X plane,
+	 * nearest face Y plane and nearest face Z plane) */
+	float local_bvmin[3], local_bvmax[3];
+
+	if (data->sign[0] == 0) {
+		local_bvmin[0] = bb_min[0] - data->ray_origin[0];
+		local_bvmax[0] = bb_max[0] - data->ray_origin[0];
+	}
+	else {
+		local_bvmin[0] = bb_max[0] - data->ray_origin[0];
+		local_bvmax[0] = bb_min[0] - data->ray_origin[0];
+	}
+
+	if (data->sign[1] == 0) {
+		local_bvmin[1] = bb_min[1] - data->ray_origin[1];
+		local_bvmax[1] = bb_max[1] - data->ray_origin[1];
+	}
+	else {
+		local_bvmin[1] = bb_max[1] - data->ray_origin[1];
+		local_bvmax[1] = bb_min[1] - data->ray_origin[1];
+	}
+
+	if (data->sign[2] == 0) {
+		local_bvmin[2] = bb_min[2] - data->ray_origin[2];
+		local_bvmax[2] = bb_max[2] - data->ray_origin[2];
+	}
+	else {
+		local_bvmin[2] = bb_max[2] - data->ray_origin[2];
+		local_bvmax[2] = bb_min[2] - data->ray_origin[2];
+	}
+
+	const float tmin[3] = {
+		local_bvmin[0] * data->idot_axis[0],
+		local_bvmin[1] * data->idot_axis[1],
+		local_bvmin[2] * data->idot_axis[2],
+	};
+
+	/* `tmax` is a vector that has the longer distances to each of the
+	 * infinite planes of the `AABB` faces (hit in farthest face X plane,
+	 * farthest face Y plane and farthest face Z plane) */
+	const float tmax[3] = {
+		local_bvmax[0] * data->idot_axis[0],
+		local_bvmax[1] * data->idot_axis[1],
+		local_bvmax[2] * data->idot_axis[2],
+	};
+	/* `v1` and `v3` is be the coordinates of the nearest `AABB` edge to the ray*/
+	float v1[3], v2[3];
+	/* `rtmin` is the highest value of the smaller distances. == max_axis_v3(tmin)
+	 * `rtmax` is the lowest value of longer distances. == min_axis_v3(tmax)*/
+	float rtmin, rtmax, mul, rdist;
+	/* `main_axis` is the axis equivalent to edge close to the ray;
+	 * `id_rate` is index of `data->project_rate`. Each edge has a proportional distance to
+	 * the distance between tmin and tmax. This proportion is stored in `data->project_rate",
+	 * getting this value, you can get the distance between the origin and
+	 * the nearest point on the ray to the edge:
+	 * `(rtmax + data->project_rate[face][id_rate] * (rtmin - rtmax))` */
+	int main_axis;
+
+	r_axis_closest[0] = false;
+	r_axis_closest[1] = false;
+	r_axis_closest[2] = false;
+
+	/* *** min_axis_v3(tmax) *** */
+	if ((tmax[0] <= tmax[1]) && (tmax[0] <= tmax[2])) {
+		// printf("# Hit in X %s\n", data->sign[0] ? "min", "max");
+		rtmax = tmax[0];
+		v1[0] = v2[0] = local_bvmax[0];
+		mul = local_bvmax[0] * data->ray_dot_axis[0];
+		main_axis = 3;
+		r_axis_closest[0] = data->sign[0];
+	}
+	else if ((tmax[1] <= tmax[0]) && (tmax[1] <= tmax[2])) {
+		// printf("# Hit in Y %s\n", data->sign[0] ? "min", "max");
+		rtmax = tmax[1];
+		v1[1] = v2[1] = local_bvmax[1];
+		mul = local_bvmax[1] * data->ray_dot_axis[1];
+		main_axis = 2;
+		r_axis_closest[1] = data->sign[1];
+	}
+	else {
+		// printf("# Hit in Z %s\n", data->sign[0] ? "min", "max");
+		rtmax = tmax[2];
+		v1[2] = v2[2] = local_bvmax[2];
+		mul = local_bvmax[2] * data->ray_dot_axis[2];
+		main_axis = 1;
+		r_axis_closest[2] = data->sign[2];
+	}
+
+	/* *** max_axis_v3(tmin) *** */
+	if ((tmin[0] >= tmin[1]) && (tmin[0] >= tmin[2])) {
+		// printf("# To X %s\n", data->sign[0] ? "min", "max");
+		rtmin = tmin[0];
+		v1[0] = v2[0] = local_bvmin[0];
+		mul += local_bvmin[0] * data->ray_dot_axis[0];
+		main_axis -= 3;
+		r_axis_closest[0] = !data->sign[0];
+	}
+	else if ((tmin[1] >= tmin[0]) && (tmin[1] >= tmin[2])) {
+		// printf("# To Y %s\n", data->sign[0] ? "min", "max");
+		rtmin = tmin[1];
+		v1[1] = v2[1] = local_bvmin[1];
+		mul += local_bvmin[1] * data->ray_dot_axis[1];
+		main_axis -= 1;
+		r_axis_closest[1] = !data->sign[1];
+	}
+	else {
+		// printf("# To Z %s\n", data->sign[0] ? "min", "max");
+		rtmin = tmin[2];
+		v1[2] = v2[2] = local_bvmin[2];
+		mul += local_bvmin[2] * data->ray_dot_axis[2];
+		main_axis -= 2;
+		r_axis_closest[2] = !data->sign[2];
+	}
+	/* *** end min/max axis *** */
+
+
+	/* `if rtmax < 0`, the whole `AABB` is behing us */
+	if ((rtmax < 0.0f) && (rtmin < 0.0f)) {
+		return FLT_MAX;
+	}
+
+	if (main_axis < 0) {
+		main_axis += 3;
+	}
+
+	if (data->sign[main_axis] == 0) {
+		v1[main_axis] = local_bvmin[main_axis];
+		v2[main_axis] = local_bvmax[main_axis];
+	}
+	else {
+		v1[main_axis] = local_bvmax[main_axis];
+		v2[main_axis] = local_bvmin[main_axis];
+	}
+
+	/* if rtmin < rtmax, ray intersect `AABB` */
+	if (rtmin <= rtmax) {
+		const float proj = rtmin * data->ray_dot_axis[main_axis];
+		rdist = 0.0f;
+		r_axis_closest[main_axis] = (proj - v1[main_axis]) < (v2[main_axis] - proj);
+	}
+	else {
+		/* `proj` equals to nearest point on the ray closest to the edge `v1 v2` of the `AABB`. */
+		const float proj = mul * data->cdot_axis[main_axis];
+		float depth;
+		if (v1[main_axis] > proj) {  /* the nearest point to the ray is the point v1 */
+			/* `depth` is equivalent the distance from the origin to the point v1,
+			 * Here's a faster way to calculate the dot product of v1 and ray
+			 * (depth = dot_v3v3(v1, data->ray.direction))*/
+			depth = mul + data->ray_dot_axis[main_axis] * v1[main_axis];
+			rdist = len_squared_v3(v1) - SQUARE(depth);
+			r_axis_closest[main_axis] = true;
+		}
+		else if (v2[main_axis] < proj) {  /* the nearest point of the ray is the point v2 */
+			depth = mul + data->ray_dot_axis[main_axis] * v2[main_axis];
+			rdist = len_squared_v3(v2) - SQUARE(depth);
+			r_axis_closest[main_axis] = false;
+		}
+		else {  /* the nearest point of the ray is on the edge of the `AABB`. */
+			float v[2];
+			mul *= data->idiag_sq[main_axis];
+			if (main_axis == 0) {
+				v[0] = (mul * data->ray_dot_axis[1]) - v1[1];
+				v[1] = (mul * data->ray_dot_axis[2]) - v1[2];
+			}
+			else if (main_axis == 1) {
+				v[0] = (mul * data->ray_dot_axis[0]) - v1[0];
+				v[1] = (mul * data->ray_dot_axis[2]) - v1[2];
+			}
+			else {
+				v[0] = (mul * data->ray_dot_axis[0]) - v1[0];
+				v[1] = (mul * data->ray_dot_axis[1]) - v1[1];
+			}
+			rdist = len_squared_v2(v);
+			r_axis_closest[main_axis] = (proj - v1[main_axis]) < (v2[main_axis] - proj);
+		}
+	}
+
+	return rdist;
+}
+
 /* find closest point to p on line through (l1, l2) and return lambda,
  * where (0 <= lambda <= 1) when cp is in the line segment (l1, l2)
  */