source: GTP/trunk/App/Demos/Vis/FriendlyCulling/src/Matrix4x4.cpp @ 3103

#include "Matrix4x4.h"
#include "Vector3.h"
#include "AxisAlignedBox3.h"


using namespace std;


namespace CHCDemoEngine 
{

Matrix4x4::Matrix4x4()
{}


Matrix4x4::Matrix4x4(const Vector3 &a, const Vector3 &b, const Vector3 &c)
{
        // first index is column [x], the second is row [y]
        x[0][0] = a.x;
        x[1][0] = b.x;
        x[2][0] = c.x;
        x[3][0] = 0.0f;

        x[0][1] = a.y;
        x[1][1] = b.y;
        x[2][1] = c.y;
        x[3][1] = 0.0f;

        x[0][2] = a.z;
        x[1][2] = b.z;
        x[2][2] = c.z;
        x[3][2] = 0.0f;

        x[0][3] = 0.0f;
        x[1][3] = 0.0f;
        x[2][3] = 0.0f;
        x[3][3] = 1.0f;
}


void Matrix4x4::SetColumns(const Vector3 &a, const Vector3 &b, const Vector3 &c)
{
        // first index is column [x], the second is row [y]
        x[0][0] = a.x;
        x[1][0] = a.y;
        x[2][0] = a.z;
        x[3][0] = 0.0f;

        x[0][1] = b.x;
        x[1][1] = b.y;
        x[2][1] = b.z;
        x[3][1] = 0.0f;

        x[0][2] = c.x;
        x[1][2] = c.y;
        x[2][2] = c.z;
        x[3][2] = 0.0f;

        x[0][3] = 0.0f;
        x[1][3] = 0.0f;
        x[2][3] = 0.0f;
        x[3][3] = 1.0f;
}

// full constructor
Matrix4x4::Matrix4x4(float x11, float x12, float x13, float x14,
                                         float x21, float x22, float x23, float x24,
                                         float x31, float x32, float x33, float x34,
                                         float x41, float x42, float x43, float x44)
{
        // first index is column [x], the second is row [y]
        x[0][0] = x11;
        x[1][0] = x12;
        x[2][0] = x13;
        x[3][0] = x14;

        x[0][1] = x21;
        x[1][1] = x22;
        x[2][1] = x23;
        x[3][1] = x24;

        x[0][2] = x31;
        x[1][2] = x32;
        x[2][2] = x33;
        x[3][2] = x34;

        x[0][3] = x41;
        x[1][3] = x42;
        x[2][3] = x43;
        x[3][3] = x44;
}


float Matrix4x4::Det3x3() const 
{
        return (x[0][0]*x[1][1]*x[2][2] + \
                x[1][0]*x[2][1]*x[0][2] + \
                x[2][0]*x[0][1]*x[1][2] - \
                x[2][0]*x[1][1]*x[0][2] - \
                x[0][0]*x[2][1]*x[1][2] - \
                x[1][0]*x[0][1]*x[2][2]);
}



// inverse matrix computation gauss_jacobiho method .. from N.R. in C
// if matrix is regular = computatation successfull = returns 0
// in case of singular matrix returns 1
int Matrix4x4::Invert()
{
        int indxc[4],indxr[4],ipiv[4];
        int i,icol,irow,j,k,l,ll,n;
        float big,dum,pivinv,temp;
        // satisfy the compiler
        icol = irow = 0;

        // the size of the matrix
        n = 4;

        for ( j = 0 ; j < n ; j++) /* zero pivots */
                ipiv[j] = 0;

        for ( i = 0; i < n; i++)
        {
                big = 0.0;
                for (j = 0 ; j < n ; j++)
                        if (ipiv[j] != 1)
                                for ( k = 0 ; k<n ; k++)
                                {
                                        if (ipiv[k] == 0)
                                        {
                                                if (fabs(x[k][j]) >= big)
                                                {
                                                        big = fabs(x[k][j]);
                                                        irow = j;
                                                        icol = k;
                                                }
                                        }
                                        else
                                                if (ipiv[k] > 1)
                                                        return 1; /* singular matrix */
                                }
                                ++(ipiv[icol]);
                                if (irow != icol)
                                {
                                        for ( l = 0 ; l<n ; l++)
                                        {
                                                temp = x[l][icol];
                                                x[l][icol] = x[l][irow];
                                                x[l][irow] = temp;
                                        }
                                }
                                indxr[i] = irow;
                                indxc[i] = icol;
                                if (x[icol][icol] == 0.0)
                                        return 1; /* singular matrix */

                                pivinv = 1.0f / x[icol][icol];
                                x[icol][icol] = 1.0f ;
                                for ( l = 0 ; l<n ; l++)
                                        x[l][icol] = x[l][icol] * pivinv ;

                                for (ll = 0 ; ll < n ; ll++)
                                        if (ll != icol)
                                        {
                                                dum = x[icol][ll];
                                                x[icol][ll] = 0.0;
                                                for ( l = 0 ; l<n ; l++)
                                                        x[l][ll] = x[l][ll] - x[l][icol] * dum ;
                                        }
        }
        for ( l = n; l--; )
        {
                if (indxr[l] != indxc[l])
                        for ( k = 0; k<n ; k++)
                        {
                                temp = x[indxr[l]][k];
                                x[indxr[l]][k] = x[indxc[l]][k];
                                x[indxc[l]][k] = temp;
                        }
        }

        return 0 ; // matrix is regular .. inversion has been succesfull
}


// Invert the given matrix using the above inversion routine.
Matrix4x4 Invert(const Matrix4x4& M)
{
        Matrix4x4 InvertMe = M;
        InvertMe.Invert();
        return InvertMe;
}


// Transpose the matrix.
void Matrix4x4::Transpose()
{
        for (int i = 0; i < 4; i++)
                for (int j = i; j < 4; j++)
                        if (i != j) {
                                float temp = x[i][j];
                                x[i][j] = x[j][i];
                                x[j][i] = temp;
                        }
}


// Transpose the given matrix using the transpose routine above.
Matrix4x4 Transpose(const Matrix4x4& M)
{
        Matrix4x4 TransposeMe = M;
        TransposeMe.Transpose();
        return TransposeMe;
}


// Construct an identity matrix.
Matrix4x4 IdentityMatrix()
{
        Matrix4x4 M;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        M.x[i][j] = (i == j) ? 1.0f : 0.0f;
        return M;
}


// Construct a zero matrix.
Matrix4x4 ZeroMatrix()
{
        Matrix4x4 M;
        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        M.x[i][j] = 0;
        return M;
}

// Construct a translation matrix given the location to translate to.
Matrix4x4
TranslationMatrix(const Vector3& Location)
{
        Matrix4x4 M = IdentityMatrix();
        M.x[3][0] = Location.x;
        M.x[3][1] = Location.y;
        M.x[3][2] = Location.z;
        return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// X axis.
Matrix4x4
RotationXMatrix(float Angle)
{
        Matrix4x4 M = IdentityMatrix();
        float Cosine = cos(Angle);
        float Sine = sin(Angle);
        M.x[1][1] = Cosine;
        M.x[2][1] = -Sine;
        M.x[1][2] = Sine;
        M.x[2][2] = Cosine;
        return M;
}

// Construct a rotation matrix.  Rotates Angle radians about the
// Y axis.
Matrix4x4 RotationYMatrix(float angle)
{
        Matrix4x4 m = IdentityMatrix();

        float cosine = cos(angle);
        float sine = sin(angle);

        m.x[0][0] = cosine;
        m.x[2][0] = -sine;
        m.x[0][2] = sine;
        m.x[2][2] = cosine;

        return m;
}


// Construct a rotation matrix.  Rotates Angle radians about the
// Z axis.
Matrix4x4 RotationZMatrix(float angle)
{
        Matrix4x4 m = IdentityMatrix();

        float cosine = cos(angle);
        float sine = sin(angle);
        
        m.x[0][0] = cosine;
        m.x[1][0] = -sine;
        m.x[0][1] = sine;
        m.x[1][1] = cosine;

        return m;
}


// Construct a yaw-pitch-roll rotation matrix.  Rotate Yaw
// radians about the XY axis, rotate Pitch radians in the
// plane defined by the Yaw rotation, and rotate Roll radians
// about the axis defined by the previous two angles.
Matrix4x4 RotationYPRMatrix(float Yaw, float Pitch, float Roll)
{
        Matrix4x4 M;
        float ch = cos(Yaw);
        float sh = sin(Yaw);
        float cp = cos(Pitch);
        float sp = sin(Pitch);
        float cr = cos(Roll);
        float sr = sin(Roll);

        M.x[0][0] = ch * cr + sh * sp * sr;
        M.x[1][0] = -ch * sr + sh * sp * cr;
        M.x[2][0] = sh * cp;
        M.x[0][1] = sr * cp;
        M.x[1][1] = cr * cp;
        M.x[2][1] = -sp;
        M.x[0][2] = -sh * cr - ch * sp * sr;
        M.x[1][2] = sr * sh + ch * sp * cr;
        M.x[2][2] = ch * cp;
        for (int i = 0; i < 4; i++)
                M.x[3][i] = M.x[i][3] = 0;
        M.x[3][3] = 1;

        return M;
}

// Construct a rotation of a given angle about a given axis.
// Derived from Eric Haines's SPD (Standard Procedural 
// Database).
Matrix4x4 RotationAxisMatrix(const Vector3& axis, float angle)
{
        Matrix4x4 M;

        float cosine = cos(angle);
        float sine = sin(angle);
        float one_minus_cosine = 1 - cosine;

        M.x[0][0] = axis.x * axis.x + (1.0f - axis.x * axis.x) * cosine;
        M.x[0][1] = axis.x * axis.y * one_minus_cosine + axis.z * sine;
        M.x[0][2] = axis.x * axis.z * one_minus_cosine - axis.y * sine;
        M.x[0][3] = 0;

        M.x[1][0] = axis.x * axis.y * one_minus_cosine - axis.z * sine;
        M.x[1][1] = axis.y * axis.y + (1.0f - axis.y * axis.y) * cosine;
        M.x[1][2] = axis.y * axis.z * one_minus_cosine + axis.x * sine;
        M.x[1][3] = 0;

        M.x[2][0] = axis.x * axis.z * one_minus_cosine + axis.y * sine;
        M.x[2][1] = axis.y * axis.z * one_minus_cosine - axis.x * sine;
        M.x[2][2] = axis.z * axis.z + (1.0f - axis.z * axis.z) * cosine;
        M.x[2][3] = 0;

        M.x[3][0] = 0;
        M.x[3][1] = 0;
        M.x[3][2] = 0;
        M.x[3][3] = 1;

        return M;
}


// Constructs the rotation matrix that rotates 'vec1' to 'vec2'
Matrix4x4 RotationVectorsMatrix(const Vector3 &vecStart, const Vector3 &vecTo)
{
        Vector3 vec = CrossProd(vecStart, vecTo);

        if (Magnitude(vec) > Limits::Small) {
                // vector exist, compute angle
                float angle = acos(DotProd(vecStart, vecTo));
                // normalize for sure
                vec.Normalize();
                return RotationAxisMatrix(vec, angle);
        }

        // opposite or colinear vectors
        Matrix4x4 ret = IdentityMatrix();
        if (DotProd(vecStart, vecTo) < 0.0f)
                ret *= -1.0f; // opposite vectors

        return ret;
}


// Construct a scale matrix given the X, Y, and Z parameters
// to scale by.  To scale uniformly, let X==Y==Z.
Matrix4x4 ScaleMatrix(float X, float Y, float Z)
{
        Matrix4x4 M = IdentityMatrix();

        M.x[0][0] = X;
        M.x[1][1] = Y;
        M.x[2][2] = Z;

        return M;
}


// Construct a rotation matrix that makes the x, y, z axes
// correspond to the vectors given.
Matrix4x4 GenRotation(const Vector3 &x, const Vector3 &y, const Vector3 &z)
{
        Matrix4x4 M = IdentityMatrix();

        //  x  y
        M.x[0][0] = x.x;
        M.x[1][0] = x.y;
        M.x[2][0] = x.z;

        M.x[0][1] = y.x;
        M.x[1][1] = y.y;
        M.x[2][1] = y.z;

        M.x[0][2] = z.x;
        M.x[1][2] = z.y;
        M.x[2][2] = z.z;

        return M;
}

// Construct a quadric matrix.  After Foley et al. pp. 528-529.
Matrix4x4
QuadricMatrix(float a, float b, float c, float d, float e,
                          float f, float g, float h, float j, float k)
{
        Matrix4x4 M;

        M.x[0][0] = a;  M.x[0][1] = d;  M.x[0][2] = f;  M.x[0][3] = g;
        M.x[1][0] = d;  M.x[1][1] = b;  M.x[1][2] = e;  M.x[1][3] = h;
        M.x[2][0] = f;  M.x[2][1] = e;  M.x[2][2] = c;  M.x[2][3] = j;
        M.x[3][0] = g;  M.x[3][1] = h;  M.x[3][2] = j;  M.x[3][3] = k;

        return M;
}

// Construct various "mirror" matrices, which flip coordinate
// signs in the various axes specified.
Matrix4x4
MirrorX()
{
        Matrix4x4 M = IdentityMatrix();
        M.x[0][0] = -1;
        return M;
}

Matrix4x4
MirrorY()
{
        Matrix4x4 M = IdentityMatrix();
        M.x[1][1] = -1;
        return M;
}

Matrix4x4
MirrorZ()
{
        Matrix4x4 M = IdentityMatrix();
        M.x[2][2] = -1;
        return M;
}

Matrix4x4
RotationOnly(const Matrix4x4& x)
{
        Matrix4x4 M = x;
        M.x[3][0] = M.x[3][1] = M.x[3][2] = 0;
        return M;
}

// Add corresponding elements of the two matrices.
Matrix4x4&
Matrix4x4::operator+= (const Matrix4x4& A)
{
        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        x[i][j] += A.x[i][j];
        return *this;
}

// Subtract corresponding elements of the matrices.
Matrix4x4&
Matrix4x4::operator-= (const Matrix4x4& A)
{
        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        x[i][j] -= A.x[i][j];
        return *this;
}

// Scale each element of the matrix by A.
Matrix4x4&
Matrix4x4::operator*= (float A)
{
        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        x[i][j] *= A;
        return *this;
}

// Multiply two matrices.
Matrix4x4&
Matrix4x4::operator*= (const Matrix4x4& A)
{
        Matrix4x4 ret = *this;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++) {
                        float subt = 0;
                        for (int k = 0; k < 4; k++)
                                subt += ret.x[i][k] * A.x[k][j];
                        x[i][j] = subt;
                }
                return *this;
}

// Add corresponding elements of the matrices.
Matrix4x4
operator+ (const Matrix4x4& A, const Matrix4x4& B)
{
        Matrix4x4 ret;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        ret.x[i][j] = A.x[i][j] + B.x[i][j];
        return ret;
}

// Subtract corresponding elements of the matrices.
Matrix4x4
operator- (const Matrix4x4& A, const Matrix4x4& B)
{
        Matrix4x4 ret;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        ret.x[i][j] = A.x[i][j] - B.x[i][j];
        return ret;
}

// Multiply matrices.
Matrix4x4
operator* (const Matrix4x4& A, const Matrix4x4& B)
{
        Matrix4x4 ret;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++) {
                        float subt = 0;
                        for (int k = 0; k < 4; k++)
                                subt += A.x[i][k] * B.x[k][j];
                        ret.x[i][j] = subt;
                }
                return ret;
}

