openmohaa/code/skeletor/SkelMat4.h

/*
===========================================================================
Copyright (C) 2023 the OpenMoHAA team

This file is part of OpenMoHAA source code.

OpenMoHAA source code is free software; you can redistribute it
and/or modify it under the terms of the GNU General Public License as
published by the Free Software Foundation; either version 2 of the License,
or (at your option) any later version.

OpenMoHAA source code is distributed in the hope that it will be
useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with OpenMoHAA source code; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
===========================================================================
*/

// SkelMat4.h : Skeletor

#pragma once

#ifdef __cplusplus

class SkelQuat;

class SkelMat4
{
public:
    float val[4][3];

protected:
    void copy(const SkelMat4&);

public:
    void MakeIdentity();
    void MakeTranslate(float, float, float);
    void MakeXRotation(float);
    void MakeYRotation(float);
    void MakeZRotation(float);

    SkelMat4(const float mat[3][3]);
    SkelMat4();

    SkelVec3 *XAxis();
    SkelVec3 *YAxis();
    SkelVec3 *ZAxis();
    SkelVec3 *XAxis() const;
    SkelVec3 *YAxis() const;
    SkelVec3 *ZAxis() const;

    operator float *();
    operator float *() const;

    bool IsOrthonormal() const;
    bool IsValid() const;

    float *operator[](int index);
    float *operator[](int index) const;

    int CompareExact(const SkelMat4& skel) const;

    void  Sum(const SkelMat4 &m1, const SkelMat4 &m2);
    void  Difference(const SkelMat4 &m1, const SkelMat4 &m2);
    void  Multiply(const SkelMat4 &m1, const SkelMat4 &m2);
    void  InvertAxis(int);
    void  RotateBy(const SkelMat3 &m);
    void  RotateByInverse(const SkelMat3 &m);
    void  RotateXAxis(float x);
    void  RotateYAxis(float y);
    void  RotateZAxis(float z);
    void  RotateXAxis(float, float);
    void  RotateYAxis(float, float);
    void  RotateZAxis(float, float);
    void  MoveOnXAxis(float x);
    void  MoveOnYAxis(float y);
    void  MoveOnZAxis(float z);
    void  TransformVector(float *) const;
    void  TransposeRot();
    void  TransposeOf(const SkelMat4 &m);
    void  TransposeRotOf(const SkelMat4 &m);
    void  InverseRotOf(const SkelMat4 &m);
    float Determinant(void);
    void  Inverse(void);
    void  GetRotationMatrix(float (*)[3]) const;
    void  GetRotationMatrix(float (*)[4]) const;
    void  ReplacePos(const float *);
    void  ReplaceRot(const SkelMat3 &m);
    void  ReplaceRot(const SkelMat4 &m);
    void  GetPos(float *pos) const;
    void  GetScale(float *scale) const;
    void  DeltaPos(const SkelMat4 &m, float *delta) const;
    void  RotateYaw(float, float);

    void GetQuat(SkelQuat& quat);
};

inline SkelMat4::SkelMat4(const float mat[3][3])
{
    VectorCopy(mat[0], val[0]);
    VectorCopy(mat[1], val[1]);
    VectorCopy(mat[2], val[2]);
    val[3][0] = 0.0f;
    val[3][1] = 0.0f;
    val[3][2] = 0.0f;
}

inline SkelMat4::SkelMat4()
{
    MakeIdentity();
}

inline void SkelMat4::copy(const SkelMat4& m)
{
    MatrixCopy(*m.val, *val);
}

inline void SkelMat4::MakeIdentity()
{
    memset(val, 0, sizeof(val));
    val[0][0] = 1.0f;
    val[1][1] = 1.0f;
    val[2][2] = 1.0f;
}

inline SkelVec3 *SkelMat4::XAxis()
{
    return (SkelVec3 *)&val[0];
}

inline SkelVec3 *SkelMat4::YAxis()
{
    return (SkelVec3 *)&val[1];
}

inline SkelVec3 *SkelMat4::ZAxis()
{
    return (SkelVec3 *)&val[2];
}

inline SkelVec3 *SkelMat4::XAxis() const
{
    return (SkelVec3 *)&val[0];
}

inline SkelVec3 *SkelMat4::YAxis() const
{
    return (SkelVec3 *)&val[1];
}

