/*-------------------------------------------------------------------------*/
/*  J3W ver 6.43  3D Animation Kit                                         */
/*  misc3d.cpp     4/ 8/2001                                               */
/*  Copyright (C) 1995 - 2001 Jun Mizutani <mizutani.jun@nifty.ne.jp>      */
/*                      All rights reserved.                               */
/*                                                                         */
/*   This file is part of the J3W 3D Animation Kit, and is covered under   */
/*  the terms of the GNU General Public License, version 2. This file has  */
/*  NO WARRANTY. See file COPYING for copyright details.                   */
/*                                                                         */
/*-------------------------------------------------------------------------*/

#include  "misc3d.h"

Matrix3  MulMatrix3(const Matrix3 &a, const Matrix3 &b)
{
    Matrix3  Result;

    Result.m1 = a.m1 * b.m1 + a.m2 * b.m4 + a.m3 * b.m7;
    Result.m2 = a.m1 * b.m2 + a.m2 * b.m5 + a.m3 * b.m8;
    Result.m3 = a.m1 * b.m3 + a.m2 * b.m6 + a.m3 * b.m9;
    Result.m4 = a.m4 * b.m1 + a.m5 * b.m4 + a.m6 * b.m7;
    Result.m5 = a.m4 * b.m2 + a.m5 * b.m5 + a.m6 * b.m8;
    Result.m6 = a.m4 * b.m3 + a.m5 * b.m6 + a.m6 * b.m9;
    Result.m7 = a.m7 * b.m1 + a.m8 * b.m4 + a.m9 * b.m7;
    Result.m8 = a.m7 * b.m2 + a.m8 * b.m5 + a.m9 * b.m8;
    Result.m9 = a.m7 * b.m3 + a.m8 * b.m6 + a.m9 * b.m9;
    return Result;
}

Vector  ConvCoordinate(const  Matrix3 &m, const Vector &plocal)
{
    Vector Result;
#ifdef WS
    Result.x = j3w_round(m.m1 * plocal.x + m.m2 * plocal.y + m.m3 * plocal.z);
    Result.y = j3w_round(m.m4 * plocal.x + m.m5 * plocal.y + m.m6 * plocal.z);
    Result.z = j3w_round(m.m7 * plocal.x + m.m8 * plocal.y + m.m9 * plocal.z);
#else //WS
    Result.x = round(m.m1 * plocal.x + m.m2 * plocal.y + m.m3 * plocal.z);
    Result.y = round(m.m4 * plocal.x + m.m5 * plocal.y + m.m6 * plocal.z);
    Result.z = round(m.m7 * plocal.x + m.m8 * plocal.y + m.m9 * plocal.z);
#endif //WS
    return Result;
}

Vector  ConvCoordinate2(const Matrix3 &m, const Vector &pworld)
{
    Vector Result;

#ifdef WS
    Result.x = j3w_round(m.m1 * pworld.x + m.m4 * pworld.y + m.m7 * pworld.z);
    Result.y = j3w_round(m.m2 * pworld.x + m.m5 * pworld.y + m.m8 * pworld.z);
    Result.z = j3w_round(m.m3 * pworld.x + m.m6 * pworld.y + m.m9 * pworld.z);
#else //WS
    Result.x = round(m.m1 * pworld.x + m.m4 * pworld.y + m.m7 * pworld.z);
    Result.y = round(m.m2 * pworld.x + m.m5 * pworld.y + m.m8 * pworld.z);
    Result.z = round(m.m3 * pworld.x + m.m6 * pworld.y + m.m9 * pworld.z);
#endif //WS
    return Result;
}

Quaternion MulQuat(const Quaternion a, const Quaternion b)
{
    Quaternion Result;

    Result._0 = a._0*b._0 - a._1*b._1 - a._2*b._2 - a._3*b._3;
    Result._1 = a._0*b._1 + a._1*b._0 + a._2*b._3 - a._3*b._2;
    Result._2 = a._0*b._2 + a._2*b._0 - a._1*b._3 + a._3*b._1;
    Result._3 = a._0*b._3 + a._3*b._0 + a._1*b._2 - a._2*b._1;
    return Result;
}

Quaternion CondugateQuat(const Quaternion a)
{
    Quaternion Result;

    Result._0 =  a._0;
    Result._1 = -a._1;
    Result._2 = -a._2;
    Result._3 = -a._3;
    return Result;
}

Quaternion NormalizeQuat(const Quaternion a)
{
    Quaternion Result;

    double s = sqrt(a._0*a._0 + a._1*a._1 + a._2*a._2 + a._3*a._3);
    Result._0 =  a._0 / s;
    Result._1 =  a._1 / s;
    Result._2 =  a._2 / s;
    Result._3 =  a._3 / s;
    return Result;
}