// Transform a vector by a matrix.
Vector3
operator* (const Matrix4x4& M, const Vector3& v)
{
        Vector3 ret;
        float denom;

        ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0] + M.x[3][0];
        ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1] + M.x[3][1];
        ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2] + M.x[3][2];
        denom = v.x * M.x[0][3] + v.y * M.x[1][3] + v.z * M.x[2][3] + M.x[3][3];

        if (denom != 1.0)
                ret /= denom;
        return ret;
}


Vector3 Matrix4x4::Transform(float &w, const Vector3 &v, float h) const
{
        Vector3 ret;
        
        ret.x = v.x * x[0][0] + v.y * x[1][0] + v.z * x[2][0] + h * x[3][0];
        ret.y = v.x * x[0][1] + v.y * x[1][1] + v.z * x[2][1] + h * x[3][1];
        ret.z = v.x * x[0][2] + v.y * x[1][2] + v.z * x[2][2] + h * x[3][2];
        w     = v.x * x[0][3] + v.y * x[1][3] + v.z * x[2][3] + h * x[3][3];

        return ret;
}


// Apply the rotation portion of a matrix to a vector.
Vector3
RotateOnly(const Matrix4x4& M, const Vector3& v)
{
        Vector3 ret;
        float denom;

        ret.x = v.x * M.x[0][0] + v.y * M.x[1][0] + v.z * M.x[2][0];
        ret.y = v.x * M.x[0][1] + v.y * M.x[1][1] + v.z * M.x[2][1];
        ret.z = v.x * M.x[0][2] + v.y * M.x[1][2] + v.z * M.x[2][2];
        denom = M.x[0][3] + M.x[1][3] + M.x[2][3] + M.x[3][3];
        if (denom != 1.0)
                ret /= denom;
        return ret;
}

