Maths - Matrix Code - Martin Baker (original) (raw)

individual matrix methods

add
scale
mul 4x4
mul 3x3
rotX
rotY
rotZ
invert
determinant
C++ nxn template

complete sftransform class

This class can represent a 3D transform which can scale, translate and rotate a 3d vector space. The transform is held as a 4x4 matrix. The class has methods to add, subtract and multiply with other matrices. Also many other methods, including the ability to load and save to from VRML and x3d.

There are 3 versions available depending on language:

A Java Implementation of Matrix

Matrix Add

public final void add(Matrix4d m1,Matrix4d m2) {
m00 = m1.m00 + m2.m00;
m01 = m1.m01 + m2.m01;
m02 = m1.m02 + m2.m02;
m03 = m1.m03 + m2.m03;
m10 = m1.m10 + m2.m10;
m11 = m1.m11 + m2.m11;
m12 = m1.m12 + m2.m12;
m13 = m1.m13 + m2.m13;
m20 = m1.m20 + m2.m20;
m21 = m1.m21 + m2.m21;
m22 = m1.m22 + m2.m22;
m23 = m1.m23 + m2.m23;
m30 = m1.m30 + m2.m30;
m31 = m1.m31 + m2.m31;
m32 = m1.m32 + m2.m32;
m33 = m1.m33 + m2.m33;
}

Scalar Multiplication

public final void scale(double scale) {
m00 *= scale;
m01 *= scale;
m02 *= scale;
m03 *= scale;
m10 *= scale;
m11 *= scale;
m12 *= scale;
m13 *= scale;
m20 *= scale;
m21 *= scale;
m22 *= scale;
m23 *= scale;
m30 *= scale;
m31 *= scale;
m32 *= scale;
m33 *= scale;
}

Matrix Multiplication

4x4 matrix:

public final void mul(Matrix4d m1,Matrix4d m2) {
m00 = m1.m00m2.m00 + m1.m01m2.m10 + m1.m02m2.m20 + m1.m03m2.m30;
m01 = m1.m00m2.m01 + m1.m01m2.m11 + m1.m02m2.m21 + m1.m03m2.m31;
m02 = m1.m00m2.m02 + m1.m01m2.m12 + m1.m02m2.m22 + m1.m03m2.m32;
m03 = m1.m00m2.m03 + m1.m01m2.m13 + m1.m02m2.m23 + m1.m03m2.m33;
m10 = m1.m10m2.m00 + m1.m11m2.m10 + m1.m12m2.m20 + m1.m13m2.m30;
m11 = m1.m10m2.m01 + m1.m11m2.m11 + m1.m12m2.m21 + m1.m13m2.m31;
m12 = m1.m10m2.m02 + m1.m11m2.m12 + m1.m12m2.m22 + m1.m13m2.m32;
m13 = m1.m10m2.m03 + m1.m11m2.m13 + m1.m12m2.m23 + m1.m13m2.m33;
m20 = m1.m20m2.m00 + m1.m21m2.m10 + m1.m22m2.m20 + m1.m23m2.m30;
m21 = m1.m20m2.m01 + m1.m21m2.m11 + m1.m22m2.m21 + m1.m23m2.m31;
m22 = m1.m20m2.m02 + m1.m21m2.m12 + m1.m22m2.m22 + m1.m23m2.m32;
m23 = m1.m20m2.m03 + m1.m21m2.m13 + m1.m22m2.m23 + m1.m23m2.m33;
m30 = m1.m30m2.m00 + m1.m31m2.m10 + m1.m32m2.m20 + m1.m33m2.m30;
m31 = m1.m30m2.m01 + m1.m31m2.m11 + m1.m32m2.m21 + m1.m33m2.m31;
m32 = m1.m30m2.m02 + m1.m31m2.m12 + m1.m32m2.m22 + m1.m33m2.m32;
m33 = m1.m30m2.m03 + m1.m31m2.m13 + m1.m32m2.m23 + m1.m33m2.m33;
}

3x3 matrix:

public final void mul(Matrix3d m1,Matrix3d m2) {
m00 = m1.m00m2.m00 + m1.m01m2.m10 + m1.m02m2.m20;
m01 = m1.m00m2.m01 + m1.m01m2.m11 + m1.m02m2.m21;
m02 = m1.m00m2.m02 + m1.m01m2.m12 + m1.m02m2.m22;
m10 = m1.m10m2.m00 + m1.m11m2.m10 + m1.m12m2.m20;
m11 = m1.m10m2.m01 + m1.m11m2.m11 + m1.m12m2.m21;
m12 = m1.m10m2.m02 + m1.m11m2.m12 + m1.m12m2.m22;
m20 = m1.m20m2.m00 + m1.m21m2.m10 + m1.m22m2.m20;
m21 = m1.m20m2.m01 + m1.m21m2.m11 + m1.m22m2.m21;
m22 = m1.m20m2.m02 + m1.m21m2.m12 + m1.m22*m2.m22;
}

Rotate

public final void rotX(double angle) {
// Assuming the angle is in radians. (?)
double c = Math.cos(angle);
double s = Math.sin(angle);
m00 = 1.0;
m01 = 0.0;
m02 = 0.0;
m10 = 0.0;
m11 = c;
m12 = s;
m20 = 0.0;
m21 = -s;
m22 = c;
}

