/* -*- c++ -*-
FILE: Math4d.cpp
RCS REVISION: $Revision: 1.10 $

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:
    Math4d implementation
*/

#include "Math4d.h"

void
Math4d::get3dRotMatrixAboutAxis(real axis[3], real angle, real mat[3][3])
{
    int         i;
    real        norm, temp[3][3], mat2[2][2];

    IDENTMAT3(mat);
    SET3(mat[2], axis);
    if (ABS(axis[0]) < ABS(axis[1]))
    {
        /* it's closer to the y axis, so cross with x axis first */
        VXV3(mat[1], mat[2], mat[0]);
        VXV3(mat[0], mat[1], mat[2]);
    }
    else
    {
        VXV3(mat[0], mat[1], mat[2]);
        VXV3(mat[1], mat[2], mat[0]);
    }
    for (i = 0; i < 3; ++i)
    {
        norm = sqrt(NORMSQRD3(mat[i]));
        VDS3(mat[i], mat[i], norm);
    }
    /*
     * mat is now an orthogonal matrix that takes the z-axis to
     * the rotation axis.
     */
    TRANSPOSE3(temp, mat);
    /*
     * temp is the matrix that takes the rotation axis to the z axis.
     * (inverse of an orthogonal matrix is its transpose).
     */

    mat2[0][0] = cos(angle);
    mat2[0][1] = sin(angle);
    XV2(mat2[1], mat2[0]);

    M2XM3r(mat, mat2, mat);
    MXM3r(mat, temp, mat);

    /* result is (mat taking rot axis to z axis) * (2d rot mat) * (mat taking
       z axis back to rot axis */
}

void
Math4d::get4dRotMatrix(real center[4], real axispoint[4], real angle,
                       real mat[4][4])
{
    int         ax, sgn, M[4][4], MINV[4][4];
    real        _3d_center[4], _3d_axispoint[4], mat3[3][3], temp[4][4];

    assert(!!center[0] + !!center[1] + !!center[2] + !!center[3] == 1);
    for (ax = 0; ax < 4; ++ax)
        if (center[ax])
            break;
    assert(INRANGE(0 <=, ax, <4));
    sgn = SGN(center[ax]);
    Math4d::getCanonicalMatThatTakesAxisToMinusW(ax, sgn, M);
    TRANSPOSE4i(MINV, M);

    VXM4(_3d_center, center, M);
    assert(ISZEROVEC3(_3d_center) && _3d_center[3] == -3);  /* it's a grip */
    VXM4(_3d_axispoint, axispoint, M);
    get3dRotMatrixAboutAxis(_3d_axispoint, angle, mat3);
    M4XM3(temp, M, mat3);
    MXM4(mat, temp, MINV);
}

void
Math4d::get4dTwistMat(int coords[4], real total_angle, int imat[4][4])
{
    int         i;
    real        center[4], real_stickercoords[4], rmat[4][4];
    /*
     * Calculate rotation center; it's the center of the face containing
     * the grip
     */
    for (i = 0; i < 4; ++i)
    {
        if (ABS(coords[i]) == 3)
            center[i] = coords[i];
        else
            center[i] = 0;
    }

    // total_angle = polymgr_get_twist_total_angle(grip, dir);
    SET4(real_stickercoords, coords);

    get4dRotMatrix(center, real_stickercoords, total_angle, rmat);
    ROUNDMAT4(imat, rmat);
}

// following routines originally from end of polymgr.c -- DG

void
Math4d::getCanonicalMatThatTakesAxisToMinusW(int ax, int sgn, int mat[4][4])
{
    IDENTMAT4(mat);
    if (ax != W)
    {
        mat[W][W] = 0;
        mat[ax][ax] = 0;
        mat[W][ax] = sgn;
        mat[ax][W] = -sgn;
    }
    else if (sgn > 0)
    {
        /* must rotate the W axis into the -W axis */
        mat[W][W] = -1;
        mat[Z][Z] = -1;
    }
}

#define VEQVXS3(v,w,s) ((v)[0] == (w)[0] * (s) && \
            (v)[1] == (w)[1] * (s) && \
            (v)[2] == (w)[2] * (s))

/*
 * Return 1 on success, 0 on failure.
 * FIX THIS-- find a better way to do this.  I would
 * be really surprised if there are no typos below.
 */
int
Math4d::get4dMatThatRotatesThese3ToThose3(
    int these0[4], int these1[4], int these2[4],
    int those0[4], int those1[4], int those2[4], int mat[4][4]) /* OUTPUT */
{
    int         i, j, k, l, ii, temp[4];
    int         these[4][4], those[4][4];   /* actually [3][4] */
    int         transpthese[4][4], transpthose[4][4];   /* actually [4][3] */

    SET4(these[0], these0);
    SET4(these[1], these1);
    SET4(these[2], these2);
    TRANSPOSE4(transpthese, these);
    SET4(those[0], those0);
    SET4(those[1], those1);
    SET4(those[2], those2);
    TRANSPOSE4(transpthose, those);

    ZEROMAT4(mat);
    for (i = 0; i < 4; ++i)
    {
        for (mat[0][i] = 1; mat[0][i] >= -1; mat[0][i] -= 2)
        {
            if (!VEQVXS3(transpthese[0], transpthose[i], mat[0][i]))
                continue;
            for (j = 0; j < 4; ++j)
            {
                if (j == i)
                    continue;
                for (mat[1][j] = 1; mat[1][j] >= -1; mat[1][j] -= 2)
                {
                    if (!VEQVXS3(transpthese[1], transpthose[j], mat[1][j]))
                        continue;
                    for (k = 0; k < 4; ++k)
                    {
                        if (k == i || k == j)
                            continue;
                        for (mat[2][k] = 1; mat[2][k] >= -1; mat[2][k] -= 2)
                        {
                            if (!VEQVXS3(transpthese[2],
                                         transpthose[k], mat[2][k]))
                                continue;
                            l = 6 - i - j - k;
                            for (mat[3][l] = 1; mat[3][l] >= -1;
                                 mat[3][l] -= 2)
                            {
                                if (!VEQVXS3(transpthese[3],
                                             transpthose[l], mat[3][l]))
                                    continue;
                                /*
                                 * Found a good matrix!!
                                 * Assert that it works.
                                 */
                                for (ii = 0; ii < 3; ++ii)
                                {
                                    VXM4(temp, these[ii], mat);
                                    assert(EQVEC4(temp, those[ii]));
                                }
                                return 1;
                            }
                            mat[3][l] = 0;
                        }
                        mat[2][k] = 0;
                    }
                }
                mat[1][j] = 0;
            }
        }
        mat[0][i] = 0;
    }
    return 0;
}

// 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: