diff options
Diffstat (limited to 'source/blender/blenlib/intern/math_geom.c')
-rw-r--r-- | source/blender/blenlib/intern/math_geom.c | 420 |
1 files changed, 354 insertions, 66 deletions
diff --git a/source/blender/blenlib/intern/math_geom.c b/source/blender/blenlib/intern/math_geom.c index a48d985faed..b89647d5b9b 100644 --- a/source/blender/blenlib/intern/math_geom.c +++ b/source/blender/blenlib/intern/math_geom.c @@ -570,6 +570,8 @@ float dist_squared_to_ray_v3( *r_depth = dot_v3v3(dvec, ray_direction); return len_squared_v3(dvec) - SQUARE(*r_depth); } + + /** * Find the closest point in a seg to a ray and return the distance squared. * \param r_point: Is the point on segment closest to ray (or to ray_origin if the ray and the segment are parallel). @@ -580,43 +582,68 @@ float dist_squared_ray_to_seg_v3( const float v0[3], const float v1[3], float r_point[3], float *r_depth) { - float a[3], t[3], n[3], lambda; - sub_v3_v3v3(a, v1, v0); - sub_v3_v3v3(t, v0, ray_origin); - cross_v3_v3v3(n, a, ray_direction); - const float nlen = len_squared_v3(n); - - /* if (nlen == 0.0f) the lines are parallel, - * has no nearest point, only distance squared.*/ - if (nlen == 0.0f) { - /* Calculate the distance to the point v0 then */ - copy_v3_v3(r_point, v0); - *r_depth = dot_v3v3(t, ray_direction); - } - else { - float c[3], cray[3]; - sub_v3_v3v3(c, n, t); - cross_v3_v3v3(cray, c, ray_direction); - lambda = dot_v3v3(cray, n) / nlen; - if (lambda <= 0) { + float lambda, depth; + if (isect_ray_seg_v3( + ray_origin, ray_direction, v0, v1, &lambda)) + { + if (lambda <= 0.0f) { copy_v3_v3(r_point, v0); - - *r_depth = dot_v3v3(t, ray_direction); } - else if (lambda >= 1) { + else if (lambda >= 1.0f) { copy_v3_v3(r_point, v1); - - sub_v3_v3v3(t, v1, ray_origin); - *r_depth = dot_v3v3(t, ray_direction); } else { - madd_v3_v3v3fl(r_point, v0, a, lambda); - - sub_v3_v3v3(t, r_point, ray_origin); - *r_depth = dot_v3v3(t, ray_direction); + interp_v3_v3v3(r_point, v0, v1, lambda); } } - return len_squared_v3(t) - SQUARE(*r_depth); + else { + /* has no nearest point, only distance squared. */ + /* Calculate the distance to the point v0 then */ + copy_v3_v3(r_point, v0); + } + + float dvec[3]; + sub_v3_v3v3(dvec, r_point, ray_origin); + depth = dot_v3v3(dvec, ray_direction); + + if (r_depth) { + *r_depth = depth; + } + + return len_squared_v3(dvec) - SQUARE(depth); +} + + +/* Returns the coordinates of the nearest vertex and + * the farthest vertex from a plane (or normal). */ +void aabb_get_near_far_from_plane( + const float plane_no[3], const float bbmin[3], const float bbmax[3], + float bb_near[3], float bb_afar[3]) +{ + if (plane_no[0] < 0.0f) { + bb_near[0] = bbmax[0]; + bb_afar[0] = bbmin[0]; + } + else { + bb_near[0] = bbmin[0]; + bb_afar[0] = bbmax[0]; + } + if (plane_no[1] < 0.0f) { + bb_near[1] = bbmax[1]; + bb_afar[1] = bbmin[1]; + } + else { + bb_near[1] = bbmin[1]; + bb_afar[1] = bbmax[1]; + } + if (plane_no[2] < 0.0f) { + bb_near[2] = bbmax[2]; + bb_afar[2] = bbmin[2]; + } + else { + bb_near[2] = bbmin[2]; + bb_afar[2] = bbmax[2]; + } } /* -------------------------------------------------------------------- */ @@ -634,7 +661,6 @@ void dist_squared_ray_to_aabb_v3_precalc( 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); } } @@ -648,30 +674,8 @@ float dist_squared_ray_to_aabb_v3( { // bool r_axis_closest[3]; float local_bvmin[3], local_bvmax[3]; - if (data->sign[0]) { - local_bvmin[0] = bb_max[0]; - local_bvmax[0] = bb_min[0]; - } - else { - local_bvmin[0] = bb_min[0]; - local_bvmax[0] = bb_max[0]; - } - if (data->sign[1]) { - local_bvmin[1] = bb_max[1]; - local_bvmax[1] = bb_min[1]; - } - else { - local_bvmin[1] = bb_min[1]; - local_bvmax[1] = bb_max[1]; - } - if (data->sign[2]) { - local_bvmin[2] = bb_max[2]; - local_bvmax[2] = bb_min[2]; - } - else { - local_bvmin[2] = bb_min[2]; - local_bvmax[2] = bb_max[2]; - } + aabb_get_near_far_from_plane( + data->ray_direction, bb_min, bb_max, local_bvmin, local_bvmax); const float tmin[3] = { (local_bvmin[0] - data->ray_origin[0]) * data->ray_inv_dir[0], @@ -693,38 +697,38 @@ float dist_squared_ray_to_aabb_v3( rtmax = tmax[0]; va[0] = vb[0] = local_bvmax[0]; main_axis = 3; - // r_axis_closest[0] = data->sign[0]; + // r_axis_closest[0] = neasrest_precalc->ray_direction[0] < 0.0f; } 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]; + // r_axis_closest[1] = neasrest_precalc->ray_direction[1] < 0.0f; } else { rtmax = tmax[2]; va[2] = vb[2] = local_bvmax[2]; main_axis = 1; - // r_axis_closest[2] = data->sign[2]; + // r_axis_closest[2] = neasrest_precalc->ray_direction[2] < 0.0f; } 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]; + // r_axis_closest[0] = neasrest_precalc->ray_direction[0] >= 0.0f; } 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]; + // r_axis_closest[1] = neasrest_precalc->ray_direction[1] >= 0.0f; } else { rtmin = tmin[2]; va[2] = vb[2] = local_bvmin[2]; main_axis -= 2; - // r_axis_closest[2] = !data->sign[2]; + // r_axis_closest[2] = neasrest_precalc->ray_direction[2] >= 0.0f; } if (main_axis < 0) { main_axis += 3; @@ -739,14 +743,14 @@ float dist_squared_ray_to_aabb_v3( return 0.0f; } - if (data->sign[main_axis]) { - va[main_axis] = local_bvmax[main_axis]; - vb[main_axis] = local_bvmin[main_axis]; - } - else { + if (data->ray_direction[main_axis] >= 0.0f) { va[main_axis] = local_bvmin[main_axis]; vb[main_axis] = local_bvmax[main_axis]; } + else { + va[main_axis] = local_bvmax[main_axis]; + vb[main_axis] = local_bvmin[main_axis]; + } return dist_squared_ray_to_seg_v3( data->ray_origin, data->ray_direction, va, vb, @@ -765,6 +769,214 @@ 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 *precalc, + const float projmat[4][4], const float winsize[2], const float mval[2]) +{ + float win_half[2], relative_mval[2], px[4], py[4]; + + mul_v2_v2fl(win_half, winsize, 0.5f); + sub_v2_v2v2(precalc->mval, mval, win_half); + + relative_mval[0] = precalc->mval[0] / win_half[0]; + relative_mval[1] = precalc->mval[1] / win_half[1]; + + copy_m4_m4(precalc->pmat, projmat); + for (int i = 0; i < 4; i++) { + px[i] = precalc->pmat[i][0] - precalc->pmat[i][3] * relative_mval[0]; + py[i] = precalc->pmat[i][1] - precalc->pmat[i][3] * relative_mval[1]; + + precalc->pmat[i][0] *= win_half[0]; + precalc->pmat[i][1] *= win_half[1]; + } +#if 0 + float projmat_trans[4][4]; + transpose_m4_m4(projmat_trans, projmat); + if (!isect_plane_plane_plane_v3( + projmat_trans[0], projmat_trans[1], projmat_trans[3], + precalc->ray_origin)) + { + /* Orthographic projection. */ + isect_plane_plane_v3( + px, py, + precalc->ray_origin, + precalc->ray_direction); + } + else { + /* Perspective projection. */ + cross_v3_v3v3(precalc->ray_direction, py, px); + //normalize_v3(precalc->ray_direction); + } +#else + if (!isect_plane_plane_v3( + px, py, + precalc->ray_origin, + precalc->ray_direction)) + { + /* Matrix with weird coplanar planes. Undetermined origin.*/ + zero_v3(precalc->ray_origin); + precalc->ray_direction[0] = precalc->pmat[0][3]; + precalc->ray_direction[1] = precalc->pmat[1][3]; + precalc->ray_direction[2] = precalc->pmat[2][3]; + } +#endif + + for (int i = 0; i < 3; i++) { + precalc->ray_inv_dir[i] = + (precalc->ray_direction[i] != 0.0f) ? + (1.0f / precalc->ray_direction[i]) : FLT_MAX; + } +} + +/* 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]; + aabb_get_near_far_from_plane( + data->ray_direction, bbmin, bbmax, local_bvmin, local_bvmax); + + 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->ray_direction[0] < 0.0f; + } + 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->ray_direction[1] < 0.0f; + } + else { + rtmax = tmax[2]; + va[2] = vb[2] = local_bvmax[2]; + main_axis = 1; + r_axis_closest[2] = data->ray_direction[2] < 0.0f; + } + + 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->ray_direction[0] >= 0.0f; + } + 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->ray_direction[1] >= 0.0f; + } + else { + rtmin = tmin[2]; + va[2] = vb[2] = local_bvmin[2]; + main_axis -= 2; + r_axis_closest[2] = data->ray_direction[2] >= 0.0f; + } + if (main_axis < 0) { + main_axis += 3; + } + + /* if rtmin <= rtmax, ray intersect `AABB` */ + if (rtmin <= rtmax) { + return 0; + } + + if (data->ray_direction[main_axis] >= 0.0f) { + va[main_axis] = local_bvmin[main_axis]; + vb[main_axis] = local_bvmax[main_axis]; + } + else { + va[main_axis] = local_bvmax[main_axis]; + vb[main_axis] = local_bvmin[main_axis]; + } + float scale = fabsf(local_bvmax[main_axis] - local_bvmin[main_axis]); + + float va2d[2] = { + (dot_m4_v3_row_x(data->pmat, va) + data->pmat[3][0]), + (dot_m4_v3_row_y(data->pmat, va) + data->pmat[3][1]), + }; + float vb2d[2] = { + (va2d[0] + data->pmat[main_axis][0] * scale), + (va2d[1] + data->pmat[main_axis][1] * scale), + }; + + float w_a = mul_project_m4_v3_zfac(data->pmat, va); + if (w_a != 1.0f) { + /* Perspective Projection. */ + float w_b = w_a + data->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 { + madd_v2_v2fl(va2d, edge, 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. * @@ -1769,6 +1981,33 @@ bool isect_ray_seg_v2( return false; } + +bool isect_ray_seg_v3( + const float ray_origin[3], const float ray_direction[3], + const float v0[3], const float v1[3], + float *r_lambda) +{ + float a[3], t[3], n[3]; + sub_v3_v3v3(a, v1, v0); + sub_v3_v3v3(t, v0, ray_origin); + cross_v3_v3v3(n, a, ray_direction); + const float nlen = len_squared_v3(n); + + if (nlen == 0.0f) { + /* the lines are parallel.*/ + return false; + } + + float c[3], cray[3]; + sub_v3_v3v3(c, n, t); + cross_v3_v3v3(cray, c, ray_direction); + + *r_lambda = dot_v3v3(cray, n) / nlen; + + return true; +} + + /** * Check if a point is behind all planes. */ @@ -1786,6 +2025,23 @@ bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3]) } /** + * Check if a point is in front all planes. + * Same as isect_point_planes_v3 but with planes facing the opposite direction. + */ +bool isect_point_planes_v3_negated( + const float(*planes)[4], const int totplane, const float p[3]) +{ + for (int i = 0; i < totplane; i++) { + if (plane_point_side_v3(planes[i], p) <= 0.0f) { + return false; + } + } + + return true; +} + + +/** * Intersect line/plane. * * \param r_isect_co The intersection point. @@ -2043,6 +2299,38 @@ static bool getLowestRoot(const float a, const float b, const float c, const flo return false; } + +/** + * Checks status of an AABB in relation to a list of planes. + * + * \returns intersection type: + * - ISECT_AABB_PLANE_BEHIND_ONE (0): AABB is completely behind at least 1 plane; + * - ISECT_AABB_PLANE_CROSS_ANY (1): AABB intersects at least 1 plane; + * - ISECT_AABB_PLANE_IN_FRONT_ALL (2): AABB is completely in front of all planes; + */ +int isect_aabb_planes_v3( + const float (*planes)[4], const int totplane, + const float bbmin[3], const float bbmax[3]) +{ + int ret = ISECT_AABB_PLANE_IN_FRONT_ALL; + + float bb_near[3], bb_far[3]; + for (int i = 0; i < totplane; i++) { + aabb_get_near_far_from_plane(planes[i], bbmin, bbmax, bb_near, bb_far); + + if (plane_point_side_v3(planes[i], bb_far) < 0.0f) { + return ISECT_AABB_PLANE_BEHIND_ANY; + } + else if ((ret != ISECT_AABB_PLANE_CROSS_ANY) && + (plane_point_side_v3(planes[i], bb_near) < 0.0f)) + { + ret = ISECT_AABB_PLANE_CROSS_ANY; + } + } + + return ret; +} + bool isect_sweeping_sphere_tri_v3(const float p1[3], const float p2[3], const float radius, const float v0[3], const float v1[3], const float v2[3], float *r_lambda, float ipoint[3]) |