/**************************************************************************
 *{@C
 *      Copyright:      1988-2023 Paul Obermeier (obermeier@poSoft.de)
 *
 *      Module:         Utilities
 *      Filename:       UT_VecIsect.c
 *
 *      Author:         Paul Obermeier
 *
 *      Description:    Operations on points, lines and planes in two
 *                      and three dimensional space.
 *
 *      Additional documentation:
 *                      None.
 *
 *      Exported functions:
 *                      UT_VecDistPointLine2D
 *                      UT_VecDistPointLine3D
 *                      UT_VecPointInTria2D
 *                      UT_VecPointInTria3D
 *                      UT_VecIsectPlaneLine2D
 *                      UT_VecIsectPlaneLine3D
 *                      UT_VecDistLineLine3D
 *                      UT_VecIsectPlanePlane3D
 *                      UT_VecTriaToPlane3D
 *                      UT_VecIsectToPlane3D
 *                      UT_VecIsectLineTria3D
 *
 **************************************************************************/

#include <stdio.h>
#include <math.h>

#include "UT_Compat.h"
#include "UT_Const.h"
#include "UT_Macros.h"

#include "UT_Vector.h"

/***************************************************************************
 *[@e
 *      Name:           UT_VecDistPointLine2D
 *
 *      Usage:          Get the distance between a point and a line.
 *
 *      Synopsis:       void UT_VecDistPointLine2D
 *                              (const UT_Vec2D a, const UT_Vec2D b, const UT_Vec2D d,
 *                               Float32 *f, UT_Vec2D r)
 *
 *      Description:    Given the vectors a, b, and d, computes a number f
 *                      and a vector r so that a + f*d + r = b and the
 *                      dot product of d and r is zero (the length of r
 *                      is the distance of point b from a straight line
 *                      defined by the points a and a+d).
 *
 *                      The length of d must be greater than 0.0.
 *                      Pass a NULL pointer for any of the output parameters
 *                      you are not interested in.
 *
 *                      There is an identical function "UT_DVecDistPointLine2D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_VecDistPointLine2D
        (const UT_Vec2D a, const UT_Vec2D b, const UT_Vec2D d, Float32 *f, UT_Vec2D r)
{
    UT_Vec2D c;
    Float32 tmp;

    c[0] = b[0] - a[0];
    c[1] = b[1] - a[1];
    tmp = UT_VecDotProd2D (c, d) / UT_VecDotProd2D (d, d);
    if (r) {
        r[0] = c[0] - tmp * d[0];
        r[1] = c[1] - tmp * d[1];
    }
    if (f) {
        *f = tmp;
    }
    return;
}

void UT_DVecDistPointLine2D
        (const UT_DVec2D a, const UT_DVec2D b, const UT_DVec2D d, Float64 *f, UT_DVec2D r)
{
    UT_DVec2D c;
    Float64 tmp;

    c[0] = b[0] - a[0];
    c[1] = b[1] - a[1];
    tmp = UT_DVecDotProd2D (c, d) / UT_DVecDotProd2D (d, d);
    if (r) {
        r[0] = c[0] - tmp * d[0];
        r[1] = c[1] - tmp * d[1];
    }
    if (f) {
        *f = tmp;
    }
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecDistPointLine3D
 *
 *      Usage:          Get the distance between a point and a line.
 *
 *      Synopsis:       void UT_VecDistPointLine3D
 *                              (const UT_Vec3D a, const UT_Vec3D b, const UT_Vec3D d,
 *                               Float32 *f, UT_Vec3D r)
 *
 *      Description:    Given the vectors a, b, and d, computes a number f
 *                      and a vector r so that a + f*d + r = b and the
 *                      dot product of d and r is zero (the length of r
 *                      is the distance of point b from a straight line
 *                      defined by the points a and a+d).
 *
 *                      The length of d must be greater than 0.0.
 *                      Pass a NULL pointer for any of the output parameters
 *                      you are not interested in.
 *
 *                      There is an identical function "UT_DVecDistPointLine3D" 
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   None.
 *
 *      See also:
 *
 ***************************************************************************/

void UT_VecDistPointLine3D
        (const UT_Vec3D a, const UT_Vec3D b, const UT_Vec3D d, Float32 *f, UT_Vec3D r)
{
    UT_Vec3D c;
    Float32 tmp;

    c[0] = b[0] - a[0];
    c[1] = b[1] - a[1];
    c[2] = b[2] - a[2];
    tmp = UT_VecDotProd3D (c, d) / UT_VecDotProd3D (d, d);
    if (r) {
        r[0] = c[0] - tmp * d[0];
        r[1] = c[1] - tmp * d[1];
        r[2] = c[2] - tmp * d[2];
    }
    if (f) {
        *f = tmp;
    }
    return;
}

