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

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->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; }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 = m12m23m31 - m13m22m31 + m13m21m32 - m11m23m32 - m12m21m33 + m11m22m33; double t01 = m03m22m31 - m02m23m31 - m03m21m32 + m01m23m32 + m02m21m33 - m01m22m33; double t02 = m02m13m31 - m03m12m31 + m03m11m32 - m01m13m32 - m02m11m33 + m01m12m33; double t03 = m03m12m21 - m02m13m21 - m03m11m22 + m01m13m22 + m02m11m23 - m01m12m23; double t10 = m13m22m30 - m12m23m30 - m13m20m32 + m10m23m32 + m12m20m33 - m10m22m33; double t11 = m02m23m30 - m03m22m30 + m03m20m32 - m00m23m32 - m02m20m33 + m00m22m33; double t12 = m03m12m30 - m02m13m30 - m03m10m32 + m00m13m32 + m02m10m33 - m00m12m33; double t13 = m02m13m20 - m03m12m20 + m03m10m22 - m00m13m22 - m02m10m23 + m00m12m23; double t20 = m11m23m30 - m13m21m30 + m13m20m31 - m10m23m31 - m11m20m33 + m10m21m33; double t21 = m03m21m30 - m01m23m30 - m03m20m31 + m00m23m31 + m01m20m33 - m00m21m33; double t22 = m01m13m30 - m03m11m30 + m03m10m31 - m00m13m31 - m01m10m33 + m00m11m33; double t23 = m03m11m20 - m01m13m20 - m03m10m21 + m00m13m21 + m01m10m23 - m00m11m23; double t30 = m12m21m30 - m11m22m30 - m12m20m31 + m10m22m31 + m11m20m32 - m10m21m32; double t31 = m01m22m30 - m02m21m30 + m02m20m31 - m00m22m31 - m01m20m32 + m00m21m32; double t32 = m02m11m30 - m01m12m30 - m02m10m31 + m00m12m31 + m01m10m32 - m00m11m32; double t33 = m01m12m20 - m02m11m20 + m02m10m21 - m00m12m21 - m01m10m22 + m00m11m22; 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 = (m11m22 - m12m21) / d; double t01 = (m02m21 - m01m22) / d; double t02 = (m01m12 - m02m11) / d; double t10 = (m12m20 - m10m22) / d; double t11 = (m00m22 - m02m20) / d; double t12 = (m02m10 - m00m12) / d; double t20 = (m10m21 - m11m20) / d; double t21 = (m01m20 - m00m21) / d; double t22 = (m00m11 - m01m10) / 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; }

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