Matrix.cpp

Go to the documentation of this file.

#include "Matrix.h"

Mat4::Mat4() {
        for (int i=0; i<16; i++) m[i] = 0.0f;
}

Mat4::Mat4(float val) {
        for (int i=0; i<16; i++) m[i] = val;
}

Mat4::Mat4(float mat[]) {
        for (int i=0; i<16; i++) m[i] = mat[i];
}

void Mat4::loadIdentity() {
        for (unsigned int i=0; i<16; i++) {
                m[i] = ((i & 3) == (i >> 2));
        }
}

void Mat4::loadRotateX(const float angle) {
        float cosA = cosf(angle);
        float sinA = sinf(angle);

        m[0]    = cosA;
        m[1]    = 0;
        m[2]    = sinA;
        m[3]    = 0;

        m[4]    = 0;
        m[5]    = 1;
        m[6]    = 0;
        m[7]    = 0;

        m[8]    = -sinA;
        m[9]    = 0;
        m[10]   = cosA;
        m[11]   = 0;

        m[12]   = 0;
        m[13]   = 0;
        m[14]   = 0;
        m[15]   = 1;
}

void Mat4::loadRotateY(const float angle) {
        float cosA = cosf(angle);
        float sinA = sinf(angle);

        m[0]    = 1;
        m[1]    = 0;
        m[2]    = 0;
        m[3]    = 0;

        m[4]    = 0;
        m[5]    = cosA;
        m[6]    = sinA;
        m[7]    = 0;

        m[8]    = 0;
        m[9]    = -sinA;
        m[10]   = cosA;
        m[11]   = 0;

        m[12]   = 0;
        m[13]   = 0;
        m[14]   = 0;
        m[15]   = 1;
}

void Mat4::loadRotateZ(const float angle) {
        float cosA = cosf(angle);
        float sinA = sinf(angle);

        m[0]    = cosA;
        m[1]    = sinA;
        m[2]    = 0;
        m[3]    = 0;

        m[4]    = -sinA;
        m[5]    = cosA;
        m[6]    = 0;
        m[7]    = 0;

        m[8]    = 0;
        m[9]    = 0;
        m[10]   = 1;
        m[11]   = 0;

        m[12]   = 0;
        m[13]   = 0;
        m[14]   = 0;
        m[15]   = 1;
}

void Mat4::loadRotateXY(const float angleX, const float angleY) {
        float cosX = cosf(angleX);
        float sinX = sinf(angleX);

        float cosY = cosf(angleY);
        float sinY = sinf(angleY);

        m[0]    = cosY;
        m[1]    = -sinX*sinY;
        m[2]    = cosX*sinY;
        m[3]    = 0;

        m[4]    = 0;
        m[5]    = cosX;
        m[6]    = sinX;
        m[7]    = 0;

        m[8]    = -sinY;
        m[9]    = -sinX*cosY;
        m[10]   = cosX*cosY;
        m[11]   = 0;

        m[12]   = 0;
        m[13]   = 0;
        m[14]   = 0;
        m[15]   = 1;
}

void Mat4::loadRotateZXY(const float angleX, const float angleY, const float angleZ) {
        float cosX = cosf(angleX);
        float sinX = sinf(angleX);

        float cosY = cosf(angleY);
        float sinY = sinf(angleY);

        float cosZ = cosf(angleZ);
        float sinZ = sinf(angleZ);

        m[0]  = cosY * cosZ + sinX * sinY * sinZ;
        m[1]  = cosY * sinZ - sinX * sinY * cosZ;
        m[2]  = cosX * sinY;
        m[3]  = 0;

        m[4]  = -cosX * sinZ;
        m[5]  = cosX * cosZ;
        m[6]  = sinX;
        m[7]  = 0;

        m[8]  = sinX * cosY * sinZ - sinY * cosZ;
        m[9]  = -sinY * sinZ - sinX * cosY * cosZ;
        m[10] = cosX * cosY;
        m[11] = 0;

        m[12] = 0;
        m[13] = 0;
        m[14] = 0;
        m[15] = 1;
}

