euclidean 0.2.0

#![no_std]

#[cfg(feature = "std")]
extern crate std;
use core::{fmt::Debug, ops::Mul};
#[cfg(feature = "std")]
use std::vec;
#[cfg(feature = "std")]
use std::vec::*;

use linear_isomorphic::*;
#[cfg(feature = "std")]
use num::traits::{float, float::TotalOrder};

/// Measure the shortest distance from a point to a segment defined by two
/// endpoints.
pub fn point_segment_distance<Vec, S>(start: &Vec, end: &Vec, point: &Vec) -> S
where
    Vec: InnerSpace<S>,
    S: RealField + core::ops::Mul<Vec, Output = Vec>,
{
    let dir = end.clone() - start.clone();
    let t = (point.clone() - start.clone()).dot(&dir) / dir.norm_squared();

    let t = linear_isomorphic::RealField::clamp(
        &t,
        S::from(0.0).unwrap(),
        S::from(1.0).unwrap(),
    );

    let closest = start.clone() + dir * t;

    (closest - point.clone()).norm()
}

/// Find the closest point on a segment to an input point.
pub fn project_point_onto_segment<Vec, S>(start: &Vec, end: &Vec, point: &Vec) -> Vec
where
    Vec: InnerSpace<S>,
    S: RealField,
{
    let dir = end.clone() - start.clone();

    let denom = dir.norm_squared();
    if denom <= S::epsilon() * S::from(100.).unwrap()
    {
        return start.clone();
    }

    let t = (point.clone() - start.clone()).dot(&dir) / denom;

    let t = t.clamp(S::from(0.0).unwrap(), S::from(1.0).unwrap());

    let closest = start.clone() + dir * t;

    closest
}

pub fn point_line_distance<Vec, S>(start: &Vec, dir: &Vec, point: &Vec) -> S
where
    Vec: InnerSpace<S>,
    S: RealField + core::ops::Mul<Vec, Output = Vec>,
{
    (point.clone() - project_onto_line(start, dir, point)).norm()
}

/// Project a point onto a line and get the resulting point.
#[inline]
pub fn project_onto_line<Vec, S>(source: &Vec, dir: &Vec, input_point: &Vec) -> Vec
where
    Vec: InnerSpace<S>,
    Vec: Default,
    S: RealField,
{
    dir.normalized().clone() * project_onto_line_distance(source, dir, input_point)
        + source.clone()
}

/// Projects a point onto a line and returns a signed distance from the
/// origin of the line to the projected point.
///
/// The projected point is thus at $s + t * d$, where $s$ is `source`, $d$
/// is `dir` and $t$ is this function's return value.
#[inline]
pub fn project_onto_line_distance<Vec, S>(source: &Vec, dir: &Vec, input_point: &Vec) -> S
where
    Vec: InnerSpace<S>,
    Vec: Default,
    S: RealField,
{
    (input_point.clone() - source.clone()).dot(&dir.normalized())
}

/// Tries to find the intersection between two lines in 3D. Returns the
/// intersection as a distance from `origin1`. i.e. the intersection point
/// is `origin1 + t * ray1` where `t` is the return value.
///
/// If no such intersection exists, the return value is `inf`.
pub fn line_line_intersection<S, Vec>(
    origin1: &Vec,
    ray1: &Vec,
    origin2: &Vec,
    ray2: &Vec,
) -> S
where
    S: RealField + Mul<Vec, Output = Vec>,
    Vec: InnerSpace<S>,
{
    let epsilon = S::from(1e-3).unwrap();
    if ((origin1.clone() - origin2.clone()).dot(&(origin1.clone() - origin2.clone()))
        - S::from(1.0).unwrap())
    .abs()
        < epsilon
    {
        return S::default();
    };

    let n1 = (origin2.clone() - origin1.clone()).cross(&ray2);
    let n2 = ray1.cross(&ray2);

    // Use this to test whether the vectors point in the same or opposite
    // directions.
    let n = n2.normalized();
    // If n2 is the 0 vector or if the cross-products are not colinear, no solution
    // exists.
    if !n[0].is_finite() || (n1.dot(&n).abs() - n1.norm()).abs() > epsilon
    {
        return S::infinity();
    }

    n1.dot(&n) / n2.dot(&n)
}

/// Computes the closest distance of two *segments* (not lines).
///
/// A segment is the set of points between two endpoints i.e. all points $v$
/// such that $v = (1 - t) p + t q$ with $t \in [0, 1]$.
pub fn segment_segment_distance<S, Vec>(p1: &Vec, p2: &Vec, p3: &Vec, p4: &Vec) -> S
where
    Vec: InnerSpace<S>,
    S: RealField + Mul<Vec, Output = Vec>,
{
    let (p, q) = segment_segment_shortest_points(p1, p2, p3, p4);

    (p - q).norm()
}

/// Find the two points closest to each other on two segments.
///
/// A segment is the set of points between two endpoints i.e. all points $v$
/// such that $v = (1 - t) p + t q$ with $t \in [0, 1]$.
pub fn segment_segment_shortest_points<S, Vec>(
    p1: &Vec,
    p2: &Vec,
    p3: &Vec,
    p4: &Vec,
) -> (Vec, Vec)
where
    Vec: InnerSpace<S>,
    S: RealField + Mul<Vec, Output = Vec>,
{
    let q1 = p1;
    let q2 = p3;
    let v1 = p2.clone() - p1.clone();
    let v2 = p4.clone() - p3.clone();
    let dq = q2.clone() - q1.clone();

    let v22 = v2.dot(&v2);
    let v11 = v1.dot(&v1);
    let v21 = v2.dot(&v1);
    let v21_1 = dq.dot(&v1);
    let v21_2 = dq.dot(&v2);
    let denom = v21 * v21 - v22 * v11;

    let mut s: S;
    let mut t: S;
    if denom.abs() < S::from(f32::EPSILON * 100.0).unwrap()
    {
        s = S::default();
        t = (v11 * s - v21_1) / v21;
    }
    else
    {
        s = (v21_2 * v21 - v22 * v21_1) / denom;
        t = (-v21_1 * v21 + v11 * v21_2) / denom;
    }

    s = s.min(S::from(1).unwrap()).max(S::from(0).unwrap());
    t = t.min(S::from(1).unwrap()).max(S::from(0).unwrap());

    let p_a = q1.clone() + v1 * s;
    let p_b = q2.clone() + v2 * t;

    return (p_a, p_b);
}

/// Find the center of the osculating circle of 3 unaligned points. returns
/// the center of the osculating circle.
///
/// (The radius can be trivially computed from the center and one of the
/// points).
///
/// #Example:
///
/// ```
/// // (Find osculating circle of basis vectors)
/// use euclidean::osculating_circle;
/// use nalgebra::Vector3;
/// let p1 = Vector3::<f32>::new(1.0, 0.0, 0.0);
/// let p2 = Vector3::<f32>::new(0.0, 1.0, 0.0);
/// let p3 = Vector3::<f32>::new(0.0, 0.0, 1.0);
///
/// let center = osculating_circle(&p1, &p2, &p3);
/// // EXPECT_NEAR(0.3333333, center.x(), 0.00001);
/// // EXPECT_NEAR(0.3333333, center.y(), 0.00001);
/// // EXPECT_NEAR(0.3333333, center.z(), 0.00001);
/// ```
pub fn osculating_circle<S, Vec>(p1: &Vec, p2: &Vec, p3: &Vec) -> Vec
where
    S: RealField + Mul<Vec, Output = Vec>,
    Vec: InnerSpace<S>,
{
    let d1 = p2.clone() - p1.clone();
    let d2 = p3.clone() - p1.clone();

    let m1 = (p1.clone() + p2.clone()) * S::from(1.0 / 2.0).unwrap();
    let m2 = (p1.clone() + p3.clone()) * S::from(1.0 / 2.0).unwrap();
    let normal = d1.cross(&d2).normalized();

    let o1 = normal.cross(&d1);
    let o2 = normal.cross(&d2);

    let t = line_line_intersection(&m1, &o1, &m2, &o2);
    debug_assert!(t.is_finite());

    m1 + o1 * t
}