inline SkelVec3 *SkelMat4::ZAxis() const
{
    return (SkelVec3 *)&val[2];
}

inline SkelMat4::operator float *()
{
    return &val[0][0];
}

inline SkelMat4::operator float *() const
{
    return (float *)&val[0][0];
}

inline float *SkelMat4::operator[](int index)
{
    return val[index];
}

inline float *SkelMat4::operator[](int index) const
{
    return (float *)val[index];
}

inline bool SkelMat4::IsOrthonormal() const
{
    // FIXME: stub
    return false;
}

inline bool SkelMat4::IsValid() const
{
    // FIXME: stub
    return false;
}

inline void SkelMat4::Sum(const SkelMat4& m1, const SkelMat4& m2)
{
    VectorAdd(m1.val[0], m2.val[0], val[0]);
    VectorAdd(m1.val[1], m2.val[1], val[1]);
    VectorAdd(m1.val[2], m2.val[2], val[2]);
}

inline void SkelMat4::Difference(const SkelMat4& m1, const SkelMat4& m2)
{
    VectorSubtract(m1.val[0], m2.val[0], val[0]);
    VectorSubtract(m1.val[1], m2.val[1], val[1]);
    VectorSubtract(m1.val[2], m2.val[2], val[2]);
}

inline void SkelMat4::Multiply(const SkelMat4& m1, const SkelMat4& m2)
{
    val[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0];
    val[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0];
    val[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0];
    val[3][0] = m1[3][0] * m2[0][0] + m1[3][1] * m2[1][0] + m1[3][2] * m2[2][0] + m2[3][0];

    val[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1];
    val[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1];
    val[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1];
    val[3][1] = m1[3][0] * m2[0][1] + m1[3][1] * m2[1][1] + m1[3][2] * m2[2][1] + m2[3][1];

    val[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2];
    val[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2];
    val[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2];
    val[3][2] = m1[3][0] * m2[0][2] + m1[3][1] * m2[1][2] + m1[3][2] * m2[2][2] + m2[3][2];
}

inline void SkelMat4::InvertAxis(int a)
{
    val[a][0] = -val[a][0];
    val[a][1] = -val[a][1];
    val[a][2] = -val[a][2];
}

inline float SkelMat4::Determinant(void)
{
    float mat4x4[4][4];

    mat4x4[0][0] = val[0][0];
    mat4x4[0][1] = val[0][1];
    mat4x4[0][2] = val[0][2];
    mat4x4[0][3] = 0;
    mat4x4[1][0] = val[1][0];
    mat4x4[1][1] = val[1][1];
    mat4x4[1][2] = val[1][2];
    mat4x4[1][3] = 0;
    mat4x4[2][0] = val[2][0];
    mat4x4[2][1] = val[2][1];
    mat4x4[2][2] = val[2][2];
    mat4x4[2][3] = 0;
    mat4x4[3][0] = val[3][0];
    mat4x4[3][1] = val[3][1];
    mat4x4[3][2] = val[3][2];
    mat4x4[3][3] = 1;

    return mat4x4[0][0]
             * (mat4x4[1][1] * (mat4x4[2][2] * mat4x4[3][3] - mat4x4[2][3] * mat4x4[3][2])
                - mat4x4[2][1] * (mat4x4[1][2] * mat4x4[3][3] - mat4x4[1][3] * mat4x4[3][2])
                + mat4x4[3][1] * (mat4x4[1][2] * mat4x4[2][3] - mat4x4[1][3] * mat4x4[2][2]))
         - mat4x4[1][0]
               * (mat4x4[0][1] * (mat4x4[2][2] * mat4x4[3][3] - mat4x4[2][3] * mat4x4[3][2])
                  - mat4x4[2][1] * (mat4x4[0][2] * mat4x4[3][3] - mat4x4[0][3] * mat4x4[3][2])
                  + mat4x4[3][1] * (mat4x4[0][2] * mat4x4[2][3] - mat4x4[0][3] * mat4x4[2][2]))
         + mat4x4[2][0]
               * (mat4x4[0][1] * (mat4x4[1][2] * mat4x4[3][3] - mat4x4[1][3] * mat4x4[3][2])
                  - mat4x4[1][1] * (mat4x4[0][2] * mat4x4[3][3] - mat4x4[0][3] * mat4x4[3][2])
                  + mat4x4[3][1] * (mat4x4[0][2] * mat4x4[1][3] - mat4x4[0][3] * mat4x4[1][2]))
         - mat4x4[3][0]
               * (mat4x4[0][1] * (mat4x4[1][2] * mat4x4[2][3] - mat4x4[1][3] * mat4x4[2][2])
                  - mat4x4[1][1] * (mat4x4[0][2] * mat4x4[2][3] - mat4x4[0][3] * mat4x4[2][2])
                  + mat4x4[2][1] * (mat4x4[0][2] * mat4x4[1][3] - mat4x4[0][3] * mat4x4[1][2]));
}

