matrix33.c

Go to the documentation of this file.

/****************************************************************************************/
/*  MATRIX33.H                                                                          */
/*                                                                                      */
/*  Author: Jason Wood                                                                  */
/*  Description: Pure 3x3 matrix                                                        */
/*                                                                                      */
/*  The contents of this file are subject to the Genesis3D Public License               */
/*  Version 1.01 (the "License"); you may not use this file except in                   */
/*  compliance with the License. You may obtain a copy of the License at                */
/*  http://www.genesis3d.com                                                            */
/*                                                                                      */
/*  Software distributed under the License is distributed on an "AS IS"                 */
/*  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied.  See                */
/*  the License for the specific language governing rights and limitations              */
/*  under the License.                                                                  */
/*                                                                                      */
/*  The Original Code is Genesis3D, released March 25, 1999.                            */
/*Genesis3D Version 1.1 released November 15, 1999                            */
/*  Copyright (C) 1999 WildTangent, Inc. All Rights Reserved           */
/*                                                                                      */
/****************************************************************************************/
#include <assert.h>
#include <math.h>

#include "vec3d.h"
#include "xform3d.h"
#include "matrix33.h"

void Matrix33_MakeCrossProductMatrix33(const geVec3d* v,
        Matrix33* m)
{
        assert(v != NULL);
        assert(m != NULL);

        m->x[0][0] = m->x[1][1] = m->x[2][2] = 0.0f;
        m->x[0][1] = -v->Z; m->x[0][2] = v->Y;
        m->x[1][0] = v->Z; m->x[1][2] = -v->X;
        m->x[2][0] = -v->Y; m->x[2][1] = v->X;
}

void Matrix33_ExtractFromXForm3d(const geXForm3d* xform, Matrix33* m)
{
        assert(xform != NULL);
        assert(m != NULL);

        m->x[0][0] = xform->AX; m->x[0][1] = xform->AY; m->x[0][2] = xform->AZ;
        m->x[1][0] = xform->BX; m->x[1][1] = xform->BY; m->x[1][2] = xform->BZ;
        m->x[2][0] = xform->CX; m->x[2][1] = xform->CY; m->x[2][2] = xform->CZ;
}

void Matrix33_MultiplyVec3d(const Matrix33* m, const geVec3d* v, geVec3d* res)
{
        assert(m != NULL);
        assert(v != NULL);
        assert(res != NULL);

        res->X = m->x[0][0] * v->X + m->x[0][1] * v->Y + m->x[0][2] * v->Z;
        res->Y = m->x[1][0] * v->X + m->x[1][1] * v->Y + m->x[1][2] * v->Z;
        res->Z = m->x[2][0] * v->X + m->x[2][1] * v->Y + m->x[2][2] * v->Z;
}

void Matrix33_Multiply(const Matrix33* m1, const Matrix33* m2, Matrix33* res)
{
        int i, j, k;

        assert(m1 != NULL);
        assert(m2 != NULL);
        assert(res != NULL);

        for (i = 0; i < 3; i++)
        {
                for (j = 0; j < 3; j++)
                {
                        res->x[i][j] = 0.0f;

                        for (k = 0; k < 3; k++)
                                res->x[i][j] += m1->x[i][k] * m2->x[k][j];
                }
        }
}

void Matrix33_GetTranspose(const Matrix33* m, Matrix33* t)
{
        int i, j;

        assert(m != NULL);
        assert(t != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                        t->x[j][i] = m->x[i][j];
}

void Matrix33_Copy(const Matrix33* m, Matrix33* c)
{
        int i, j;

        assert(m != NULL);
        assert(c != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                        c->x[i][j] = m->x[i][j];
}

void Matrix33_SetIdentity(Matrix33* m)
{
        int i, j;

        assert(m != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                {
                        if (i == j) 
                                m->x[i][j] = 1.f;
                        else m->x[i][j] = 0.f;
                }
}

void Matrix33_Add(const Matrix33* m1, const Matrix33* m2, Matrix33* res)
{
        int i, j;

        assert(m1 != NULL);
        assert(m2 != NULL);
        assert(res != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                        res->x[i][j] = m1->x[i][j] + m2->x[i][j];
}

void Matrix33_Subtract(const Matrix33* m1, const Matrix33* m2, Matrix33* res)
{
        int i, j;

        assert(m1 != NULL);
        assert(m2 != NULL);
        assert(res != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                        res->x[i][j] = m1->x[i][j] - m2->x[i][j];
}

void Matrix33_MultiplyScalar(geFloat s, const Matrix33* m, Matrix33* res)
{
        int i, j;

        assert(m != NULL);
        assert(res != NULL);

        for (i = 0; i < 3; i++)
                for (j = 0; j < 3; j++)
                        res->x[i][j] = s * m->x[i][j];
}

void Matrix33_GetInverse(const Matrix33* m, Matrix33* inv)
{
        int i, j, k;
        Matrix33 copy;

        assert(m != NULL);
        assert(inv != NULL);

        Matrix33_Copy(m, &copy);
        Matrix33_SetIdentity(inv);

        for (i = 0; i < 3; i++)
        {
                if (copy.x[i][i] != 1.0f)
                {
                        geFloat divby = copy.x[i][i];

                        assert(fabs(divby) >= 1e-5);
                        
                        for (j = 0; j < 3; j++)
                        {
                                inv->x[i][j] /= divby;
                                copy.x[i][j] /= divby;
                        }
                }
                for (j = 0; j < 3; j++)
                {
                        if (j != i)
                        {
                                if (copy.x[j][i] != 0.0f)
                                {
                                        geFloat mulby = copy.x[j][i];
                                        for (k = 0; k < 3; k++)
                                        {
                                                copy.x[j][k] -= mulby * copy.x[i][k];
                                                inv->x[j][k] -= mulby * inv->x[i][k];
                                        }
                                }
                        }
                }
        }
}

---