/// Computes the intersection of a line and a segment.
pub fn line_segment_intersection<S, Vec>(
    l0: &Vec,
    l1: &Vec,
    s0: &Vec,
    s1: &Vec,
) -> (bool, Vec)
where
    Vec: InnerSpace<S>,
    S: RealField + core::ops::Mul<Vec, Output = Vec>,
{
    segment_segment_intersection_tolerance(
        l0,
        l1,
        s0,
        s1,
        S::min_value(),
        -(s0.clone() - s1.clone()).norm() * S::from(0.01).unwrap(),
    )
}

// Page 304 of geometry gems "Intersection of two lines in 3 space":
// https://theswissbay.ch/pdf/Gentoomen%20Library/Game%20Development/Programming/Graphics%20Gems%201.pdf
// Also:
// https://stackoverflow.com/questions/34602761/intersecting-3d-line-segments

/// Computes the intersection of 2 segments.
///
/// A segment is the set of points between two endpoints i.e. all points $v$
/// such that $v = (1 - t) p + t q$ with $t \in [0, 1]$.
pub fn segment_segment_intersection<S, Vec>(
    p0: &Vec,
    p1: &Vec,
    q0: &Vec,
    q1: &Vec,
) -> (bool, Vec)
where
    Vec: InnerSpace<S>,
    S: RealField + Mul<Vec, Output = Vec>,
{
    segment_segment_intersection_tolerance(p0, p1, q0, q1, S::default(), S::default())
}

/// Like `segment_segment_intersection` but it allows specifying a tolerance
/// value for the end points. i.e. if the intersection occurs near either
/// endpoint, such as in the vertex joining 2 sides of a triangle, it won't
/// be considered an intersection. Use positive epsilons to make the
/// test stricter and negative to make it more permissive.
pub fn segment_segment_intersection_tolerance<S, Vec>(
    p0: &Vec,
    p1: &Vec,
    q0: &Vec,
    q1: &Vec,
    epsilon1: S,
    epsilon2: S,
) -> (bool, Vec)
where
    Vec: InnerSpace<S>,
    S: RealField + core::ops::Mul<Vec, Output = Vec>,
{
    let dp = p1.clone() - p0.clone();
    let dq = q1.clone() - q0.clone();
    let pq = q0.clone() - p0.clone();

    let a = dp.dot(&dp);
    let b = dp.dot(&dq);
    let c = dq.dot(&dq);
    let d = dp.dot(&pq);
    let e = dq.dot(&pq);

    let dd = -(a * c - b * b);
    let cos_angle = dp.normalized().dot(&dq.normalized());
    // Lines are parallel, so we need to take care of that.
    if dd.abs() < S::from(f32::EPSILON).unwrap() * S::from(10.0).unwrap()
        && cos_angle > S::from(0.99).unwrap()
    {
        // If this is true, lines do not overlap.
        if (d - dp.norm() * pq.norm()) >= S::from(f32::EPSILON * 100.0).unwrap()
        {
            return (false, p0.clone() * S::infinity());
        }

        let distance = |a: &Vec| (a.clone() - p0.clone()).dot(&dp);

        let dp0 = distance(p0);
        let dp1 = distance(p1);
        let dq0 = distance(q0);
        let dq1 = distance(q1);

        if dp0 >= dq0 + epsilon2 && dp0 <= dq1 - epsilon2
        {
            return (true, p0.clone());
        }

        if dp1 >= dq0 + epsilon2 && dp1 <= dq1 - epsilon2
        {
            return (true, p0.clone());
        }

        return (false, p0.clone());
    }

    let t = (b * e - c * d) / dd;
    let s = (a * e - b * d) / dd;

    let pi = p0.clone() + dp.clone() * t;
    let qi = q0.clone() + dq.clone() * s;

    // If this fails, then the points are skewed. Use the norm of one direction as
    // the epsilon.
    let skewness_test =
        (pi.clone() - qi.clone()).norm() <= dp.norm() * S::from(0.01).unwrap();

    let validity_test = (S::from(0.0).unwrap() + epsilon1
        ..=S::from(S::from(1.0).unwrap() - epsilon1).unwrap())
        .contains(&t)
        && (S::from(0.0).unwrap() + epsilon2
            ..=S::from(1.0).unwrap() - S::from(epsilon2).unwrap())
            .contains(&s);

    (validity_test && skewness_test, pi)
}