inline void SkelMat4::Inverse(void)
{
    float       mat4x4[4][4];
    float       outmat4x4[4][4];
    const float Det = Determinant();

    mat4x4[0][0] = val[0][0];
    mat4x4[0][1] = val[0][1];
    mat4x4[0][2] = val[0][2];
    mat4x4[0][3] = 0;
    mat4x4[1][0] = val[1][0];
    mat4x4[1][1] = val[1][1];
    mat4x4[1][2] = val[1][2];
    mat4x4[1][3] = 0;
    mat4x4[2][0] = val[2][0];
    mat4x4[2][1] = val[2][1];
    mat4x4[2][2] = val[2][2];
    mat4x4[2][3] = 0;
    mat4x4[3][0] = val[3][0];
    mat4x4[3][1] = val[3][1];
    mat4x4[3][2] = val[3][2];
    mat4x4[3][3] = 1;

    if (Det == 0.0f) {
        MakeIdentity();
    } else {
        VectorMatrixInverse(outmat4x4, mat4x4);

        val[0][0] = outmat4x4[0][0];
        val[0][1] = outmat4x4[0][1];
        val[0][2] = outmat4x4[0][2];
        val[1][0] = outmat4x4[1][0];
        val[1][1] = outmat4x4[1][1];
        val[1][2] = outmat4x4[1][2];
        val[2][0] = outmat4x4[2][0];
        val[2][1] = outmat4x4[2][1];
        val[2][2] = outmat4x4[2][2];
        val[3][0] = outmat4x4[3][0];
        val[3][1] = outmat4x4[3][1];
        val[3][2] = outmat4x4[3][2];
    }
}

inline void SkelMat4::RotateBy(const SkelMat3& m)
{
    SkelMat4 temp = *this;

    val[0][0] = temp[0][0] * m[0][0] + temp[0][1] * m[1][0] + temp[0][2] * m[2][0];
    val[0][1] = temp[0][0] * m[0][1] + temp[0][1] * m[1][1] + temp[0][2] * m[2][1];
    val[0][2] = temp[0][0] * m[0][2] + temp[0][1] * m[1][2] + temp[0][2] * m[2][2];

    val[1][0] = temp[1][0] * m[0][0] + temp[1][1] * m[1][0] + temp[1][2] * m[2][0];
    val[1][1] = temp[1][0] * m[0][1] + temp[1][1] * m[1][1] + temp[1][2] * m[2][1];
    val[1][2] = temp[1][0] * m[0][2] + temp[1][1] * m[1][2] + temp[1][2] * m[2][2];

    val[2][0] = temp[2][0] * m[0][0] + temp[2][1] * m[1][0] + temp[2][2] * m[2][0];
    val[2][1] = temp[2][0] * m[0][1] + temp[2][1] * m[1][1] + temp[2][2] * m[2][1];
    val[2][2] = temp[2][0] * m[0][2] + temp[2][1] * m[1][2] + temp[2][2] * m[2][2];
}

inline void SkelMat4::RotateByInverse(const SkelMat3& m)
{
    // FIXME: stub
}

inline void SkelMat4::TransposeRotOf(const SkelMat4& m)
{
    val[0][0] = m.val[0][0];
    val[0][1] = m.val[1][0];
    val[0][2] = m.val[2][0];
    val[1][0] = m.val[0][1];
    val[1][1] = m.val[1][1];
    val[1][2] = m.val[2][1];
    val[2][0] = m.val[0][2];
    val[2][1] = m.val[1][2];
    val[2][2] = m.val[2][2];
}

inline void SkelMat4::GetQuat(SkelQuat& quat)
{
    MatToQuat(val, (float *)&quat);
}

#else

typedef struct {
    float val[4][3];
} SkelMat4;

#endif