void Mat4::loadRotate(const Vec3 &v, const float angle) {
        float cosA = cosf(angle);
        float sinA = sinf(angle);

        m[0]  = v.x * v.x * (1 - cosA) + cosA;
        m[1]  = v.x * v.y * (cosA - 1) + v.z * sinA;
        m[2]  = v.x * v.z * (1 - cosA) + v.y * sinA;
        m[3]  = 0;
        m[4]  = v.x * v.y * (cosA - 1) - v.z * sinA;
        m[5]  = v.y * v.y * (1 - cosA) + cosA;
        m[6]  = v.y * v.z * (cosA - 1) + v.x * sinA;
        m[7]  = 0;
        m[8]  = v.x * v.z * (1 - cosA) - v.y * sinA;
        m[9]  = v.y * v.z * (cosA - 1) - v.x * sinA;
        m[10] = v.z * v.z * (1 - cosA) + cosA;
        m[11] = 0;
        m[12] = 0;
        m[13] = 0;
        m[14] = 0;
        m[15] = 1;
}

void Mat4::loadProjectionMatrix(float fov, float aspect, float zNear, float zFar) {
        float w, h;

        w = tanf(0.5f *fov);
        h = w*aspect;

        m[0] = 1.0f / w;
        m[1] = 0;
        m[2] = 0;
        m[3] = 0;

        m[4] = 0;
        m[5] = 1.0f / h;
        m[6] = 0;
        m[7] = 0;
                
        m[8]  = 0;
        m[9]  = 0;
        m[10] = (zFar + zNear) / (zFar - zNear);
        m[11] = 1;

        m[12] = 0;
        m[13] = 0;
        m[14] = -(2 * zFar * zNear) / (zFar - zNear);
        m[15] = 0;
}

void Mat4::translate(const Vec3 &v) {
        m[12] += (v.x * m[0] + v.y * m[4] + v.z * m[8]);
        m[13] += (v.x * m[1] + v.y * m[5] + v.z * m[9]);
        m[14] += (v.x * m[2] + v.y * m[6] + v.z * m[10]);
        m[15] += (v.x * m[3] + v.y * m[7] + v.z * m[11]);
}

void Mat4::setTranslate(const Vec3 &v) {
        m[12] = v.x;
        m[13] = v.y;
        m[14] = v.z;
}

Vec3 Mat4::translateVec(const Vec3 &v) {
        Vec3 vec;
        vec.x = v.x + m[12];
        vec.y = v.y + m[13];
        vec.z = v.z + m[14];

        return vec;
}

void Mat4::scale(const float sx, const float sy, const float sz) {
        m[0] *= sx; m[1] *= sx; m[2]  *= sx; m[3]  *= sx;
        m[4] *= sy; m[5] *= sy; m[6]  *= sy; m[7]  *= sy;
        m[8] *= sz; m[9] *= sz; m[10] *= sz; m[11] *= sz;
}

Vec3 Mat4::rotateVec(const Vec3 &v) {
        Vec3 vec;
        vec.x = v.x * m[0] + v.y * m[4] + v.z * m[8];
        vec.y = v.x * m[1] + v.y * m[5] + v.z * m[9];
        vec.y = v.x * m[2] + v.y * m[6] + v.z * m[10];

        return vec;
}

void Mat4::rotateX(const float angle) {
        Mat4 temp;

        temp.loadRotateX(angle);
        (*this) *= temp;
}

void Mat4::rotateY(const float angle) {
        Mat4 temp;

        temp.loadRotateY(angle);
        (*this) *= temp;
}

void Mat4::rotateZ(const float angle) {
        Mat4 temp;

        temp.loadRotateZ(angle);
        (*this) *= temp;
}

void Mat4::rotateXY(const float angleX, const float angleY) {
        Mat4 temp;

        temp.loadRotateXY(angleX, angleY);
        (*this) *= temp;
}