/// Orthogonally project a point onto a plane.
pub fn project_point_onto_plane<S, Vec>(
    plane_point: &Vec,
    plane_normalized_normal: &Vec,
    input_point: &Vec,
) -> Vec
where
    Vec: InnerSpace<S>,
    S: RealField + Mul<Vec, Output = Vec>,
{
    let d1 = input_point.clone() - plane_point.clone();
    let proj = plane_normalized_normal.clone() * d1.dot(&plane_normalized_normal);

    input_point.clone() - proj.clone()
}

/// Find an orthonormal basis for a planar set of points in some dimension.
/// The order of the return elements is, origin, first basis, second basis.
pub fn find_planar_orthonormal_basis<V, S>(
    points: &dyn Fn(usize) -> V,
    point_count: usize,
) -> (V, V, V)
where
    V: InnerSpace<S> + Debug,
    S: RealField + Mul<V, Output = V>,
{
    let pi = find_best_ortho_index(points, point_count);
    let e1 = (points(1) - points(0)).normalized();
    let e2 = (points(pi) - points(0)).normalized();
    let e2 = orthogonal_vec_from_basis(&e1, &e2);

    (points(0), e1, e2)
}

/// Find the index of a vector to use for a basis of a set of points.
/// The first vector is assumed to be $(p_1 - p_0)$, the second is $(p_i -
/// p_0)$ where $p_i$ is the most orthogonal vector relative to $(p_1 -
/// p_0)$.
pub fn find_best_ortho_index<V, S>(
    points: &dyn Fn(usize) -> V,
    point_count: usize,
) -> usize
where
    V: InnerSpace<S> + VectorSpace<Scalar = S>,
    S: linear_isomorphic::RealField + core::ops::Mul<V, Output = V>,
{
    // Find a direction that is numerically good to orthonormalize.
    let dir = (points(1) - points(0)).normalized();
    let mut dot = S::from(-1.0).unwrap();
    let mut selected_i = 0;
    for i in 2..point_count
    {
        let other_dir = (points(i) - points(0)).normalized();
        let test = other_dir.dot(&dir);
        if test.abs() < S::abs(dot)
        {
            dot = test;
            selected_i = i;
        }
    }
    debug_assert!(
        S::from(1.0).unwrap() - dot.abs() > S::from(1e-3).unwrap(),
        "Point set is too colinear, cannot define plane."
    );

    selected_i
}

// The returned vector will have a positive dot product with v.
// https://math.stackexchange.com/q/4746227/460810
pub fn orthogonal_vec_from_basis<V, S>(u: &V, v: &V) -> V
where
    V: InnerSpace<S>,
    S: RealField + core::ops::Mul<V, Output = V>,
{
    let a = -u.dot(v) / u.dot(u);

    let w = u.clone() * a + v.clone();

    // The result better be orthogonal.
    debug_assert!(
        w.normalized().dot(&u.normalized()).abs() <= S::from(1e5).unwrap(),
        "{:?}",
        w.dot(u).abs()
    );

    w.normalized()
}

