Maths - Matrix Code - C++

related classes

h file

/*Title: mjbWorld
   Copyright (c) 1998-2007 Martin John Baker
   This program 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.
   This program 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.For information about the GNU General public: License see http://www.gnu.org/To discuss this program http://sourceforge.net/forum/forum.php?forum_id=122133
   also see website https://www.euclideanspace.com/
   */__gc class sftransform : public property {
   public: double m00;
   public: double m01;
   public: double m02;
   public: double m03;
   public: double m10;
   public: double m11;
   public: double m12;
   public: double m13;
   public: double m20;
   public: double m21;
   public: double m22;
   public: double m23;
   public: double m30;
   public: double m31;
   public: double m32;
   public: double m33; public: sftransform() ;
   public: ~sftransform() ;public: sftransform(sftransform* a);public: sftransform(sftransform* a,sftransform* b); public: void setIdentity(); public: String* vrmlType(); public: static String* vrmlType_s(){
   return "SFTransform";
   } public: property* clone() ; /** create an array of the appropriate type
   * with a size given by the parameter
   */
   public: property* createArray(int size)[]; public: void calcTransform(sfvec3f* translation,
   sfrotation* rotation,
   sfvec3f* center,
   sfvec3f* sc,
   sfrotation* scaleOrientation); public: void combine(sftransform* m1) ; public: void combineInverse(sftransform* m1); /**
   * Sets the value of this matrix to the result of multiplying
   * the two argument matrices together.
   * @param m1 the first matrix
   * @param m2 the second matrix
   */
   public: void combine(sftransform* m1,sftransform* m2);
   public: void transform(sfvec3f* v); public: void translate(sfvec3f* v);// public: void translate(Vector3d v); public: void scale(sfvec3f* v); //public: void scale(Point3d v); public: void rotate(sfrotation* rot,sfvec3f* A);
   void setRotate(sfrotation* rot,sfvec3f* A) ; void setRotate(sfvec3f* A, // rotate around a line between A & B by alpha    degrees
   sfvec3f* B,
   double alpha); void setRotate(sfvec3f* A, // rotate about a line centred on A
   double theta,
   double phi,
   double alpha); String* ToString(); public: void writeJava(filter* f,int mode,int maxInstances);/** output as a string
   * mode values
   * 0 - output modified values
   * 1 - output original values
   * 2 - output attribute
   * 3 - output attribute in brackets
   * 4 - output with f prefix
   */
   public: String* outstring(int i);/** write to file
   * filter = information about output
   * mode values
   * 0 - output VRML97 modified values
   * 1 - output VRML97 original values
   * 2 - output xml (x3d)
   * 3 - output attribute in brackets
   * 4 - output with f prefix
   */
   public: void write(filter* f,int mode,int indent); /** used by mfparam.vrml2par
   */
   public: bool instring(filter* f,sfparam* sfp,nodeBean* n,int mode); public: static Type* getEditClass(){
   return __typeof(sftransformEditor);
   }
   public: void invert();
   public: void invertAffine();
   public: void invertOrthogonal();
   public: void invertOrthoNormal();
   public: double determinant();
   public: double determinantAffine();
   public: void scale(double scale);
   public: void transpose();
   };
 

cpp file

