flanchan
/
blender
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
98280 Commits
1 Branch
0 Tags
454 MiB
 
 
 
 
 
 
Tree: 701a9d3917
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '701a9d3917'
${ noResults }
blender/source/blender/blenlib/intern/math_geom.c

6155 lines
169 KiB
Raw Blame History

/*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: some of this file.
*
* */
/** \file
* \ingroup bli
*/
#include "MEM_guardedalloc.h"
#include "BLI_math.h"
#include "BLI_math_bits.h"
#include "BLI_utildefines.h"
#include "BLI_strict_flags.h"
/********************************** Polygons *********************************/
void cross_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
{
float n1[3], n2[3];
n1[0] = v1[0] - v2[0];
n2[0] = v2[0] - v3[0];
n1[1] = v1[1] - v2[1];
n2[1] = v2[1] - v3[1];
n1[2] = v1[2] - v2[2];
n2[2] = v2[2] - v3[2];
n[0] = n1[1] * n2[2] - n1[2] * n2[1];
n[1] = n1[2] * n2[0] - n1[0] * n2[2];
n[2] = n1[0] * n2[1] - n1[1] * n2[0];
}
float normal_tri_v3(float n[3], const float v1[3], const float v2[3], const float v3[3])
{
float n1[3], n2[3];
n1[0] = v1[0] - v2[0];
n2[0] = v2[0] - v3[0];
n1[1] = v1[1] - v2[1];
n2[1] = v2[1] - v3[1];
n1[2] = v1[2] - v2[2];
n2[2] = v2[2] - v3[2];
n[0] = n1[1] * n2[2] - n1[2] * n2[1];
n[1] = n1[2] * n2[0] - n1[0] * n2[2];
n[2] = n1[0] * n2[1] - n1[1] * n2[0];
return normalize_v3(n);
}
float normal_quad_v3(
float n[3], const float v1[3], const float v2[3], const float v3[3], const float v4[3])
{
/* real cross! */
float n1[3], n2[3];
n1[0] = v1[0] - v3[0];
n1[1] = v1[1] - v3[1];
n1[2] = v1[2] - v3[2];
n2[0] = v2[0] - v4[0];
n2[1] = v2[1] - v4[1];
n2[2] = v2[2] - v4[2];
n[0] = n1[1] * n2[2] - n1[2] * n2[1];
n[1] = n1[2] * n2[0] - n1[0] * n2[2];
n[2] = n1[0] * n2[1] - n1[1] * n2[0];
return normalize_v3(n);
}
/**
* Computes the normal of a planar
* polygon See Graphics Gems for
* computing newell normal.
*/
float normal_poly_v3(float n[3], const float verts[][3], unsigned int nr)
{
cross_poly_v3(n, verts, nr);
return normalize_v3(n);
}
float area_quad_v3(const float v1[3], const float v2[3], const float v3[3], const float v4[3])
{
const float verts[4][3] = {{UNPACK3(v1)}, {UNPACK3(v2)}, {UNPACK3(v3)}, {UNPACK3(v4)}};
return area_poly_v3(verts, 4);
}
float area_squared_quad_v3(const float v1[3],
const float v2[3],
const float v3[3],
const float v4[3])
{
const float verts[4][3] = {{UNPACK3(v1)}, {UNPACK3(v2)}, {UNPACK3(v3)}, {UNPACK3(v4)}};
return area_squared_poly_v3(verts, 4);
}
/* Triangles */
float area_tri_v3(const float v1[3], const float v2[3], const float v3[3])
{
float n[3];
cross_tri_v3(n, v1, v2, v3);
return len_v3(n) * 0.5f;
}
float area_squared_tri_v3(const float v1[3], const float v2[3], const float v3[3])
{
float n[3];
cross_tri_v3(n, v1, v2, v3);
mul_v3_fl(n, 0.5f);
return len_squared_v3(n);
}
float area_tri_signed_v3(const float v1[3],
const float v2[3],
const float v3[3],
const float normal[3])
{
float area, n[3];
cross_tri_v3(n, v1, v2, v3);
area = len_v3(n) * 0.5f;
/* negate area for flipped triangles */
if (dot_v3v3(n, normal) < 0.0f) {
area = -area;
}
return area;
}
float area_poly_v3(const float verts[][3], unsigned int nr)
{
float n[3];
cross_poly_v3(n, verts, nr);
return len_v3(n) * 0.5f;
}
float area_squared_poly_v3(const float verts[][3], unsigned int nr)
{
float n[3];
cross_poly_v3(n, verts, nr);
mul_v3_fl(n, 0.5f);
return len_squared_v3(n);
}
/**
* Scalar cross product of a 2d polygon.
*
* - equivalent to ``area * 2``
* - useful for checking polygon winding (a positive value is clockwise).
*/
float cross_poly_v2(const float verts[][2], unsigned int nr)
{
unsigned int a;
float cross;
const float *co_curr, *co_prev;
/* The Trapezium Area Rule */
co_prev = verts[nr - 1];
co_curr = verts[0];
cross = 0.0f;
for (a = 0; a < nr; a++) {
cross += (co_curr[0] - co_prev[0]) * (co_curr[1] + co_prev[1]);
co_prev = co_curr;
co_curr += 2;
}
return cross;
}
void cross_poly_v3(float n[3], const float verts[][3], unsigned int nr)
{
const float *v_prev = verts[nr - 1];
const float *v_curr = verts[0];
unsigned int i;
zero_v3(n);
/* Newell's Method */
for (i = 0; i < nr; v_prev = v_curr, v_curr = verts[++i]) {
add_newell_cross_v3_v3v3(n, v_prev, v_curr);
}
}
float area_poly_v2(const float verts[][2], unsigned int nr)
{
return fabsf(0.5f * cross_poly_v2(verts, nr));
}
float area_poly_signed_v2(const float verts[][2], unsigned int nr)
{
return (0.5f * cross_poly_v2(verts, nr));
}
float area_squared_poly_v2(const float verts[][2], unsigned int nr)
{
float area = area_poly_signed_v2(verts, nr);
return area * area;
}
float cotangent_tri_weight_v3(const float v1[3], const float v2[3], const float v3[3])
{
float a[3], b[3], c[3], c_len;
sub_v3_v3v3(a, v2, v1);
sub_v3_v3v3(b, v3, v1);
cross_v3_v3v3(c, a, b);
c_len = len_v3(c);
if (c_len > FLT_EPSILON) {
return dot_v3v3(a, b) / c_len;
}
else {
return 0.0f;
}
}
/********************************* Planes **********************************/
/**
* Calculate a plane from a point and a direction,
* \note \a point_no isn't required to be normalized.
*/
void plane_from_point_normal_v3(float r_plane[4], const float plane_co[3], const float plane_no[3])
{
copy_v3_v3(r_plane, plane_no);
r_plane[3] = -dot_v3v3(r_plane, plane_co);
}
/**
* Get a point and a direction from a plane.
*/
void plane_to_point_vector_v3(const float plane[4], float r_plane_co[3], float r_plane_no[3])
{
mul_v3_v3fl(r_plane_co, plane, (-plane[3] / len_squared_v3(plane)));
copy_v3_v3(r_plane_no, plane);
}
/**
* version of #plane_to_point_vector_v3 that gets a unit length vector.
*/
void plane_to_point_vector_v3_normalized(const float plane[4],
float r_plane_co[3],
float r_plane_no[3])
{
const float length = normalize_v3_v3(r_plane_no, plane);
mul_v3_v3fl(r_plane_co, r_plane_no, (-plane[3] / length));
}
/********************************* Volume **********************************/
/**
* The volume from a tetrahedron, points can be in any order
*/
float volume_tetrahedron_v3(const float v1[3],
const float v2[3],
const float v3[3],
const float v4[3])
{
float m[3][3];
sub_v3_v3v3(m[0], v1, v2);
sub_v3_v3v3(m[1], v2, v3);
sub_v3_v3v3(m[2], v3, v4);
return fabsf(determinant_m3_array(m)) / 6.0f;
}
/**
* The volume from a tetrahedron, normal pointing inside gives negative volume
*/
float volume_tetrahedron_signed_v3(const float v1[3],
const float v2[3],
const float v3[3],
const float v4[3])
{
float m[3][3];
sub_v3_v3v3(m[0], v1, v2);
sub_v3_v3v3(m[1], v2, v3);
sub_v3_v3v3(m[2], v3, v4);
return determinant_m3_array(m) / 6.0f;
}
/**
* The volume from a triangle that is made into a tetrahedron.
* This uses a simplified formula where the tip of the tetrahedron is in the world origin.
* Using this method, the total volume of a closed triangle mesh can be calculated.
* Note that you need to divide the result by 6 to get the actual volume.
*/
float volume_tri_tetrahedron_signed_v3_6x(const float v1[3], const float v2[3], const float v3[3])
{
float v_cross[3];
cross_v3_v3v3(v_cross, v1, v2);
float tetra_volume = dot_v3v3(v_cross, v3);
return tetra_volume;
}
float volume_tri_tetrahedron_signed_v3(const float v1[3], const float v2[3], const float v3[3])
{
return volume_tri_tetrahedron_signed_v3_6x(v1, v2, v3) / 6.0f;
}
/********************************* Distance **********************************/
/* distance p to line v1-v2
* using Hesse formula, NO LINE PIECE! */
float dist_squared_to_line_v2(const float p[2], const float l1[2], const float l2[2])
{
float closest[2];
closest_to_line_v2(closest, p, l1, l2);
return len_squared_v2v2(closest, p);
}
float dist_to_line_v2(const float p[2], const float l1[2], const float l2[2])
{
return sqrtf(dist_squared_to_line_v2(p, l1, l2));
}
/* distance p to line-piece v1-v2 */
float dist_squared_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
{
float closest[2];
closest_to_line_segment_v2(closest, p, l1, l2);
return len_squared_v2v2(closest, p);
}
float dist_to_line_segment_v2(const float p[2], const float l1[2], const float l2[2])
{
return sqrtf(dist_squared_to_line_segment_v2(p, l1, l2));
}
/* point closest to v1 on line v2-v3 in 2D */
void closest_to_line_segment_v2(float r_close[2],
const float p[2],
const float l1[2],
const float l2[2])
{
float lambda, cp[2];
lambda = closest_to_line_v2(cp, p, l1, l2);
/* flip checks for !finite case (when segment is a point) */
if (!(lambda > 0.0f)) {
copy_v2_v2(r_close, l1);
}
else if (!(lambda < 1.0f)) {
copy_v2_v2(r_close, l2);
}
else {
copy_v2_v2(r_close, cp);
}
}
/* point closest to v1 on line v2-v3 in 3D */
void closest_to_line_segment_v3(float r_close[3],
const float p[3],
const float l1[3],
const float l2[3])
{
float lambda, cp[3];
lambda = closest_to_line_v3(cp, p, l1, l2);
/* flip checks for !finite case (when segment is a point) */
if (!(lambda > 0.0f)) {
copy_v3_v3(r_close, l1);
}
else if (!(lambda < 1.0f)) {
copy_v3_v3(r_close, l2);
}
else {
copy_v3_v3(r_close, cp);
}
}
/**
* Find the closest point on a plane.
*
* \param r_close: Return coordinate
* \param plane: The plane to test against.
* \param pt: The point to find the nearest of
*
* \note non-unit-length planes are supported.
*/
void closest_to_plane_v3(float r_close[3], const float plane[4], const float pt[3])
{
const float len_sq = len_squared_v3(plane);
const float side = plane_point_side_v3(plane, pt);
madd_v3_v3v3fl(r_close, pt, plane, -side / len_sq);
}
void closest_to_plane_normalized_v3(float r_close[3], const float plane[4], const float pt[3])
{
const float side = plane_point_side_v3(plane, pt);
BLI_ASSERT_UNIT_V3(plane);
madd_v3_v3v3fl(r_close, pt, plane, -side);
}
void closest_to_plane3_v3(float r_close[3], const float plane[3], const float pt[3])
{
const float len_sq = len_squared_v3(plane);
const float side = dot_v3v3(plane, pt);
madd_v3_v3v3fl(r_close, pt, plane, -side / len_sq);
}
void closest_to_plane3_normalized_v3(float r_close[3], const float plane[3], const float pt[3])
{
const float side = dot_v3v3(plane, pt);
BLI_ASSERT_UNIT_V3(plane);
madd_v3_v3v3fl(r_close, pt, plane, -side);
}
float dist_signed_squared_to_plane_v3(const float pt[3], const float plane[4])
{
const float len_sq = len_squared_v3(plane);
const float side = plane_point_side_v3(plane, pt);
const float fac = side / len_sq;
return copysignf(len_sq * (fac * fac), side);
}
float dist_squared_to_plane_v3(const float pt[3], const float plane[4])
{
const float len_sq = len_squared_v3(plane);
const float side = plane_point_side_v3(plane, pt);
const float fac = side / len_sq;
/* only difference to code above - no 'copysignf' */
return len_sq * (fac * fac);
}
float dist_signed_squared_to_plane3_v3(const float pt[3], const float plane[3])
{
const float len_sq = len_squared_v3(plane);
const float side = dot_v3v3(plane, pt); /* only difference with 'plane[4]' version */
const float fac = side / len_sq;
return copysignf(len_sq * (fac * fac), side);
}
float dist_squared_to_plane3_v3(const float pt[3], const float plane[3])
{
const float len_sq = len_squared_v3(plane);
const float side = dot_v3v3(plane, pt); /* only difference with 'plane[4]' version */
const float fac = side / len_sq;
/* only difference to code above - no 'copysignf' */
return len_sq * (fac * fac);
}
/**
* Return the signed distance from the point to the plane.
*/
float dist_signed_to_plane_v3(const float pt[3], const float plane[4])
{
const float len_sq = len_squared_v3(plane);
const float side = plane_point_side_v3(plane, pt);
const float fac = side / len_sq;
return sqrtf(len_sq) * fac;
}
float dist_to_plane_v3(const float pt[3], const float plane[4])
{
return fabsf(dist_signed_to_plane_v3(pt, plane));
}
float dist_signed_to_plane3_v3(const float pt[3], const float plane[3])
{
const float len_sq = len_squared_v3(plane);
const float side = dot_v3v3(plane, pt); /* only difference with 'plane[4]' version */
const float fac = side / len_sq;
return sqrtf(len_sq) * fac;
}
float dist_to_plane3_v3(const float pt[3], const float plane[3])
{
return fabsf(dist_signed_to_plane3_v3(pt, plane));
}
/* distance v1 to line-piece l1-l2 in 3D */
float dist_squared_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
{
float closest[3];
closest_to_line_segment_v3(closest, p, l1, l2);
return len_squared_v3v3(closest, p);
}
float dist_to_line_segment_v3(const float p[3], const float l1[3], const float l2[3])
{
return sqrtf(dist_squared_to_line_segment_v3(p, l1, l2));
}
float dist_squared_to_line_v3(const float p[3], const float l1[3], const float l2[3])
{
float closest[3];
closest_to_line_v3(closest, p, l1, l2);
return len_squared_v3v3(closest, p);
}
float dist_to_line_v3(const float p[3], const float l1[3], const float l2[3])
{
return sqrtf(dist_squared_to_line_v3(p, l1, l2));
}
/**
* Check if \a p is inside the 2x planes defined by ``(v1, v2, v3)``
* where the 3x points define 2x planes.
*
* \param axis_ref: used when v1,v2,v3 form a line and to check if the corner is concave/convex.
*
* \note the distance from \a v1 & \a v3 to \a v2 doesn't matter
* (it just defines the planes).
*
* \return the lowest squared distance to either of the planes.
* where ``(return < 0.0)`` is outside.
*
* <pre>
* v1
* +
* /
* x - out / x - inside
* /
* +----+
* v2 v3
* x - also outside
* </pre>
*/
float dist_signed_squared_to_corner_v3v3v3(const float p[3],
const float v1[3],
const float v2[3],
const float v3[3],
const float axis_ref[3])
{
float dir_a[3], dir_b[3];
float plane_a[3], plane_b[3];
float dist_a, dist_b;
float axis[3];
float s_p_v2[3];
bool flip = false;
sub_v3_v3v3(dir_a, v1, v2);
sub_v3_v3v3(dir_b, v3, v2);
cross_v3_v3v3(axis, dir_a, dir_b);
if ((len_squared_v3(axis) < FLT_EPSILON)) {
copy_v3_v3(axis, axis_ref);
}
else if (dot_v3v3(axis, axis_ref) < 0.0f) {
/* concave */
flip = true;
negate_v3(axis);
}
cross_v3_v3v3(plane_a, dir_a, axis);
cross_v3_v3v3(plane_b, axis, dir_b);
#if 0
plane_from_point_normal_v3(plane_a, v2, plane_a);
plane_from_point_normal_v3(plane_b, v2, plane_b);
dist_a = dist_signed_squared_to_plane_v3(p, plane_a);
dist_b = dist_signed_squared_to_plane_v3(p, plane_b);
#else
/* calculate without the planes 4th component to avoid float precision issues */
sub_v3_v3v3(s_p_v2, p, v2);
dist_a = dist_signed_squared_to_plane3_v3(s_p_v2, plane_a);
dist_b = dist_signed_squared_to_plane3_v3(s_p_v2, plane_b);
#endif
if (flip) {
return min_ff(dist_a, dist_b);
}
else {
return max_ff(dist_a, dist_b);
}
}
/**
* Compute the squared distance of a point to a line (defined as ray).
* \param ray_origin: A point on the line.
* \param ray_direction: Normalized direction of the line.
* \param co: Point to which the distance is to be calculated.
*/
float dist_squared_to_ray_v3_normalized(const float ray_origin[3],
const float ray_direction[3],
const float co[3])
{
float origin_to_co[3];
sub_v3_v3v3(origin_to_co, co, ray_origin);
float origin_to_proj[3];
project_v3_v3v3_normalized(origin_to_proj, origin_to_co, ray_direction);
float co_projected_on_ray[3];
add_v3_v3v3(co_projected_on_ray, ray_origin, origin_to_proj);
return len_squared_v3v3(co, co_projected_on_ray);
}
/**
* 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).
* \param r_depth: the distance of r_point projection on ray to the ray_origin.
*/
float dist_squared_ray_to_seg_v3(const float ray_origin[3],
const float ray_direction[3],
const float v0[3],
const float v1[3],
float r_point[3],
float *r_depth)
{
float lambda, depth;
if (isect_ray_line_v3(ray_origin, ray_direction, v0, v1, &lambda)) {
if (lambda <= 0.0f) {
copy_v3_v3(r_point, v0);
}
else if (lambda >= 1.0f) {
copy_v3_v3(r_point, v1);
}
else {
interp_v3_v3v3(r_point, v0, v1, lambda);
}
}
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_f(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];
}
}
/* -------------------------------------------------------------------- */
/** \name dist_squared_to_ray_to_aabb and helpers
* \{ */
void dist_squared_ray_to_aabb_v3_precalc(struct DistRayAABB_Precalc *neasrest_precalc,
const float ray_origin[3],
const float ray_direction[3])
{
copy_v3_v3(neasrest_precalc->ray_origin, ray_origin);
copy_v3_v3(neasrest_precalc->ray_direction, ray_direction);
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;
}
}
/**
* Returns the distance from a ray to a bound-box (projected on ray)
*/
float dist_squared_ray_to_aabb_v3(const struct DistRayAABB_Precalc *data,
const float bb_min[3],
const float bb_max[3],
float r_point[3],
float *r_depth)
{
// bool r_axis_closest[3];
float local_bvmin[3], local_bvmax[3];
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],
(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] = 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] = 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] = 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] = 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] = 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] = neasrest_precalc->ray_direction[2] >= 0.0f;
}
if (main_axis < 0) {
main_axis += 3;
}
/* if rtmin <= rtmax, ray intersect `AABB` */
if (rtmin <= rtmax) {
float dvec[3];
copy_v3_v3(r_point, local_bvmax);
sub_v3_v3v3(dvec, local_bvmax, data->ray_origin);
*r_depth = dot_v3v3(dvec, data->ray_direction);
return 0.0f;
}
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, r_point, r_depth);
}
float dist_squared_ray_to_aabb_v3_simple(const float ray_origin[3],
const float ray_direction[3],
const float bbmin[3],
const float bbmax[3],
float r_point[3],
float *r_depth)
{
struct DistRayAABB_Precalc data;
dist_squared_ray_to_aabb_v3_precalc(&data, ray_origin, ray_direction);
return dist_squared_ray_to_aabb_v3(&data, bbmin, bbmax, r_point, r_depth);
}
/** \} */
/* -------------------------------------------------------------------- */
/** \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;
r_axis_closest[0] = false;
r_axis_closest[1] = false;
r_axis_closest[2] = false;
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.
*
* Set 'r' to the point in triangle (a, b, c) closest to point 'p' */
void closest_on_tri_to_point_v3(
float r[3], const float p[3], const float a[3], const float b[3], const float c[3])
{
float ab[3], ac[3], ap[3], d1, d2;
float bp[3], d3, d4, vc, cp[3], d5, d6, vb, va;
float denom, v, w;
/* Check if P in vertex region outside A */
sub_v3_v3v3(ab, b, a);
sub_v3_v3v3(ac, c, a);
sub_v3_v3v3(ap, p, a);
d1 = dot_v3v3(ab, ap);
d2 = dot_v3v3(ac, ap);
if (d1 <= 0.0f && d2 <= 0.0f) {
/* barycentric coordinates (1,0,0) */
copy_v3_v3(r, a);
return;
}
/* Check if P in vertex region outside B */
sub_v3_v3v3(bp, p, b);
d3 = dot_v3v3(ab, bp);
d4 = dot_v3v3(ac, bp);
if (d3 >= 0.0f && d4 <= d3) {
/* barycentric coordinates (0,1,0) */
copy_v3_v3(r, b);
return;
}
/* Check if P in edge region of AB, if so return projection of P onto AB */
vc = d1 * d4 - d3 * d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
v = d1 / (d1 - d3);
/* barycentric coordinates (1-v,v,0) */
madd_v3_v3v3fl(r, a, ab, v);
return;
}
/* Check if P in vertex region outside C */
sub_v3_v3v3(cp, p, c);
d5 = dot_v3v3(ab, cp);
d6 = dot_v3v3(ac, cp);
if (d6 >= 0.0f && d5 <= d6) {
/* barycentric coordinates (0,0,1) */
copy_v3_v3(r, c);
return;
}
/* Check if P in edge region of AC, if so return projection of P onto AC */
vb = d5 * d2 - d1 * d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
w = d2 / (d2 - d6);
/* barycentric coordinates (1-w,0,w) */
madd_v3_v3v3fl(r, a, ac, w);
return;
}
/* Check if P in edge region of BC, if so return projection of P onto BC */
va = d3 * d6 - d5 * d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
/* barycentric coordinates (0,1-w,w) */
sub_v3_v3v3(r, c, b);
mul_v3_fl(r, w);
add_v3_v3(r, b);
return;
}
/* P inside face region. Compute Q through its barycentric coordinates (u,v,w) */
denom = 1.0f / (va + vb + vc);
v = vb * denom;
w = vc * denom;
/* = u*a + v*b + w*c, u = va * denom = 1.0f - v - w */
/* ac * w */
mul_v3_fl(ac, w);
/* a + ab * v */
madd_v3_v3v3fl(r, a, ab, v);
/* a + ab * v + ac * w */
add_v3_v3(r, ac);
}
/******************************* Intersection ********************************/
/* intersect Line-Line, shorts */
int isect_seg_seg_v2_int(const int v1[2], const int v2[2], const int v3[2], const int v4[2])
{
float div, lambda, mu;
div = (float)((v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]));
if (div == 0.0f) {
return ISECT_LINE_LINE_COLINEAR;
}
lambda = (float)((v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
mu = (float)((v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) {
return ISECT_LINE_LINE_EXACT;
}
return ISECT_LINE_LINE_CROSS;
}
return ISECT_LINE_LINE_NONE;
}
/* intersect Line-Line, floats - gives intersection point */
int isect_line_line_v2_point(
const float v0[2], const float v1[2], const float v2[2], const float v3[2], float r_vi[2])
{
float s10[2], s32[2];
float div;
sub_v2_v2v2(s10, v1, v0);
sub_v2_v2v2(s32, v3, v2);
div = cross_v2v2(s10, s32);
if (div != 0.0f) {
const float u = cross_v2v2(v1, v0);
const float v = cross_v2v2(v3, v2);
r_vi[0] = ((s32[0] * u) - (s10[0] * v)) / div;
r_vi[1] = ((s32[1] * u) - (s10[1] * v)) / div;
return ISECT_LINE_LINE_CROSS;
}
else {
return ISECT_LINE_LINE_COLINEAR;
}
}
/* intersect Line-Line, floats */
int isect_seg_seg_v2(const float v1[2], const float v2[2], const float v3[2], const float v4[2])
{
float div, lambda, mu;
div = (v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]);
if (div == 0.0f) {
return ISECT_LINE_LINE_COLINEAR;
}
lambda = ((float)(v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
mu = ((float)(v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
if (lambda >= 0.0f && lambda <= 1.0f && mu >= 0.0f && mu <= 1.0f) {
if (lambda == 0.0f || lambda == 1.0f || mu == 0.0f || mu == 1.0f) {
return ISECT_LINE_LINE_EXACT;
}
return ISECT_LINE_LINE_CROSS;
}
return ISECT_LINE_LINE_NONE;
}
/* Returns a point on each segment that is closest to the other. */
void isect_seg_seg_v3(const float a0[3],
const float a1[3],
const float b0[3],
const float b1[3],
float r_a[3],
float r_b[3])
{
float fac_a, fac_b;
float a_dir[3], b_dir[3], a0b0[3], crs_ab[3];
sub_v3_v3v3(a_dir, a1, a0);
sub_v3_v3v3(b_dir, b1, b0);
sub_v3_v3v3(a0b0, b0, a0);
cross_v3_v3v3(crs_ab, b_dir, a_dir);
const float nlen = len_squared_v3(crs_ab);
if (nlen == 0.0f) {
/* Parallel Lines */
/* In this case return any point that
* is between the closest segments. */
float a0b1[3], a1b0[3], len_a, len_b, fac1, fac2;
sub_v3_v3v3(a0b1, b1, a0);
sub_v3_v3v3(a1b0, b0, a1);
len_a = len_squared_v3(a_dir);
len_b = len_squared_v3(b_dir);
if (len_a) {
fac1 = dot_v3v3(a0b0, a_dir);
fac2 = dot_v3v3(a0b1, a_dir);
CLAMP(fac1, 0.0f, len_a);
CLAMP(fac2, 0.0f, len_a);
fac_a = (fac1 + fac2) / (2 * len_a);
}
else {
fac_a = 0.0f;
}
if (len_b) {
fac1 = -dot_v3v3(a0b0, b_dir);
fac2 = -dot_v3v3(a1b0, b_dir);
CLAMP(fac1, 0.0f, len_b);
CLAMP(fac2, 0.0f, len_b);
fac_b = (fac1 + fac2) / (2 * len_b);
}
else {
fac_b = 0.0f;
}
}
else {
float c[3], cray[3];
sub_v3_v3v3(c, crs_ab, a0b0);
cross_v3_v3v3(cray, c, b_dir);
fac_a = dot_v3v3(cray, crs_ab) / nlen;
cross_v3_v3v3(cray, c, a_dir);
fac_b = dot_v3v3(cray, crs_ab) / nlen;
CLAMP(fac_a, 0.0f, 1.0f);
CLAMP(fac_b, 0.0f, 1.0f);
}
madd_v3_v3v3fl(r_a, a0, a_dir, fac_a);
madd_v3_v3v3fl(r_b, b0, b_dir, fac_b);
}
/**
* Get intersection point of two 2D segments.
*
* \param endpoint_bias: Bias to use when testing for end-point overlap.
* A positive value considers intersections that extend past the endpoints,
* negative values contract the endpoints.
* Note the bias is applied to a 0-1 factor, not scaled to the length of segments.
*
* \returns intersection type:
* - -1: collinear.
* - 1: intersection.
* - 0: no intersection.
*/
int isect_seg_seg_v2_point_ex(const float v0[2],
const float v1[2],
const float v2[2],
const float v3[2],
const float endpoint_bias,
float r_vi[2])
{
float s10[2], s32[2], s30[2], d;
const float eps = 1e-6f;
const float endpoint_min = -endpoint_bias;
const float endpoint_max = endpoint_bias + 1.0f;
sub_v2_v2v2(s10, v1, v0);
sub_v2_v2v2(s32, v3, v2);
sub_v2_v2v2(s30, v3, v0);
d = cross_v2v2(s10, s32);
if (d != 0) {
float u, v;
u = cross_v2v2(s30, s32) / d;
v = cross_v2v2(s10, s30) / d;
if ((u >= endpoint_min && u <= endpoint_max) && (v >= endpoint_min && v <= endpoint_max)) {
/* intersection */
float vi_test[2];
float s_vi_v2[2];
madd_v2_v2v2fl(vi_test, v0, s10, u);
/* When 'd' approaches zero, float precision lets non-overlapping co-linear segments
* detect as an intersection. So re-calculate 'v' to ensure the point overlaps both.
* see T45123 */
/* inline since we have most vars already */
#if 0
v = line_point_factor_v2(ix_test, v2, v3);
#else
sub_v2_v2v2(s_vi_v2, vi_test, v2);
v = (dot_v2v2(s32, s_vi_v2) / dot_v2v2(s32, s32));
#endif
if (v >= endpoint_min && v <= endpoint_max) {
copy_v2_v2(r_vi, vi_test);
return 1;
}
}
/* out of segment intersection */
return -1;
}
else {
if ((cross_v2v2(s10, s30) == 0.0f) && (cross_v2v2(s32, s30) == 0.0f)) {
/* equal lines */
float s20[2];
float u_a, u_b;
if (equals_v2v2(v0, v1)) {
if (len_squared_v2v2(v2, v3) > square_f(eps)) {
/* use non-point segment as basis */
SWAP(const float *, v0, v2);
SWAP(const float *, v1, v3);
sub_v2_v2v2(s10, v1, v0);
sub_v2_v2v2(s30, v3, v0);
}
else { /* both of segments are points */
if (equals_v2v2(v0, v2)) { /* points are equal */
copy_v2_v2(r_vi, v0);
return 1;
}
/* two different points */
return -1;
}
}
sub_v2_v2v2(s20, v2, v0);
u_a = dot_v2v2(s20, s10) / dot_v2v2(s10, s10);
u_b = dot_v2v2(s30, s10) / dot_v2v2(s10, s10);
if (u_a > u_b) {
SWAP(float, u_a, u_b);
}
if (u_a > endpoint_max || u_b < endpoint_min) {
/* non-overlapping segments */
return -1;
}
else if (max_ff(0.0f, u_a) == min_ff(1.0f, u_b)) {
/* one common point: can return result */
madd_v2_v2v2fl(r_vi, v0, s10, max_ff(0, u_a));
return 1;
}
}
/* lines are collinear */
return -1;
}
}
int isect_seg_seg_v2_point(
const float v0[2], const float v1[2], const float v2[2], const float v3[2], float r_vi[2])
{
const float endpoint_bias = 1e-6f;
return isect_seg_seg_v2_point_ex(v0, v1, v2, v3, endpoint_bias, r_vi);
}
bool isect_seg_seg_v2_simple(const float v1[2],
const float v2[2],
const float v3[2],
const float v4[2])
{
#define CCW(A, B, C) ((C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0]))
return CCW(v1, v3, v4) != CCW(v2, v3, v4) && CCW(v1, v2, v3) != CCW(v1, v2, v4);
#undef CCW
}
/**
* If intersection == ISECT_LINE_LINE_CROSS or ISECT_LINE_LINE_NONE:
* <pre>
* pt = v1 + lambda * (v2 - v1) = v3 + mu * (v4 - v3)
* </pre>
* \returns intersection type:
* - ISECT_LINE_LINE_COLINEAR: collinear.
* - ISECT_LINE_LINE_EXACT: intersection at an endpoint of either.
* - ISECT_LINE_LINE_CROSS: interaction, not at an endpoint.
* - ISECT_LINE_LINE_NONE: no intersection.
* Also returns lambda and mu in r_lambda and r_mu.
*/
int isect_seg_seg_v2_lambda_mu_db(const double v1[2],
const double v2[2],
const double v3[2],
const double v4[2],
double *r_lambda,
double *r_mu)
{
double div, lambda, mu;
div = (v2[0] - v1[0]) * (v4[1] - v3[1]) - (v2[1] - v1[1]) * (v4[0] - v3[0]);
if (fabs(div) < DBL_EPSILON) {
return ISECT_LINE_LINE_COLINEAR;
}
lambda = ((v1[1] - v3[1]) * (v4[0] - v3[0]) - (v1[0] - v3[0]) * (v4[1] - v3[1])) / div;
mu = ((v1[1] - v3[1]) * (v2[0] - v1[0]) - (v1[0] - v3[0]) * (v2[1] - v1[1])) / div;
if (r_lambda) {
*r_lambda = lambda;
}
if (r_mu) {
*r_mu = mu;
}
if (lambda >= 0.0 && lambda <= 1.0 && mu >= 0.0 && mu <= 1.0) {
if (lambda == 0.0 || lambda == 1.0 || mu == 0.0 || mu == 1.0) {
return ISECT_LINE_LINE_EXACT;
}
return ISECT_LINE_LINE_CROSS;
}
return ISECT_LINE_LINE_NONE;
}
/**
* \param l1, l2: Coordinates (point of line).
* \param sp, r: Coordinate and radius (sphere).
* \return r_p1, r_p2: Intersection coordinates.
*
* \note The order of assignment for intersection points (\a r_p1, \a r_p2) is predictable,
* based on the direction defined by ``l2 - l1``,
* this direction compared with the normal of each point on the sphere:
* \a r_p1 always has a >= 0.0 dot product.
* \a r_p2 always has a <= 0.0 dot product.
* For example, when \a l1 is inside the sphere and \a l2 is outside,
* \a r_p1 will always be between \a l1 and \a l2.
*/
int isect_line_sphere_v3(const float l1[3],
const float l2[3],
const float sp[3],
const float r,
float r_p1[3],
float r_p2[3])
{
/* adapted for use in blender by Campbell Barton - 2011
*
* atelier iebele abel - 2001
* atelier@iebele.nl
* http://www.iebele.nl
*
* sphere_line_intersection function adapted from:
* http://astronomy.swin.edu.au/pbourke/geometry/sphereline
* Paul Bourke pbourke@swin.edu.au
*/
const float ldir[3] = {
l2[0] - l1[0],
l2[1] - l1[1],
l2[2] - l1[2],
};
const float a = len_squared_v3(ldir);
const float b = 2.0f * (ldir[0] * (l1[0] - sp[0]) + ldir[1] * (l1[1] - sp[1]) +
ldir[2] * (l1[2] - sp[2]));
const float c = len_squared_v3(sp) + len_squared_v3(l1) - (2.0f * dot_v3v3(sp, l1)) - (r * r);
const float i = b * b - 4.0f * a * c;
float mu;
if (i < 0.0f) {
/* no intersections */
return 0;
}
else if (i == 0.0f) {
/* one intersection */
mu = -b / (2.0f * a);
madd_v3_v3v3fl(r_p1, l1, ldir, mu);
return 1;
}
else if (i > 0.0f) {
const float i_sqrt = sqrtf(i); /* avoid calc twice */
/* first intersection */
mu = (-b + i_sqrt) / (2.0f * a);
madd_v3_v3v3fl(r_p1, l1, ldir, mu);
/* second intersection */
mu = (-b - i_sqrt) / (2.0f * a);
madd_v3_v3v3fl(r_p2, l1, ldir, mu);
return 2;
}
else {
/* math domain error - nan */
return -1;
}
}
/* keep in sync with isect_line_sphere_v3 */
int isect_line_sphere_v2(const float l1[2],
const float l2[2],
const float sp[2],
const float r,
float r_p1[2],
float r_p2[2])
{
const float ldir[2] = {l2[0] - l1[0], l2[1] - l1[1]};
const float a = dot_v2v2(ldir, ldir);
const float b = 2.0f * (ldir[0] * (l1[0] - sp[0]) + ldir[1] * (l1[1] - sp[1]));
const float c = dot_v2v2(sp, sp) + dot_v2v2(l1, l1) - (2.0f * dot_v2v2(sp, l1)) - (r * r);
const float i = b * b - 4.0f * a * c;
float mu;
if (i < 0.0f) {
/* no intersections */
return 0;
}
else if (i == 0.0f) {
/* one intersection */
mu = -b / (2.0f * a);
madd_v2_v2v2fl(r_p1, l1, ldir, mu);
return 1;
}
else if (i > 0.0f) {
const float i_sqrt = sqrtf(i); /* avoid calc twice */
/* first intersection */
mu = (-b + i_sqrt) / (2.0f * a);
madd_v2_v2v2fl(r_p1, l1, ldir, mu);
/* second intersection */
mu = (-b - i_sqrt) / (2.0f * a);
madd_v2_v2v2fl(r_p2, l1, ldir, mu);
return 2;
}
else {
/* math domain error - nan */
return -1;
}
}
/* point in polygon (keep float and int versions in sync) */
bool isect_point_poly_v2(const float pt[2],
const float verts[][2],
const unsigned int nr,
const bool UNUSED(use_holes))
{
unsigned int i, j;
bool isect = false;
for (i = 0, j = nr - 1; i < nr; j = i++) {
if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
(pt[0] <
(verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) +
verts[i][0])) {
isect = !isect;
}
}
return isect;
}
bool isect_point_poly_v2_int(const int pt[2],
const int verts[][2],
const unsigned int nr,
const bool UNUSED(use_holes))
{
unsigned int i, j;
bool isect = false;
for (i = 0, j = nr - 1; i < nr; j = i++) {
if (((verts[i][1] > pt[1]) != (verts[j][1] > pt[1])) &&
(pt[0] <
(verts[j][0] - verts[i][0]) * (pt[1] - verts[i][1]) / (verts[j][1] - verts[i][1]) +
verts[i][0])) {
isect = !isect;
}
}
return isect;
}
/* point in tri */
/* only single direction */
bool isect_point_tri_v2_cw(const float pt[2],
const float v1[2],
const float v2[2],
const float v3[2])
{
if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
return true;
}
}
}
return false;
}
int isect_point_tri_v2(const float pt[2], const float v1[2], const float v2[2], const float v3[2])
{
if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
if (line_point_side_v2(v3, v1, pt) >= 0.0f) {
return 1;
}
}
}
else {
if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
if (!(line_point_side_v2(v3, v1, pt) >= 0.0f)) {
return -1;
}
}
}
return 0;
}
/* point in quad - only convex quads */
int isect_point_quad_v2(
const float pt[2], const float v1[2], const float v2[2], const float v3[2], const float v4[2])
{
if (line_point_side_v2(v1, v2, pt) >= 0.0f) {
if (line_point_side_v2(v2, v3, pt) >= 0.0f) {
if (line_point_side_v2(v3, v4, pt) >= 0.0f) {
if (line_point_side_v2(v4, v1, pt) >= 0.0f) {
return 1;
}
}
}
}
else {
if (!(line_point_side_v2(v2, v3, pt) >= 0.0f)) {
if (!(line_point_side_v2(v3, v4, pt) >= 0.0f)) {
if (!(line_point_side_v2(v4, v1, pt) >= 0.0f)) {
return -1;
}
}
}
}
return 0;
}
/* moved from effect.c
* test if the line starting at p1 ending at p2 intersects the triangle v0..v2
* return non zero if it does
*/
bool isect_line_segment_tri_v3(const float p1[3],
const float p2[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2])
{
float p[3], s[3], d[3], e1[3], e2[3], q[3];
float a, f, u, v;
sub_v3_v3v3(e1, v1, v0);
sub_v3_v3v3(e2, v2, v0);
sub_v3_v3v3(d, p2, p1);
cross_v3_v3v3(p, d, e2);
a = dot_v3v3(e1, p);
if (a == 0.0f) {
return false;
}
f = 1.0f / a;
sub_v3_v3v3(s, p1, v0);
u = f * dot_v3v3(s, p);
if ((u < 0.0f) || (u > 1.0f)) {
return false;
}
cross_v3_v3v3(q, s, e1);
v = f * dot_v3v3(d, q);
if ((v < 0.0f) || ((u + v) > 1.0f)) {
return false;
}
*r_lambda = f * dot_v3v3(e2, q);
if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) {
return false;
}
if (r_uv) {
r_uv[0] = u;
r_uv[1] = v;
}
return true;
}
/* like isect_line_segment_tri_v3, but allows epsilon tolerance around triangle */
bool isect_line_segment_tri_epsilon_v3(const float p1[3],
const float p2[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2],
const float epsilon)
{
float p[3], s[3], d[3], e1[3], e2[3], q[3];
float a, f, u, v;
sub_v3_v3v3(e1, v1, v0);
sub_v3_v3v3(e2, v2, v0);
sub_v3_v3v3(d, p2, p1);
cross_v3_v3v3(p, d, e2);
a = dot_v3v3(e1, p);
if (a == 0.0f) {
return false;
}
f = 1.0f / a;
sub_v3_v3v3(s, p1, v0);
u = f * dot_v3v3(s, p);
if ((u < -epsilon) || (u > 1.0f + epsilon)) {
return false;
}
cross_v3_v3v3(q, s, e1);
v = f * dot_v3v3(d, q);
if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) {
return false;
}
*r_lambda = f * dot_v3v3(e2, q);
if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) {
return false;
}
if (r_uv) {
r_uv[0] = u;
r_uv[1] = v;
}
return true;
}
/* moved from effect.c
* test if the ray starting at p1 going in d direction intersects the triangle v0..v2
* return non zero if it does
*/
bool isect_ray_tri_v3(const float ray_origin[3],
const float ray_direction[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2])
{
/* note: these values were 0.000001 in 2.4x but for projection snapping on
* a human head (1BU == 1m), subsurf level 2, this gave many errors - campbell */
const float epsilon = 0.00000001f;
float p[3], s[3], e1[3], e2[3], q[3];
float a, f, u, v;
sub_v3_v3v3(e1, v1, v0);
sub_v3_v3v3(e2, v2, v0);
cross_v3_v3v3(p, ray_direction, e2);
a = dot_v3v3(e1, p);
if ((a > -epsilon) && (a < epsilon)) {
return false;
}
f = 1.0f / a;
sub_v3_v3v3(s, ray_origin, v0);
u = f * dot_v3v3(s, p);
if ((u < 0.0f) || (u > 1.0f)) {
return false;
}
cross_v3_v3v3(q, s, e1);
v = f * dot_v3v3(ray_direction, q);
if ((v < 0.0f) || ((u + v) > 1.0f)) {
return false;
}
*r_lambda = f * dot_v3v3(e2, q);
if ((*r_lambda < 0.0f)) {
return false;
}
if (r_uv) {
r_uv[0] = u;
r_uv[1] = v;
}
return true;
}
/**
* if clip is nonzero, will only return true if lambda is >= 0.0
* (i.e. intersection point is along positive \a ray_direction)
*
* \note #line_plane_factor_v3() shares logic.
*/
bool isect_ray_plane_v3(const float ray_origin[3],
const float ray_direction[3],
const float plane[4],
float *r_lambda,
const bool clip)
{
float h[3], plane_co[3];
float dot;
dot = dot_v3v3(plane, ray_direction);
if (dot == 0.0f) {
return false;
}
mul_v3_v3fl(plane_co, plane, (-plane[3] / len_squared_v3(plane)));
sub_v3_v3v3(h, ray_origin, plane_co);
*r_lambda = -dot_v3v3(plane, h) / dot;
if (clip && (*r_lambda < 0.0f)) {
return false;
}
return true;
}
bool isect_ray_tri_epsilon_v3(const float ray_origin[3],
const float ray_direction[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2],
const float epsilon)
{
float p[3], s[3], e1[3], e2[3], q[3];
float a, f, u, v;
sub_v3_v3v3(e1, v1, v0);
sub_v3_v3v3(e2, v2, v0);
cross_v3_v3v3(p, ray_direction, e2);
a = dot_v3v3(e1, p);
if (a == 0.0f) {
return false;
}
f = 1.0f / a;
sub_v3_v3v3(s, ray_origin, v0);
u = f * dot_v3v3(s, p);
if ((u < -epsilon) || (u > 1.0f + epsilon)) {
return false;
}
cross_v3_v3v3(q, s, e1);
v = f * dot_v3v3(ray_direction, q);
if ((v < -epsilon) || ((u + v) > 1.0f + epsilon)) {
return false;
}
*r_lambda = f * dot_v3v3(e2, q);
if ((*r_lambda < 0.0f)) {
return false;
}
if (r_uv) {
r_uv[0] = u;
r_uv[1] = v;
}
return true;
}
void isect_ray_tri_watertight_v3_precalc(struct IsectRayPrecalc *isect_precalc,
const float ray_direction[3])
{
float inv_dir_z;
/* Calculate dimension where the ray direction is maximal. */
int kz = axis_dominant_v3_single(ray_direction);
int kx = (kz != 2) ? (kz + 1) : 0;
int ky = (kx != 2) ? (kx + 1) : 0;
/* Swap kx and ky dimensions to preserve winding direction of triangles. */
if (ray_direction[kz] < 0.0f) {
SWAP(int, kx, ky);
}
/* Calculate the shear constants. */
inv_dir_z = 1.0f / ray_direction[kz];
isect_precalc->sx = ray_direction[kx] * inv_dir_z;
isect_precalc->sy = ray_direction[ky] * inv_dir_z;
isect_precalc->sz = inv_dir_z;
/* Store the dimensions. */
isect_precalc->kx = kx;
isect_precalc->ky = ky;
isect_precalc->kz = kz;
}
bool isect_ray_tri_watertight_v3(const float ray_origin[3],
const struct IsectRayPrecalc *isect_precalc,
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2])
{
const int kx = isect_precalc->kx;
const int ky = isect_precalc->ky;
const int kz = isect_precalc->kz;
const float sx = isect_precalc->sx;
const float sy = isect_precalc->sy;
const float sz = isect_precalc->sz;
/* Calculate vertices relative to ray origin. */
const float a[3] = {v0[0] - ray_origin[0], v0[1] - ray_origin[1], v0[2] - ray_origin[2]};
const float b[3] = {v1[0] - ray_origin[0], v1[1] - ray_origin[1], v1[2] - ray_origin[2]};
const float c[3] = {v2[0] - ray_origin[0], v2[1] - ray_origin[1], v2[2] - ray_origin[2]};
const float a_kx = a[kx], a_ky = a[ky], a_kz = a[kz];
const float b_kx = b[kx], b_ky = b[ky], b_kz = b[kz];
const float c_kx = c[kx], c_ky = c[ky], c_kz = c[kz];
/* Perform shear and scale of vertices. */
const float ax = a_kx - sx * a_kz;
const float ay = a_ky - sy * a_kz;
const float bx = b_kx - sx * b_kz;
const float by = b_ky - sy * b_kz;
const float cx = c_kx - sx * c_kz;
const float cy = c_ky - sy * c_kz;
/* Calculate scaled barycentric coordinates. */
const float u = cx * by - cy * bx;
const float v = ax * cy - ay * cx;
const float w = bx * ay - by * ax;
float det;
if ((u < 0.0f || v < 0.0f || w < 0.0f) && (u > 0.0f || v > 0.0f || w > 0.0f)) {
return false;
}
/* Calculate determinant. */
det = u + v + w;
if (UNLIKELY(det == 0.0f || !isfinite(det))) {
return false;
}
else {
/* Calculate scaled z-coordinates of vertices and use them to calculate
* the hit distance.
*/
const int sign_det = (float_as_int(det) & (int)0x80000000);
const float t = (u * a_kz + v * b_kz + w * c_kz) * sz;
const float sign_t = xor_fl(t, sign_det);
if ((sign_t < 0.0f)
/* Differ from Cycles, don't read r_lambda's original value
* otherwise we won't match any of the other intersect functions here...
* which would be confusing. */
#if 0
|| (sign_T > *r_lambda * xor_signmask(det, sign_mask))
#endif
) {
return false;
}
else {
/* Normalize u, v and t. */
const float inv_det = 1.0f / det;
if (r_uv) {
r_uv[0] = u * inv_det;
r_uv[1] = v * inv_det;
}
*r_lambda = t * inv_det;
return true;
}
}
}
bool isect_ray_tri_watertight_v3_simple(const float ray_origin[3],
const float ray_direction[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2])
{
struct IsectRayPrecalc isect_precalc;
isect_ray_tri_watertight_v3_precalc(&isect_precalc, ray_direction);
return isect_ray_tri_watertight_v3(ray_origin, &isect_precalc, v0, v1, v2, r_lambda, r_uv);
}
#if 0 /* UNUSED */
/**
* A version of #isect_ray_tri_v3 which takes a threshold argument
* so rays slightly outside the triangle to be considered as intersecting.
*/
bool isect_ray_tri_threshold_v3(const float ray_origin[3],
const float ray_direction[3],
const float v0[3],
const float v1[3],
const float v2[3],
float *r_lambda,
float r_uv[2],
const float threshold)
{
const float epsilon = 0.00000001f;
float p[3], s[3], e1[3], e2[3], q[3];
float a, f, u, v;
float du, dv;
sub_v3_v3v3(e1, v1, v0);
sub_v3_v3v3(e2, v2, v0);
cross_v3_v3v3(p, ray_direction, e2);
a = dot_v3v3(e1, p);
if ((a > -epsilon) && (a < epsilon)) {
return false;
}
f = 1.0f / a;
sub_v3_v3v3(s, ray_origin, v0);
cross_v3_v3v3(q, s, e1);
*r_lambda = f * dot_v3v3(e2, q);
if ((*r_lambda < 0.0f)) {
return false;
}
u = f * dot_v3v3(s, p);
v = f * dot_v3v3(ray_direction, q);
if (u > 0 && v > 0 && u + v > 1) {
float t = (u + v - 1) / 2;
du = u - t;
dv = v - t;
}
else {
if (u < 0) {
du = u;
}
else if (u > 1) {
du = u - 1;
}
else {
du = 0.0f;
}
if (v < 0) {
dv = v;
}
else if (v > 1) {
dv = v - 1;
}
else {
dv = 0.0f;
}
}
mul_v3_fl(e1, du);
mul_v3_fl(e2, dv);
if (len_squared_v3(e1) + len_squared_v3(e2) > threshold * threshold) {
return false;
}
if (r_uv) {
r_uv[0] = u;
r_uv[1] = v;
}
return true;
}
#endif
bool isect_ray_seg_v2(const float ray_origin[2],
const float ray_direction[2],
const float v0[2],
const float v1[2],
float *r_lambda,
float *r_u)
{
float v0_local[2], v1_local[2];
sub_v2_v2v2(v0_local, v0, ray_origin);
sub_v2_v2v2(v1_local, v1, ray_origin);
float s10[2];
float det;
sub_v2_v2v2(s10, v1_local, v0_local);
det = cross_v2v2(ray_direction, s10);
if (det != 0.0f) {
const float v = cross_v2v2(v0_local, v1_local);
float p[2] = {(ray_direction[0] * v) / det, (ray_direction[1] * v) / det};
const float t = (dot_v2v2(p, ray_direction) / dot_v2v2(ray_direction, ray_direction));
if ((t >= 0.0f) == 0) {
return false;
}
float h[2];
sub_v2_v2v2(h, v1_local, p);
const float u = (dot_v2v2(s10, h) / dot_v2v2(s10, s10));
if ((u >= 0.0f && u <= 1.0f) == 0) {
return false;
}
if (r_lambda) {
*r_lambda = t;
}
if (r_u) {
*r_u = u;
}
return true;
}
return false;
}
bool isect_ray_line_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.
*/
bool isect_point_planes_v3(float (*planes)[4], int totplane, const float p[3])
{
int i;
for (i = 0; i < totplane; i++) {
if (plane_point_side_v3(planes[i], p) > 0.0f) {
return false;
}
}
return true;
}
/**
* 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.
* \param l1: The first point of the line.
* \param l2: The second point of the line.
* \param plane_co: A point on the plane to intersect with.
* \param plane_no: The direction of the plane (does not need to be normalized).
*
* \note #line_plane_factor_v3() shares logic.
*/
bool isect_line_plane_v3(float r_isect_co[3],
const float l1[3],
const float l2[3],
const float plane_co[3],
const float plane_no[3])
{
float u[3], h[3];
float dot;
sub_v3_v3v3(u, l2, l1);
sub_v3_v3v3(h, l1, plane_co);
dot = dot_v3v3(plane_no, u);
if (fabsf(dot) > FLT_EPSILON) {
float lambda = -dot_v3v3(plane_no, h) / dot;
madd_v3_v3v3fl(r_isect_co, l1, u, lambda);
return true;
}
else {
/* The segment is parallel to plane */
return false;
}
}
/**
* Intersect three planes, return the point where all 3 meet.
* See Graphics Gems 1 pg 305
*
* \param plane_a, plane_b, plane_c: Planes.
* \param r_isect_co: The resulting intersection point.
*/
bool isect_plane_plane_plane_v3(const float plane_a[4],
const float plane_b[4],
const float plane_c[4],
float r_isect_co[3])
{
float det;
det = determinant_m3(UNPACK3(plane_a), UNPACK3(plane_b), UNPACK3(plane_c));
if (det != 0.0f) {
float tmp[3];
/* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
* plane_c.xyz.cross(plane_a.xyz) * -plane_b[3] +
* plane_a.xyz.cross(plane_b.xyz) * -plane_c[3]) / det; */
cross_v3_v3v3(tmp, plane_c, plane_b);
mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
cross_v3_v3v3(tmp, plane_a, plane_c);
madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
cross_v3_v3v3(tmp, plane_b, plane_a);
madd_v3_v3fl(r_isect_co, tmp, plane_c[3]);
mul_v3_fl(r_isect_co, 1.0f / det);
return true;
}
else {
return false;
}
}
/**
* Intersect two planes, return a point on the intersection and a vector
* that runs on the direction of the intersection.
* \note this is a slightly reduced version of #isect_plane_plane_plane_v3
*
* \param plane_a, plane_b: Planes.
* \param r_isect_co: The resulting intersection point.
* \param r_isect_no: The resulting vector of the intersection.
*
* \note \a r_isect_no isn't unit length.
*/
bool isect_plane_plane_v3(const float plane_a[4],
const float plane_b[4],
float r_isect_co[3],
float r_isect_no[3])
{
float det, plane_c[3];
/* direction is simply the cross product */
cross_v3_v3v3(plane_c, plane_a, plane_b);
/* in this case we don't need to use 'determinant_m3' */
det = len_squared_v3(plane_c);
if (det != 0.0f) {
float tmp[3];
/* (plane_b.xyz.cross(plane_c.xyz) * -plane_a[3] +
* plane_c.xyz.cross(plane_a.xyz) * -plane_b[3]) / det; */
cross_v3_v3v3(tmp, plane_c, plane_b);
mul_v3_v3fl(r_isect_co, tmp, plane_a[3]);
cross_v3_v3v3(tmp, plane_a, plane_c);
madd_v3_v3fl(r_isect_co, tmp, plane_b[3]);
mul_v3_fl(r_isect_co, 1.0f / det);
copy_v3_v3(r_isect_no, plane_c);
return true;
}
else {
return false;
}
}
/**
* Intersect two triangles.
*
* \param r_i1, r_i2: Optional arguments to retrieve the overlapping edge between the 2 triangles.
* \return true when the triangles intersect.
*
* \note intersections between coplanar triangles are currently undetected.
*/
bool isect_tri_tri_epsilon_v3(const float t_a0[3],
const float t_a1[3],
const float t_a2[3],
const float t_b0[3],
const float t_b1[3],
const float t_b2[3],
float r_i1[3],
float r_i2[3],
const float epsilon)
{
const float *tri_pair[2][3] = {{t_a0, t_a1, t_a2}, {t_b0, t_b1, t_b2}};
float plane_a[4], plane_b[4];
float plane_co[3], plane_no[3];
BLI_assert((r_i1 != NULL) == (r_i2 != NULL));
/* normalizing is needed for small triangles T46007 */
normal_tri_v3(plane_a, UNPACK3(tri_pair[0]));
normal_tri_v3(plane_b, UNPACK3(tri_pair[1]));
plane_a[3] = -dot_v3v3(plane_a, t_a0);
plane_b[3] = -dot_v3v3(plane_b, t_b0);
if (isect_plane_plane_v3(plane_a, plane_b, plane_co, plane_no) &&
(normalize_v3(plane_no) > epsilon)) {
/**
* Implementation note: its simpler to project the triangles onto the intersection plane
* before intersecting their edges with the ray, defined by 'isect_plane_plane_v3'.
* This way we can use 'line_point_factor_v3_ex' to see if an edge crosses 'co_proj',
* then use the factor to calculate the world-space point.
*/
struct {
<