#[cfg(feature = "std")]
// https://stackoverflow.com/a/38245767/6202327
/// Find an arbitrary point that is guaranteed to lie on the inside of the
/// simple polygon described by its ordered vertex sequence.
pub fn interior_polygon_point<S, Vec>(polygon: &[Vec]) -> Vec
where
    Vec: InnerSpace<S>,
    Vec: Default,
    Vec: Debug,
    S: Mul<Vec, Output = Vec> + RealField,
{
    let (center, _x_axis, y_axis) =
        find_planar_orthonormal_basis(&|i| polygon[i].clone(), polygon.len());

    let start = (polygon[0].clone() + polygon[1].clone()) * S::from(0.5).unwrap()
        - y_axis.clone() * S::from(2.0).unwrap();
    let mut intersections = vec![];
    for i in 0..polygon.len()
    {
        let p1 = polygon[i].clone();
        let p2 = polygon[(i + 1) % polygon.len()].clone();

        let (test, intersection) = line_segment_intersection(
            &start,
            &(start.clone() + y_axis.normalized()),
            &p1,
            &p2,
        );
        if test
        {
            intersections.push((test, intersection));
        }
    }

    intersections.sort_by(|a, b| {
        let x_1 = (a.1.clone() - center.clone()).dot(&y_axis);
        let x_2 = (b.1.clone() - center.clone()).dot(&y_axis);

        x_1.partial_cmp(&x_2).unwrap()
    });

    (intersections[0].1.clone() + intersections[1].1.clone()) * S::from(0.5).unwrap()
}

#[cfg(feature = "std")]
/// Find the point closest to an input point amongst all segments of a
/// polyline.
pub fn project_onto_open_poly_line<Vec, S>(p: &Vec, polyline: &[Vec]) -> (Vec, S, usize)
where
    Vec: InnerSpace<S>,
    S: RealField + std::iter::Sum + core::ops::Mul<Vec, Output = Vec>,
{
    let arclength: S = polyline
        .windows(2)
        .map(|vs| (vs[0].clone() - vs[1].clone()).norm())
        .sum();

    let mut closest_segment = 0;
    let mut closest_projection = polyline[0].clone();
    let mut cum_length = S::from(0.).unwrap();
    let mut best_length = S::from(0.).unwrap();
    let mut best_distance = S::from(f32::MAX).unwrap();
    for i in 0..polyline.len() - 1
    {
        let v1 = &polyline[i];
        let v2 = &polyline[i + 1];

        let proj = project_point_onto_segment(v1, v2, p);

        let d = (p.clone() - proj.clone()).norm();
        if d < best_distance
        {
            best_distance = d;
            closest_segment = i;
            closest_projection = proj.clone();
            best_length = cum_length + (v1.clone() - proj.clone()).norm();
        }

        cum_length += (v1.clone() - v2.clone()).norm();
    }

    (closest_projection, best_length / arclength, closest_segment)
}

/// Project a point onto a triangle. Meaning the shortest distance from any
/// point in the triangle to the input point.
pub fn project_point_onto_triangle<S, V>(p: &V, triangle: &[V; 3]) -> V
where
    V: InnerSpace<S>,
    S: RealField + core::ops::Mul<V, Output = V>,
{
    // Vectors from a.
    let ab = triangle[1].clone() - triangle[0].clone();
    let ac = triangle[2].clone() - triangle[0].clone();
    let ap = p.clone() - triangle[0].clone();

    // Compute dot products
    let d1 = ab.dot(&ap);
    let d2 = ac.dot(&ap);
    let d3 = ab.dot(&ab);
    let d4 = ab.dot(&ac);
    let d5 = ac.dot(&ac);

    let denom = d3 * d5 - d4 * d4;
    // Handle the degenerate case.
    if denom <= S::epsilon() * S::from(1_000).unwrap()
    {
        let mut best_d = S::infinity();
        let mut best_p = triangle[0].clone();
        for i in 0..3
        {
            let proj =
                project_point_onto_segment(&triangle[i], &triangle[(i + 1) % 3], p);

            let new_d = (proj.clone() - p.clone()).norm();
            if new_d < best_d
            {
                best_d = new_d;
                best_p = proj;
            }
        }
        return best_p;
    }

    // Barycentric coordinates
    let v = (d5 * d1 - d4 * d2) / denom;
    let w = (d3 * d2 - d4 * d1) / denom;

    let o = S::from(1.).unwrap();
    let z = S::from(0.).unwrap();
    let u = o - v - w;

    // Clamp to triangle
    let u = u.clamp(z, o);
    let v = v.clamp(z, o - u);
    let w = o - u - v;

    // Reconstruct the projected point
    triangle[0].clone() * u + triangle[1].clone() * v + triangle[2].clone() * w
}