void EularToQuat(long h, long p, long b, Quaternion &qq)
{
    double cosB = cos(RAD(b) * 0.5);
    double cosP = cos(RAD(p) * 0.5);
    double cosH = cos(RAD(h) * 0.5);
    double sinB = sin(RAD(b) * 0.5);
    double sinP = sin(RAD(p) * 0.5);
    double sinH = sin(RAD(h) * 0.5);

    double cosBcosP = cosB * cosP;
    double sinBsinP = sinB * sinP;
    double cosBsinP = cosB * sinP;
    double sinBcosP = sinB * cosP;

    qq._0 = cosBcosP * cosH + sinBsinP * sinH;
    qq._1 = sinBcosP * cosH - cosBsinP * sinH;
    qq._2 = cosBsinP * cosH + sinBcosP * sinH;
    qq._3 = cosBcosP * sinH - sinBsinP * cosH;
}

int CheckShortestPath(const Quaternion q1, const Quaternion q2)
{
    Quaternion qa, qs;

    qs._0 = q1._0 - q2._0;
    qs._1 = q1._1 - q2._1;
    qs._2 = q1._2 - q2._2;
    qs._3 = q1._3 - q2._3;
    qa._0 = q1._0 + q2._0;
    qa._1 = q1._1 + q2._1;
    qa._2 = q1._2 + q2._2;
    qa._3 = q1._3 + q2._3;
    double s = qs._0*qs._0 + qs._1*qs._1 + qs._2*qs._2 + qs._3*qs._3;
    double a = qa._0*qa._0 + qa._1*qa._1 + qa._2*qa._2 + qa._3*qa._3;
    return s < a;
}

void QuatToMat(const Quaternion q, Matrix3 &m)
{
    double q00 = q._0 * q._0;
    double q01 = q._0 * q._1;
    double q02 = q._0 * q._2;
    double q03 = q._0 * q._3;
    double q11 = q._1 * q._1;
    double q12 = q._1 * q._2;
    double q13 = q._1 * q._3;
    double q22 = q._2 * q._2;
    double q23 = q._2 * q._3;
    double q33 = q._3 * q._3;
    m.m1 = q00 + q11 - q22 - q33;
    m.m5 = q00 - q11 + q22 - q33;
    m.m9 = q00 - q11 - q22 + q33;
    m.m4 = (q12 + q03) * 2;
    m.m7 = (q13 - q02) * 2;
    m.m2 = (q12 - q03) * 2;
    m.m8 = (q23 + q01) * 2;
    m.m3 = (q13 + q02) * 2;
    m.m6 = (q23 - q01) * 2;
}

void Slerp(const Quaternion &a, const Quaternion &b, Quaternion &q, double t)
{
    double s, p, cosp, sinp, scale0, scale1;

    cosp = a._0 * b._0 + a._1 * b._1 + a._2 * b._2 + a._3 * b._3;
    p = acos(cosp);
    sinp = sin(p);
    if (sinp < 0.0) s = -sinp;
    else s = sinp;
    if (s > 0.002) {
        scale0 = sin((1.0 - t) * p) / sinp;
        scale1 = sin(t * p) / sinp;
        q._0 = scale0 * a._0 + scale1 * b._0;
        q._1 = scale0 * a._1 + scale1 * b._1;
        q._2 = scale0 * a._2 + scale1 * b._2;
        q._3 = scale0 * a._3 + scale1 * b._3;
    } else q = b;
}

void MatrixToQuat(const Matrix3 &m, Quaternion &q)
{
    double S;
    if (m.m1 + m.m5 + m.m9 + 1) {
        q._0 = sqrt(m.m1 + m.m5 + m.m9 + 1)/2;
        S = 4 * q._0;
        if (S != 0.0) {
            q._1 = (m.m8 - m.m6)/S;
            q._2 = (m.m3 - m.m7)/S;
            q._3 = (m.m4 - m.m2)/S;
        }
    } else {
        int i = 0;
        if (m.m5 > m.m1) {
          i=1;
          if (m.m9 > m.m5) i=2;
        } else if (m.m9 > m.m1) i=2;

        switch (i) {
            case 0:
                q._1 = sqrt(1 + m.m1 - m.m5 - m.m9)/2;
                S = 4 * q._1;
                if (S != 0.0) {
                    q._0 = (m.m8 - m.m6)/S;
                    q._2 = (m.m2 + m.m4)/S;
                    q._3 = (m.m3 + m.m7)/S;
                }
                break;
            case 1:
                q._2 = sqrt(1 -m.m1 + m.m5 - m.m9)/2;
                S = 4 * q._2;
                if (S != 0.0) {
                    q._0 = (m.m3-m.m7)/S;
                    q._1 = (m.m2+m.m4)/S;
                    q._3 = (m.m6+m.m8)/S;
                }
                break;
            case 2:
                q._3 = sqrt(1 -m.m1 - m.m5 + m.m9)/2;
                S = 4 * q._3;
                if (S != 0.0) {
                    q._0 = (m.m4 - m.m2)/S;
                    q._1 = (m.m3 + m.m7)/S;
                    q._2 = (m.m6 + m.m8)/S;
                }
            break;
        }
    }
}