// Scale each element of the matrix by B.
Matrix4x4
operator* (const Matrix4x4& A, float B)
{
        Matrix4x4 ret;

        for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                        ret.x[i][j] = A.x[i][j] * B;
        return ret;
}

// Overloaded << for C++-style output.
ostream&
operator<< (ostream& s, const Matrix4x4& M)
{
        for (int i = 0; i < 4; i++) { // y
                for (int j = 0; j < 4; j++) { // x
                        //  x  y 
                        s << M.x[j][i];
                }
                s << '\n';
        }
        return s;
}


Vector3 PlaneRotate(const Matrix4x4& tform, const Vector3& p)
{
        // I sure hope that matrix is invertible...
        Matrix4x4 use = Transpose(Invert(tform));

        return RotateOnly(use, p);
}

// Transform a normal
Vector3 TransformNormal(const Matrix4x4& tform, const Vector3& n)
{
        Matrix4x4 use = NormalTransformMatrix(tform);

        return RotateOnly(use, n);
}


Matrix4x4  NormalTransformMatrix(const Matrix4x4 &tform)
{
        Matrix4x4 m = tform;
        // for normal translation vector must be zero!
        m.x[3][0] = m.x[3][1] = m.x[3][2] = 0.0; 
        // I sure hope that matrix is invertible...
        return Transpose(Invert(m));
}


