text: /* graphene-ray.c: A ray emitted from an origin in a given direction
   *
   * SPDX-License-Identifier: MIT
   *
   * Copyright 2015  Emmanuele Bassi
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to deal
   * in the Software without restriction, including without limitation the rights
   * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in
   * all copies or substantial portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
   * THE SOFTWARE.
   */
  
  /**
   * SECTION:graphene-ray
   * @Title: Ray
   * @Short_Description: A ray emitted from an origin in a given direction
   *
   * #graphene_ray_t is a structure representing a ray emitted by an origin,
   * identified by a point in 3D space, in a given direction, identified
   * by a vector with 3 components.
   *
   * A common use of #graphene_ray_t is ray-casting to find which objects in
   * a 3D scene are under the coordinates of the pointer.
   */
  
  #include "graphene-private.h"
  
  #include "graphene-ray.h"
  
  #include "graphene-alloc-private.h"
  #include "graphene-box.h"
  #include "graphene-plane.h"
  #include "graphene-point3d.h"
  #include "graphene-simd4f.h"
  #include "graphene-sphere.h"
  #include "graphene-vec3.h"
  #include "graphene-triangle.h"
  
  #include <math.h>
  #include <float.h>
  
  #if defined(_M_X64) && ! defined(isnanf)
  # define isnanf(x) _isnanf(x)
  # define HAVE_ISNANF
  #endif
  
  /**
   * graphene_ray_alloc: (constructor)
   *
   * Allocates a new #graphene_ray_t structure.
   *
   * The contents of the returned structure are undefined.
   *
   * Returns: (transfer full): the newly allocated #graphene_ray_t.
   *   Use graphene_ray_free() to free the resources allocated by
   *   this function
   *
   * Since: 1.4
   */
  graphene_ray_t *
  graphene_ray_alloc (void)
  {
    return graphene_aligned_alloc (sizeof (graphene_ray_t), 1, 16);
  }
  
  /**
   * graphene_ray_free:
   * @r: a #graphene_ray_t
   *
   * Frees the resources allocated by graphene_ray_alloc().
   *
   * Since: 1.4
   */
  void
  graphene_ray_free (graphene_ray_t *r)
  {
    graphene_aligned_free (r);
  }
  
  /**
   * graphene_ray_init:
   * @r: the #graphene_ray_t to initialize
   * @origin: (nullable): the origin of the ray
   * @direction: (nullable): the direction vector
   *
   * Initializes the given #graphene_ray_t using the given @origin
   * and @direction values.
   *
   * Returns: (transfer none): the initialized ray
   *
   * Since: 1.4
   */
  graphene_ray_t *
  graphene_ray_init (graphene_ray_t           *r,
                     const graphene_point3d_t *origin,
                     const graphene_vec3_t    *direction)
  {
    if (origin != NULL)
      graphene_point3d_to_vec3 (origin, &r->origin);
    else
      graphene_vec3_init_from_vec3 (&r->origin, graphene_vec3_zero ());
  
    if (direction != NULL)
      graphene_vec3_normalize (direction, &r->direction);
    else
      graphene_vec3_init_from_vec3 (&r->direction, graphene_vec3_zero ());
  
    return r;
  }
  
  /**
   * graphene_ray_init_from_ray:
   * @r: the #graphene_ray_t to initialize
   * @src: a #graphene_ray_t
   *
   * Initializes the given #graphene_ray_t using the origin and direction
   * values of another #graphene_ray_t.
   *
   * Returns: (transfer none): the initialized ray
   *
   * Since: 1.4
   */
  graphene_ray_t *
  graphene_ray_init_from_ray (graphene_ray_t       *r,
                              const graphene_ray_t *src)
  {
    return graphene_ray_init_from_vec3 (r, &src->origin, &src->direction);
  }
  
  /**
   * graphene_ray_init_from_vec3:
   * @r: the #graphene_ray_t to initialize
   * @origin: (nullable): a #graphene_vec3_t
   * @direction: (nullable): a #graphene_vec3_t
   *
   * Initializes the given #graphene_ray_t using the given vectors.
   *
   * Returns: (transfer none): the initialized ray
   *
   * Since: 1.4
   */
  graphene_ray_t *
  graphene_ray_init_from_vec3 (graphene_ray_t        *r,
                               const graphene_vec3_t *origin,
                               const graphene_vec3_t *direction)
  {
    if (origin != NULL)
      graphene_vec3_init_from_vec3 (&r->origin, origin);
    else
      graphene_vec3_init_from_vec3 (&r->origin, graphene_vec3_zero ());
  
    if (direction != NULL)
      graphene_vec3_normalize (direction, &r->direction);
    else
      graphene_vec3_init_from_vec3 (&r->direction, graphene_vec3_zero ());
  
    return r;
  }
  
  /**
   * graphene_ray_get_origin:
   * @r: a #graphene_ray_t
   * @origin: (out caller-allocates): return location for the origin
   *
   * Retrieves the origin of the given #graphene_ray_t.
   *
   * Since: 1.4
   */
  void
  graphene_ray_get_origin (const graphene_ray_t *r,
                           graphene_point3d_t   *origin)
  {
    graphene_point3d_init_from_vec3 (origin, &r->origin);
  }
  
  /**
   * graphene_ray_get_direction:
   * @r: a #graphene_ray_t
   * @direction: (out caller-allocates): return location for the direction
   *
   * Retrieves the direction of the given #graphene_ray_t.
   *
   * Since: 1.4
   */
  void
  graphene_ray_get_direction (const graphene_ray_t *r,
                              graphene_vec3_t      *direction)
  {
    graphene_vec3_init_from_vec3 (direction, &r->direction);
  }
  
  /**
   * graphene_ray_get_position_at:
   * @r: a #graphene_ray_t
   * @t: the distance along the ray
   * @position: (out caller-allocates): return location for the position
   *
   * Retrieves the coordinates of a point at the distance @t along the
   * given #graphene_ray_t.
   *
   * Since: 1.4
   */
  void
  graphene_ray_get_position_at (const graphene_ray_t *r,
                                float                 t,
                                graphene_point3d_t   *position)
  {
    graphene_vec3_t res;
  
    graphene_vec3_scale (&r->direction, t, &res);
    graphene_vec3_add (&res, &r->origin, &res);
  
    graphene_point3d_init_from_vec3 (position, &res);
  }
  
  /**
   * graphene_ray_get_distance_to_point:
   * @r: a #graphene_ray_t
   * @p: a #graphene_point3d_t
   *
   * Computes the distance of the closest approach between the
   * given #graphene_ray_t @r and the point @p.
   *
   * The closest approach to a ray from a point is the distance
   * between the point and the projection of the point on the
   * ray itself.
   *
   * Returns: the distance of the point
   *
   * Since: 1.4
   */
  float
  graphene_ray_get_distance_to_point (const graphene_ray_t     *r,
                                      const graphene_point3d_t *p)
  {
    graphene_vec3_t point;
    graphene_vec3_t tmp;
    float distance;
  
    graphene_point3d_to_vec3 (p, &point);
  
    graphene_vec3_subtract (&point, &r->origin, &tmp);
    distance = graphene_vec3_dot (&tmp, &r->direction);
  
    /* the point is behind the ray */
    if (distance < 0)
      {
        graphene_vec3_subtract (&r->origin, &point, &tmp);
        return graphene_vec3_length (&tmp);
      }
  
    /* get the position on the ray at the given distance */
    graphene_vec3_scale (&r->direction, distance, &tmp);
    graphene_vec3_add (&tmp, &r->origin, &tmp);
  
    /* distance */
    graphene_vec3_subtract (&tmp, &point, &tmp);
    return graphene_vec3_length (&tmp);
  }
  
  /**
   * graphene_ray_get_distance_to_plane:
   * @r: a #graphene_ray_t
   * @p: a #graphene_plane_t
   *
   * Computes the distance of the origin of the given #graphene_ray_t from the
   * given plane.
   *
   * If the ray does not intersect the plane, this function returns `INFINITY`.
   *
   * Returns: the distance of the origin of the ray from the plane
   *
   * Since: 1.4
   */
  float
  graphene_ray_get_distance_to_plane (const graphene_ray_t   *r,
                                      const graphene_plane_t *p)
  {
    float denom, t;
  
    denom = graphene_vec3_dot (&p->normal, &r->direction);
    if (fabsf (denom) < GRAPHENE_FLOAT_EPSILON)
      {
        graphene_point3d_t tmp;
  
        /* If the ray is coplanar, return 0 */
        graphene_point3d_init_from_vec3 (&tmp, &r->origin);
        if (fabsf (graphene_plane_distance (p, &tmp)) < GRAPHENE_FLOAT_EPSILON)
          return 0.f;
  
        return INFINITY;
      }
  
    t = -1.f * (graphene_vec3_dot (&r->origin, &p->normal) + p->constant) / denom;
    if (t >= 0.f)
      return t;
  
    return INFINITY;
  }
  
  static bool
  ray_equal (const void *p1,
             const void *p2)
  {
    const graphene_ray_t *a = p1;
    const graphene_ray_t *b = p2;
  
    return graphene_vec3_equal (&a->origin, &b->origin) &&
           graphene_vec3_equal (&a->direction, &b->direction);
  }
  
  /**
   * graphene_ray_equal:
   * @a: a #graphene_ray_t
   * @b: a #graphene_ray_t
   *
   * Checks whether the two given #graphene_ray_t are equal.
   *
   * Returns: `true` if the given rays are equal
   *
   * Since: 1.4
   */
  bool
  graphene_ray_equal (const graphene_ray_t *a,
                      const graphene_ray_t *b)
  {
    return graphene_pointer_equal (a, b, ray_equal);
  }
  
  /**
   * graphene_ray_get_closest_point_to_point:
   * @r: a #graphene_ray_t
   * @p: a #graphene_point3d_t
   * @res: (out caller-allocates): return location for the closest point3d
   *
   * Computes the point on the given #graphene_ray_t that is closest to the
   * given point @p.
   *
   * Since: 1.4
   */
  void
  graphene_ray_get_closest_point_to_point (const graphene_ray_t     *r,
                                           const graphene_point3d_t *p,
                                           graphene_point3d_t       *res)
  {
    graphene_vec3_t point, result;
    float distance;
  
    graphene_point3d_to_vec3 (p, &point);
    graphene_vec3_subtract (&point, &r->origin, &result);
  
    distance = graphene_vec3_dot (&result, &r->direction);
    if (distance < 0)
      graphene_vec3_init_from_vec3 (&result, &r->origin);
    else
      {
        graphene_vec3_scale (&r->direction, distance, &result);
        graphene_vec3_add (&result, &r->origin, &result);
      }
  
    graphene_point3d_init_from_vec3 (res, &result);
  }
  
  /**
   * graphene_ray_intersect_sphere:
   * @r: a #graphene_ray_t
   * @s: a #graphene_sphere_t
   * @t_out: (out): the distance of the point on the ray that intersects the sphere
   *
   * Intersects the given #graphene_ray_t @r with the given
   * #graphene_sphere_t @s.
   *
   * Returns: the type of intersection
   *
   * Since: 1.10
   */
  graphene_ray_intersection_kind_t
  graphene_ray_intersect_sphere (const graphene_ray_t    *r,
                                 const graphene_sphere_t *s,
                                 float                   *t_out)
  {
    graphene_vec3_t v1;
  
    graphene_vec3_subtract (&s->center, &r->origin, &v1);
  
    /* initialize t_out, if set, so we don't have to do that every
     * time we bail out with no intersection
     */
    if (t_out != NULL)
      *t_out = 0.f;
  
    /* (signed) distance along ray to point nearest sphere center */
    float tca = graphene_vec3_dot (&v1, &r->direction);
  
    /* square of distance from ray line to sphere center */
    float d2 = graphene_vec3_dot (&v1, &v1) - tca * tca;
  
    float radius2 = s->radius * s->radius;
    if (d2 > radius2)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    /* Distance to entry/exit point */
    float thc = sqrtf (radius2 - d2);
  
    /* t0 = first intersect point - entrance on front of sphere */
    float t0 = tca - thc;
  
    /* t1 = second intersect point - exit point on back of sphere */
    float t1 = tca + thc;
  
    // test to see if both t0 and t1 are behind the ray - if so, no intersection
    if (t0 < 0 && t1 < 0)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    /* test to see if t0 is behind the ray.
     *
     * 1) if it is, the ray is inside the sphere, so return t1,
     * in order to always return an intersect point that is
     * in front of the ray.
     */
    if (t0 < 0)
      {
        if (t_out)
          *t_out = t1;
  
        return GRAPHENE_RAY_INTERSECTION_KIND_LEAVE;
      }
  
    /* 2) t0 is in front of the ray, so return t0 */
    if (t_out)
      *t_out = t0;
  
    return GRAPHENE_RAY_INTERSECTION_KIND_ENTER;
  }
  
  /**
   * graphene_ray_intersects_sphere:
   * @r: a #graphene_ray_t
   * @s: a #graphene_sphere_t
   *
   * Checks if the given #graphene_ray_t @r intersects the
   * given #graphene_sphere_t @s.
   *
   * See also: graphene_ray_intersect_sphere()
   *
   * Returns: `true` if the ray intersects the sphere
   *
   * Since: 1.10
   */
  bool
  graphene_ray_intersects_sphere (const graphene_ray_t    *r,
                                  const graphene_sphere_t *s)
  {
    return graphene_ray_intersect_sphere (r, s, NULL) != GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  }
  
  static inline float
  nudge_off_axis (float v)
  {
    if (graphene_approx_val (v, 0.f))
      {
        if (v < 0.f)
          return -2 * FLT_EPSILON;
        else
          return 2 * FLT_EPSILON;
      }
    else
      {
        return v;
      }
  }
  
  /**
   * graphene_ray_intersect_box:
   * @r: a #graphene_ray_t
   * @b: a #graphene_box_t
   * @t_out: (out): the distance of the point on the ray that intersects the box
   *
   * Intersects the given #graphene_ray_t @r with the given
   * #graphene_box_t @b.
   *
   * Returns: the type of intersection
   *
   * Since: 1.10
   */
  graphene_ray_intersection_kind_t
  graphene_ray_intersect_box (const graphene_ray_t *r,
                              const graphene_box_t *b,
                              float                *t_out)
  {
    graphene_vec3_t safe_direction;
    graphene_vec3_t inv_dir;
    float d[3];
  
    graphene_vec3_to_float (&r->direction, d);
    graphene_vec3_init (&safe_direction,
                        nudge_off_axis (d[0]),
                        nudge_off_axis (d[1]),
                        nudge_off_axis (d[2]));
  
    /* FIXME: Needs a graphene_vec3_reciprocal() */
    inv_dir.value = graphene_simd4f_reciprocal (safe_direction.value);
  
    graphene_vec3_t inv_min;
    graphene_vec3_subtract (&(b->min), &r->origin, &inv_min);
    graphene_vec3_multiply (&inv_min, &inv_dir, &inv_min);
  
    graphene_vec3_t inv_max;
    graphene_vec3_subtract (&(b->max), &r->origin, &inv_max);
    graphene_vec3_multiply (&inv_max, &inv_dir, &inv_max);
  
    float tx_min, tx_max;
    if (graphene_vec3_get_x (&inv_dir) >= 0.f)
      {
        tx_min = graphene_vec3_get_x (&inv_min);
        tx_max = graphene_vec3_get_x (&inv_max);
      }
    else
      {
        tx_min = graphene_vec3_get_x (&inv_max);
        tx_max = graphene_vec3_get_x (&inv_min);
      }
  
    float ty_min, ty_max;
    if (graphene_vec3_get_y (&inv_dir) >= 0.f)
      {
        ty_min = graphene_vec3_get_y (&inv_min);
        ty_max = graphene_vec3_get_y (&inv_max);
      }
    else
      {
        ty_min = graphene_vec3_get_y (&inv_max);
        ty_max = graphene_vec3_get_y (&inv_min);
      }
  
    if (t_out != NULL)
      *t_out = 0.f;
  
    if ((tx_min > ty_max) || (ty_min > tx_max))
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    /* These lines also handle the case where tx_min or tx_max is NaN
     * (result of 0 * INFINITY): NaN != NaN
     */
  #ifdef HAVE_ISNANF
    if (ty_min > tx_min || isnanf (tx_min))
      tx_min = ty_min;
    if (ty_max < tx_max || isnanf (tx_max))
      tx_max = ty_max;
  #else
    if (ty_min > tx_min || fpclassify (tx_min) == FP_NAN)
      tx_min = ty_min;
    if (ty_max > tx_max || fpclassify (tx_max) == FP_NAN)
      tx_max = ty_max;
  #endif
  
    float tz_min, tz_max;
    if (graphene_vec3_get_z (&inv_dir) >= 0.f)
      {
        tz_min = graphene_vec3_get_z (&inv_min);
        tz_max = graphene_vec3_get_z (&inv_max);
      }
    else
      {
        tz_min = graphene_vec3_get_z (&inv_max);
        tz_max = graphene_vec3_get_z (&inv_min);
      }
  
    if ((tx_min > tz_max) || (tz_min > tx_max))
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
  #ifdef HAVE_ISNANF
    if (tz_min > tx_min || isnanf (tx_min))
      tx_min = tz_min;
    if (tz_max < tx_max || isnanf (tx_max))
      tx_max = tz_max;
  #else
    if (tz_min > tx_min || fpclassify (tx_min) == FP_NAN)
      tx_min = tz_min;
    if (tz_max < tx_max || fpclassify (tx_max) == FP_NAN)
      tx_max = tz_max;
  #endif
  
    /* return the point closest to the ray (positive side) */
    if (tx_max < 0)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    if (tx_min >= 0)
      {
        if (t_out)
          *t_out = tx_min;
  
        return GRAPHENE_RAY_INTERSECTION_KIND_ENTER;
      }
  
    if (t_out)
      *t_out = tx_max;
  
    return GRAPHENE_RAY_INTERSECTION_KIND_LEAVE;
  }
  
  /**
   * graphene_ray_intersects_box:
   * @r: a #graphene_ray_t
   * @b: a #graphene_box_t
   *
   * Checks whether the given #graphene_ray_t @r intersects the
   * given #graphene_box_t @b.
   *
   * See also: graphene_ray_intersect_box()
   *
   * Returns: `true` if the ray intersects the box
   *
   * Since: 1.10
   */
  bool
  graphene_ray_intersects_box (const graphene_ray_t *r,
                               const graphene_box_t *b)
  {
    return graphene_ray_intersect_box (r, b, NULL) != GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  }
  
  /**
   * graphene_ray_intersect_triangle:
   * @r: a #graphene_ray_t
   * @t: a #graphene_triangle_t
   * @t_out: (out): the distance of the point on the ray that intersects the triangle
   *
   * Intersects the given #graphene_ray_t @r with the given
   * #graphene_triangle_t @t.
   *
   * Returns: the type of intersection
   *
   * Since: 1.10
   */
  graphene_ray_intersection_kind_t
  graphene_ray_intersect_triangle (const graphene_ray_t      *r,
                                   const graphene_triangle_t *t,
                                   float                     *t_out)
  {
    graphene_vec3_t diff, edge1, edge2, normal;
    graphene_ray_intersection_kind_t kind;
  
    /* from http://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrRay3Triangle3.h */
  
    /* Compute the offset origin, edges, and normal */
    graphene_vec3_subtract (&t->b, &t->a, &edge1);
    graphene_vec3_subtract (&t->c, &t->a, &edge2);
    graphene_vec3_cross (&edge1, &edge2, &normal);
  
    /* Solve:
     *
     *   Q + t * D = b1 * E1 + b2 * E2
     *
     * Where:
     *  * Q = kDiff
     *  * D = ray direction
     *  * E1 = kEdge1
     *  * E2 = kEdge2
     *  * N = Cross(E1,E2)
     *
     * by:
     *
     *  |Dot(D,N)| * b1 = sign(Dot(D,N)) * Dot(D,Cross(Q,E2))
     *  |Dot(D,N)| * b2 = sign(Dot(D,N)) * Dot(D,Cross(E1,Q))
     *  |Dot(D,N)| * t = -sign(Dot(D,N)) * Dot(Q,N)
     */
    float DdN = graphene_vec3_dot (&r->direction, &normal);
    float sign;
  
    /* Ray and triangle are parallel, call it a "no intersection"
     * even if the ray does intersect
     */
    if (graphene_approx_val (DdN, 0.f))
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
    else if (DdN > 0)
      {
        kind = GRAPHENE_RAY_INTERSECTION_KIND_LEAVE;
        sign = 1.f;
  
      }
    else
      {
        kind = GRAPHENE_RAY_INTERSECTION_KIND_ENTER;
        sign = -1.f;
        DdN = -DdN;
      }
  
    graphene_vec3_subtract (&r->origin, &t->a, &diff);
    graphene_vec3_cross (&diff, &edge2, &edge2);
    float DdQxE2 = sign * graphene_vec3_dot (&r->direction, &edge2);
  
    /* b1 < 0, no intersection */
    if (DdQxE2 < 0)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    graphene_vec3_cross (&edge1, &diff, &edge1);
  
    float DdE1xQ = sign * graphene_vec3_dot (&r->direction, &edge1);
  
    /* b2 < 0, no intersection */
    if (DdE1xQ < 0)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    /* b1 + b2 > 1, no intersection */
    if (DdQxE2 + DdE1xQ > DdN)
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    /* Line intersects triangle, check if ray does */
    float QdN = -sign * graphene_vec3_dot (&diff,
                                           &normal);
  
    /* t < 0, no intersection */
    if ( QdN < 0 )
      return GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  
    if (t_out != NULL)
      *t_out = QdN / DdN;
  
    return kind;
  }
  
  /**
   * graphene_ray_intersects_triangle:
   * @r: a #graphene_ray_t
   * @t: a #graphene_triangle_t
   *
   * Checks whether the given #graphene_ray_t @r intersects the
   * given #graphene_triangle_t @b.
   *
   * See also: graphene_ray_intersect_triangle()
   *
   * Returns: `true` if the ray intersects the triangle
   *
   * Since: 1.10
   */
  bool
  graphene_ray_intersects_triangle (const graphene_ray_t      *r,
                                    const graphene_triangle_t *t)
  {
    return graphene_ray_intersect_triangle (r, t, NULL) != GRAPHENE_RAY_INTERSECTION_KIND_NONE;
  }
  