From bfabb9d3c5057a5eb5796ba8457990a80e1a74e5 Mon Sep 17 00:00:00 2001 From: Germano Cavalcante Date: Mon, 25 Jan 2016 18:17:45 +1100 Subject: Math Lib: Add dist_squared_ray_to_aabb_v3 utility --- source/blender/blenlib/intern/math_geom.c | 223 ++++++++++++++++++++++++++++++ 1 file changed, 223 insertions(+) (limited to 'source/blender/blenlib/intern/math_geom.c') 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) */ -- cgit v1.2.3