Vector3 GetTranslation(const Matrix4x4 &M)
{
        return Vector3(M.x[3][0], M.x[3][1], M.x[3][2]);
}


Matrix4x4 GetFittingProjectionMatrix(const AxisAlignedBox3 &box)
{
        Matrix4x4 m;

        m.x[0][0] = 2.0f / (box.Max()[0] - box.Min()[0]);
        m.x[0][1] = .0f;
        m.x[0][2] = .0f;
        m.x[0][3] = .0f;

        m.x[1][0] = .0f;
        m.x[1][1] = 2.0f / (box.Max()[1] - box.Min()[1]);
        m.x[1][2] = .0f;
        m.x[1][3] = .0f;

        m.x[2][0] = .0f;
        m.x[2][1] = .0f;
        m.x[2][2] = 2.0f / (box.Max()[2] - box.Min()[2]);
        m.x[2][3] = .0f;

        m.x[3][0] = -(box.Max()[0] + box.Min()[0]) / (box.Max()[0] - box.Min()[0]);
        m.x[3][1] = -(box.Max()[1] + box.Min()[1]) / (box.Max()[1] - box.Min()[1]);
        m.x[3][2] = -(box.Max()[2] + box.Min()[2]) / (box.Max()[2] - box.Min()[2]);
        m.x[3][3] = 1.0f;

        return m;
}