void Mat4::rotateZXY(const float angleX, const float angleY, const float angleZ) {
        Mat4 temp;

        temp.loadRotateZXY(angleX, angleY, angleZ);
        (*this) *= temp;
}

void Mat4::rotate(const Vec3 &v, const float angle) {
        Mat4 temp;

        temp.loadRotate(v, angle);
        (*this) *= temp;
}

void Mat4::transpose() {
        float temp;

        temp  = m[4];
        m[4]  = m[1];
        m[1]  = temp;
        
        temp  = m[8];
        m[8]  = m[2];
        m[2]  = temp;
        
        temp  = m[12];
        m[12] = m[3];
        m[3]  = temp;
        
        temp  = m[9];
        m[9]  = m[6];
        m[6]  = temp;
        
        temp  = m[13];
        m[13] = m[7];
        m[7]  = temp;
        
        temp  = m[14];
        m[14] = m[11];
        m[11] = temp;
}

Mat4 Mat4::operator + (const Mat4 &v) {
        Mat4 mat;
        for (unsigned int i = 0; i < 16; i++){
                mat.m[i] = m[i] + v.m[i];
        }
        return mat;
}

Mat4 Mat4::operator + (const float s) {
        Mat4 mat;
        for (unsigned int i = 0; i < 16; i++){
                mat.m[i] = m[i] + s;
        }
        return mat;
}

Mat4 Mat4::operator - (const Mat4 &v) {
        Mat4 mat;
        for (unsigned int i = 0; i < 16; i++){
                mat.m[i] = m[i] - v.m[i];
        }
        return mat;
}

Mat4 Mat4::operator - (const float s) {
        Mat4 mat;
        for (unsigned int i = 0; i < 16; i++){
                mat.m[i] = m[i] - s;
        }
        return mat;
}

Mat4 Mat4::operator - () {
        Mat4 mat;
        for (unsigned int i = 0; i < 16; i++){
                mat.m[i] = -m[i];
        }
        return mat;
}

void Mat4::operator += (const Mat4 &v) {
        for (unsigned int i = 0; i < 16; i++){
                m[i] += v.m[i];
        }
}

void Mat4::operator -= (const Mat4 &v) {
        for (unsigned int i = 0; i < 16; i++){
                m[i] -= v.m[i];
        }
}

void Mat4::operator += (const float s) {
        for (unsigned int i = 0; i < 16; i++){
                m[i] += s;
        }
}

void Mat4::operator -= (const float s) {
        for (unsigned int i = 0; i < 16; i++){
                m[i] -= s;
        }
}


Mat4 Mat4::operator * (const Mat4 &v) {
        Mat4 mat;
        unsigned int i,us,vs;

        for (i = 0; i < 16; i++){
                us = (i & 0x3);
                vs = (i & 0xC);

                mat.m[i] =  m[us     ] * v.m[vs    ] + 
                                        m[us +  4] * v.m[vs + 1] + 
                                        m[us +  8] * v.m[vs + 2] + 
                                        m[us + 12] * v.m[vs + 3];
        }
        return mat;
}

Mat4 Mat4::operator * (const float s) {
        Mat4 mat;
        unsigned int i;

        for (i = 0; i < 16; i++){
                mat.m[i] = m[i] * s;
        }
        return mat;
}

void Mat4::operator *= (const float s) {
        for (unsigned int i=0; i<16; i++) {
                m[i] *= s;
        }
}

void Mat4::operator *= (const Mat4 &v) {
        unsigned int i, us, vs;
        float nm[16];

        for (i = 0; i < 16; i++){
                us = (i & 0x3);
                vs = (i & 0xC);

                nm[i] = m[us     ] * v.m[vs    ] + 
                                m[us +  4] * v.m[vs + 1] + 
                                m[us +  8] * v.m[vs + 2] + 
                                m[us + 12] * v.m[vs + 3];
        }
        for (i = 0; i < 16; i++){
                m[i] = nm[i];
        }
}

