/*---------------------------------------------------------------------------*
  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;
}

//
// 3Dジオメトリのメンバ関数
//
void
PLANE::Set(const VEC3* P0, const VEC3* P1, const VEC3* P2)
{
    // P0, P1, P2は上から見て時計まわり
    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;

    // 新しいX軸における最小値と最大値を求める
    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; }

    // 新しいY軸における最小値と最大値を求める
    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; }

    // 新しいZ軸における最小値と最大値を求める
    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);
    // 逆行列のアサートが入る

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

    f32 f_n = f / n;

    // 右手系
    // P[0]はnearの左上
    P[0].x = left; P[0].y = top; P[0].z = -n;

    // P[1]はnearの右上
    P[1].x = right; P[1].y = top; P[1].z = -n;

    // P[2]はnearの右下
    P[2].x = right; P[2].y = bottom; P[2].z = -n;

    // P[3]はnearの左下
    P[3].x = left; P[3].y = bottom; P[3].z = -n;

    // P[4]はfarの左上
    P[4].x = f_n * left; P[4].y = f_n * top; P[4].z = -f;

    // P[5]はfarの右上
    P[5].x = f_n * right; P[5].y = f_n * top; P[5].z = -f;

    // P[6]はfarの右下
    P[6].x = f_n * right; P[6].y = f_n * bottom; P[6].z = -f;

    // P[7]はfarの左下
    P[7].x = f_n * left; P[7].y = f_n * bottom; P[7].z = -f;

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

    // 右手系である
    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]);

    // P[0-7]をワールド座標系に変換して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)
{
    // 平面の法線ベクトルは正規化されていることが前提になっています。
    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)
{
    // nVerticesは2以上
    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;

    // 最初のセグメント(線)との距離を計算する。
    SEGMENT3 S(vertices[0], vertices[1]);
    dSq = DistSqPoint3ToSegment3(P, &S, NULL);

    // 続くセグメントを出来る限りリジェクトする。
    // リジェクトできなければ距離を計算
    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)))
        {
            // リジェクト
            ;
        }
        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;
}



//
// 線・レイ・線分の間の距離の計算(合計6種類)
//
f32
DistSqLine3ToLine3(const LINE3* L0, const LINE3* L1, f32* s, f32* t)
{
    // 線の方向ベクトルは正規化されていることが前提になっています。
    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;

    // 平行かどうかをチェック
    if (det < internal::epsilon)
    {
        // 平行な場合はL0の起点から算出
        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

    // 線分を線とした場合の最近点を求める
    if (D < internal::epsilon) { // 2つの線分は平行
        sN = 0.0;              // S1の始点とS2の距離をとる
        sD = 1.0;              // 後のコードの除算対策
        tN = e;
        tD = c;
    }
    else {                     // 線分でなく線とした場合最近点を計算
        sN = (b*e - c*d);
        tN = (a*e - b*d);

        if (sN < 0.0f) {       // sc < 0ならば、s=0の辺が「見える」
            sN = 0.0f;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {    // sc > 1ならば、s=1の辺が「見える」
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0f) {            // tc < 0ならば、t=0の辺が「見える」
        tN = 0.0f;

        // tc = 0としてscの最近点を再計算
        if (-d < 0.0f)
            sN = 0.0f;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {        // tc > 1ならば、t=1の辺が「見える」
        tN = tD;
        // tc = 1としてscの最近点を再計算
        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;

    // 最近点同士の差をとる
    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)
    {
        // 適当に決める
        sNum = 0.f;
        tNum = e;
        tDenom = 1.f;
        sDenom = det;
    }
    else
    {
        sNum = b * e - d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // t(Rayのパラメータ)が0以下の場合をチェック
    if (tNum < 0)
    {
        // Rayの起点とそれに一番近い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;

    // 平行かどうかをチェック
    if (det < internal::epsilon)
    {
        // 適当に決める
        sNum = 0.f;
        tNum = e;
        sDenom = det;
        tDenom = c;
    }
    else
    {
        sNum = b * e - c * d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // t(Segnemtのパラメータ)が0未満、1より大きい場合のチェック
    if (tNum < 0.f)
    {
        // Segmentの始点
        tNum = 0.f;
        sNum = -d;
        sDenom = 1.f;
    }
    else if (tNum > tDenom)
    {
        // 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;

    // 平行かどうかをチェック
    if (det < internal::epsilon)
    {
        // 適当に決める
        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)
    {
        // Ray0を起点にする。
        sNum = 0.f;
        tNum = e;
        tDenom = 1.f;
    }

    if (tNum < 0.f)
    {
        // Ray1の起点
        tNum = 0.f;
        if (-d < 0)
        {
            // Ray0, Ray1の起点同士が最も近い
            sNum = 0.f;
        }
        else
        {
            // Ray0上のRay0に垂直な線分がRay1の起点に到達する場合
            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;

    // 平行かどうかをチェック
    if (det < internal::epsilon)
    {
        // 適当に決める
        sNum = 0.f;
        tNum = e;
        tDenom = c;
        sDenom = det;
    }
    else
    {
        sNum = b * e - c * d;
        tNum = e - b * d;
        sDenom = tDenom = det;
    }

    // Rayをチェック
    if (sNum < 0.f)
    {
        // Rayの起点を使う。
        sNum = 0.f;
        tNum = e;
        tDenom = c;
    }

    // Segmentをチェック
    if (tNum < 0.f)
    {
        // Segmentの始点
        tNum = 0.f;
        if (-d < 0.f)
        {
            sNum = 0.f;
        }
        else
        {
            // Rayの上にSegmentの始点への最近点がある場合
            sNum = -d;
            sDenom = 1.f;
        }
    }
    else if (tNum > tDenom)
    {
        // Segmentの終点
        tNum = tDenom;
        if ((-d + b) < 0.f)
        {
            sNum = 0.f;
        }
        else
        {
            // Rayの上にSegmentの終点への最近点がある場合
            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;
                // 交点の計算
                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)
    {
        // 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)
    {
        // 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)は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
        {
            // ここで、t0_ < t1_が保証されている
            if (t0_ >= 0.f)
            {
                // Rayは2点でSphereと交わる
                if (t0)
                    *t0 = t0_;
                if (t1)
                    *t1 = t1_;
                return result;
            }
            else if (t1_ >= 0.f)
            {
                // Rayは1点でSphereと交わる
                if (t0)
                    *t0 = t1_;
                return INTERSECTION_1;
            }
            else
            {
                return INTERSECTION_NONE;
            }
        }
    }
    else
    {
        return result;
    }
}



// Ray, Sphereその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
        {
            // ここで、t0_ < t1_が保証されている
            if (t0_ >= 0.f)
            {
                if (t1_ <= P0P1)
                {
                    f32 tmp = 1.f / P0P1;
                    // 2点が交わる
                    if (t0)
                        *t0 = t0_ * tmp;
                    if (t1)
                        *t1 = t1_ * tmp;

                    return result;
                }
                else if (t0_ <= P0P1)
                {
                    // t0_のみ交わる
                    if (t0)
                        *t0 = t0_ / P0P1;
                    return INTERSECTION_1;
                }
                else
                {
                    return INTERSECTION_NONE;
                }
            }
            else if (t1_ >= 0.f && t1_ <= P0P1)
            {
                // t1_のみ交わる
                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)
    {
        // 面に対して平行
        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); // ここでEの要素は全て正である。

    // AABBの中心からAABBの各点へのベクトルとNとの内積で最大のものはrで最小のものは-rである。
    // つまり法線Nを持ち、AABBの中心を通る平面からr以内はAABBの内部である。
    f32 r = E.x * FAbs(J->N.x) + 
            E.y * FAbs(J->N.y) +
            E.z * FAbs(J->N.z);

    // 平面とAABBの中心の距離
    f32 s = VEC3Dot(&J->N, &C) + J->d;

    // 平面とAABBの中心の距離が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
{
    // 球はワールド座標系

    f32 Dist;
    VEC3 viewPos;

    // 球の中心のZ座標をビュー座標系に変換する
    viewPos.z = cam.f._20 * S->C.x +
                cam.f._21 * S->C.y +
                cam.f._22 * S->C.z +
                cam.f._23;

    // ニアプレーンの前かどうかを判定
    if (viewPos.z - S->r > znear)
        return false;

    // ファープレーンの後ろかどうかを判定 
    if (viewPos.z + S->r < zfar)
        return false;

    // 球の中心のX座標をビュー座標系に変換する
    viewPos.x = cam.f._00 * S->C.x +
                cam.f._01 * S->C.y +
                cam.f._02 * S->C.z +
                cam.f._03;

    // 左クリップ平面とテスト
    // 左クリップ平面は原点を通るのでN.d = 0で、y軸に平行なのでN.y = 0である。
    Dist = viewPos.x * leftPlane.N.x +
           viewPos.z * leftPlane.N.z;
    if (Dist > S->r)
        return false;

    // 右クリップ平面とテスト
    // 右クリップ平面は原点を通るのでN.d = 0で、y軸に平行なのでN.y = 0である。
    Dist = viewPos.x * rightPlane.N.x +
           viewPos.z * rightPlane.N.z;
    if (Dist > S->r)
        return false;

    // 球の中心のY座標をビュー座標系に変換する
    viewPos.y = cam.f._10 * S->C.x +
                cam.f._11 * S->C.y +
                cam.f._12 * S->C.z +
                cam.f._13;

    // 上クリップ平面とテスト
    // 上クリップ平面は原点を通るのでN.d = 0で、x軸に平行なのでN.x = 0である。
    Dist = viewPos.y * topPlane.N.y +
           viewPos.z * topPlane.N.z;
    if (Dist > S->r)
        return false;

    // 下クリップ平面とテスト
    // 下クリップ平面は原点を通るのでN.d = 0で、x軸に平行なのでN.x = 0である。
    Dist = viewPos.y * bottomPlane.N.y +
           viewPos.z * bottomPlane.N.z;
    if (Dist > S->r)
        return false;

    // 球の一部または全部がフラスタム内にある
    return true;
}

bool
FRUSTUM::IntersectAABB(const AABB* B) const
{
    // フラスタムを包むAABBでフィルタリング
    if (!IntersectionAABB(B, &box))
        return false;

    int i;
    VEC3 p;
    for (i = 0; i < 6; ++i)
    {
        // フラスタムの各面に対して
        // Test: ax + by + cz + dを最小にする点を決める。
        // これはa,b,cの符号を見ることでで決めることができる。
        // Testが正ならAABBの全ての点はフラスタムの外ということになる。
        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 (planes[i].Test(p) > 0)
            return false;
    }

    return true;
}

IntersectionResult
FRUSTUM::IntersectAABB_Ex(const AABB* B) const
{
    // フラスタムを包むAABBでフィルタリング
    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 (planes[i].Test(p) > 0)
            return INTERSECTION_OUTSIDE;
        if (planes[i].Test(n) > 0)
            result = INTERSECTION_INTERSECT;
    }

    return result;
}




//
// 領域のマージ
//


SPHERE*
MergeSphere(SPHERE* s2, const SPHERE* s0, const SPHERE* s1)
{
    // s2 == s0,s1でも問題ない
    
    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