#[cfg(feature = "std")]
/// Performs triangle-AABB intersection test using SAT in $R^3$.
pub fn triangle_aabb_intersection_test<V, S>(triangle: &[V; 3], aabb: &[V; 2]) -> bool
where
    V: InnerSpace<S>,
    S: RealField,
{
    let triangle_edges = (0..3)
        .map(|i| triangle[(i + 1) % 3].clone() - triangle[i].clone())
        .collect::<Vec<_>>();

    let triangle_normal = triangle_edges[0].cross(&triangle_edges[1]).normalized();

    // Box, AA, axes.
    let box_axes = ((0..3).map(|i| {
        let mut d1 = V::default();
        d1[i] = S::from(1.).unwrap();
        d1
    }))
    .collect::<Vec<_>>();

    let mut axes = vec![];
    for axis in box_axes.iter()
    {
        for triangle_edge in triangle_edges.iter()
        {
            axes.push(axis.cross(&triangle_edge).normalized());
        }
    }

    axes.push(triangle_normal);
    axes.extend(box_axes.into_iter());

    assert_eq!(axes.len(), 13);

    let extents = aabb[1].clone() - aabb[0].clone();
    let box_corners: Vec<_> = (0..8)
        .map(|i| {
            let x = S::from(((i & (1 << 0)) != 0) as u8).unwrap();
            let y = S::from(((i & (1 << 1)) != 0) as u8).unwrap();
            let z = S::from(((i & (1 << 2)) != 0) as u8).unwrap();

            let mut corner = aabb[0].clone();
            corner[0] += x * extents[0];
            corner[1] += y * extents[1];
            corner[2] += z * extents[2];

            corner
        })
        .collect();

    let overlap = |i1: [S; 2], i2: [S; 2]| i1[1] >= i2[0] && i2[1] >= i1[0];

    // Test against the axes of the triangle and the box.
    for axis in axes.iter()
    {
        let mut t_min = S::infinity();
        let mut t_max = S::neg_infinity();

        let mut b_min = S::infinity();
        let mut b_max = S::neg_infinity();

        // Test the triangle vertices.
        for j in 0..3
        {
            let t = project_onto_line_distance(&V::default(), axis, &triangle[j]);
            t_min = t_min.min(t);
            t_max = t_max.max(t);
        }

        // Test the box corners.
        for b in &box_corners
        {
            let t = project_onto_line_distance(&V::default(), axis, b);
            b_min = b_min.min(t);
            b_max = b_max.max(t);
        }

        // If we found a non-overlapping axis, then the triangle does not intersect
        // the box.
        if !overlap([t_min, t_max], [b_min, b_max])
        {
            return false;
        }
    }

    // We failed to find a separating plane, by the SAT the shapes intersect.
    true
}

/// Construct an orthogonal frame of reference in $R^3$ from a single
/// vector.
#[cfg(feature = "std")]
pub fn arbitrary_orthogonal_frame<S, V>(normal: &V) -> [V; 2]
where
    V: InnerSpace<S>,
    S: RealField + TotalOrder + core::ops::Mul<V, Output = V>,
{
    let v1 = orthogonal_vector(normal).normalized();
    let v2 = normal.cross(&v1).normalized();

    [v1, v2]
}