void UT_DVecDistPointLine3D
        (const UT_DVec3D a, const UT_DVec3D b, const UT_DVec3D d, Float64 *f, UT_DVec3D r)
{
    UT_DVec3D c;
    Float64 tmp;

    c[0] = b[0] - a[0];
    c[1] = b[1] - a[1];
    c[2] = b[2] - a[2];
    tmp = UT_DVecDotProd3D (c, d) / UT_DVecDotProd3D (d, d);
    if (r) {
        r[0] = c[0] - tmp * d[0];
        r[1] = c[1] - tmp * d[1];
        r[2] = c[2] - tmp * d[2];
    }
    if (f) {
        *f = tmp;
    }
    return;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecPointInTria2D
 *
 *      Usage:          Test if a point is inside a triangle.
 *
 *      Synopsis:       UT_Bool UT_VecPointInTria2D
 *                              (const UT_Vec2D t1, const UT_Vec2D t2,
 *                               const UT_Vec2D t3, const UT_Vec2D p)
 *
 *      Description:    There is an identical function "UT_DVecPointInTria2D" 
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   UT_True if point p is inside the triangle defined
 *                      by t1, t2 and t3, or if p is very close to the
 *                      edges of the triangle, so that it is not possible
 *                      to determine exactly if p is inside or outside.
 *                      If p is clearly outside the triangle, UT_False
 *                      is returned.
 *                      P must be an element of the plane defined by t1,
 *                      t2 and t3.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecPointInTria2D
        (const UT_Vec2D t1, const UT_Vec2D t2, const UT_Vec2D t3,
         const UT_Vec2D p)
{
    UT_Vec2D a1, a2, a3, b1, b2, b3;
    Float32 c1, c2, c3;

    UT_VecSub2D (t2, t1, a1);
    UT_VecSub2D (t3, t2, a2);
    UT_VecSub2D (t1, t3, a3);
    (void) UT_VecUnit2D (a1);
    (void) UT_VecUnit2D (a2);
    (void) UT_VecUnit2D (a3);
    UT_VecSub2D (p, t1, b1);
    UT_VecSub2D (p, t2, b2);
    UT_VecSub2D (p, t3, b3);
    (void) UT_VecUnit2D (b1);
    (void) UT_VecUnit2D (b2);
    (void) UT_VecUnit2D (b3);
    c1 = UT_VecCrossProd2D (a1, b1);
    c2 = UT_VecCrossProd2D (a2, b2);
    c3 = UT_VecCrossProd2D (a3, b3);
    if (c1 * c2 < -Float32Prec ||
        c2 * c3 < -Float32Prec || 
        c3 * c1 < -Float32Prec) {
        return UT_False;
    }
    return UT_True;
}

UT_Bool UT_DVecPointInTria2D
        (const UT_DVec2D t1, const UT_DVec2D t2, const UT_DVec2D t3,
         const UT_DVec2D p)
{
    UT_DVec2D a1, a2, a3, b1, b2, b3;
    Float64 c1, c2, c3;

    UT_DVecSub2D (t2, t1, a1);
    UT_DVecSub2D (t3, t2, a2);
    UT_DVecSub2D (t1, t3, a3);
    (void) UT_DVecUnit2D (a1);
    (void) UT_DVecUnit2D (a2);
    (void) UT_DVecUnit2D (a3);
    UT_DVecSub2D (p, t1, b1);
    UT_DVecSub2D (p, t2, b2);
    UT_DVecSub2D (p, t3, b3);
    (void) UT_DVecUnit2D (b1);
    (void) UT_DVecUnit2D (b2);
    (void) UT_DVecUnit2D (b3);
    c1 = UT_DVecCrossProd2D (a1, b1);
    c2 = UT_DVecCrossProd2D (a2, b2);
    c3 = UT_DVecCrossProd2D (a3, b3);
    if (c1 * c2 < -Float64Prec ||
        c2 * c3 < -Float64Prec ||
        c3 * c1 < -Float64Prec) {
        return UT_False;
    }
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecPointInTria3D
 *
 *      Usage:          Test if a point is inside a triangle.
 *
 *      Synopsis:       UT_Bool UT_VecPointInTria3D
 *                              (const UT_Vec3D t1, const UT_Vec3D t2,
 *                               const UT_Vec3D t3, const UT_Vec3D p)
 *
 *      Description:    There is an identical function "UT_DVecPointInTria3D" 
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   UT_True if point p is inside the triangle defined
 *                      by t1, t2 and t3, or if p is very close to the
 *                      edges of the triangle, so that it is not possible
 *                      to determine exactly if p is inside or outside.
 *                      If p is clearly outside the triangle, UT_False
 *                      is returned.
 *                      P must be an element of the plane defined by t1,
 *                      t2 and t3.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecPointInTria3D
        (const UT_Vec3D t1, const UT_Vec3D t2, const UT_Vec3D t3,
         const UT_Vec3D p)
{
    UT_Vec3D a1, a2, a3, b1, b2, b3, c1, c2, c3;

    UT_VecSub3D (t2, t1, a1);
    UT_VecSub3D (t3, t2, a2);
    UT_VecSub3D (t1, t3, a3);
    (void) UT_VecUnit3D (a1);
    (void) UT_VecUnit3D (a2);
    (void) UT_VecUnit3D (a3);
    UT_VecSub3D (p, t1, b1);
    UT_VecSub3D (p, t2, b2);
    UT_VecSub3D (p, t3, b3);
    (void) UT_VecUnit3D (b1);
    (void) UT_VecUnit3D (b2);
    (void) UT_VecUnit3D (b3);
    UT_VecCrossProd3D (a1, b1, c1);
    UT_VecCrossProd3D (a2, b2, c2);
    UT_VecCrossProd3D (a3, b3, c3);
    if (UT_VecDotProd3D (c1, c2) < -Float32Prec ||
        UT_VecDotProd3D (c2, c3) < -Float32Prec ||
        UT_VecDotProd3D (c3, c1) < -Float32Prec) {
        return UT_False;
    }
    return UT_True;
}

UT_Bool UT_DVecPointInTria3D
        (const UT_DVec3D t1, const UT_DVec3D t2, const UT_DVec3D t3,
         const UT_DVec3D p)
{
    UT_DVec3D a1, a2, a3, b1, b2, b3, c1, c2, c3;

    UT_DVecSub3D (t2, t1, a1);
    UT_DVecSub3D (t3, t2, a2);
    UT_DVecSub3D (t1, t3, a3);
    (void) UT_DVecUnit3D (a1);
    (void) UT_DVecUnit3D (a2);
    (void) UT_DVecUnit3D (a3);
    UT_DVecSub3D (p, t1, b1);
    UT_DVecSub3D (p, t2, b2);
    UT_DVecSub3D (p, t3, b3);
    (void) UT_DVecUnit3D (b1);
    (void) UT_DVecUnit3D (b2);
    (void) UT_DVecUnit3D (b3);
    UT_DVecCrossProd3D (a1, b1, c1);
    UT_DVecCrossProd3D (a2, b2, c2);
    UT_DVecCrossProd3D (a3, b3, c3);
    if (UT_DVecDotProd3D (c1, c2) < -Float64Prec ||
        UT_DVecDotProd3D (c2, c3) < -Float64Prec ||
        UT_DVecDotProd3D (c3, c1) < -Float64Prec) {
        return UT_False;
    }
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecIsectPlaneLine2D
 *
 *      Usage:          Find the intersection point of a plane and a line.
 *
 *      Synopsis:       UT_Bool UT_VecIsectPlaneLine2D
 *                              (const UT_Vec2D a, const UT_Vec2D n,
 *                              const UT_Vec2D b, const UT_Vec2D d, Float32 *f)
 *
 *      Description:    Find the intersection point of a plane and a straight
 *                      line. The plane is defined by its normal, n, and
 *                      one point on the plane, a (in the two dimensional
 *                      version of this routine, the "plane" is a straight
 *                      line). The line is defined by its direction, d, and
 *                      a point on the line, b.
 *
 *                      There is an identical function "UT_DVecIsectPlaneLine2D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   If the plane and the line are not parallel,
 *                      f is set so that the intersection point is b + f*d,
 *                      and UT_True is returned.
 *                      If no intersection point can be found, i.e. the
 *                      plane and the line are parallel, UT_False is returned
 *                      and f is left untouched.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecIsectPlaneLine2D
        (const UT_Vec2D a, const UT_Vec2D n,
         const UT_Vec2D b, const UT_Vec2D d, Float32 *f)
{
    UT_Vec2D c;
    Float32 ndotd;

    UT_VecSub2D (a, b, c);
    ndotd = UT_VecDotProd2D (n, d);
    if (UT_ABS (ndotd) < Float32Prec) {
        return UT_False;
    }
    *f = UT_VecDotProd2D (n, c) / ndotd;
    return UT_True;
}

UT_Bool UT_DVecIsectPlaneLine2D
        (const UT_DVec2D a, const UT_DVec2D n,
         const UT_DVec2D b, const UT_DVec2D d, Float64 *f)
{
    UT_DVec2D c;
    Float64 ndotd;

    UT_DVecSub2D (a, b, c);
    ndotd = UT_DVecDotProd2D (n, d);
    if (UT_ABS (ndotd) < Float64Prec) {
        return UT_False;
    }
    *f = UT_DVecDotProd2D (n, c) / ndotd;
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecIsectPlaneLine3D
 *
 *      Usage:          Find the intersection point of a plane and a line.
 *
 *      Synopsis:       UT_Bool UT_VecIsectPlaneLine3D
 *                              (const UT_Vec3D a, const UT_Vec3D n,
 *                               const UT_Vec3D b, const UT_Vec3D d, Float32 *f)
 *
 *      Description:    Find the intersection point of a plane and a straight
 *                      line. The plane is defined by its normal, n, and
 *                      one point on the plane, a (in the two dimensional
 *                      version of this routine, the "plane" is a straight
 *                      line). The line is defined by its direction, d, and
 *                      a point on the line, b.
 *
 *                      There is an identical function "UT_DVecIsectPlaneLine3D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   If the plane and the line are not parallel,
 *                      f is set so that the intersection point is b + f*d,
 *                      and UT_True is returned.
 *                      If no intersection point can be found, i.e. the
 *                      plane and the line are parallel, UT_False is returned
 *                      and f is left untouched.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecIsectPlaneLine3D
        (const UT_Vec3D a, const UT_Vec3D n,
         const UT_Vec3D b, const UT_Vec3D d, Float32 *f)
{
    UT_Vec3D c;
    Float32 ndotd;

    UT_VecSub3D (a, b, c);
    ndotd = UT_VecDotProd3D (n, d);
    if (UT_ABS (ndotd) < Float32Prec) {
        return UT_False;
    }
    *f = UT_VecDotProd3D (n, c) / ndotd;
    return UT_True;
}

UT_Bool UT_DVecIsectPlaneLine3D
        (const UT_DVec3D a, const UT_DVec3D n,
         const UT_DVec3D b, const UT_DVec3D d, Float64 *f)
{
    UT_DVec3D c;
    Float64 ndotd;

    UT_DVecSub3D (a, b, c);
    ndotd = UT_DVecDotProd3D (n, d);
    if (UT_ABS (ndotd) < Float64Prec) {
        return UT_False;
    }
    *f = UT_DVecDotProd3D (n, c) / ndotd;
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecDistLineLine3D
 *
 *      Usage:          Get the distance between two lines.
 *
 *      Synopsis:       UT_Bool UT_VecDistLineLine3D
 *                              (const UT_Vec3D a1, const UT_Vec3D b1,
 *                               const UT_Vec3D a2, const UT_Vec3D b2,
 *                               Float32 *d, Float32 *s1, Float32 *s2)
 *
 *      Description:    Given two lines in three dimensional space (defined
 *                      by one point on each line, a1 and a2, and the
 *                      directions of the lines, b2 and b2), finds the
 *                      distance, d, between these lines, and two numbers,
 *                      s1 and s2, so that the distance beween a1 + s1*b1
 *                      and a2 + s2*b2 is d.
 *
 *                      Pass NULL instead a pointer to a Float32 for d,
 *                      s1 or s2, if you are not interested in their values.
 *
 *                      There is an identical function "UT_DVecDistLineLine3D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   UT_True if d, s1 and s2 could be found, i.e. b1 and
 *                      b2 are not parallel; otherwise UT_False.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecDistLineLine3D
        (const UT_Vec3D a1, const UT_Vec3D b1,
         const UT_Vec3D a2, const UT_Vec3D b2,
         Float32 *d, Float32 *s1, Float32 *s2)
{
    UT_Vec3D c, n;
    Float32 b1dotb1, b2dotb1, b2dotb2, cdotb1, cdotb2, cdotn, divisor;

    UT_VecSub3D (a2, a1, c);
    if (d) {
        UT_VecCrossProd3D (b1, b2, n);
        if (!UT_VecUnit3D (n)) {
            return UT_False;
        }
        cdotn = UT_VecDotProd3D (c, n);
        *d = UT_ABS (cdotn);
    }
    if (s1 || s2) {
        b1dotb1 = UT_VecDotProd3D (b1, b1);
        b2dotb1 = UT_VecDotProd3D (b2, b1);
        b2dotb2 = UT_VecDotProd3D (b2, b2);
        divisor = b1dotb1 * b2dotb2 - b2dotb1 * b2dotb1;
        if (UT_ABS (divisor) < Float32Prec) {
            return UT_False;
        }
        cdotb1 = UT_VecDotProd3D (c, b1);
        cdotb2 = UT_VecDotProd3D (c, b2);
        if (s1) {
            *s1 = (cdotb1 * b2dotb2 - cdotb2 * b2dotb1) / divisor;
        }
        if (s2) {
            *s2 = (cdotb1 * b2dotb1 - cdotb2 * b1dotb1) / divisor;
        }
    }
    return UT_True;
}

UT_Bool UT_DVecDistLineLine3D
        (const UT_DVec3D a1, const UT_DVec3D b1,
         const UT_DVec3D a2, const UT_DVec3D b2,
         Float64 *d, Float64 *s1, Float64 *s2)
{
    UT_DVec3D c, n;
    Float64 b1dotb1, b2dotb1, b2dotb2, cdotb1, cdotb2, cdotn, divisor;

    UT_DVecSub3D (a2, a1, c);
    if (d) {
        UT_DVecCrossProd3D (b1, b2, n);
        if (!UT_DVecUnit3D (n)) {
            return UT_False;
        }
        cdotn = UT_DVecDotProd3D (c, n);
        *d = UT_ABS (cdotn);
    }
    if (s1 || s2) {
        b1dotb1 = UT_DVecDotProd3D (b1, b1);
        b2dotb1 = UT_DVecDotProd3D (b2, b1);
        b2dotb2 = UT_DVecDotProd3D (b2, b2);
        divisor = b1dotb1 * b2dotb2 - b2dotb1 * b2dotb1;
        if (UT_ABS (divisor) < Float64Prec) {
            return UT_False;
        }
        cdotb1 = UT_DVecDotProd3D (c, b1);
        cdotb2 = UT_DVecDotProd3D (c, b2);
        if (s1) {
            *s1 = (cdotb1 * b2dotb2 - cdotb2 * b2dotb1) / divisor;
        }
        if (s2) {
            *s2 = (cdotb1 * b2dotb1 - cdotb2 * b1dotb1) / divisor;
        }
    }
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecIsectPlanePlane3D
 *
 *      Usage:          Find the intersection line of two planes.
 *
 *      Synopsis:       UT_Bool UT_VecIsectPlanePlane3D
 *                              (const UT_Vec3D n1, Float32 d1,
 *                               const UT_Vec3D n2, Float32 d2,
 *                               UT_Vec3D dir, UT_Vec3D a)
 *
 *      Description:    Given two planes, defined by their normals, n1 and
 *                      n2, and the distances of these planes from the
 *                      coordinate origin, d1 and d2, the intersection
 *                      line of these two planes is computed.
 *                      The line is described by a direction vector, dir, and
 *                      a point on the line, a.
 *                      The direction of the line is the cross product of n1
 *                      and n2. To obtain a point on the line, we introduce a
 *                      plane constructed to have normal dir and to pass
 *                      through some point in the one of the other planes.
 *                      This yields a system of three linear equations which
 *                      should have a singular solution, a.
 *
 *                      Note: The length of n1 and n2 must be 1.0.
 *
 *                      There is an identical function "UT_DVecIsectPlanePlane3D"
 *                      for dealing with vectors in double precision.
 *
 *      Return value:   If a and dir could be found, UT_True is returned; if
 *                      the normals of the two planes are very similar,
 *                      a and dir can not be computed and 
 *                      "UT_VecIsectPlanePlane3D" returns UT_False.
 *
 *      See also:
 *
 ***************************************************************************/

UT_Bool UT_VecIsectPlanePlane3D
    (const UT_Vec3D n1, Float32 d1,
     const UT_Vec3D n2, Float32 d2,
     UT_Vec3D dir, UT_Vec3D a)
{
    Float32 maxval, scale, M[3][4];
    Int32   i, j, k, pivline=0;

    /* Find the direction vector. */
    UT_VecCrossProd3D (n1, n2, dir);
    if (!UT_VecUnit3D (dir)) {
        return UT_False;
    }

    /* Construct a set of three linear equations (the first three columns of
    M form the left side and the fourth column is the right side of the
    equations): Line 0 of M is the equation for the first plane, line 1 is
    the equation for the second plane, and line 2 is the equation of a
    plane orthogonal to the first two planes. */

    M[0][0] = n1[0];
    M[0][1] = n1[1];
    M[0][2] = n1[2];
    M[0][3] = d1;
    M[1][0] = n2[0];
    M[1][1] = n2[1];
    M[1][2] = n2[2];
    M[1][3] = d2;
    M[2][0] = dir[0];
    M[2][1] = dir[1];
    M[2][2] = dir[2];
    M[2][3] = d1 * UT_VecDotProd3D (dir, n1);

    /* Try to solve the equations using the Gaussian Algorithm. */

    for (j = 0; j < 3; ++j) {
        /* Find a good pivot element in column j of M. */
        maxval = 0.0;
        for (i = j; i < 3; ++i) {
            if (maxval < UT_ABS (M[i][j])) {
                maxval = UT_ABS (M[i][j]);
                pivline = i;
            }
        }
        if (maxval < Float32Prec) {
            /* No usable pivot element was found, the equations
            can not be solved. */
            return UT_False;
        }
        if (pivline != j) {
            /* Exchange line j and line pivline of M, so
            that M[j][j] becomes the pivot element. */
            for (i = j; i < 4; ++i) {
                UT_SWAP (M[j][i], M[pivline][i], Float32);
            }
        }

        /* Add a multiple of line j of M to all lines below j, so that all
        elements below the pivot element become equal to zero. */
        for (i = j + 1; i < 3; ++i) {
            scale = M[i][j] / M[j][j];
            for (k = j; k < 4; ++k) {
                M[i][k] -= scale * M[j][k];
            }
        }
    }

    for (j = 2; j >= 0; --j) {
        /* Backward substitution: Add a multiple of line j to all lines above
        j, so that all elements above M[j][j] become equal to zero. */
        for (i = j - 1; i <= 0; --i) {
            scale = M[i][j] / M[j][j];
            for (k = j; k < 4; ++k) {
                M[i][k] -= scale * M[j][k];
            }
        }
    }

    /* Finally extract a from the equations. */
    a[0] = M[0][3] / M[0][0];
    a[1] = M[1][3] / M[1][1];
    a[2] = M[2][3] / M[2][2];
    return UT_True;
}

UT_Bool UT_DVecIsectPlanePlane3D
        (const UT_DVec3D n1, Float64 d1,
         const UT_DVec3D n2, Float64 d2,
         UT_DVec3D dir, UT_DVec3D a)
{
    Float64 maxval, scale, M[3][4];
    Int32   i, j, k, pivline=0;

    /* Find the direction vector. */
    UT_DVecCrossProd3D (n1, n2, dir);
    if (!UT_DVecUnit3D (dir)) {
        return UT_False;
    }

    /* Construct a set of three linear equations (the first three columns of
    M form the left side and the fourth column is the right side of the
    equations): Line 0 of M is the equation for the first plane, line 1 is
    the equation for the second plane, and line 2 is the equation of a
    plane orthogonal to the first two planes. */

    M[0][0] = n1[0];
    M[0][1] = n1[1];
    M[0][2] = n1[2];
    M[0][3] = d1;
    M[1][0] = n2[0];
    M[1][1] = n2[1];
    M[1][2] = n2[2];
    M[1][3] = d2;
    M[2][0] = dir[0];
    M[2][1] = dir[1];
    M[2][2] = dir[2];
    M[2][3] = d1 * UT_DVecDotProd3D (dir, n1);

    /* Try to solve the equations using the Gaussian Algorithm. */

    for (j = 0; j < 3; ++j) {
        /* Find a good pivot element in column j of M. */
        maxval = 0.0;
        for (i = j; i < 3; ++i) {
            if (maxval < UT_ABS (M[i][j])) {
                maxval = UT_ABS (M[i][j]);
                pivline = i;
            }
        }
        if (maxval < Float64Prec) {
            /* No usable pivot element was found, the equations
            can not be solved. */
            return UT_False;
        }
        if (pivline != j) {
            /* Exchange line j and line pivline of M, so
            that M[j][j] becomes the pivot element. */
            for (i = j; i < 4; ++i) {
                UT_SWAP (M[j][i], M[pivline][i], Float64);
            }
        }

        /* Add a multiple of line j of M to all lines below j, so that all
        elements below the pivot element become equal to zero. */
        for (i = j + 1; i < 3; ++i) {
            scale = M[i][j] / M[j][j];
            for (k = j; k < 4; ++k) {
                M[i][k] -= scale * M[j][k];
            }
        }
    }

    for (j = 2; j >= 0; --j) {
        /* Backward substitution: Add a multiple of line j to all lines above
        j, so that all elements above M[j][j] become equal to zero. */
        for (i = j - 1; i <= 0; --i) {
            scale = M[i][j] / M[j][j];
            for (k = j; k < 4; ++k) {
                M[i][k] -= scale * M[j][k];
            }
        }
    }

    /* Finally extract a from the equations. */
    a[0] = M[0][3] / M[0][0];
    a[1] = M[1][3] / M[1][1];
    a[2] = M[2][3] / M[2][2];
    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecTriaToPlane3D
 *
 *      Usage:          Compute the plane equation of a triangle.
 *
 *      Synopsis:       UT_Bool UT_VecTriaToPlane3D
 *                              (const UT_DVec3D pos0,
 *                               const UT_DVec3D pos1,
 *                               const UT_DVec3D pos2,
 *                               UT_Plane3D *pl)
 *
 *      Description:    Compute the equation, "pl", for a plane containing a
 *                      triangle whose corners are at positions "pos0", "pos1"
 *                      and "pos2".
 *                      If the triangle's area is very close to zero, produce
 *                      an equation without solutions. To avoid rounding
 *                      errors, the plane equation is computed with double
 *                      and then converted to single precision.
 *
 *      Return value:   UT_True, if the plane equation could be calculated.
 *                      UT_False, if the equation calculated is not valid.
 *                      The normal of the plane is set to (0,0,0) and the 
 *                      distance is set to -1.
 * 
 *      See also:       UT_VecIsectToPlane3D
 *
 ***************************************************************************/

UT_Bool UT_VecTriaToPlane3D
        (const UT_DVec3D pos0,
         const UT_DVec3D pos1,
         const UT_DVec3D pos2,
         UT_Plane3D *pl)
{
    UT_DVec3D   dir1, dir2, nrm;
    Float64     tmp, odist;

    UT_DVecSub3D (pos1, pos0, dir1);
    UT_DVecSub3D (pos2, pos0, dir2);
    UT_DVecCrossProd3D (dir1, dir2, nrm);
    tmp = UT_DVecLength3D (nrm);

    if (tmp < MinNrmFloat64) {
        pl->dist = -1.0;
        pl->nrm[0] = 0.0;
        pl->nrm[1] = 0.0;
        pl->nrm[2] = 0.0;
        return UT_False;
    }

    tmp = 1.0 / tmp;
    UT_DVecScale3D (tmp, nrm, nrm);
    odist = -UT_DVecDotProd3D (pos0, nrm);
    pl->dist = odist;
    pl->nrm[0] = nrm[0];
    pl->nrm[1] = nrm[1];
    pl->nrm[2] = nrm[2];

    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecIsectToPlane3D
 *
 *      Usage:          Compute a plane equation triangle intersection testing.
 *
 *      Synopsis:       UT_Bool UT_VecIsectToPlane3D
 *                              (const UT_DVec3D pos0,
 *                               const UT_DVec3D pos1,
 *                               const UT_DVec3D pos2,
 *                               UT_Plane3D *pl)
 *
 *      Description:    Compute a plane equation for quick point-in-triangle
 *                      testing: "Pos0", "pos1" and "pos2" define the the 
 *                      corners of a triangle. 
 *                      A plane equation, "pl", is produced, so that
 *                      (UT_VecDotProd3D (pl->nrm, pos0) + pl->dist) == 0.0 &&
 *                      (UT_VecDotProd3D (pl->nrm, pos1) + pl->dist) == 0.0 &&
 *                      (UT_VecDotProd3D (pl->nrm, pos2) + pl->dist) == 1.0
 *                      
 *                      If the triangle's area is very small, an equation
 *                      without solutions is produced.
 *                      To avoid rounding errors, the plane equation is computed
 *                      with double and then converted to single precision.
 *
 *      Return value:   UT_True, if the plane equation could be calculated.
 *                      UT_False, if the equation calculated is not valid.
 *                      The normal of the plane is set to (0,0,0) and the 
 *                      distance is set to -1.
 * 
 *      See also:       UT_VecTriaToPlane3D
 *
 ***************************************************************************/

UT_Bool UT_VecIsectToPlane3D
        (const UT_DVec3D pos0,
         const UT_DVec3D pos1,
         const UT_DVec3D pos2,
         UT_Plane3D *pl)
{
    UT_DVec3D   dir1, dir2, nrm;
    Float64     tmp, odist;

    UT_DVecSub3D (pos1, pos0, dir1);
    UT_DVecSub3D (pos2, pos0, dir2);
    tmp = UT_DVecLength3D (dir1);

    if (tmp < MinNrmFloat64) {
        pl->dist = -1.0;
        pl->nrm[0] = 0.0;
        pl->nrm[1] = 0.0;
        pl->nrm[2] = 0.0;
        return UT_False;
    }

    tmp = 1.0 / tmp;
    UT_DVecScale3D (tmp, dir1, dir1);
    tmp = UT_DVecDotProd3D (dir1, dir2);
    UT_DVecScale3D (tmp, dir1, dir1);
    UT_DVecSub3D (dir2, dir1, nrm);
    tmp = UT_DVecDotProd3D (nrm, nrm);

    if (tmp < MinNrmFloat64) {
        pl->dist = -1.0;
        pl->nrm[0] = 0.0;
        pl->nrm[1] = 0.0;
        pl->nrm[2] = 0.0;
        return UT_False;
    }

    tmp = 1.0 / tmp;
    UT_DVecScale3D (tmp, nrm, nrm);
    odist = -UT_DVecDotProd3D (pos0, nrm);
    pl->dist = odist;
    pl->nrm[0] = nrm[0];
    pl->nrm[1] = nrm[1];
    pl->nrm[2] = nrm[2];

    return UT_True;
}

/***************************************************************************
 *[@e
 *      Name:           UT_VecIsectLineTria3D
 *
 *      Usage:          Find the intersection point of a line and a triangle.
 *
 *      Synopsis:       UT_Bool UT_VecIsectLineTria3D
 *                              (const UT_Vec3D lineStart,
 *                               const UT_Vec3D lineDir,
 *                               const UT_DVec3D pos0,
 *                               const UT_DVec3D pos1, 
 *                               const UT_DVec3D pos2,
 *                               UT_Vec3D isectPos,
 *                               UT_Vec3D isectNrm,
 *                               Float32 *distance)
 *
 *      Description:    Find the intersection point of a a straight line and
 *                      a triangle.
 *                      The line is defined by its start point "lineStart" and
 *                      it's direction "lineDir".
 *                      The triangle is defined by its 3 control points: "pos0",
 *                      "pos1" and "pos2".
 *                      The intersection between the line and the triangle are
 *                      returned in parameters "isectPos", "isectNrm" and 
 *                      "distance".
 *                      "isectPos" is the position of the intersection point.
 *                      "isectNrm" is the normal of the triangle at the 
 *                      intersection point.
 *                      "distance" is the distance between "lineStart" and
 *                      the intersection point.
 *
 *                      Description of the algorithm:
 *                      The control points of the triangle are converted into 3
 *                      plane equations, "pl0", "pl1" and "pl2".
 *                      Each plane is defined by its normal vector and its
 *                      distance from the coordinate origin.
 *                      "pl0" is the plane containing all three corners of the
 *                      triangle. The normal vector of "pl0" has been scaled to
 *                      length 1.0 and points to the triangle's outer side.
 *                      With "pl0", it is easy to determine if a given
 *                      line approaches the triangle from the inner or from 
 *                      the outer side. The position, "ipoint", of the 
 *                      intersection of the line and "pl0" can also be computed
 *                      quickly. The other two plane equations are used to
 *                      determine if "ipoint" is inside the triangle:
 *                      "Pl1" describes a plane which is perpendicular to
 *                      "pl0" and contains the first two vertices of the
 *                      triangle. The normal vector of "pl1" has been scaled so
 *                      that substituting the triangle's third vertex into "pl1"
 *                      yields 1.0. "Pl2" is analogous; "pl2" contains the
 *                      triangle's first and third vertex, and substituting the
 *                      second vertex yields 1.0.
 *                      Substituting "ipoint" into "pl1" and "pl2" results in
 *                      two factors, "u" and "v". 
 *                      "Ipoint" is inside the triangle if (u >= 0.0) and
 *                      (v >= 0.0) and (u + v <= 1.0).
 *
 *                      Note: A slightly modified and inlined version of this
 *                            function is used in the ray-tracer for 
 *                            ray-triangle intersection tests.
 *
 *      Return value:   If the triangle and the line are not parallel,
 *                      "isectPos", "isectNrm" and "distance" are set
 *                      and UT_True is returned.
 *                      If no intersection point can be found, i.e. the
 *                      triangle and the line are parallel, UT_False is returned
 *                      and "isectPos", "isectNrm" and "distance" are not set.
 *
 *      See also:       UT_VecTriaToPlane3D
 *                      UT_VecIsectToPlane3D
 *
 ***************************************************************************/

UT_Bool UT_VecIsectLineTria3D 
        (const UT_Vec3D lineStart, const UT_Vec3D lineDir,
         const UT_DVec3D pos0, const UT_DVec3D pos1, const UT_DVec3D pos2, 
         UT_Vec3D isectPos, UT_Vec3D isectNrm, Float32 *distance)
{
    Float32     rn_dot_ir, idist, u, v;
    UT_Vec3D    ipoint;
    UT_Plane3D  pl0, pl1, pl2;

    /* First create the plane equations. */
    if (!UT_VecTriaToPlane3D  (pos0, pos1, pos2, &pl0) ||
        !UT_VecIsectToPlane3D (pos0, pos2, pos1, &pl1) ||
        !UT_VecIsectToPlane3D (pos0, pos1, pos2, &pl2)) {
        return UT_False;
    }

    /* Test if the line approaches the triangle from the inner or from
       the outer side. */
    rn_dot_ir = UT_VecDotProd3D (lineDir, pl0.nrm);
    if (UT_ABS (rn_dot_ir) < 10.0 * MinNrmFloat32) {
        /* The ray is nearly parallel to the triangle's plane so that we cannot
         * compute the intersection point.
         */
        return UT_False;
    }

    /* Compute the distance between the ray's origin and the intersection
     * of the ray and the plane containing the triangle.
     */
    idist = -(pl0.dist + UT_VecDotProd3D (lineStart, pl0.nrm)) / rn_dot_ir;

   /* Find the intersection point, "ipoint", and test if it is
    * inside the triangle.
    */
    UT_VecAddScaled3D (lineDir, lineStart, idist, ipoint); 

    u = pl1.dist + UT_VecDotProd3D (ipoint, pl1.nrm);
    v = pl2.dist + UT_VecDotProd3D (ipoint, pl2.nrm);

    if ((u + UT_LineTriaPrec >= 0.0) && 
        (v + UT_LineTriaPrec >= 0.0) && 
        (u + v - UT_LineTriaPrec <= 1.0)) {
        UT_VecCopy3D (ipoint,  isectPos);
        UT_VecCopy3D (pl0.nrm, isectNrm);
        *distance = idist;
        return UT_True;
    } else {
        /* "ipoint" is not inside the triangle, the triangle and
         * the ray do not intersect. */
        return UT_False;
    }
}