/** The resultig matrix that is equal to the result of glFrustum
*/
Matrix4x4 GetFrustum(float left, float right, 
                                         float bottom, float top, 
                                         float near, float far)
{
        Matrix4x4 m;

        const float xDif = 1.0f / (right - left);
        const float yDif = 1.0f / (top - bottom);
        const float zDif = 1.0f / (near - far);

        m.x[0][0] = 2.0f * near * xDif;
        m.x[0][1] = .0f;
        m.x[0][2] = .0f;
        m.x[0][3] = .0f;

        m.x[1][0] = .0f;
        m.x[1][1] = 2.0f * near * yDif;
        m.x[1][2] = .0f;
        m.x[1][3] = .0f;

        m.x[2][0] = (right + left) * xDif;
        m.x[2][1] = (top + bottom)* yDif;
        m.x[2][2] = (far + near) * zDif;
        m.x[2][3] = -1.0f;

        m.x[3][0] = .0f;
        m.x[3][1] = .0f;
        m.x[3][2] = 2.0f * near * far * zDif;
        m.x[3][3] = .0f;

        return m;
}


Matrix4x4 LookAt(const Vector3 &pos, const Vector3 &dir, const Vector3& up) 
{
        const Vector3 nDir = Normalize(dir);
        Vector3 nUp = Normalize(up);

        Vector3 nRight = Normalize(CrossProd(nDir, nUp));
        nUp = Normalize(CrossProd(nRight, nDir));

        Matrix4x4 m(nRight.x, nRight.y, nRight.z, 0,
                        nUp.x,    nUp.y,    nUp.z,    0,
                                -nDir.x,  -nDir.y,   -nDir.z, 0,
                                0,        0,        0,        1);

        // note: left handed system => we go into positive z
        m.x[3][0] = -DotProd(nRight, pos);
        m.x[3][1] = -DotProd(nUp, pos);
        m.x[3][2] = DotProd(nDir, pos);

        return m;
}


