/* -*- c++ -*-
FILE: SQuat.cpp
RCS REVISION: $Revision: 1.8 $

COPYRIGHT: (c) 1999 -- 2003 Melinda Green, Don Hatch, and Jay Berkenbilt - Superliminal Software

LICENSE: Free to use and modify for non-commercial purposes as long as the
    following conditions are adhered to:
    1) Obvious credit for the source of this code and the designs it embodies
       are clearly made, and
    2) Ports and derived versions of 4D Magic Cube programs are not distributed
       without the express written permission of the authors.

DESCRIPTION:
    Implementation ofsimple quaternion library
*/

#include "squat.h"
#include "vec.h"

#define X 0
#define Y 1
#define Z 2


Vector3::Vector3 ()
{
    // ZEROVEC3(elements);
}

Vector3::Vector3 (double x, double y, double z)
{
    elements[X] = x;
    elements[Y] = y;
    elements[Z] = z;
}

double Vector3::Length() const
{
    return sqrt(NORMSQRD3(elements));
}

Vector3 Vector3::Normal() const
{
    Vector3     norm(*this);
    double      len = Length();
    VDS3(norm, norm, len);
    return norm;
}

void SQuat::InitFromVector(const Vector3 &a, double r)
{
    Vector3     normvec(a.Normal());
    double      half_angle = r * 0.5;
    double      sin_half_angle = sin(half_angle);
    rot = cos(half_angle);
    VXS3(axis, normvec, sin_half_angle);
}

SQuat::SQuat (const Vector3 &a, double r, int homo_vals)
{
    if (homo_vals)
    {
        axis = a;
        rot = r;
    }
    else
        InitFromVector(a, r);
}

SQuat::SQuat (double x, double y, double z, double r, int homo_vals)
{
    if (homo_vals)
    {
        rot = r;
        axis = Vector3 (x, y, z);
    }
    else
        InitFromVector(Vector3 (x, y, z), r);
}


SQuat::SQuat (const SQuat &q) :
    axis(q.axis),
    rot(q.rot)
{
}

SQuat::SQuat (const Matrix3 &m)
{
    double      trace = TRACE3(m);
#ifdef PREMULT
    if (trace > 0.0)
    {
        double      s = sqrt(trace + 1.0);
        rot = s * 0.5;
        s = 0.5 / s;
        const Vector3 a(m[2][1] - m[1][2],
                        m[0][2] - m[2][0], m[1][0] - m[0][1]);
        VXS3(axis, a, s);
    }
    else
    {
        int         i, j, k;
        i = 0;
        if (m[1][1] > m[0][0])
            i = 1;
        if (m[2][2] > m[i][i])
            i = 2;
        j = (i + 1) % 3;
        k = (i + 2) % 3;
        double      s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
        axis[i] = s * 0.5;
        s = 0.5 / s;
        rot = s * (m[k][j] - m[j][k]);
        axis[j] = s * m[j][i] + m[i][j];
        axis[k] = s * m[k][i] + m[i][k];
    }
#else
    if (trace > 0.0)
    {
        double      s = sqrt(trace + 1.0);
        rot = s * 0.5;
        s = 0.5 / s;
        const Vector3 a(m[1][2] - m[2][1],
                        m[2][0] - m[0][2], m[0][1] - m[1][0]);
        VXS3(axis, a, s);
    }
    else
    {
        int         i, j, k;
        i = 0;
        if (m[1][1] > m[0][0])
            i = 1;
        if (m[2][2] > m[i][i])
            i = 2;
        j = (i + 1) % 3;
        k = (i + 2) % 3;
        double      s = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
        axis[i] = s * 0.5;
        s = 0.5 / s;
        rot = s * (m[j][k] - m[k][j]);
        axis[j] = s * m[i][j] + m[j][i];
        axis[k] = s * m[i][k] + m[k][i];
    }
#endif
}