/*Title: mjbWorld
   Copyright (c) 1998-2002 Martin John BakerThis program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.This program 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 License for more details.For information about the GNU General License see http://www.gnu.org/To discuss this program http://sourceforge.net/forum/forum.php?forum_id=122133
   also see website https://www.euclideanspace.com/
   */#include "mjbModel.h"sftransform::sftransform() {
   m00 = 0.0;
   m01 = 0.0;
   m02 = 0.0;
   m03 = 0.0;
   m10 = 0.0;
   m11 = 0.0;
   m12 = 0.0;
   m13 = 0.0;
   m20 = 0.0;
   m21 = 0.0;
   m22 = 0.0;
   m23 = 0.0;
   m30 = 0.0;
   m31 = 0.0;
   m32 = 0.0;
   m33 = 0.0;
   } sftransform::sftransform(sftransform* a) {
   m00 = a->m00;
   m01 = a->m01;
   m02 = a->m02;
   m03 = a->m03;
   m10 = a->m10;
   m11 = a->m11;
   m12 = a->m12;
   m13 = a->m13;
   m20 = a->m20;
   m21 = a->m21;
   m22 = a->m22;
   m23 = a->m23;
   m30 = a->m30;
   m31 = a->m31;
   m32 = a->m32;
   m33 = a->m33;
   }sftransform::sftransform(sftransform* a,sftransform* b) {
   combine(a,b);
   }sftransform::~sftransform() {
   }void sftransform::setIdentity(){
   m00 = 1.0;
   m01 = 0.0;
   m02 = 0.0;
   m03 = 0.0;
   m10 = 0.0;
   m11 = 1.0;
   m12 = 0.0;
   m13 = 0.0;
   m20 = 0.0;
   m21 = 0.0;
   m22 = 1.0;
   m23 = 0.0;
   m30 = 0.0;
   m31 = 0.0;
   m32 = 0.0;
   m33 = 1.0;
   }String* sftransform::vrmlType(){
   return "SFTransform";
   }property* sftransform::clone() {
   //Console::WriteLine("sfparam::clone");
   return new sftransform(this);
   }/** create an array of the appropriate type
   * with a size given by the parameter
   */
   property* sftransform::createArray(int size)[]{
   return new sftransform*[size];
   }void sftransform::calcTransform(sfvec3f* translation,
   sfrotation* rotation,
   sfvec3f* center,
   sfvec3f* sc,
   sfrotation* scaleOrientation){
   //Console::WriteLine("sftransform->calcTransform started translate=");
   //if (translation ) Console::WriteLine("translation="+translation->toString());
   //if (center ) Console::WriteLine("centre="+center->toString());
   //if (rotation ) Console::WriteLine("rotation="+rotation->toString());
   //if (sc ) Console::WriteLine("sc="+sc->toString());
   //if (scaleOrientation ) Console::WriteLine("scaleOrientation="+scaleOrientation->toString());
   setIdentity();
   if (translation ) translate(translation);
   if (rotation ) rotate(rotation,center);
   if (scaleOrientation ) rotate(scaleOrientation,center);
   if (sc ) { // if scale is (0,0,0), such as when scale first
   // enabled this will generate a non-afine error
   // so do following check
   if ((sc->x != 0) & (sc->y != 0) & (sc->z != 0))
   scale(sc);
   }
   if (scaleOrientation ) rotate(scaleOrientation->minus(),center);
   //Console::WriteLine("matrix="+this);
   }void sftransform::combine(sftransform* m1) {
   sftransform* tmp = new sftransform(this);
   combine(tmp,m1);
   }void sftransform::combineInverse(sftransform* m1) {
   sftransform* tmp = new sftransform(this);
   combine(m1,tmp);
   }/** This matrix set to m1 * m2 */
   void sftransform::combine(sftransform* m1,sftransform* m2) {
   m00 = m1->m00*m2->m00 + m1->m01*m2->m10 + m1->m02*m2->m20    + m1->m03*m2->m30;
   m01 = m1->m00*m2->m01 + m1->m01*m2->m11 + m1->m02*m2->m21    + m1->m03*m2->m31;
   m02 = m1->m00*m2->m02 + m1->m01*m2->m12 + m1->m02*m2->m22    + m1->m03*m2->m32;
   m03 = m1->m00*m2->m03 + m1->m01*m2->m13 + m1->m02*m2->m23    + m1->m03*m2->m33; m10 = m1->m10*m2->m00 + m1->m11*m2->m10 + m1->m12*m2->m20    + m1->m13*m2->m30;
   m11 = m1->m10*m2->m01 + m1->m11*m2->m11 + m1->m12*m2->m21    + m1->m13*m2->m31;
   m12 = m1->m10*m2->m02 + m1->m11*m2->m12 + m1->m12*m2->m22    + m1->m13*m2->m32;
   m13 = m1->m10*m2->m03 + m1->m11*m2->m13 + m1->m12*m2->m23    + m1->m13*m2->m33; m20 = m1->m20*m2->m00 + m1->m21*m2->m10 + m1->m22*m2->m20    + m1->m23*m2->m30;
   m21 = m1->m20*m2->m01 + m1->m21*m2->m11 + m1->m22*m2->m21    + m1->m23*m2->m31;
   m22 = m1->m20*m2->m02 + m1->m21*m2->m12 + m1->m22*m2->m22    + m1->m23*m2->m32;
   m23 = m1->m20*m2->m03 + m1->m21*m2->m13 + m1->m22*m2->m23    + m1->m23*m2->m33; m30 = m1->m30*m2->m00 + m1->m31*m2->m10 + m1->m32*m2->m20    + m1->m33*m2->m30;
   m31 = m1->m30*m2->m01 + m1->m31*m2->m11 + m1->m32*m2->m21    + m1->m33*m2->m31;
   m32 = m1->m30*m2->m02 + m1->m31*m2->m12 + m1->m32*m2->m22    + m1->m33*m2->m32;
   m33 = m1->m30*m2->m03 + m1->m31*m2->m13 + m1->m32*m2->m23    + m1->m33*m2->m33;
   }void sftransform::transform(sfvec3f* v){
   sfvec3f* temp = new sfvec3f(v);
   v->x = m00 * temp->x + m01 * temp->y + m02 * temp->z + m03;
   v->y = m10 * temp->x + m11 * temp->y + m12 * temp->z + m13;
   v->z = m20 * temp->x + m21 * temp->y + m22 * temp->z + m23;
   }void sftransform::translate(sfvec3f* v){
   if (!v) {
   Console::WriteLine("sftransform::translate v==null");
   return;
   }
   m03 += v->x;
   m13 += v->y;
   m23 += v->z;
   m33 = 1.0;
   }void sftransform::scale(sfvec3f* v){
   m00 *= v->x;
   m01 *= v->x;
   m02 *= v->x;
   m10 *= v->y;
   m11 *= v->y;
   m12 *= v->y;
   m20 *= v->z;
   m21 *= v->z;
   m22 *= v->z;
   m33 = 1.0;
   }void sftransform::rotate(sfrotation* rot,sfvec3f* A) {// rotate around A
   sftransform* t1 = new sftransform(this);
   sftransform* t2 = new sftransform();
   t2->setRotate(rot,A);
   combine(t1,t2);
   }void sftransform::setRotate(sfrotation* rot,sfvec3f* A) {// rotate around A
   double v1 = rot->x,
   v2 = rot->y,
   v3 = rot->z;
   double theta = Math::Atan2(v2, v1);
   double phi = Math::Atan2(Math::Sqrt(v1 * v1 + v2 * v2), v3);
   double alpha = rot->angle;
   setRotate(A, theta, phi, alpha);
   }void sftransform::setRotate(sfvec3f* A, // rotate around a line between A &    B by alpha degrees
   sfvec3f* B,
   double alpha) {
   if ((!A)||(!B)) return;
   double v1 = B->x - A->x,
   v2 = B->y - A->y,
   v3 = B->z - A->z,
   theta = Math::Atan2(v2, v1),
   phi = Math::Atan2(Math::Sqrt(v1 * v1 + v2 * v2), v3);
   setRotate(A, theta, phi, alpha);
   }void sftransform::setRotate(sfvec3f* A, // rotate about a line centred on A
   double theta,
   double phi,
   double alpha) {
   double cosAlpha, sinAlpha, cosPhi, sinPhi,
   cosTheta, sinTheta, cosPhi2, sinPhi2,
   cosTheta2, sinTheta2, c, a1,a2,a3;
   if (!A) {
   a1=a2=a3=0;
   } else {
   a1 = A->x;
   a2 = A->y;
   a3 = A->z;
   }
   cosPhi = Math::Cos(phi); sinPhi = Math::Sin(phi);
   cosPhi2 = cosPhi * cosPhi; sinPhi2 = sinPhi * sinPhi;
   cosTheta = Math::Cos(theta);
   sinTheta = Math::Sin(theta);
   cosTheta2 = cosTheta * cosTheta;
   sinTheta2 = sinTheta * sinTheta;
   cosAlpha = Math::Cos(alpha);
   sinAlpha = Math::Sin(alpha);
   c = 1.0 - cosAlpha;
   m00 = cosTheta2 * (cosAlpha * cosPhi2 + sinPhi2)
   + cosAlpha * sinTheta2;
   m10 = sinAlpha * cosPhi + c * sinPhi2 * cosTheta * sinTheta;
   m20 = sinPhi * (cosPhi * cosTheta * c - sinAlpha * sinTheta);
   m30 = 0.0; m01 = sinPhi2 * cosTheta * sinTheta * c - sinAlpha * cosPhi;
   m11 = sinTheta2 * (cosAlpha * cosPhi2 + sinPhi2)
   + cosAlpha * cosTheta2;
   m21 = sinPhi * (cosPhi * sinTheta * c + sinAlpha * cosTheta);
   m31 = 0.0; m02 = sinPhi * (cosPhi * cosTheta * c + sinAlpha * sinTheta);
   m12 = sinPhi * (cosPhi * sinTheta * c - sinAlpha * cosTheta);
   m22 = cosAlpha * sinPhi2 + cosPhi2;
   m32 = 0.0; m03 = a1 - a1 * m00 - a2 * m01 - a3 * m02;
   m13 = a2 - a1 * m10 - a2 * m11 - a3 * m12;
   m23 = a3 - a1 * m20 - a2 * m21 - a3 * m22;
   m33 = 1.0;
   }String* sftransform::ToString(){
   String* s1=String::Concat(S"[",__box(m00)->ToString(),S",",__box(m01)->ToString(),S",",__box(m02)->ToString(),S",",__box(m03)->ToString(),S"]");
   String* s2=String::Concat(S"[",__box(m10)->ToString(),S",",__box(m11)->ToString(),S",",__box(m12)->ToString(),S",",__box(m13)->ToString(),S"]");
   String* s3=String::Concat(S"[",__box(m20)->ToString(),S",",__box(m21)->ToString(),S",",__box(m22)->ToString(),S",",__box(m23)->ToString(),S"]");
   String* s4=String::Concat(S"[",__box(m30)->ToString(),S",",__box(m31)->ToString(),S",",__box(m32)->ToString(),S",",__box(m33)->ToString(),S"]");
   return String::Concat(s1,S"\n",s2,S"\n",s3,S"\n",s4);
   }void sftransform::writeJava(filter* f,int mode,int maxInstances){
   try {
   // String* name = generateUniqueName();
   if (mode != 0) return; // no procedure defn required
   f->status(GetType()->get_Name());
   f->writeln(String::Concat("// code for ",GetType()->get_Name()),0);
   f->writeln("Transform3D t3d = new Transform3D();",2);
   f->writeln("Matrix4d m = new Matrix4d();",2);
   f->writeln("double []d = {",2);
   f->writeln(String::Concat(__box(m00)->ToString(),S",",__box(m01)->ToString(),S",",__box(m02)->ToString(),S",",__box(m03)->ToString(),S","),2);
   f->writeln(String::Concat(__box(m10)->ToString(),S",",__box(m11)->ToString(),S",",__box(m12)->ToString(),S",",__box(m13)->ToString(),S","),2);
   f->writeln(String::Concat(__box(m20)->ToString(),S",",__box(m21)->ToString(),S",",__box(m22)->ToString(),S",",__box(m23)->ToString(),S","),2);
   f->writeln(String::Concat(__box(m30)->ToString(),S",",__box(m31)->ToString(),S",",__box(m32)->ToString(),S",",__box(m33)->ToString(),S","),2);
   f->writeln("};",2);
   f->writeln("m.set(d);",2);
   f->writeln("t3d.set(m);",2);
   } catch (Exception* e) {
   Console::WriteLine("sftransform->writeJava error: {0}",e);
   }
   return;
   }/** output as a string
   * mode values
   * 0 - output modified values
   * 1 - output original values
   * 2 - output attribute
   * 3 - output attribute in brackets
   * 4 - output with f prefix
   */
   String* sftransform::outstring(int i) {
   return "null";
   }/** write to file
   * filter = information about output
   * mode values
   * 0 - output VRML97 modified values
   * 1 - output VRML97 original values
   * 2 - output xml (x3d)
   * 3 - output attribute in brackets
   * 4 - output with f prefix
   */
   void sftransform::write(filter* f,int mode,int indent){
   f->write(outstring(mode));
   }/** used by mfparam::vrml2par
   */
   bool sftransform::instring(filter* f,sfparam* sfp,nodeBean* n,int mode) {
   return false;
   }void sftransform::invert() {
   double det = determinant();
   double t00 = m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33    + m11*m22*m33;
   double t01 = m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33    - m01*m22*m33;
   double t02 = m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33    + m01*m12*m33;
   double t03 = m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23    - m01*m12*m23;
   double t10 = m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33    - m10*m22*m33;
   double t11 = m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33    + m00*m22*m33;
   double t12 = m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33    - m00*m12*m33;
   double t13 = m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23    + m00*m12*m23;
   double t20 = m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33    + m10*m21*m33;
   double t21 = m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33    - m00*m21*m33;
   double t22 = m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33    + m00*m11*m33;
   double t23 = m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23    - m00*m11*m23;
   double t30 = m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32    - m10*m21*m32;
   double t31 = m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32    + m00*m21*m32;
   double t32 = m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32    - m00*m11*m32;
   double t33 = m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22    + m00*m11*m22;
   m00 = t00;
   m01 = t01;
   m02 = t02;
   m03 = t03;
   m10 = t10;
   m11 = t11;
   m12 = t12;
   m13 = t13;
   m20 = t20;
   m21 = t21;
   m22 = t22;
   m23 = t23;
   m30 = t30;
   m31 = t31;
   m32 = t32;
   m33 = t33;
   scale(1/det);
   }void sftransform::invertAffine() {
   double d = determinantAffine();
   if (d != 0.0) {
   double t00 = (m11*m22 - m12*m21) / d;
   double t01 = (m02*m21 - m01*m22) / d;
   double t02 = (m01*m12 - m02*m11) / d;
   double t10 = (m12*m20 - m10*m22) / d;
   double t11 = (m00*m22 - m02*m20) / d; 
   double t12 = (m02*m10 - m00*m12) / d;
   double t20 = (m10*m21 - m11*m20) / d;
   double t21 = (m01*m20 - m00*m21) / d;
   double t22 = (m00*m11 - m01*m10) / d;
   m00 = t00;
   m01 = t01;
   m02 = t02;
   m10 = t10;
   m11 = t11;
   m12 = t12;
   m20 = t20;
   m21 = t21;
   m22 = t22;
   }
   m03 *= -1.0;
   m13 *= -1.0;
   m23 *= -1.0;
   }/**
   * Sets the value of this matrix to its inverse assuming its rotational
   * component is Orthogonal.
   */
   void sftransform::invertOrthogonal() {
   transpose();
   }/**
   * Sets the value of this matrix to its inverse assuming its rotational
   * component is ortho-normal.
   */
   void sftransform::invertOrthoNormal() {
   transpose();
   }/** Computes the determinant of this matrix. 
   * @return the determinant of the matrix 
   */
   double sftransform::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;
   }/**
   * Computes the affine determinant of this matrix. 
   * @return the determinant of the matrix 
   */
   double sftransform::determinantAffine() {
   double value;
   value = m00 * ( m11 * m22 - m21 * m12 );
   value -= m01 * ( m10 * m22 - m20 * m12 );
   value += m02 * ( m10 * m21 - m20 * m11 );
   return value;
   }/**
   * Sets the value of this matrix to a scale matrix with the
   * passed scale amount. 
   * @param scale the scale factor for the matrix 
   */
   void sftransform::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;
   }void sftransform::transpose() {
   double tmp = m01;
   m01 = m10;
   m10 = tmp; tmp = m02;
   m02 = m20;
   m20 = tmp; tmp = m03;
   m03 = m30;
   m30 = tmp; tmp = m12;
   m12 = m21;
   m21 = tmp; tmp = m13;
   m13 = m31;
   m31 = tmp; tmp = m23;
   m23 = m32;
   m32 = tmp;
   }

metadata block
see also:

 

Correspondence about this page david

Book Shop - Further reading.

Where I can, I have put links to Amazon for books that are relevant to the subject, click on the appropriate country flag to get more details of the book or to buy it from them.

cover Mathematics for 3D game Programming - Includes introduction to Vectors, Matrices, Transforms and Trigonometry. (But no euler angles or quaternions). Also includes ray tracing and some linear & rotational physics also collision detection (but not collision response).

Terminology and Notation

Specific to this page here:

 

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

Copyright (c) 1998-2017 Martin John Baker - All rights reserved - privacy policy.