public final void rotY(double angle) {
// Assuming the angle is in radians. (?)
Double c = Math.cos(angle);
double s = Math.sin(angle);
m00 = c;
m01 = 0.0;
m02 = -s;
m10 = 0.0;
m11 = 1;
m12 = 0.0;
m20 = s;
m21 = 0.0;
m22 = c;
}

public final void rotZ(double angle) {
// Assuming the angle is in radians. (?)
Double c = Math.cos(angle);
double s = Math.sin(angle);
m00 = c;
m01 = s;
m02 = 0.0;
m10 = -s;
m11 = c;
m12 = 0.0;
m20 = 0.0;
m21 = 0.0;
m22 = 1.0;
}

Invert

public final void invert(Matrix4d m1) {
m00 = m12m23m31 - m13m22m31 + m13m21m32 - m11m23m32 - m12m21m33 + m11m22m33;
m01 = m03m22m31 - m02m23m31 - m03m21m32 + m01m23m32 + m02m21m33 - m01m22m33;
m02 = m02m13m31 - m03m12m31 + m03m11m32 - m01m13m32 - m02m11m33 + m01m12m33;
m03 = m03m12m21 - m02m13m21 - m03m11m22 + m01m13m22 + m02m11m23 - m01m12m23;
m10 = m13m22m30 - m12m23m30 - m13m20m32 + m10m23m32 + m12m20m33 - m10m22m33;
m11 = m02m23m30 - m03m22m30 + m03m20m32 - m00m23m32 - m02m20m33 + m00m22m33;
m12 = m03m12m30 - m02m13m30 - m03m10m32 + m00m13m32 + m02m10m33 - m00m12m33;
m13 = m02m13m20 - m03m12m20 + m03m10m22 - m00m13m22 - m02m10m23 + m00m12m23;
m20 = m11m23m30 - m13m21m30 + m13m20m31 - m10m23m31 - m11m20m33 + m10m21m33;
m21 = m03m21m30 - m01m23m30 - m03m20m31 + m00m23m31 + m01m20m33 - m00m21m33;
m22 = m01m13m30 - m03m11m30 + m03m10m31 - m00m13m31 - m01m10m33 + m00m11m33;
m23 = m03m11m20 - m01m13m20 - m03m10m21 + m00m13m21 + m01m10m23 - m00m11m23;
m30 = m12m21m30 - m11m22m30 - m12m20m31 + m10m22m31 + m11m20m32 - m10m21m32;
m31 = m01m22m30 - m02m21m30 + m02m20m31 - m00m22m31 - m01m20m32 + m00m21m32;
m32 = m02m11m30 - m01m12m30 - m02m10m31 + m00m12m31 + m01m10m32 - m00m11m32;
m33 = m01m12m20 - m02m11m20 + m02m10m21 - m00m12m21 - m01m10m22 + m00m11m22;
scale(1/m1.determinant());
}To look at more general C++ code see below

determinant

public final double determinant() { double value; value = m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13 * m22 * m30+ m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13 * m20 * m31+ m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12 * m23 * m31+ m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13 * m21 * m32+ m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12 * m20 * m33+ m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11 * m22 * m33; return value; }To look at more general C++ code see below

The following C++ matrix are more optimised than the java code above and can be used for any dimension matrix, It relies on templates, it would also be useful to overload the +, - ,* and / operators for matrix arithmetic. This code is written by sudeep das ( das_sudeep@hotmail.com ) who kindly allowed me to include it here.

template < int order> class Matx {protected: double mtx[ order][ order];public: double &element( int i, int j) { return mtx[ i][ j]; } const double &element( int i, int j) const { return mtx[ i][ j]; } /////////////////////////////////////// Matx multiply( const Matx &mat) const Matx< order> a; for ( int i = 0; i < order; ++i) for ( int l = 0; l < order; ++l) { double &x = a.element( i, l) = 0; for ( int j = 0; j < order; ++j) x += element( i, j) * mat.element( j, l); } return a; }

////////////////////////////////////////// template Matx Matx::inverse( void) const { Matx b; for ( int i = 0; i < order; ++i) for ( int j = 0; j < order; ++j) { int sgn = ( (i+j)%2) ? -1 : 1; b.element( i, j) = sgn * MatxCofactor< order -1>( *this, i,j).determinant(); } b.transpose(); b /= determinant(); return b; } ////////////////////////////////////////// template double Matx::determinant( void) const { double d = 0; for ( int i = 0; i < order; ++i) { int sgn = ( i % 2) ? -1 : 1; Matx<order -1> cf = MatxCofactor< order -1>( *this, i, 0); d += sgn * element( i, 0) * cf.determinant(); } return d; } ////////////////////////////////////////// void transpose( void) { for ( int i = 0; i < order; ++i) { for ( int j = i +1; j < order; ++j) ::swap( &element( i, j), &element( j, i)); // need a swap function not provided here } } };//////////////////////////////////////////////////// template class MatxCofactor : public Matx< corder> { public: MatxCofactor( const Matx< corder+1> &a, int aI, int aJ) { for ( int i = 0, k = 0; i < ( corder+1); ++i) { if ( i != aI) { for ( int j = 0, l = 0; j < ( corder+1); ++j) { if ( j != aJ) { element( k, l) = a.element( i, j); ++l; } } ++k; } } } };

This site may have errors. Don't use for critical systems.