/*-------------------------------------------------------------------------*/
/*  J3W ver 6.50  3D Animation Kit                                         */
/*  axis.cpp      10/16/2000                                               */
/*  Copyright (C) 1996 - 2000 Jun Mizutani <jun.mizutani@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  <stdlib.h>
#include  "axis.h"

TAxis::TAxis()
{
    change = 1;
    origin = Vector(0,0,0);
    SetAngle(0, 0, 0);
}

void TAxis::move(Vector v)
{

    v = ConvCoordinate(m, v);
    origin = origin + v;
    change = 1;
}

void TAxis::rotate(long h, long p, long b)
{
    EulerAngle(h, p, b);
    CalcRotationMatrix();
    change = 1;
}

void TAxis::set_origin(long x, long y, long z)
{
    origin = Vector(x, y, z);
    change = 1;
}

Vector TAxis::get_origin()
{
    return origin;
}

Matrix3 TAxis::get_matrix()
{
    CalcRotationMatrix();
    return m;
}

void TAxis::set_matrix(Matrix3 tm)
{
    m = tm;
}

Quaternion TAxis::get_quat()
{
    return q;
}

Vector TAxis::RelativePosition(Vector v)
{
    return ConvCoordinate2(m, v);
}

void TAxis::attitude(long h, long p, long b)
{
    SetAngle(h, p, b);
    CalcRotationMatrix();
    change = 1;
}

void TAxis::get_attitude(long &h, long &p, long &b)
{
    GetAngle(h,p,b);
}

void TAxis::CalcRotationMatrix()
{
    QuatToMat(q, m);
}

void TAxis::SetAngle(long h, long p, long b)
{
    EularToQuat(h, p, b, q);
}

void TAxis::GetAngle(long &h, long &p, long &b)
{
    CalcRotationMatrix();
    b  = DEG( atan2(m.m8, m.m9) );
    p  = DEG( - asin(m.m7) );
    h  = DEG( atan2(m.m4, m.m1) );
}

void TAxis::EulerAngle(long DH, long DP, long DB)
{
    Quaternion nq;

    long M = DH;
    if (abs(M) < abs(DP)) M = DP;
    if (abs(M) < abs(DB)) M = DB;
    M = M / 64;
    if (M < 0) M = -M;
    M++;
    double tDH = RAD(DH) / M;
    double tDP = RAD(DP) / M;
    double tDB = RAD(DB) / M;
    for(int n = 1; n<=M; n++) {
        double AL = tDB*tDB + tDP*tDP + tDH*tDH;
        double D1 = (AL*tDB )/48.0;
        double D2 = (AL*tDP )/48.0;
        double D3 = (AL*tDH )/48.0;

        double AQ = 1.0 - 0.125 * AL;
        double BQ = -0.5 * tDB + D1;
        double CQ = -0.5 * tDP + D2;
        double DQ = -0.5 * tDH + D3;

        nq._0 =  AQ*q._0 + BQ*q._1 + CQ*q._2 + DQ*q._3;
        nq._1 = -BQ*q._0 + AQ*q._1 - DQ*q._2 + CQ*q._3;
        nq._2 = -CQ*q._0 + DQ*q._1 + AQ*q._2 - BQ*q._3;
        nq._3 = -DQ*q._0 - CQ*q._1 + BQ*q._2 + AQ*q._3;
        q = nq;
    }
}

void TAxis::attitudeq(Quaternion qq)
{

    q = qq;
    CalcRotationMatrix();
    change = 1;
}

