Maths - Matrix Code - Martin Baker (original) (raw)
/Title: mjbWorld
Copyright (c) 1998-2007 Martin John BakerThis 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/
/package mjbModel;
import java.lang.ref.;
import java.util.; // for StringTokenizer/** a class to hold a 4x4 matrix and to allow various transforms on it. /
public class sftransform extends property {/* element 0,0 of matrix /
public double m00;
/* element 0,1 of matrix /
public double m01;
/* element 0,2 of matrix /
public double m02;
/* element 0,3 of matrix /
public double m03;
/* element 1,0 of matrix /
public double m10;
/element 1,1 of matrix /
public double m11;
/* element 1,2 of matrix */
public double m12;
/ element 1,3 of matrix /
public double m13;
/ element 2,0 of matrix /
public double m20;
/* element 2,1 of matrix /
public double m21;
/* element 2,2 of matrix /
public double m22;
/* element 2,3 of matrix /
public double m23;
/* element 3,0 of matrix /
public double m30;
/* element 3,1 of matrix /
public double m31;
/* element 3,2 of matrix /
public double m32;
/* element 3,3 of matrix /
public double m33;/* constructor for initialy zero matrix /
public 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;
}/* copy constructor
- @param a class to copy
- / public 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; }/* constructor set to value of a times b
- @param a first matrix
- @param b second matrix / public sftransform(sftransform a,sftransform b) { combine(a,b); }/* a static class to return the VRML name of this property
- @return the VRML name of this property / public static String vrmlType_s(){ return "SFTransform"; }/* gets the VRML name of this property, we need a non static class so that it can be overriden
- @return the VRML name of this property / public String vrmlType(){ return "SFTransform"; }/* returns the type of a class which can edit this transform
- @return type of a class which can edit this transform / static public Class getEditClass(){ return sftransformEditor.class; }/* sets this transform to the identity matrix, multiplying by the
- identity matrix does not alter the other matrix.
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- / public void 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; }/* overrides the clone method for this class
- @return new instance with same values as this / public Object clone() { return new sftransform(this); }/* sets the value of this transform to a copy of the transform supplied to it
- @param input instance to copy
- / public void copy(sftransform input){ m00 = input.m00; m01 = input.m01; m02 = input.m02; m03 = input.m03; m10 = input.m10; m11 = input.m11; m12 = input.m12; m13 = input.m13; m20 = input.m20; m21 = input.m21; m22 = input.m22; m23 = input.m23; m30 = input.m30; m31 = input.m31; m32 = input.m32; m33 = input.m33; }/* create an array of the appropriate type
- with a size given by the parameter
- @param size size of array
- @return the array of transforms created / public property[] createArray(int size){ return new sftransform[size]; }/* create an array of doubles with the values of this matrix
- / public double[] toArray(){ double[] matrix = new double[16]; matrix[0] = m00; matrix[1] = m01; matrix[2] = m02; matrix[3] = m03; matrix[4] = m10; matrix[5] = m11; matrix[6] = m12; matrix[7] = m13; matrix[8] = m20; matrix[9] = m21; matrix[10] = m22; matrix[11] = m23; matrix[12] = m30; matrix[13] = m31; matrix[14] = m32; matrix[15] = m33; return matrix; }/* sets this transform to the value calculated form a VRML tranform parameter
- which are: translation,rotation,center,sc and scaleOrientation
- for theory see:
- https://www.euclideanspace.com/maths/geometry/rotations/rotationAndTranslation/nonMatrix/index.htm
- @param translation inier offset
- @param rotation rotation
- @param center centre of rotation
- @param sc scale factor in x,y and z dimensions
- @param scaleOrientation orientation of scale factor / public void calcTransform(sfvec3f translation, sfrotation rotation, sfvec3f center, sfvec3f sc, sfrotation scaleOrientation){ setIdentity(); if (translation!=null) translate(translation); if (rotation!=null) rotate(rotation,center); if (scaleOrientation!=null) rotate(scaleOrientation,center); if (sc!=null) { // 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!=null) rotate(scaleOrientation.getMinus(),center); }/* multiply this matrix with the matrix supplied (m1) ** for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- @param m1 matrix to multiply by
- / public void combine(sftransform m1) { sftransform tmp = new sftransform(this); combine(tmp,m1); }/* multiply the matrix supplied (m1) with this matrix. This is different
- from the combine because the order of multipication is significant
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- @param m1 matrix which we multiply this by
- / public void combineInverse(sftransform m1) { sftransform tmp = new sftransform(this); combine(m1,tmp); }/* sets the value of this to m1 * m2
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- @param m1 martix 1
- @param m2 matrix 2
- / public void 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; }/* transform the vector supplied using this transform
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/transforms/index.htm
- @param v vector to be transformed
- / public void transform(sfvec3f v){ if (v==null) { System.out.println("sftransform.transform v==null"); return; } 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; }/* linear translation, ie add the vector supplied to the translation part of this matrix
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/transforms/index.htm
- @param v ammount of translation
- / public void translate(sfvec3f v){ if (v==null) { System.out.println("sftransform.translate v==null"); return; } m03 += v.x; m13 += v.y; m23 += v.z; m33 = 1.0; }/* scale this transform,
- for theory see:
- https://www.euclideanspace.com/maths/geometry/rotations/rotationAndTranslation/nonMatrix/index.htm
- @param v scale factor in x,y and z dimensions */ public void 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; }/ scale equally in all dimensions
- https://www.euclideanspace.com/maths/geometry/rotations/rotationAndTranslation/nonMatrix/index.htm
- @param scale the scale factor for the matrix
- */ public 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; }/ set this translation to rotate around a point
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/index.htm
- @param rot ammount of rotation
- @param centre centre of rotation
- / public void rotate(sfrotation rot,sfvec3f centre) { sftransform t1 = new sftransform(this); sftransform t2 = new sftransform(); t2.setRotate(rot,centre); combine(t1,t2); }/* rotate around a point
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/index.htm
- @param rot
- @param centre
- / public void setRotate(sfrotation rot,sfvec3f centre) { if (rot.coding == sfrotation.CODING_AXISANGLE | rot.coding == sfrotation.CODING_AXISANGLE_SAVEASQUAT) { double c = Math.cos(rot.angle); double s = Math.sin(rot.angle); double t = 1.0 - c; m00 = c + rot.xrot.xt; m11 = c + rot.yrot.yt; m22 = c + rot.zrot.zt; double tmp1 = rot.xrot.yt; double tmp2 = rot.zs; m10 = tmp1 + tmp2; m01 = tmp1 - tmp2; tmp1 = rot.xrot.zt; tmp2 = rot.ys; m20 = tmp1 - tmp2; m02 = tmp1 + tmp2; tmp1 = rot.yrot.zt; tmp2 = rot.xs; m21 = tmp1 + tmp2; m12 = tmp1 - tmp2; } else { double sqw = rot.anglerot.angle; double sqx = rot.xrot.x; double sqy = rot.yrot.y; double sqz = rot.zrot.z; m00 = sqx - sqy - sqz + sqw; // since sqw + sqx + sqy + sqz =1 m11 = -sqx + sqy - sqz + sqw; m22 = -sqx - sqy + sqz + sqw; double tmp1 = rot.xrot.y; double tmp2 = rot.zrot.angle; m10 = 2.0 * (tmp1 + tmp2); m01 = 2.0 * (tmp1 - tmp2); tmp1 = rot.xrot.z; tmp2 = rot.yrot.angle; m20 = 2.0 * (tmp1 - tmp2); m02 = 2.0 * (tmp1 + tmp2); tmp1 = rot.yrot.z; tmp2 = rot.xrot.angle; m21 = 2.0 * (tmp1 + tmp2); m12 = 2.0 * (tmp1 - tmp2); } double a1,a2,a3; if (centre == null) { a1=a2=a3=0; } else { a1 = centre.x; a2 = centre.y; a3 = centre.z; } m03 = a1 - a1 * m00 - a2 * m01 - a3 * m02; m13 = a2 - a1 * m10 - a2 * m11 - a3 * m12; m23 = a3 - a1 * m20 - a2 * m21 - a3 * m22; m30 = m31 = m32 = 0.0; m33 = 1.0; }/** rotate about a point, rotation given by euler angles
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/index.htm
- @param centre">point to rotate around
- @param theta">angle in radians
- @param phi">angle in radians
- @param alpha">angle in radians */ public void setRotate(sfvec3f centre, double theta,double phi,double alpha) { double cosAlpha, sinAlpha, cosPhi, sinPhi, cosTheta, sinTheta, cosPhi2, sinPhi2, cosTheta2, sinTheta2, c, a1,a2,a3; if (centre==null) { a1=a2=a3=0; } else { a1 = centre.x; a2 = centre.y; a3 = centre.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; }/** format values into a string with square brackets
- @return a string representation of this class
- / public String toString(){ String s1="["+m00+","+m01+","+m02+","+m03+"]"; String s2="["+m10+","+m11+","+m12+","+m13+"]"; String s3="["+m20+","+m21+","+m22+","+m23+"]"; String s4="["+m30+","+m31+","+m32+","+m33+"]"; return ""+s1+"\n"+s2+"\n"+s3+"\n"+s4; }/* output as a string
- @param mode possible values:
- 0 - output modified values
- 1 - output original values
- 2 - output attribute
- 3 - output attribute in brackets
- 4 - output with f prefix
- @return a string representation of this class / public String outstring(int i) { return "\n"+m00+","+m01+","+m02+","+m03+"\n"+ m10+","+m11+","+m12+","+m13+"\n"+ m20+","+m21+","+m22+","+m23+"\n"+ m30+","+m31+","+m32+","+m33; }/* find inverse matrix
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- / public void 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); }/* for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm / public void 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 - 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; }/ Calculates the determinant of this matrix
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- @return the determinant of the matrix
- / public 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; }/* calculates the transpose
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- / public void 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; }/* calculates the affine determinant of this matrix.
- for theory see:
- https://www.euclideanspace.com/maths/algebra/matrix/arithmetic/index.htm
- @return the determinant of the matrix
- / public double determinantAffine() { double value; value = m00 * ( m11 * m22 - m21 * m12 ); value -= m01 * ( m10 * m22 - m20 * m12 ); value += m02 * ( m10 * m21 - m20 * m11 ); return value; }/* generate java code to file
- @param f
- @param mode
- @param maxInstances
- / public void writeJava(filter f,int mode,int maxInstances){ try { if (mode != 0) return; // no procedure defn required f.status(getClass().getName()); f.writeln("// code for "+getClass().getName(),0); f.writeln("Transform3D t3d = new Transform3D();",2); f.writeln("Matrix4d m = new Matrix4d();",2); f.writeln("double []d = {",2); f.writeln(""+m00+","+m01+","+m02+","+m03+",",2); f.writeln(""+m10+","+m11+","+m12+","+m13+",",2); f.writeln(""+m20+","+m21+","+m22+","+m23+",",2); f.writeln(""+m30+","+m31+","+m32+","+m33+",",2); f.writeln("};",2); f.writeln("m.set(d);",2); f.writeln("t3d.set(m);",2); } catch (Exception e) { System.out.println("sftransform.writeJava " + e.toString()); } return; }/* write in VRML mode (except that SFTransform does not exist in VRML)
- @param f file parameters and methods
- @param mode 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
- @param indent / public void write(filter f,int mode,int indent){ f.write(outstring(mode)); }/* read in VRML mode (except that SFTransform does not exist in VRML)
- used by mfparam.vrml2par
- @param f">file parameters and methods
- @param sfp">
- @param n">
- @param mode">
- @return */ public boolean instring(filter f,sfparam sfp,nodeBean n,int mode) { return false; } }
This site may have errors. Don't use for critical systems.
Copyright (c) 1998-2023 Martin John Baker - All rights reserved - privacy policy.