/*  Starshatter OpenSource Distribution
    Copyright (c) 1997-2004, Destroyer Studios LLC.
    All Rights Reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.
    * Neither the name "Destroyer Studios" nor the names of its contributors
      may be used to endorse or promote products derived from this software
      without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    POSSIBILITY OF SUCH DAMAGE.

    SUBSYSTEM:    nGenEx.lib
    FILE:         Geometry.cpp
    AUTHOR:       John DiCamillo


    OVERVIEW
    ========
    Geometric Utilities
*/

#include "MemDebug.h"
#include "Geometry.h"

// +--------------------------------------------------------------------+

void Rect::Inflate(int dx, int dy)
{
    x -= dx;
    w += dx*2;
    y -= dy;
    h += dy*2;
}

void Rect::Deflate(int dx, int dy)
{
    x += dx;
    w -= dx*2;
    y += dy;
    h -= dy*2;
}

void Rect::Inset(int l, int r, int t, int b)
{
    x += l;
    y += t;
    w -= l + r;
    h -= t + b;
}

int Rect::Contains(int ax, int ay) const
{
    if (ax < x)    return 0;
    if (ax > x+w)  return 0;
    if (ay < y)    return 0;
    if (ay > y+h)  return 0;

    return 1;
}

// +--------------------------------------------------------------------+

double
Point::Normalize()
{
    double scale = 1.0;
    double len   = length();

    if (len)
    scale /= len;

    x *= scale;
    y *= scale;
    z *= scale;

    return len;
}

// +--------------------------------------------------------------------+

void
Point::SetElement(int i, double v)
{
    switch (i) {
    case 0:  x = v; break;
    case 1:  y = v; break;
    case 2:  z = v; break;
    default:        break;
    }
}

// +--------------------------------------------------------------------+

Point
Point::operator*(const Matrix& m) const
{
    Point result;

    result.x = (m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z);
    result.y = (m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z);
    result.z = (m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z);

    return result;
}

// +--------------------------------------------------------------------+

double ClosestApproachTime(const Point& loc1, const Point& vel1,
const Point& loc2, const Point& vel2)
{
    double t = 0;

    Point D  = loc1-loc2;
    Point Dv = vel1-vel2;

    if (Dv.x || Dv.y || Dv.z)
    t = -1 * (Dv*D) / (Dv*Dv);

    return t;
}

// +--------------------------------------------------------------------+

float
Vec2::Normalize()
{
    float  scale = 1.0f;
    float  len   = length();

    if (len)
    scale /= len;

    x *= scale;
    y *= scale;

    return len;
}

// +--------------------------------------------------------------------+

float
Vec3::Normalize()
{
    float  scale = 1.0f;
    float  len   = length();

    if (len)
    scale /= len;

    x *= scale;
    y *= scale;
    z *= scale;

    return len;
}

// +--------------------------------------------------------------------+

Vec3
Vec3::operator*(const Matrix& m) const
{
    Vec3 result;

    result.x = (float) ((m.elem[0][0] * x) + (m.elem[1][0] * y) + (m.elem[2][0] * z));
    result.y = (float) ((m.elem[0][1] * x) + (m.elem[1][1] * y) + (m.elem[2][1] * z));
    result.z = (float) ((m.elem[0][2] * x) + (m.elem[1][2] * y) + (m.elem[2][2] * z));

    return result;
}

// +--------------------------------------------------------------------+

double ClosestApproachTime(const Vec3& loc1, const Vec3& vel1,
const Vec3& loc2, const Vec3& vel2)
{
    double t = 0;

    Point D  = loc1-loc2;
    Point Dv = vel1-vel2;

    if (Dv.x || Dv.y || Dv.z)
    t = -1 * (Dv*D) / (Dv*Dv);

    return t;
}

// +--------------------------------------------------------------------+

double
Quaternion::Normalize()
{
    double scale = 1.0;
    double len   = length();

    if (len)
    scale /= len;

    x *= scale;
    y *= scale;
    z *= scale;
    w *= scale;

    return len;
}

// +--------------------------------------------------------------------+

Matrix::Matrix()
{
    Identity();
}

Matrix::Matrix(const Matrix& m)
{
    CopyMemory(elem, m.elem, sizeof(elem));
}

Matrix::Matrix(const Point& vrt, const Point& vup, const Point& vpn)
{
    elem[0][0] = vrt.x;
    elem[0][1] = vrt.y;
    elem[0][2] = vrt.z;

    elem[1][0] = vup.x;
    elem[1][1] = vup.y;
    elem[1][2] = vup.z;

    elem[2][0] = vpn.x;
    elem[2][1] = vpn.y;
    elem[2][2] = vpn.z;
}

// +--------------------------------------------------------------------+