/** The resulting matrix is equal to the result of glOrtho
*/
Matrix4x4 GetOrtho(float left, float right, float bottom, float top, float near, float far)
{
        Matrix4x4 m;

        const float xDif = 1.0f / (right - left);
        const float yDif = 1.0f / (top - bottom);
        const float zDif = 1.0f / (far - near);

        m.x[0][0] = 2.0f * xDif;
        m.x[0][1] = .0f;
        m.x[0][2] = .0f;
        m.x[0][3] = .0f;

        m.x[1][0] = .0f;
        m.x[1][1] = 2.0f * yDif;
        m.x[1][2] = .0f;
        m.x[1][3] = .0f;

        m.x[2][0] = .0f;
        m.x[2][1] = .0f;
        m.x[2][2] = -2.0f * zDif;
        m.x[2][3] = .0f;
        
        m.x[3][0] = -(right + left) * xDif;
        m.x[3][1] = -(top + bottom) * yDif;
        m.x[3][2] = -(far + near)   * zDif;
        m.x[3][3] = 1.0f;

        return m;
}

/** The resultig matrix that is equal to the result of gluPerspective
*/
Matrix4x4 GetPerspective(float fov, float aspect, float near, float far)
{
        Matrix4x4 m;

        const float x = 1.0f / tan(fov * 0.5f);
        const float zDif = 1.0f / (near - far);

        m.x[0][0] = x / aspect;
        m.x[0][1] = .0f;
        m.x[0][2] = .0f;
        m.x[0][3] = .0f;

        m.x[1][0] = .0f;
        m.x[1][1] = x;
        m.x[1][2] = .0f;
        m.x[1][3] = .0f;

        m.x[2][0] = .0f;
        m.x[2][1] = .0f;
        m.x[2][2] = (far + near) * zDif;
        m.x[2][3] = -1.0f;
        
        m.x[3][0] = .0f;
        m.x[3][1] = .0f;
        m.x[3][2] = 2.0f *(far * near) * zDif;
        m.x[3][3] = 0.0f;

        return m;
}


}