Vec3 Mat4::operator * (const Vec3 &v) {
        return Vec3(
                m[0] * v.x + m[4] * v.y + m[8]  * v.z, 
                m[1] * v.x + m[5] * v.y + m[9]  * v.z, 
                m[2] * v.x + m[6] * v.y + m[10] * v.z);
}

Vec4 Mat4::operator * (const Vec4 &v) {
        return Vec4(
                m[0] * v.x + m[4] * v.y + m[8]  * v.z + m[12], 
                m[1] * v.x + m[5] * v.y + m[9]  * v.z + m[13], 
                m[2] * v.x + m[6] * v.y + m[10] * v.z + m[14],
                m[3] * v.x + m[7] * v.y + m[11] * v.z + m[15]);
}

Mat4 Mat4::operator / (const float d) {
        Mat4 mat;
        unsigned int i;

        for (i = 0; i < 16; i++){
                mat.m[i] = m[i] / d;
        }
        return mat;
}

void Mat4::operator /= (const float s) {
        for (unsigned int i=0; i<16; i++) {
                m[i] /= s;
        }
}

Mat4 Mat4::operator ! () {
        Mat4 mat;
        unsigned int i,j,k;
        float temp, newMat[4][8];

        for (k = i = 0; i < 4; i++){
                for (j = 0; j < 4; j++){
                        newMat[j][i] = m[k];
                        newMat[i][j + 4] = (i == j);
                        k++;
                }
        }


        for (i = 0; i < 4; i++){
                if (newMat[i][i] == 0){
                        j = 0;
                        do {
                                j++;
                        } while (newMat[j][i] == 0);

                        for (k = i; k < 8; k++){
                                temp = newMat[j][k];
                                newMat[j][k] = newMat[i][k];
                                newMat[i][k] = temp;
                        }
                }
                
                for (j = i+1; j < 8; j++){
                        newMat[i][j] /= newMat[i][i];
                }

                for (j = 0; j < 4; j++){
                        if (j != i){
                                for (k = i+1; k < 8; k++){
                                        newMat[j][k] -= newMat[j][i] * newMat[i][k];
                                }
                        }
                }
        }

        for (k = 0, i = 4; i < 8; i++){
                for (j = 0; j < 4; j++){
                        mat.m[k] = newMat[j][i];
                        k++;
                }
        }

        return mat;
}


// Used with MilkShape

void Mat4::postMultiply(const Mat4 &matrix) {
        float newMatrix[16];
        const float *m1 = m, *m2 = matrix.m;

        newMatrix[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2];
        newMatrix[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2];
        newMatrix[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2];
        newMatrix[3] = 0;

        newMatrix[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6];
        newMatrix[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6];
        newMatrix[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6];
        newMatrix[7] = 0;

        newMatrix[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10];
        newMatrix[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10];
        newMatrix[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10];
        newMatrix[11] = 0;

        newMatrix[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12];
        newMatrix[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13];
        newMatrix[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14];
        newMatrix[15] = 1;

        set( newMatrix );
}

void Mat4::setTranslation(const float *translation) {
        m[12] = translation[0];
        m[13] = translation[1];
        m[14] = translation[2];
}

void Mat4::setInverseTranslation(const float *translation) {
        m[12] = -translation[0];
        m[13] = -translation[1];
        m[14] = -translation[2];
}

void Mat4::setRotationRadians(const float *angles) {
        double cr = cos( angles[0] );
        double sr = sin( angles[0] );
        double cp = cos( angles[1] );
        double sp = sin( angles[1] );
        double cy = cos( angles[2] );
        double sy = sin( angles[2] );

        m[0] = ( float )( cp*cy );
        m[1] = ( float )( cp*sy );
        m[2] = ( float )( -sp );

        double srsp = sr*sp;
        double crsp = cr*sp;

        m[4] = ( float )( srsp*cy-cr*sy );
        m[5] = ( float )( srsp*sy+cr*cy );
        m[6] = ( float )( sr*cp );

        m[8] = ( float )( crsp*cy+sr*sy );
        m[9] = ( float )( crsp*sy-sr*cy );
        m[10] = ( float )( cr*cp );
}

---