Matrix&
Matrix::operator =(const Matrix& m)
{
    CopyMemory(elem, m.elem, sizeof(elem));

    return *this;
}

// +--------------------------------------------------------------------+

Matrix&
Matrix::operator*=(const Matrix& m)
{
    return *this = *this * m;
}

// +--------------------------------------------------------------------+

void
Matrix::Identity()
{
    elem[0][0] = 1;
    elem[0][1] = 0;
    elem[0][2] = 0;

    elem[1][0] = 0;
    elem[1][1] = 1;
    elem[1][2] = 0;

    elem[2][0] = 0;
    elem[2][1] = 0;
    elem[2][2] = 1;
}

// +--------------------------------------------------------------------+

inline void swap_elem(double& a, double& b) { double t=a; a=b; b=t; }

void
Matrix::Transpose()
{
    swap_elem(elem[0][1], elem[1][0]);
    swap_elem(elem[0][2], elem[2][0]);
    swap_elem(elem[1][2], elem[2][1]);
}

// +--------------------------------------------------------------------+

void
Matrix::Rotate(double roll, double pitch, double yaw)
{
    double e[3][3];
    CopyMemory(e, elem, sizeof(elem));

    double sr = sin(roll);
    double cr = cos(roll);
    double sp = sin(pitch);
    double cp = cos(pitch);
    double sy = sin(yaw);
    double cy = cos(yaw);

    double a,b,c;

    a = cy*cr;
    b = cy*sr;
    c = -sy;

    elem[0][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
    elem[0][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
    elem[0][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];

    a = cp*-sr + sp*sy*cr;
    b = cp* cr + sp*sy*sr;
    c = sp*cy;

    elem[1][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
    elem[1][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
    elem[1][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];

    a = -sp*-sr + cp*sy*cr;
    b = -sp* cr + cp*sy*sr;
    c = cp*cy;

    elem[2][0] = a*e[0][0] + b*e[1][0] + c*e[2][0];
    elem[2][1] = a*e[0][1] + b*e[1][1] + c*e[2][1];
    elem[2][2] = a*e[0][2] + b*e[1][2] + c*e[2][2];
}

// +--------------------------------------------------------------------+

void
Matrix::Roll(double roll)
{
    double s = sin(roll);
    double c = cos(roll);

    double e00 = elem[0][0];
    double e01 = elem[0][1];
    double e02 = elem[0][2];
    double e10 = elem[1][0];
    double e11 = elem[1][1];
    double e12 = elem[1][2];

    elem[0][0] =  c*e00 + s*e10;
    elem[0][1] =  c*e01 + s*e11;
    elem[0][2] =  c*e02 + s*e12;

    elem[1][0] = -s*e00 + c*e10;
    elem[1][1] = -s*e01 + c*e11;
    elem[1][2] = -s*e02 + c*e12;
}

// +--------------------------------------------------------------------+

void
Matrix::Pitch(double pitch)
{
    double s = sin(pitch);
    double c = cos(pitch);

    double e10 = elem[1][0];
    double e11 = elem[1][1];
    double e12 = elem[1][2];
    double e20 = elem[2][0];
    double e21 = elem[2][1];
    double e22 = elem[2][2];

    elem[1][0] =  c*e10 + s*e20;
    elem[1][1] =  c*e11 + s*e21;
    elem[1][2] =  c*e12 + s*e22;

    elem[2][0] = -s*e10 + c*e20;
    elem[2][1] = -s*e11 + c*e21;
    elem[2][2] = -s*e12 + c*e22;
}

// +--------------------------------------------------------------------+

void
Matrix::Yaw(double yaw)
{
    double s = sin(yaw);
    double c = cos(yaw);

    double e00 = elem[0][0];
    double e01 = elem[0][1];
    double e02 = elem[0][2];
    double e20 = elem[2][0];
    double e21 = elem[2][1];
    double e22 = elem[2][2];

    elem[0][0] = c*e00 - s*e20;
    elem[0][1] = c*e01 - s*e21;
    elem[0][2] = c*e02 - s*e22;

    elem[2][0] = s*e00 + c*e20;
    elem[2][1] = s*e01 + c*e21;
    elem[2][2] = s*e02 + c*e22;
}

// +--------------------------------------------------------------------+

inline int sign(double d) { return (d >= 0); }

void
Matrix::ComputeEulerAngles(double& roll, double& pitch, double& yaw) const
{
    double cy;

    yaw   = asin(-elem[0][2]);
    cy    = cos(yaw);
    roll  = asin(elem[0][1] / cy);
    pitch = asin(elem[1][2] / cy);

    if (sign(cos(roll)*cy) != sign(elem[0][0]))
    roll = PI - roll;

    if (sign(cos(pitch)*cy) != sign(elem[2][2]))
    pitch = PI - pitch;
}

// +--------------------------------------------------------------------+

Matrix
Matrix::operator*(const Matrix& m) const
{
    Matrix r;

    r.elem[0][0] = elem[0][0]*m.elem[0][0] + elem[0][1]*m.elem[1][0] + elem[0][2]*m.elem[2][0];
    r.elem[0][1] = elem[0][0]*m.elem[0][1] + elem[0][1]*m.elem[1][1] + elem[0][2]*m.elem[2][1];
    r.elem[0][2] = elem[0][0]*m.elem[0][2] + elem[0][1]*m.elem[1][2] + elem[0][2]*m.elem[2][2];

    r.elem[1][0] = elem[1][0]*m.elem[0][0] + elem[1][1]*m.elem[1][0] + elem[1][2]*m.elem[2][0];
    r.elem[1][1] = elem[1][0]*m.elem[0][1] + elem[1][1]*m.elem[1][1] + elem[1][2]*m.elem[2][1];
    r.elem[1][2] = elem[1][0]*m.elem[0][2] + elem[1][1]*m.elem[1][2] + elem[1][2]*m.elem[2][2];

    r.elem[2][0] = elem[2][0]*m.elem[0][0] + elem[2][1]*m.elem[1][0] + elem[2][2]*m.elem[2][0];
    r.elem[2][1] = elem[2][0]*m.elem[0][1] + elem[2][1]*m.elem[1][1] + elem[2][2]*m.elem[2][1];
    r.elem[2][2] = elem[2][0]*m.elem[0][2] + elem[2][1]*m.elem[1][2] + elem[2][2]*m.elem[2][2];

    return r;
}

// +--------------------------------------------------------------------+

Point
Matrix::operator*(const Point& p) const
{
    Point result;

    result.x = (elem[0][0] * p.x) + (elem[0][1] * p.y) + (elem[0][2] * p.z);
    result.y = (elem[1][0] * p.x) + (elem[1][1] * p.y) + (elem[1][2] * p.z);
    result.z = (elem[2][0] * p.x) + (elem[2][1] * p.y) + (elem[2][2] * p.z);

    return result;
}

// +--------------------------------------------------------------------+

Vec3
Matrix::operator*(const Vec3& v) const
{
    Vec3 result;

    result.x = (float) ((elem[0][0] * v.x) + (elem[0][1] * v.y) + (elem[0][2] * v.z));
    result.y = (float) ((elem[1][0] * v.x) + (elem[1][1] * v.y) + (elem[1][2] * v.z));
    result.z = (float) ((elem[2][0] * v.x) + (elem[2][1] * v.y) + (elem[2][2] * v.z));

    return result;
}

// +--------------------------------------------------------------------+

double
Matrix::Cofactor(int i, int j) const
{
    int i1=0;
    int i2=2;
    int j1=0;
    int j2=2;

    if (i==0) i1=1; else if (i==2) i2=1;
    if (j==0) j1=1; else if (j==2) j2=1;

    double factor = elem[i1][j1]*elem[i2][j2] - elem[i1][j2]*elem[i2][j1];

    if ((i+j) & 1)
    factor *= -1;

    return factor;
}

// +--------------------------------------------------------------------+

void
Matrix::Invert()
{
    double f[3][3];
    int    i, j;

    for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
    f[i][j] = Cofactor(j,i);

    double det = elem[0][0] * f[0][0] +
    elem[0][1] * f[1][0] +
    elem[0][2] * f[2][0];

    if (det != 0) {
        double inv = 1/det;

        for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++)
        elem[i][j] = f[i][j] * inv;
    }
}

// +--------------------------------------------------------------------+
// +--------------------------------------------------------------------+
// +--------------------------------------------------------------------+

Plane::Plane()
: distance(0.0f)
{ }

Plane::Plane(const Point& p0, const Point& p1, const Point& p2)
{
    Point d1 = p1 - p0;
    Point d2 = p2 - p0;

    normal = (Vec3) d1.cross(d2);
    normal.Normalize();

    distance = (float) (normal * p0);
}

Plane::Plane(const Vec3& v0, const Vec3& v1, const Vec3& v2)
{
    Vec3 d1 = v1 - v0;
    Vec3 d2 = v2 - v0;

    normal = d1.cross(d2);
    normal.Normalize();

    distance = normal * v0;
}

void Plane::Rotate(const Vec3& v0, const Matrix& m)
{
    normal   = normal * m;
    distance = normal * v0;
}

void Plane::Translate(const Vec3& v0)
{
    distance = normal * v0;
}

// +--------------------------------------------------------------------+
// 3-D dot product.

double DotProduct(const Point& a, const Point& b)
{
    return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
}

// +--------------------------------------------------------------------+
// 3-D cross product.

void CrossProduct(const Point& a, const Point& b, Point& out)
{
    out.x = (a.y * b.z) - (a.z * b.y);
    out.y = (a.z * b.x) - (a.x * b.z);
    out.z = (a.x * b.y) - (a.y * b.x);
}

// +--------------------------------------------------------------------+
// Concatenate two 3x3 matrices.

void MConcat(double in1[3][3], double in2[3][3], double out[3][3])
{
    int     i, j;

    for (i=0 ; i<3 ; i++) {
        for (j=0 ; j<3 ; j++) {
            out[i][j] = in1[i][0] * in2[0][j] +
            in1[i][1] * in2[1][j] +
            in1[i][2] * in2[2][j];
        }
    }
}

/* GRAPHICS GEMS II ----------------------------------------------------
*
* lines_intersect:  AUTHOR: Mukesh Prasad
*
*   This function computes whether two line segments,
*   respectively joining the input points (x1,y1) -- (x2,y2)
*   and the input points (x3,y3) -- (x4,y4) intersect.
*   If the lines intersect, the output variables x, y are
*   set to coordinates of the point of intersection.
*
*   All values are in integers.  The returned value is rounded
*   to the nearest integer point.
*
*   If non-integral grid points are relevant, the function
*   can easily be transformed by substituting floating point
*   calculations instead of integer calculations.
*
*   Entry
*        x1, y1,  x2, y2   Coordinates of endpoints of one segment.
*        x3, y3,  x4, y4   Coordinates of endpoints of other segment.
*
*   Exit
*        x, y              Coordinates of intersection point.
*
*   The value returned by the function is one of:
*
*        DONT_INTERSECT    0
*        DO_INTERSECT      1
*        COLLINEAR         2
*
* Error conditions:
*
*     Depending upon the possible ranges, and particularly on 16-bit
*     computers, care should be taken to protect from overflow.
*
*     In the following code, 'long' values have been used for this
*     purpose, instead of 'int'.
*
*/

#define DONT_INTERSECT    0
#define DO_INTERSECT      1
#define COLLINEAR         2

inline int SAME_SIGNS(double a, double b)
{
    return ((a>=0 && b>=0) || (a<0 && b<0));
}

int
lines_intersect(
/* 1st line segment */ double x1, double y1, double x2, double y2,
/* 2nd line segment */ double x3, double y3, double x4, double y4,
/* pt of intersect  */ double& ix, double& iy)
{
    double a1, a2, b1, b2, c1, c2; /* Coefficients of line eqns. */
    double r1, r2, r3, r4;         /* 'Sign' values */
    double denom, offset, num;     /* Intermediate values */

    /* Compute a1, b1, c1, where line joining points 1 and 2
    * is "a1 x  +  b1 y  +  c1  =  0".  */

    a1 = y2 - y1;
    b1 = x1 - x2;
    c1 = x2 * y1 - x1 * y2;

    /* Compute r3 and r4.  */

    r3 = a1 * x3 + b1 * y3 + c1;
    r4 = a1 * x4 + b1 * y4 + c1;

    /* Check signs of r3 and r4.  If both point 3 and point 4 lie on
    * same side of line 1, the line segments do not intersect.  */

    if ( r3 != 0 &&
            r4 != 0 &&
            SAME_SIGNS( r3, r4 ))
    return ( DONT_INTERSECT );

    /* Compute a2, b2, c2 */

    a2 = y4 - y3;
    b2 = x3 - x4;
    c2 = x4 * y3 - x3 * y4;

    /* Compute r1 and r2 */

    r1 = a2 * x1 + b2 * y1 + c2;
    r2 = a2 * x2 + b2 * y2 + c2;

    /* Check signs of r1 and r2.  If both point 1 and point 2 lie
    * on same side of second line segment, the line segments do
    * not intersect.  */

    if ( r1 != 0 &&
            r2 != 0 &&
            SAME_SIGNS( r1, r2 ))
    return ( DONT_INTERSECT );

    /* Line segments intersect: compute intersection point.  */

    denom = a1 * b2 - a2 * b1;
    if ( denom == 0 )
    return ( DONT_INTERSECT );
    offset = denom < 0 ? - denom / 2 : denom / 2;

    /* The denom/2 is to get rounding instead of truncating.  It
    * is added or subtracted to the numerator, depending upon the
    * sign of the numerator.  */

    num = b1 * c2 - b2 * c1;
    ix = ( num < 0 ? num - offset : num + offset ) / denom;

    num = a2 * c1 - a1 * c2;
    iy = ( num < 0 ? num - offset : num + offset ) / denom;

    return ( DO_INTERSECT );
}