/*---------------------------------------------------------------------------*
  Project:  Horizon
  File:     math_Geometry.cpp
  Copyright (C)2009 Nintendo Co., Ltd.  All rights reserved.
  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law. They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.
  $Rev: 24051 $
 *---------------------------------------------------------------------------


*/

#include <nn/math.h>

#include <nn/math/math_Geometry.h>
#include <algorithm>

namespace nn { namespace math {

namespace internal
{
    f32 epsilon = 0.0001F;
    f32 infinity = 100000000.F;
}

//
// Member function of the 3D geometry
//
void
PLANE::Set(const VEC3* P0, const VEC3* P1, const VEC3* P2)
{
    // P0, P1, P2 is clockwise looking from above.
    VEC3 v0, v1, v2;
    VEC3Sub(&v0, P2, P0);
    VEC3Sub(&v1, P1, P0);

    VEC3Normalize(&N, VEC3Cross(&v2, &v0, &v1));
    d = -VEC3Dot(&N, P0);
}

void
AABB::Normalize()
{
    if (Pmin.x > Pmax.x)
        ::std::swap(Pmin.x, Pmax.x);
    if (Pmin.y > Pmax.y)
        ::std::swap(Pmin.y, Pmax.y);
    if (Pmin.z > Pmax.z)
        ::std::swap(Pmin.z, Pmax.z);
}

void
AABB::Set(const VEC3* arrayPoint, unsigned int numPoints)
{
    Pmin = arrayPoint[0];
    Pmax = arrayPoint[0];
    
    for (unsigned int i = 1; i < numPoints; ++i)
    {
        if (arrayPoint[i].x < Pmin.x)
            Pmin.x = arrayPoint[i].x;
        else if (arrayPoint[i].x > Pmax.x)
            Pmax.x = arrayPoint[i].x;

        if (arrayPoint[i].y < Pmin.y)
            Pmin.y = arrayPoint[i].y;
        else if (arrayPoint[i].y > Pmax.y)
            Pmax.y = arrayPoint[i].y;

        if (arrayPoint[i].z < Pmin.z)
            Pmin.z = arrayPoint[i].z;
        else if (arrayPoint[i].z > Pmax.z)
            Pmax.z = arrayPoint[i].z;
    }
}

void
AABB::Set(const AABB* box, const MTX34* M)
{
    f32 x0, y0, z0;
    f32 x1, y1, z1;

    f32 a0, a1;
    f32 b0, b1;

    // Obtain the minimum and maximum values for the new x-axis.
    x0 = M->f._00 * box->Pmin.x + M->f._03;
    x1 = M->f._00 * box->Pmax.x + M->f._03;
    a0 = M->f._01 * box->Pmin.y;
    a1 = M->f._01 * box->Pmax.y;
    b0 = M->f._02 * box->Pmin.z;
    b1 = M->f._02 * box->Pmax.z;

    if (x0 > x1)
        ::std::swap(x0, x1);
    if (a0 < a1) { x0 += a0;  x1 += a1; }
    else { x0 += a1; x1 += a0; }
    if (b0 < b1) { x0 += b0; x1 += b1; }
    else { x0 += b1; x1 += b0; }

    // Obtain the minimum and maximum values for the new y-axis.
    y0 = M->f._10 * box->Pmin.x + M->f._13;
    y1 = M->f._10 * box->Pmax.x + M->f._13;
    a0 = M->f._11 * box->Pmin.y;
    a1 = M->f._11 * box->Pmax.y;
    b0 = M->f._12 * box->Pmin.z;
    b1 = M->f._12 * box->Pmax.z;

    if (y0 > y1)
        ::std::swap(y0, y1);
    if (a0 < a1) { y0 += a0; y1 += a1; }
    else { y0 += a1; y1 += a0; }
    if (b0 < b1) { y0 += b0; y1 += b1; }
    else { y0 += b1; y1 += b0; }

    // Obtain the minimum and maximum values for the new z-axis.
    z0 = M->f._20 * box->Pmin.x + M->f._23;
    z1 = M->f._20 * box->Pmax.x + M->f._23;
    a0 = M->f._21 * box->Pmin.y;
    a1 = M->f._21 * box->Pmax.y;
    b0 = M->f._22 * box->Pmin.z;
    b1 = M->f._22 * box->Pmax.z;

    if (z0 > z1)
        ::std::swap(z0, z1);
    if (a0 < a1) { z0 += a0; z1 += a1; }
    else { z0 += a1; z1 += a0; }
    if (b0 < b1) { z0 += b0; z1 += b1; }
    else { z0 += b1; z1 += b0; }

    Pmin.x = x0;
    Pmin.y = y0;
    Pmin.z = z0;

    Pmax.x = x1;
    Pmax.y = y1;
    Pmax.z = z1;
}




void
SPHERE::Set(const VEC3* arrayPoint, unsigned int numPoints)
{
    AABB tmp;
    tmp.Set(arrayPoint, numPoints);
    VEC3Lerp(&C, &tmp.Pmin, &tmp.Pmax, 0.5f);

    f32 maxDistance = VEC3SquareDist(&C, &arrayPoint[0]);
    for (unsigned int i = 1; i < numPoints; ++i)
    {
        f32 dist = VEC3SquareDist(&C, &arrayPoint[i]);
        if (dist > maxDistance)
            maxDistance = dist;
    }
    r = FSqrt(maxDistance);
}

void
FRUSTUM::Set(f32 top, f32 bottom, f32 left, f32 right, f32 n, f32 f, const MTX34& camera)
{
    MTX34 invCamera;
    MTX34Inverse(&invCamera, &camera);
    MTX34Copy(&cam, &camera);
    // An assert of inverse matrix will be inserted.

    VEC3 P0(0.f, 0.f, 0.f);
    VEC3 P[8];

    f32 f_n = f / n;

    // right hand type
    // P[0] is at upper left of 'near'
    P[0].x = left; P[0].y = top; P[0].z = -n;

    // P[1] is at upper right of 'near'
    P[1].x = right; P[1].y = top; P[1].z = -n;

    // P[2] is at lower right of 'near'
    P[2].x = right; P[2].y = bottom; P[2].z = -n;

    // P[3] is at lower left of 'near'
    P[3].x = left; P[3].y = bottom; P[3].z = -n;

    // P[4] is at upper left of 'far'
    P[4].x = f_n * left; P[4].y = f_n * top; P[4].z = -f;

    // P[5] is at upper right of 'far'
    P[5].x = f_n * right; P[5].y = f_n * top; P[5].z = -f;

    // P[6] is at lower right of 'far'
    P[6].x = f_n * right; P[6].y = f_n * bottom; P[6].z = -f;

    // P[7] is at lower left of 'far'
    P[7].x = f_n * left; P[7].y = f_n * bottom; P[7].z = -f;

    // near, far
    znear = -n;
    zfar = -f;

    // right hand type
    leftPlane.Set(&P0, &P[3], &P[0]);
    rightPlane.Set(&P0, &P[1], &P[2]);
    topPlane.Set(&P0, &P[0], &P[1]);
    bottomPlane.Set(&P0, &P[2], &P[3]);

    // Convert P[0-7] to world coordinate system and set AABB.
    int i;
    for (i = 0; i < 8; ++i)
    {
        VEC3Transform(&P[i], &invCamera, &P[i]);
    }
    
    VEC3Transform(&P0, &invCamera, &P0);
    box.Set(&P[0], 8);

    planes[0].Set(&P0, &P[3], &P[0]);   // Left
    planes[1].Set(&P0, &P[1], &P[2]);   // Right
    planes[2].Set(&P[0], &P[1], &P[2]); // Near
    planes[3].Set(&P[4], &P[7], &P[6]); // Far
    planes[4].Set(&P0, &P[0], &P[1]);   // Up
    planes[5].Set(&P0, &P[2], &P[3]);   // down
}






f32
DistSqPoint3ToLine3(const VEC3* P, const LINE3* L, f32* t)
{
    f32 distSq;

    // P, L
    VEC3 LP;
    f32 t_ = VEC3Dot(&L->d, VEC3Sub(&LP, P, &L->P));

    VEC3 PP;
    VEC3Add(&PP, &L->P, VEC3Scale(&PP, &L->d, t_));

    VEC3 PP_P;
    VEC3Sub(&PP_P, P, &PP);
    distSq = VEC3SquareLen(&PP_P);

    if (t)
        *t = t_;
    return distSq;
}


f32
DistSqPoint3ToRay3(const VEC3* P, const RAY3* R, f32* t)
{
    f32 distSq;
    f32 t_;

    distSq = DistSqPoint3ToLine3(P, (const LINE3*)R, &t_);

    if (t_ < 0)
    {
        t_ = 0;

        VEC3 vec;
        VEC3Sub(&vec, P, &R->P);
        distSq = VEC3SquareLen(&vec);
    }

    
    if (t)
        *t = t_;
    return distSq;
}

f32
DistSqPoint3ToSegment3(const VEC3* P, const SEGMENT3* S, f32* t)
{
    f32 distSq;
    f32 t_;
    LINE3 L;
    L.Set(S);

    f32 P0P1 = FSqrt((S->P0 - S->P1).LenSq());
    distSq = DistSqPoint3ToLine3(P, &L, &t_);

    if (t_ < 0.f)
    {
        if (t)
            *t = 0.f;

        VEC3 vec;
        VEC3Sub(&vec, P, &S->P0);
        distSq = VEC3SquareLen(&vec);
    }
    else if (t_ > P0P1)
    {
        if (t)
            *t = 1.f;

        VEC3 vec;
        VEC3Sub(&vec, P, &S->P1);
        distSq = VEC3SquareLen(&vec);
    }
    else
    {
        if (t)
            *t = t_ / P0P1;
    }

    return distSq;
}


f32
DistSqPoint3ToPlane(const VEC3* P, const PLANE* J, VEC3* Q)
{
    // The normal vector of the plane is assumed to be normalized.
    f32 k = J->Test(*P);
    
    if (Q)
    {
        VEC3 tmp;
        VEC3Sub(Q, P, VEC3Scale(&tmp, &J->N, k));
    }

    return k * k;
}


f32
DistSqSphereToPlane(const SPHERE* S, const PLANE* J)
{
    f32 distance = J->Test(S->C);
    if (distance > S->r)
        return (distance - S->r) * (distance - S->r);
    else if (distance < -S->r)
        return (distance + S->r) * (distance + S->r);
    else
        return 0.f;
}

f32
DistSqPoint3ToPolyline3(const VEC3* P, const VEC3* vertices, unsigned int nVertices)
{
    // There must be two or more nVertices
    unsigned int nSegments = nVertices - 1;

    f32 dSq = internal::infinity;
    f32 xMinusA, yMinusB, zMinusC;
    f32 xNextMinusA, yNextMinusB, zNextMinusC;
    f32 xMinusASq, yMinusBSq, zMinusCSq;
    f32 xNextMinusASq, yNextMinusBSq, zNextMinusCSq;

    xMinusA = vertices[0].x - P->x;
    yMinusB = vertices[0].y - P->y;
    zMinusC = vertices[0].z - P->z;

    xMinusASq = xMinusA * xMinusA;
    yMinusBSq = yMinusB * yMinusB;
    zMinusCSq = zMinusC * zMinusC;

    xNextMinusA = vertices[1].x - P->x;
    yNextMinusB = vertices[1].y - P->y;
    zNextMinusC = vertices[1].z - P->z;

    xNextMinusASq = xNextMinusA * xNextMinusA;
    yNextMinusBSq = yNextMinusB * yNextMinusB;
    zNextMinusCSq = zNextMinusC * zNextMinusC;

    // Calculate the distance with the first segment (line).
    SEGMENT3 S(vertices[0], vertices[1]);
    dSq = DistSqPoint3ToSegment3(P, &S, NULL);

    // Reject the subsequent segments as much as possible.
    // Calculate distance if cannot be rejected
    for (unsigned int i = 1; i < nSegments - 1; ++i)
    {
        // Rejection Test
        if (((FAbs(xMinusASq) >= dSq) &&
             (FAbs(xNextMinusASq) >= dSq) &&
             (xMinusA * xNextMinusA > 0)) ||
            ((FAbs(yMinusBSq) >= dSq) &&
             (FAbs(yNextMinusBSq) >= dSq) &&
             (yMinusB * yNextMinusB > 0)) ||
            ((FAbs(zMinusCSq) >= dSq) &&
             (FAbs(zNextMinusCSq) >= dSq) &&
             (zMinusC * zNextMinusC > 0)))
        {
            // Reject
            ;
        }
        else
        {
            SEGMENT3 S2(vertices[i], vertices[i + 1]);
            f32 dSq_ = DistSqPoint3ToSegment3(P, &S2, NULL);

            if (dSq_ < dSq)
                dSq = dSq_;
        }

        if (i != nSegments - 2)
        {
            xMinusA = xNextMinusA;
            yMinusB = yNextMinusB;
            zMinusC = zNextMinusC;
            
            xMinusASq = xMinusA * xMinusA;
            yMinusBSq = yMinusB * yMinusB;
            zMinusCSq = zMinusC * zMinusC;
            
            xNextMinusA = vertices[i + 2].x - P->x;
            yNextMinusB = vertices[i + 2].y - P->y;
            zNextMinusC = vertices[i + 2].z - P->z;
            
            xNextMinusASq = xNextMinusA * xNextMinusA;
            yNextMinusBSq = yNextMinusB * yNextMinusB;
            zNextMinusCSq = zNextMinusC * zNextMinusC;
        }
    }

    return dSq;
}


f32
DistSqPoint3ToAABB(const VEC3* P, const AABB* B, VEC3* q)
{
    f32 sqDist = 0.f;
    f32 v;
    f32 vv;

    vv = v = P->x;
    if (v < B->Pmin.x)
    {
        sqDist += (B->Pmin.x - v) * (B->Pmin.x - v);
        vv = B->Pmin.x;
    }
    else if (v > B->Pmax.x)
    {
        sqDist += (B->Pmax.x - v) * (B->Pmax.x - v);
        vv = B->Pmax.x;
    }
    if (q)
        q->x = vv;

    vv = v = P->y;
    if (v < B->Pmin.y)
    {
        sqDist += (B->Pmin.y - v) * (B->Pmin.y - v);
        vv = B->Pmin.y;
    }
    else if (v > B->Pmax.y)
    {
        sqDist += (B->Pmax.y - v) * (B->Pmax.y - v);
        vv = B->Pmax.y;
    }
    if (q)
        q->y = vv;

    vv = v = P->z;
    if (v < B->Pmin.z)
    {
        sqDist += (B->Pmin.z - v) * (B->Pmin.z - v);
        vv = B->Pmin.z;
    }
    else if (v > B->Pmax.z)
    {
        sqDist += (B->Pmax.z - v) * (B->Pmax.z - v);
        vv = B->Pmax.z;
    }
    if (q)
        q->z = vv;

    return sqDist;
}



//
// Distance calculation between line/ray/line segment (6 total)
//
f32
DistSqLine3ToLine3(const LINE3* L0, const LINE3* L1, f32* s, f32* t)
{
    // The direction vector of the line is assumed to be normalized.
    VEC3 u;
    f32 b, d, e, f, det;

    VEC3Sub(&u, &L0->P, &L1->P);
    //a = 1.f; // a = VEC3SquareLen(&L0->d);
    b = VEC3Dot(&L0->d, &L1->d);
    //c = 1.f; // c = VEC3SquareLen(&L1->d);
    d = VEC3Dot(&L0->d, &u);
    e = VEC3Dot(&L1->d, &u);
    f = VEC3SquareLen(&u);
    det = 1.f - b * b; // det = a * c - b * b;

    // Check whether parallel
    if (det < internal::epsilon)
    {
        // Calculate from starting point of L0 if parallel
        if (s)
            *s = 0.f;
        if (t)
            *t = e;

        return f;
    }
    else
    {
        f32 invDet = 1.f / det;
        f32 s_, t_;
        
        s_ = (b * e - d) * invDet;
        t_ = (e - b * d) * invDet;

        if (s)
            *s = s_;
        if (t)
            *t = t_;

        VEC3 tmp0, tmp1, tmp2;
        VEC3Sub(&tmp2,
                VEC3Scale(&tmp0, &L0->d, s_),
                VEC3Scale(&tmp1, &L1->d, t_));
        VEC3Add(&tmp2, &tmp2, &u);
        return VEC3SquareLen(&tmp2);
    }
}


f32
DistSqSegment3ToSegment3(const SEGMENT3* S1, const SEGMENT3* S2, f32* s, f32* t)
{
    VEC3   u(S1->P1 - S1->P0);
    VEC3   v(S2->P1 - S2->P0);
    VEC3   w(S1->P0 - S2->P0);
    f32    a = VEC3SquareLen(&u);        // always >= 0
    f32    b = VEC3Dot(&u, &v);
    f32    c = VEC3SquareLen(&v);        // always >= 0
    f32    d = VEC3Dot(&u, &w);
    f32    e = VEC3Dot(&v, &w);
    f32    D = a*c - b*b;       // always >= 0
    f32    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
    f32    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0

    // Obtain the nearest point when the line segment is taken as line
    if (D < internal::epsilon) { // The two line segments are parallel
        sN = 0.0;              // Take the distance between S1 starting point and S2.
        sD = 1.0;              // Measure against division operation in the code to follow
        tN = e;
        tD = c;
    }
    else {                     // Calculate the nearest point when taken as line, not line segment
        sN = (b*e - c*d);
        tN = (a*e - b*d);

        if (sN < 0.0f) {       // The side (s = 0) is "visible" if sc < 0.
            sN = 0.0f;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {    // The side (s = 1) is "visible" if sc > 1.
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0f) {            // The side (t = 0) is "visible" if tc < 0.
        tN = 0.0f;

        // Recalculate the nearest point of sc with tc = 0.
        if (-d < 0.0f)
            sN = 0.0f;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {        // The side (t = 1) is "visible" if tc > 1.
        tN = tD;
        // Recalculate the nearest point of sc with tc = 1.
        if ((-d + b) < 0.0f)
            sN = 0.0f;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }
    // finally do the division to get sc and tc
    sc = (FAbs(sN) < internal::epsilon ? 0.0f : sN / sD);
    tc = (FAbs(tN) < internal::epsilon ? 0.0f : tN / tD);

    if (s)
        *s = sc;
    if (t)
        *t = tc;

    // Take the difference between the nearest points.
    VEC3   dP = w + (u * sc) - (v * tc);  // = S1(sc) - S2(tc)

    return VEC3SquareLen(&dP);
}


f32
DistSqLine3ToRay3(const LINE3* L, const RAY3* R, f32* s, f32* t)
{
    VEC3 u;
    f32 b, d, e, det;
    f32 sNum, tNum, tDenom, sDenom;

    VEC3Sub(&u, &L->P, &R->P);
    //a = 1.f; // a = VEC3SquareLen(&L0->d);
    b = VEC3Dot(&L->d, &R->d);
    //c = 1.f; // c = VEC3SquareLen(&L1->d);
    d = VEC3Dot(&L->d, &u);
    e = VEC3Dot(&R->d, &u);
    det = 1.f - b * b; // det = a * c - b * b;

    if (det < internal::epsilon)
    {
        // Randomly choose
        sNum = 0.f;
        tNum = e;
        tDenom = 1.f;
        sDenom = det;
    }
    else
    {
        sNum = b * e - d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // Check the case where t (parameter of Ray) is 0 or smaller.
    if (tNum < 0)
    {
        // Combination of the starting point of Ray and its closest position on the Line.
        tNum = 0;
        sNum = -d;
        sDenom = 1.f;
    }

    f32 s_ = sNum / sDenom;
    f32 t_ = tNum / tDenom;

    if (s)
        *s = s_;
    if (t)
        *t = t_;

    // v = (L.P + (s * L.d)) - (R.P + (t * R.d))
    VEC3 v;
    VEC3 tmp;
    VEC3Add(&v, &L->P, VEC3Scale(&tmp, &L->d, s_));
    VEC3Sub(&v, &v, &R->P);
    VEC3Sub(&v, &v, VEC3Scale(&tmp, &R->d, t_));

    return VEC3SquareLen(&v);
}

f32
DistSqLine3ToSegment3(const LINE3* L0, const SEGMENT3* S, f32* s, f32* t)
{
    VEC3 segDir;
    VEC3Sub(&segDir, &S->P1, &S->P0);

    VEC3 u;
    f32 b, c, d, e, det;
    f32 sNum, tNum, tDenom, sDenom;

    VEC3Sub(&u, &L0->P, &S->P0);
    // const f32 a = 1.f; // a = VEC3SquareLen(&L0->d);
    b = VEC3Dot(&L0->d, &segDir);
    c = VEC3SquareLen(&segDir);
    d = VEC3Dot(&L0->d, &u);
    e = VEC3Dot(&segDir, &u);
    det = c - b * b; // det = a * c - b * b;

    // Check whether parallel
    if (det < internal::epsilon)
    {
        // Randomly choose
        sNum = 0.f;
        tNum = e;
        sDenom = det;
        tDenom = c;
    }
    else
    {
        sNum = b * e - c * d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // Check when t (parameter of Segment) is smaller than 0 or larger than 1.
    if (tNum < 0.f)
    {
        // Starting point of Segment
        tNum = 0.f;
        sNum = -d;
        sDenom = 1.f;
    }
    else if (tNum > tDenom)
    {
        // Ending point of Segment.
        tNum = tDenom;
        sNum = -d + b;
        sDenom = 1.f;
    }
    
    f32 s_ = sNum / sDenom;
    f32 t_ = tNum / tDenom;

    if (s)
        *s = s_;
    if (t)
        *t = t_;

    VEC3 v;
    VEC3 tmp;
    VEC3Add(&v, &L0->P, VEC3Scale(&tmp, &L0->d, s_));
    VEC3Sub(&v, &v, &S->P0);
    VEC3Sub(&v, &v, VEC3Scale(&tmp, &segDir, t_));

    return VEC3SquareLen(&v);
}


f32
DistSqRay3ToRay3(const RAY3* R0, const RAY3* R1, f32* s, f32* t)
{
    VEC3 u;
    f32 b, d, e, det;
    f32 sNum, tNum, tDenom, sDenom;

    VEC3Sub(&u, &R0->P, &R1->P);
    // const f32 a = 1.f; // a = VEC3SquareLen(&R0->d);
    b = VEC3Dot(&R0->d, &R1->d);
    // const f32 c = 1.f; // c = VEC3SquareLen(&R1->d);
    d = VEC3Dot(&R0->d, &u);
    e = VEC3Dot(&R1->d, &u);
    det = 1.f - b * b; // det = a * c - b * b;

    // Check whether parallel
    if (det < internal::epsilon)
    {
        // Randomly choose
        sNum = 0.f;
        tNum = e;
        tDenom = 1.f;
        sDenom = det;
    }
    else
    {
        sNum = b * e - d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    if (sNum < 0.f)
    {
        // Take Ray0 as the starting point.
        sNum = 0.f;
        tNum = e;
        tDenom = 1.f;
    }

    if (tNum < 0.f)
    {
        // Starting point of Ray1
        tNum = 0.f;
        if (-d < 0)
        {
            // The distance between the starting points of Ray0 and Ray1 will be the closest
            sNum = 0.f;
        }
        else
        {
            // When the line segment on Ray0 vertical to the Ray0 reaches the Ray1 starting point.
            sNum = -d;
            sDenom = 1.f;
        }
    }

    f32 s_ = sNum / sDenom;
    f32 t_ = tNum / tDenom;

    if (s)
        *s = s_;
    if (t)
        *t = t_;

    // v = (L0.P + (s * L0.d)) - (L1.P + (t * L1.d))
    VEC3 v;
    VEC3 tmp;
    VEC3Add(&v, &R0->P, VEC3Scale(&tmp, &R0->d, s_));
    VEC3Sub(&v, &v, &R1->P);
    VEC3Sub(&v, &v, VEC3Scale(&tmp, &R1->d, t_));

    return VEC3SquareLen(&v);
}

f32
DistSqRay3ToSegment3(const RAY3* R0, const SEGMENT3* S, f32* s, f32* t)
{
    VEC3 segDir;
    VEC3 u;
    f32 b, c, d, e, det;
    f32 sNum, tNum, tDenom, sDenom;

    VEC3Sub(&segDir, &S->P1, &S->P0);
    VEC3Sub(&u, &R0->P, &S->P0);
    // const f32 a = 1.f; // a = VEC3SquareLen(&R0->d);
    b = VEC3Dot(&R0->d, &segDir);
    c = VEC3SquareLen(&segDir);
    d = VEC3Dot(&R0->d, &u);
    e = VEC3Dot(&segDir, &u);
    det = c - b * b; // det = a * c - b * b;

    // Check whether parallel
    if (det < internal::epsilon)
    {
        // Randomly choose
        sNum = 0.f;
        tNum = e;
        tDenom = c;
        sDenom = det;
    }
    else
    {
        sNum = b * e - c * d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // Check Ray
    if (sNum < 0.f)
    {
        // Use the starting position of Ray.
        sNum = 0.f;
        tNum = e;
        tDenom = c;
    }

    // Check Segment
    if (tNum < 0.f)
    {
        // Starting point of Segment
        tNum = 0.f;
        if (-d < 0.f)
        {
            sNum = 0.f;
        }
        else
        {
            // If the nearest point to the Segment starting point is on Ray
            sNum = -d;
            sDenom = 1.f;
        }
    }
    else if (tNum > tDenom)
    {
        // Ending point of Segment.
        tNum = tDenom;
        if ((-d + b) < 0.f)
        {
            sNum = 0.f;
        }
        else
        {
            // If the nearest point to the Segment ending point is on Ray
            sNum = -d + b;
            sDenom = 1.f;
        }
    }

    f32 s_ = sNum / sDenom;
    f32 t_ = tNum / tDenom;

    if (s)
        *s = s_;
    if (t)
        *t = t_;

    // v = (L0.P + (s * L0.d)) - (S.P + (t * S.d))
    VEC3 v;
    VEC3 tmp;
    VEC3Add(&v, &R0->P, VEC3Scale(&tmp, &R0->d, s_));
    VEC3Sub(&v, &v, &S->P0);
    VEC3Sub(&v, &v, VEC3Scale(&tmp, &segDir, t_));

    return VEC3SquareLen(&v);
}



//
// Intersection
//

// Line, Plane
IntersectionResult
IntersectionLine3Plane(const LINE3* L, const PLANE* J, f32* t, VEC3* I)
{
    f32 denom;
    denom = VEC3Dot(&L->d, &J->N);

    if (FAbs(denom) < internal::epsilon)
    {
        if (FAbs(VEC3Dot(&J->N, &L->P) + J->d) < internal::epsilon)
            return INTERSECTION_LINE3_ON_PLANE;
        else
            return INTERSECTION_NONE;
    }
    else
    {
        if (I != NULL || t != NULL)
        {
            f32 t_ = -(VEC3Dot(&J->N, &L->P) + J->d) / denom;

            if (I)
            {
                VEC3 tmp;
                // Intersect calculation
                VEC3Add(I, &L->P, VEC3Scale(&tmp, &L->d, t_));
            }

            if (t)
                *t = t_;
        }

        return INTERSECTION_1;
    }
}

// Ray, Plane
IntersectionResult
IntersectionRay3Plane(const RAY3* R, const PLANE* J, f32* t, VEC3* I)
{
    f32 t_;
    VEC3 I_;
    IntersectionResult result;
    result = IntersectionLine3Plane((const LINE3*)R, J, &t_, (I != NULL ? &I_ : NULL));

    if (result == INTERSECTION_1)
    {
        // Decide based on whether t_ >= 0.
        if (t_ >= 0)
        {
            if (t)
                *t = t_;
            if (I)
                *I = I_;
            return result;
        }
        else
        {
            return INTERSECTION_NONE;
        }
    }
    else
    {
        return result;
    }
}

// Segment, Plane
IntersectionResult
IntersectionSegment3Plane(const SEGMENT3* S, const PLANE* J, f32* t, VEC3* I)
{
    f32 t_;
    VEC3 I_;
    VEC3 dir;
    VEC3Sub(&dir, &S->P1, &S->P0);
    f32 P0P1 = FSqrt(dir.LenSq());
    LINE3 L(S->P0, dir);
    
    IntersectionResult result;
    result = IntersectionLine3Plane(&L, J, &t_, (I != NULL ? &I_ : NULL));

    if (result == INTERSECTION_1)
    {
        // Decide using 0 <= t_ <= P0P1
        if (t_ >= 0 && t_ <= P0P1)
        {
            if (t)
                *t = t_ / P0P1;
            if (I)
                *I = I_;
            return result;
        }
        else
        {
            return INTERSECTION_NONE;
        }
    }
    else
    {
        return result;
    }
}

// Line, Sphere
IntersectionResult
IntersectionLine3Sphere(const LINE3* L, const SPHERE* sphere, f32* t0, f32* t1)
{
    f32 b, c, discrm;
    VEC3 PmC;
    VEC3Sub(&PmC, &L->P, &sphere->C);
    // a = 1.f; // dot(L->d, L->d) is 1.f
    b = 2.f * VEC3Dot(&L->d, &PmC);
    c = VEC3SquareLen(&PmC) - sphere->r * sphere->r;
    discrm = b * b - 4.f * c /* * a*/;

    if (discrm > 0.f)
    {
        f32 sq = FSqrt(discrm);

        if (t0)
        {
            *t0 = (-b - sq) * 0.5f/* inv(2 * a) */;
        }

        if (t1)
        {
            *t1 = (-b + sq) * 0.5f/* inv(2 * a) */;
        }

        return INTERSECTION_2;
    }
    else if (discrm == 0.f)
    {
        if (t0)
        {
            *t0 = -0.5f * b;
            //*t0 = -b / (2.f * a);
        }
        return INTERSECTION_1;
    }
    else
    {
        return INTERSECTION_NONE;
    }
}

// Ray, Sphere
IntersectionResult
IntersectionRay3Sphere(const RAY3* R, const SPHERE* sphere, f32* t0, f32* t1)
{
    IntersectionResult result;
    f32 t0_, t1_;
    result = IntersectionLine3Sphere((const LINE3*)R, sphere, &t0_, &t1_);
    
    if (result != INTERSECTION_NONE)
    {
        if (result == INTERSECTION_1)
        {
            if (t0_ >= 0.f)
            {
                if (t0)
                    *t0 = t0_;
                return result;
            }
            else
            {
                return INTERSECTION_NONE;
            }
        }
        else
        {
            // Here (t0_ < t1_) is guaranteed
            if (t0_ >= 0.f)
            {
                // Ray will intersect with Sphere at two points
                if (t0)
                    *t0 = t0_;
                if (t1)
                    *t1 = t1_;
                return result;
            }
            else if (t1_ >= 0.f)
            {
                // Ray will intersect with Sphere at one point
                if (t0)
                    *t0 = t1_;
                return INTERSECTION_1;
            }
            else
            {
                return INTERSECTION_NONE;
            }
        }
    }
    else
    {
        return result;
    }
}



// Ray, Sphere - number 2
bool
IntersectionRay3Sphere(const RAY3* R, const SPHERE* sphere)
{
    VEC3 w;
    VEC3Sub(&w, &sphere->C, &R->P);
    f32 wsq = VEC3SquareLen(&w);
    f32 proj = VEC3Dot(&w, &R->d);
    f32 rsq = sphere->r * sphere->r;

    if (proj < 0.f && wsq > rsq)
        return false;

    f32 vsq = VEC3SquareLen(&R->d);

    if (vsq * wsq - proj * proj <= vsq * rsq)
        return true;
    else
        return false;
}

// Segment, Sphere
IntersectionResult
IntersectionSegment3Sphere(const SEGMENT3* S, const SPHERE* sphere, f32* t0, f32* t1)
{
    VEC3 dir;
    VEC3Sub(&dir, &S->P1, &S->P0);
    f32 P0P1 = FSqrt(dir.LenSq());
    LINE3 L(S->P0, dir);
    IntersectionResult result;
    f32 t0_, t1_;
    result = IntersectionLine3Sphere(&L, sphere, &t0_, &t1_);
    
    if (result != INTERSECTION_NONE)
    {
        if (result == INTERSECTION_1)
        {
            if (t0_ >= 0.f && t0_ <= P0P1)
            {
                if (t0)
                    *t0 = t0_ / P0P1;
                return result;
            }
            else
            {
                return INTERSECTION_NONE;
            }
        }
        else
        {
            // Here (t0_ < t1_) is guaranteed
            if (t0_ >= 0.f)
            {
                if (t1_ <= P0P1)
                {
                    f32 tmp = 1.f / P0P1;
                    // Two points intersect
                    if (t0)
                        *t0 = t0_ * tmp;
                    if (t1)
                        *t1 = t1_ * tmp;

                    return result;
                }
                else if (t0_ <= P0P1)
                {
                    // Only t0_ intersects
                    if (t0)
                        *t0 = t0_ / P0P1;
                    return INTERSECTION_1;
                }
                else
                {
                    return INTERSECTION_NONE;
                }
            }
            else if (t1_ >= 0.f && t1_ <= P0P1)
            {
                // Only t1_ intersects
                if (t0)
                    *t0 = t1_ / P0P1;
                return INTERSECTION_1;
            }
            else
            {
                return INTERSECTION_NONE;
            }

        }
    }
    else
    {
        return result;
    }
}

static bool
IntersectionRay3AABB_(f32 min, f32 max, f32 o, f32 dir, f32& tNear, f32& tFar)
{
    if (FAbs(dir) < internal::epsilon)
    {
        // Parallel to plane
        if (o < min || o > max)
            return false;
        else
            return true;
    }

    f32 inv_dir = 1.f / dir;
    f32 t0 = (min - o) * inv_dir;
    f32 t1 = (max - o) * inv_dir;

    if (t0 > t1)
        ::std::swap(t0, t1);

    if (t0 > tNear)
        tNear = t0;
    if (t1 < tFar)
        tFar = t1;

    if (tNear > tFar)
        return false;
    if (tFar < 0.f)
        return false;
    return true;
}


// Ray, AABB
bool
IntersectionRay3AABB(const RAY3* R, const AABB* box, f32* t)
{
    f32 tNear = -internal::infinity;
    f32 tFar  =  internal::infinity;
    bool result;

    result = IntersectionRay3AABB_(box->Pmin.x, box->Pmax.x, R->P.x, R->d.x, tNear, tFar);
    if (!result)
        return false;

    result = IntersectionRay3AABB_(box->Pmin.y, box->Pmax.y, R->P.y, R->d.y, tNear, tFar);
    if (!result)
        return false;

    result = IntersectionRay3AABB_(box->Pmin.z, box->Pmax.z, R->P.z, R->d.z, tNear, tFar);
    if (!result)
        return false;

    if (t)
    {
        if (tNear > 0.f)
            *t = tNear;
        else
            *t = tFar;
    }

    return true;
}

// AABB, AABB
bool
IntersectionAABB(const AABB* a, const AABB* b)
{
    if (a->Pmin.x > b->Pmax.x || b->Pmin.x > a->Pmax.x ||
        a->Pmin.y > b->Pmax.y || b->Pmin.y > a->Pmax.y ||
        a->Pmin.z > b->Pmax.z || b->Pmin.z > a->Pmax.z)
        return false;
    else
        return true;
}


// Sphere, AABB
bool
IntersectionSphereAABB(const SPHERE* sphere, const AABB* aabb)
{
    f32 distSq = 0.f;

    if (sphere->C.x < aabb->Pmin.x)
        distSq += (sphere->C.x - aabb->Pmin.x) * (sphere->C.x - aabb->Pmin.x);
    else if (sphere->C.x > aabb->Pmax.x)
        distSq += (sphere->C.x - aabb->Pmax.x) * (sphere->C.x - aabb->Pmax.x);

    if (sphere->C.y < aabb->Pmin.y)
        distSq += (sphere->C.y - aabb->Pmin.y) * (sphere->C.y - aabb->Pmin.y);
    else if (sphere->C.y > aabb->Pmax.y)
        distSq += (sphere->C.y - aabb->Pmax.y) * (sphere->C.y - aabb->Pmax.y);

    if (sphere->C.z < aabb->Pmin.z)
        distSq += (sphere->C.z - aabb->Pmin.z) * (sphere->C.z - aabb->Pmin.z);
    else if (sphere->C.z > aabb->Pmax.z)
        distSq += (sphere->C.z - aabb->Pmax.z) * (sphere->C.z - aabb->Pmax.z);

    if (distSq <= sphere->r * sphere->r)
        return true;
    else
        return false;
}

bool
IntersectionSphere(const SPHERE* s0, const SPHERE* s1)
{
    VEC3 centerDiff;
    VEC3Sub(&centerDiff, &s0->C, &s1->C);
    f32 radiusSum = s0->r + s1->r;

    if (VEC3SquareLen(&centerDiff) <= radiusSum * radiusSum)
        return true;
    else
        return false;
}

bool
IntersectionPlaneAABB(const PLANE* J, const AABB* B)
{
    VEC3 C;
    VEC3 E;
    VEC3Lerp(&C, &B->Pmin, &B->Pmax, .5f);
    VEC3Sub(&E, &B->Pmax, &C); // Here, the elements of E will all be positive.

    // The largest of the dot product between N and the vector from the center of AABB to each point of AABB will be r, and the smallest -r.
    // That is, a location within distance r of the plane that passes through center of AABB and a normal of N will be inside AABB.
    f32 r = E.x * FAbs(J->N.x) + 
            E.y * FAbs(J->N.y) +
            E.z * FAbs(J->N.z);

    // Distance between the plane and the center of AABB
    f32 s = VEC3Dot(&J->N, &C) + J->d;

    // Intersects if the distance between the plane and the center of AABB is within r.
    return (FAbs(s) <= r);
}

bool
IntersectionCapsule(const CAPSULE* C0, const CAPSULE* C1)
{
    f32 radiusSum = C0->r + C1->r;
    if (DistSqSegment3ToSegment3(&C0->S, &C1->S, NULL, NULL)
            <= radiusSum * radiusSum)
        return true;
    else
        return false;
}


bool
IntersectionRay3Capsule(const RAY3* R, const CAPSULE* C)
{
    if (DistSqRay3ToSegment3(R, &C->S, NULL, NULL) <= C->r * C->r)
        return true;
    else
        return false;
}

bool
IntersectionLine3Capsule(const LINE3* L, const CAPSULE* C)
{
    if (DistSqLine3ToSegment3(L, &C->S, NULL, NULL) <= C->r * C->r)
        return true;
    else
        return false;
}

bool
IntersectionPlaneCapsule(const PLANE* J, const CAPSULE* C)
{
    f32 s0 = J->Test(C->S.P0);
    f32 s1 = J->Test(C->S.P1);

    if (s0 * s1 < 0.f ||
        FAbs(s0) <= C->r ||
        FAbs(s1) <= C->r)
        return true;
    else
        return false;
}

bool
FRUSTUM::IntersectSphere(const SPHERE* S) const
{
    // Sphere is in world coordinate system

    f32 Dist;
    VEC3 viewPos;

    // Convert the z-coordinate of the center of sphere to view coordinate system coordinate
    viewPos.z = cam.f._20 * S->C.x +
                cam.f._21 * S->C.y +
                cam.f._22 * S->C.z +
                cam.f._23;

    // Determine whether it is in front of the near plane
    if (viewPos.z - S->r > znear)
        return false;

    // Determine whether it is behind the far plane 
    if (viewPos.z + S->r < zfar)
        return false;

    // Convert the x-coordinate of the center of sphere to view coordinate system coordinate
    viewPos.x = cam.f._00 * S->C.x +
                cam.f._01 * S->C.y +
                cam.f._02 * S->C.z +
                cam.f._03;

    // Test with left clip plane
    // Because the left clip plane passes through the origin, N.d = 0. It is also parallel to the y-axis, so N.y = 0.
    Dist = viewPos.x * leftPlane.N.x +
           viewPos.z * leftPlane.N.z;
    if (Dist > S->r)
        return false;

    // Test with right clip plane
    // Because the right clip plane passes through the origin, N.d = 0. It is also parallel to the y-axis, so N.y = 0.
    Dist = viewPos.x * rightPlane.N.x +
           viewPos.z * rightPlane.N.z;
    if (Dist > S->r)
        return false;

    // Convert the y-coordinate of the center of sphere to view coordinate system
    viewPos.y = cam.f._10 * S->C.x +
                cam.f._11 * S->C.y +
                cam.f._12 * S->C.z +
                cam.f._13;

    // Test with upper clip plane
    // Because the upper clip plane passes through the origin, N.d = 0. It is also parallel to the x-axis, so N.x = 0.
    Dist = viewPos.y * topPlane.N.y +
           viewPos.z * topPlane.N.z;
    if (Dist > S->r)
        return false;

    // Test with lower clip plane
    // Because the lower clip plane passes through the origin, N.d = 0. It is also parallel to the x-axis, so N.x = 0.
    Dist = viewPos.y * bottomPlane.N.y +
           viewPos.z * bottomPlane.N.z;
    if (Dist > S->r)
        return false;

    // Part or whole of the sphere is inside frustum.
    return true;
}

bool
FRUSTUM::IntersectAABB(const AABB* B) const
{
    // Filter using AABB surrounding the frustum
    if (!IntersectionAABB(B, &box))
        return false;

    int i;
    VEC3 p;
    for (i = 0; i < 6; ++i)
    {
        // For each sides of the frustum
        // Test: determine the point that will minimize ax + by + cz + d.
        // This can be determined by looking at the sign of a, b, c.
        // If Test is positive, all points of AABB will be outside the frustum.
        p.x = (planes[i].N.x >= 0) ? B->Pmin.x : B->Pmax.x;
        p.y = (planes[i].N.y >= 0) ? B->Pmin.y : B->Pmax.y;
        p.z = (planes[i].N.z >= 0) ? B->Pmin.z : B->Pmax.z;

        // FALSE if the nearest point to the frustum is outside.
        if (planes[i].Test(p) > 0)
            return false;
    }

    return true;
}

IntersectionResult
FRUSTUM::IntersectAABB_Ex(const AABB* B) const
{
    // Filter using AABB surrounding the frustum
    if (!IntersectionAABB(B, &box))
        return INTERSECTION_OUTSIDE;

    IntersectionResult result = INTERSECTION_INSIDE;

    int i;
    VEC3 p, n;
    for (i = 0; i < 6; ++i)
    {
        if (planes[i].N.x >= 0)
        {
            p.x = B->Pmin.x; n.x = B->Pmax.x;
        }
        else
        {
            p.x = B->Pmax.x; n.x = B->Pmin.x;
        }

        if (planes[i].N.y >= 0)
        {
            p.y = B->Pmin.y; n.y = B->Pmax.y;
        }
        else
        {
            p.y = B->Pmax.y; n.y = B->Pmin.y;
        }

        if (planes[i].N.z >= 0)
        {
            p.z = B->Pmin.z; n.z = B->Pmax.z;
        }
        else
        {
            p.z = B->Pmax.z; n.z = B->Pmin.z;
        }

        // 'outside' if the nearest point to the frustum is outside.
        if (planes[i].Test(p) > 0)
            return INTERSECTION_OUTSIDE;
        if (planes[i].Test(n) > 0)
            result = INTERSECTION_INTERSECT;
    }

    return result;
}




//
// Merge area
//


SPHERE*
MergeSphere(SPHERE* s2, const SPHERE* s0, const SPHERE* s1)
{
    // s2 == s0,s1 will still be fine.
    
    VEC3 diff;
    VEC3Sub(&diff, &s1->C, &s0->C);
    f32 distsq = VEC3SquareLen(&diff);
    f32 radiusDiff = s1->r - s0->r;

    if (distsq <= radiusDiff * radiusDiff)
    {
        if (s0->r > s1->r)
            *s2 = *s0;
        else
            *s2 = *s1;
    }
    else
    {
        f32 dist = FSqrt(distsq);
        f32 newRadius = 0.5f * (s0->r + s1->r + dist);

        if (FAbs(dist) > internal::epsilon)
        {
            VEC3Add(&s2->C, &s0->C, VEC3Scale(&diff, &diff, ((newRadius - s0->r)/dist)));
        }
        else
        {
            s2->C = s0->C;
        }

        s2->r = newRadius;
    }
    return s2;
}

AABB*
MergeAABB(AABB* a2, const AABB* a0, const AABB* a1)
{
    a2->Pmin.x = ::std::min(a0->Pmin.x, a1->Pmin.x);
    a2->Pmin.y = ::std::min(a0->Pmin.y, a1->Pmin.y);
    a2->Pmin.z = ::std::min(a0->Pmin.z, a1->Pmin.z);

    a2->Pmax.x = ::std::max(a0->Pmax.x, a1->Pmax.x);
    a2->Pmax.y = ::std::max(a0->Pmax.y, a1->Pmax.y);
    a2->Pmax.z = ::std::max(a0->Pmax.z, a1->Pmax.z);

    return a2;
}


}}  // nw::math