/// Robustly obtain a vector orthogonal to an input vector.
#[cfg(feature = "std")]
pub fn orthogonal_vector<S, V>(v: &V) -> V
where
    V: InnerSpace<S>,
    S: linear_isomorphic::RealField + float::TotalOrder + core::ops::Mul<V, Output = V>,
{
    // Canonical axes.
    let mut v1 = V::default();
    v1[0] = S::from(1.).unwrap();
    let mut v2 = V::default();
    v2[1] = S::from(1.).unwrap();
    let mut v3 = V::default();
    v3[2] = S::from(1.).unwrap();

    let scores = [v.dot(&v1).abs(), v.dot(&v2).abs(), v.dot(&v3).abs()];
    let dirs = [v1, v2, v3];

    let (min_index, _min_value) = scores
        .iter()
        .enumerate()
        .min_by(|(_, x), (_, y)| x.total_cmp(y))
        .unwrap();

    let v: V = dirs[min_index].cross(v);
    v
}

#[cfg(test)]
mod tests
{
    use ::core::f32::consts::PI;

    use super::*;
    use crate::segment_segment_intersection;
    type Vec2 = nalgebra::Vector2<f32>;
    type Vec3 = nalgebra::Vector3<f32>;

    #[test]
    fn test_project_onto_open_poly_line()
    {
        let curve: Vec<_> = (0..50)
            .map(|i| {
                let t = i as f32 / 49.;
                let t = t * PI;

                Vec2::new(t.cos(), t.sin())
            })
            .collect();

        let point = Vec2::new(0., 10.);
        let (proj, param, interval) = project_onto_open_poly_line(&point, &curve);

        assert!((proj - Vec2::new(0., 1.0)).norm() < 0.001,);
        assert!((param - 0.5).abs() < 0.001);
        assert!(interval == 24);
    }

    #[test]
    fn test_segment_segment_intersection_3d()
    {
        let p1 = Vec3::new(-1.0, 0.0, 0.0);
        let p2 = Vec3::new(1.0, 0.0, 0.0);
        let p3 = Vec3::new(0.0, -1.0, 0.0);
        let p4 = Vec3::new(0.0, 1.0, 0.0);

        let (test, point) = segment_segment_intersection(&p1, &p2, &p3, &p4);

        assert!(test);
        assert!(point.norm() <= f32::EPSILON * 10.0);

        let p1 = Vec3::new(-1.0, 0.0, 0.0);
        let p2 = Vec3::new(1.0, 0.0, 0.0);
        let p3 = Vec3::new(0.5, -1.0, 0.0);
        let p4 = Vec3::new(0.5, 1.0, 0.0);

        let (test, point) = segment_segment_intersection(&p1, &p2, &p3, &p4);

        assert!(test);
        assert!((point - Vec3::new(0.5, 0.0, 0.0)).norm() <= f32::EPSILON * 10.0);

        let p1 = Vec3::new(-1.0, 0.5, 0.0);
        let p2 = Vec3::new(1.0, 0.5, 0.0);
        let p3 = Vec3::new(0.5, -1.0, 0.0);
        let p4 = Vec3::new(0.5, 1.0, 0.0);

        let (test, point) = segment_segment_intersection(&p1, &p2, &p3, &p4);

        assert!(test);
        assert!((point - Vec3::new(0.5, 0.5, 0.0)).norm() <= f32::EPSILON * 10.0);

        let p1 = Vec3::new(-1.0, -1.0, 0.0);
        let p2 = Vec3::new(1.0, 1.0, 0.0);
        let p3 = Vec3::new(1.0, -1.0, 0.0);
        let p4 = Vec3::new(-1.0, 1.0, 0.0);

        let (test, point) = segment_segment_intersection(&p1, &p2, &p3, &p4);

        assert!(test);
        assert!((point - Vec3::new(0.0, 0.0, 0.0)).norm() <= f32::EPSILON * 10.0);
    }
}