From be8add5d505b14fcdc5d2caf6dc4bb345f057a7c Mon Sep 17 00:00:00 2001 From: Germano Date: Fri, 11 May 2018 20:20:51 -0300 Subject: Refactor: Move functions to get the distance to a projected aabb to BLI_match and remove confusing parameters. --- source/blender/blenlib/BLI_math_geom.h | 21 +++ source/blender/blenlib/intern/math_geom.c | 235 ++++++++++++++++++++++++++++++ 2 files changed, 256 insertions(+) (limited to 'source/blender/blenlib') diff --git a/source/blender/blenlib/BLI_math_geom.h b/source/blender/blenlib/BLI_math_geom.h index ff80d15ea5d..d4112254fcf 100644 --- a/source/blender/blenlib/BLI_math_geom.h +++ b/source/blender/blenlib/BLI_math_geom.h @@ -140,6 +140,27 @@ float dist_squared_ray_to_aabb_v3_simple( const float bb_min[3], const float bb_max[3], float r_point[3], float *r_depth); +struct DistProjectedAABBPrecalc { + float ray_origin[3]; + float ray_direction[3]; + float ray_inv_dir[3]; + + float pmat[4][4]; + + float mval[2]; + bool sign[3]; +}; +void dist_squared_to_projected_aabb_precalc( + struct DistProjectedAABBPrecalc *neasrest_precalc, + const float projmat[4][4], const float winsize[2], const float mval[2]); +float dist_squared_to_projected_aabb( + struct DistProjectedAABBPrecalc *data, + const float bbmin[3], const float bbmax[3], + bool r_axis_closest[3]); +float dist_squared_to_projected_aabb_simple( + const float projmat[4][4], const float winsize[2], const float mval[2], + const float bbmin[3], const float bbmax[3]); + float closest_to_line_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2]); float closest_to_line_v3(float r_close[3], const float p[3], const float l1[3], const float l2[3]); void closest_to_line_segment_v2(float r_close[2], const float p[2], const float l1[2], const float l2[2]); diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c index ee6a3dcc9b3..582ecb92274 100644 --- a/source/blender/blenlib/intern/math_geom.c +++ b/source/blender/blenlib/intern/math_geom.c @@ -765,6 +765,241 @@ float dist_squared_ray_to_aabb_v3_simple( /** \} */ +/* -------------------------------------------------------------------- */ +/** \name dist_squared_to_projected_aabb and helpers +* \{ */ + +/** + * \param projmat: Projection Matrix (usually perspective + * matrix multiplied by object matrix). + */ +void dist_squared_to_projected_aabb_precalc( + struct DistProjectedAABBPrecalc *neasrest_precalc, + const float projmat[4][4], const float winsize[2], const float mval[2]) +{ + float relative_mval[2] = { + 2 * mval[0] / winsize[0] - 1.0f, + 2 * mval[1] / winsize[1] - 1.0f, + }; + + float px[4], py[4]; + px[0] = projmat[0][0] - projmat[0][3] * relative_mval[0]; + px[1] = projmat[1][0] - projmat[1][3] * relative_mval[0]; + px[2] = projmat[2][0] - projmat[2][3] * relative_mval[0]; + px[3] = projmat[3][0] - projmat[3][3] * relative_mval[0]; + + py[0] = projmat[0][1] - projmat[0][3] * relative_mval[1]; + py[1] = projmat[1][1] - projmat[1][3] * relative_mval[1]; + py[2] = projmat[2][1] - projmat[2][3] * relative_mval[1]; + py[3] = projmat[3][1] - projmat[3][3] * relative_mval[1]; + +#if 0 + if (!isect_plane_plane_plane_v3( + projmat[0], projmat[1], projmat[3], neasrest_precalc->ray_origin)) + { + /* Orthographic projection. */ + copy_v3_v3(neasrest_precalc->ray_direction, projmat[3]); + } + else { + /* Perspective projection. */ + cross_v3_v3v3(neasrest_precalc->ray_direction, py, px); + //normalize_v3(neasrest_precalc->ray_direction); + } +#else + isect_plane_plane_v3( + px, py, + neasrest_precalc->ray_origin, + neasrest_precalc->ray_direction); +#endif + float win_half[2]; + mul_v2_v2fl(win_half, winsize, 0.5f); + + copy_v2_v2(neasrest_precalc->mval, mval); + sub_v2_v2(neasrest_precalc->mval, win_half); + + copy_m4_m4(neasrest_precalc->pmat, projmat); + + neasrest_precalc->pmat[0][0] *= win_half[0]; + neasrest_precalc->pmat[1][0] *= win_half[0]; + neasrest_precalc->pmat[2][0] *= win_half[0]; + neasrest_precalc->pmat[3][0] *= win_half[0]; + + neasrest_precalc->pmat[0][1] *= win_half[1]; + neasrest_precalc->pmat[1][1] *= win_half[1]; + neasrest_precalc->pmat[2][1] *= win_half[1]; + neasrest_precalc->pmat[3][1] *= win_half[1]; + + for (int i = 0; i < 3; i++) { + neasrest_precalc->ray_inv_dir[i] = + (neasrest_precalc->ray_direction[i] != 0.0f) ? + (1.0f / neasrest_precalc->ray_direction[i]) : FLT_MAX; + neasrest_precalc->sign[i] = (neasrest_precalc->ray_inv_dir[i] < 0.0f); + } +} + +/* Returns the distance from a 2d coordinate to a BoundBox (Projected) */ +float dist_squared_to_projected_aabb( + struct DistProjectedAABBPrecalc *data, + const float bbmin[3], const float bbmax[3], + bool r_axis_closest[3]) +{ + float local_bvmin[3], local_bvmax[3]; + if (data->sign[0]) { + local_bvmin[0] = bbmax[0]; + local_bvmax[0] = bbmin[0]; + } + else { + local_bvmin[0] = bbmin[0]; + local_bvmax[0] = bbmax[0]; + } + if (data->sign[1]) { + local_bvmin[1] = bbmax[1]; + local_bvmax[1] = bbmin[1]; + } + else { + local_bvmin[1] = bbmin[1]; + local_bvmax[1] = bbmax[1]; + } + if (data->sign[2]) { + local_bvmin[2] = bbmax[2]; + local_bvmax[2] = bbmin[2]; + } + else { + local_bvmin[2] = bbmin[2]; + local_bvmax[2] = bbmax[2]; + } + + const float tmin[3] = { + (local_bvmin[0] - data->ray_origin[0]) * data->ray_inv_dir[0], + (local_bvmin[1] - data->ray_origin[1]) * data->ray_inv_dir[1], + (local_bvmin[2] - data->ray_origin[2]) * data->ray_inv_dir[2], + }; + const float tmax[3] = { + (local_bvmax[0] - data->ray_origin[0]) * data->ray_inv_dir[0], + (local_bvmax[1] - data->ray_origin[1]) * data->ray_inv_dir[1], + (local_bvmax[2] - data->ray_origin[2]) * data->ray_inv_dir[2], + }; + /* `va` and `vb` are the coordinates of the AABB edge closest to the ray */ + float va[3], vb[3]; + /* `rtmin` and `rtmax` are the minimum and maximum distances of the ray hits on the AABB */ + float rtmin, rtmax; + int main_axis; + + if ((tmax[0] <= tmax[1]) && (tmax[0] <= tmax[2])) { + rtmax = tmax[0]; + va[0] = vb[0] = local_bvmax[0]; + main_axis = 3; + r_axis_closest[0] = data->sign[0]; + } + else if ((tmax[1] <= tmax[0]) && (tmax[1] <= tmax[2])) { + rtmax = tmax[1]; + va[1] = vb[1] = local_bvmax[1]; + main_axis = 2; + r_axis_closest[1] = data->sign[1]; + } + else { + rtmax = tmax[2]; + va[2] = vb[2] = local_bvmax[2]; + main_axis = 1; + r_axis_closest[2] = data->sign[2]; + } + + if ((tmin[0] >= tmin[1]) && (tmin[0] >= tmin[2])) { + rtmin = tmin[0]; + va[0] = vb[0] = local_bvmin[0]; + main_axis -= 3; + r_axis_closest[0] = !data->sign[0]; + } + else if ((tmin[1] >= tmin[0]) && (tmin[1] >= tmin[2])) { + rtmin = tmin[1]; + va[1] = vb[1] = local_bvmin[1]; + main_axis -= 1; + r_axis_closest[1] = !data->sign[1]; + } + else { + rtmin = tmin[2]; + va[2] = vb[2] = local_bvmin[2]; + main_axis -= 2; + r_axis_closest[2] = !data->sign[2]; + } + if (main_axis < 0) { + main_axis += 3; + } + + /* if rtmin <= rtmax, ray intersect `AABB` */ + if (rtmin <= rtmax) { + return 0; + } + + if (data->sign[main_axis]) { + va[main_axis] = local_bvmax[main_axis]; + vb[main_axis] = local_bvmin[main_axis]; + } + else { + va[main_axis] = local_bvmin[main_axis]; + vb[main_axis] = local_bvmax[main_axis]; + } + float scale = fabsf(local_bvmax[main_axis] - local_bvmin[main_axis]); + + float (*pmat)[4] = data->pmat; + + float va2d[2] = { + (dot_m4_v3_row_x(pmat, va) + pmat[3][0]), + (dot_m4_v3_row_y(pmat, va) + pmat[3][1]), + }; + float vb2d[2] = { + (va2d[0] + pmat[main_axis][0] * scale), + (va2d[1] + pmat[main_axis][1] * scale), + }; + + float w_a = mul_project_m4_v3_zfac(pmat, va); + float w_b = w_a + pmat[main_axis][3] * scale; + va2d[0] /= w_a; + va2d[1] /= w_a; + vb2d[0] /= w_b; + vb2d[1] /= w_b; + + float dvec[2], edge[2], lambda, rdist_sq; + sub_v2_v2v2(dvec, data->mval, va2d); + sub_v2_v2v2(edge, vb2d, va2d); + lambda = dot_v2v2(dvec, edge); + if (lambda != 0.0f) { + lambda /= len_squared_v2(edge); + if (lambda <= 0.0f) { + rdist_sq = len_squared_v2v2(data->mval, va2d); + r_axis_closest[main_axis] = true; + } + else if (lambda >= 1.0f) { + rdist_sq = len_squared_v2v2(data->mval, vb2d); + r_axis_closest[main_axis] = false; + } + else { + va2d[0] += edge[0] * lambda; + va2d[1] += edge[1] * lambda; + rdist_sq = len_squared_v2v2(data->mval, va2d); + r_axis_closest[main_axis] = lambda < 0.5f; + } + } + else { + rdist_sq = len_squared_v2v2(data->mval, va2d); + } + + return rdist_sq; +} + +float dist_squared_to_projected_aabb_simple( + const float projmat[4][4], const float winsize[2], const float mval[2], + const float bbmin[3], const float bbmax[3]) +{ + struct DistProjectedAABBPrecalc data; + dist_squared_to_projected_aabb_precalc(&data, projmat, winsize, mval); + + bool dummy[3] = {true, true, true}; + return dist_squared_to_projected_aabb(&data, bbmin, bbmax, dummy); +} +/** \} */ + + /* Adapted from "Real-Time Collision Detection" by Christer Ericson, * published by Morgan Kaufmann Publishers, copyright 2005 Elsevier Inc. * -- cgit v1.2.3