SQuat SQuat::operator * (const SQuat &q) const
{
    Vector3     v1, v2, scaled_sum, cross, new_axis;
    VXS3(v1, q.axis, rot);
    VXS3(v2, axis, q.rot);
    VPV3(scaled_sum, v1, v2);  // scaled_sum = rot*q.axis +
    // q.rot*axis
#ifdef PREMULT
    VXV3(cross, axis, q.axis);
#else
    VXV3(cross, q.axis, axis);
#endif
    VPV3(new_axis, scaled_sum, cross); // new_axis =
    // scaled_sum + cross
    return SQuat (new_axis, rot * q.rot - DOT3(axis, q.axis), 1);
}


SQuat &SQuat::operator *= (const SQuat &q)
{
    return *this = *this * q;
}

double SQuat::GetRotation() const
{
    return 2.0 * acos(rot);
}

void SQuat::GetAxis(Vector3 &v) const
{
    double      theta = GetRotation();
    double      f = 1.0 / sin(theta * 0.5);
    VXS3(v, axis, f);
}

Vector3 SQuat::GetAxis() const
{
    Vector3     vec;
    GetAxis(vec);
    return vec;
}

double SQuat::GetHomoRotation() const
{
    return rot;
}

void SQuat::GetHomoAxis(Vector3 &v) const
{
    SET3(v, axis);
}

Matrix3::Matrix3 ()
{
    IDENTMAT3(rows);
}

Matrix3::Matrix3 (const Matrix3 &from)
{
    SET3(rows, from.rows);
}

Matrix3::Matrix3 (const Vector3 &x, const Vector3 &y, const Vector3 &z)
{
    rows[X] = x;
    rows[Y] = y;
    rows[Z] = z;
}

Matrix3::Matrix3 (const SQuat &q)
{
    double      W = q.GetHomoRotation();
    Vector3     axis;
    q.GetHomoAxis(axis);

    double      XX = axis[X] + axis[X];
    double      YY = axis[Y] + axis[Y];
    double      ZZ = axis[Z] + axis[Z];

    double      WXX = W * XX;
    double      XXX = axis[X] * XX;

    double      WYY = W * YY;
    double      XYY = axis[X] * YY;
    double      YYY = axis[Y] * YY;

    double      WZZ = W * ZZ;
    double      XZZ = axis[X] * ZZ;
    double      YZZ = axis[Y] * ZZ;
    double      ZZZ = axis[Z] * ZZ;

#ifdef PREMULT
    rows[X][X] = 1.0 - (YYY + ZZZ);
    rows[X][Y] = XYY - WZZ;
    rows[X][Z] = XZZ + WYY;

    rows[Y][X] = XYY + WZZ;
    rows[Y][Y] = 1.0 - (XXX + ZZZ);
    rows[Y][Z] = YZZ - WXX;

    rows[Z][X] = XZZ - WYY;
    rows[Z][Y] = YZZ + WXX;
    rows[Z][Z] = 1.0 - (XXX + YYY);
#else
    rows[X][X] = 1.0 - (YYY + ZZZ);
    rows[Y][X] = XYY - WZZ;
    rows[Z][X] = XZZ + WYY;

    rows[X][Y] = XYY + WZZ;
    rows[Y][Y] = 1.0 - (XXX + ZZZ);
    rows[Z][Y] = YZZ - WXX;

    rows[X][Z] = XZZ - WYY;
    rows[Y][Z] = YZZ + WXX;
    rows[Z][Z] = 1.0 - (XXX + YYY);
#endif
}

// Local Variables:
// c-basic-offset: 4
// c-comment-only-line-offset: 0
// c-file-offsets: ((defun-block-intro . +) (block-open . 0) (substatement-open . 0) (statement-cont . +) (statement-case-open . +4) (arglist-intro . +) (arglist-close . +) (inline-open . 0))
// indent-tabs-mode: nil
// End: