Matrix3D.invert()

/**
 * Copyright umhr ( http://wonderfl.net/user/umhr )
 * MIT License ( http://www.opensource.org/licenses/mit-license.php )
 * Downloaded from: http://wonderfl.net/c/4OxX
 */

/*
Matrix3D.invert();
と同じ機能の関数を作ってみる。
MAC 10,0,2,54,MAC 10,0,12,36で動作確認。



◆処理内容
余因子(cofactor)を作り、
サルス(Sarrus)の公式(関－サルスの公式)で
随伴行列(余因子行列,adjugate matrix)を作る。
行列式(determinant)で反転可能かを確認し、
可能なら随伴行列を行列式で割り、逆行列を得る。


◆確認方法
平行移動と回転にrandomを入れてrecomposeしたものを、
interpolateし、行列の各要素を比較して確認とした。

◆参考
「実例で学ぶゲーム3D数学」オライリーP126
ベクトルと行列　行列式と外積
http://www.snap-tck.com/room04/c01/matrix/matrix08.html
線形代数@wiki 行列式の展開公式
http://www22.atwiki.jp/linearalgebra/pages/21.html


↓こちらは掃き出し法(sweep out method)を使ってるのかな
３Ｄゲーム用の４行４列のマトリックスの関数を作ろう#逆行列を作成する
http://hakuhin.hp.infoseek.co.jp/main/as/matrix_44.html#MATRIX_11


Matrix3D.invert()
http://help.adobe.com/ja_JP/AS3LCR/Flash_10.0/flash/geom/Matrix3D.html#invert()
*/
package {
	import flash.display.Sprite;
	import flash.geom.Matrix3D;
	import flash.text.TextField;
	import flash.geom.Vector3D;
	import flash.system.Capabilities;
	public class FlashTest extends Sprite {
		public function FlashTest() {
			var txt:String="◆同機能関数Mtrx3D.invert　"+Capabilities.version+"での実行結果\n\n";
			
			//確認用の値を用意
			var parameter:Matrix3D=new Matrix3D(Util.random9());
			var parameter1:Matrix3D=parameter.clone();
			var parameter2:Matrix3D=parameter.clone();
			
			//実行
			parameter1.invert();
			Mtrx3D.invert(parameter2);
			
			//確認
			var entity1RawData:Vector.<Number>=parameter1.rawData;
			var entity2RawData:Vector.<Number>=parameter2.rawData;
			txt+="↓Matrix3D.invertの結果\n"+entity1RawData+"\n";
			txt+="\n↓同機能関数Mtrx3D.invertの結果\n"+entity2RawData+"\n\n";
			txt+=Util.hikaku(entity1RawData,entity2RawData);
			
			//テキストフィールドを作りtxtを流し込み。
			var tf:TextField = new TextField();
			tf.width=stage.stageWidth;
			tf.height=stage.stageHeight;
			tf.wordWrap=true;
			stage.addChild(tf);
			tf.text=txt;
		}
	}
}
import flash.display.Sprite;
class Mtrx3D extends Sprite {
	import flash.geom.Matrix3D;
	import flash.geom.Vector3D;
	public static function invert(entity:Matrix3D):Boolean {
		var e:Vector.<Number>=Vector.<Number>(entity.rawData);
		var a:Vector.<Number>=new Vector.<Number>(16,true);
		//aはadjugateから
		a[0]=sarrus([e[5],e[9],e[13],e[6],e[10],e[14],e[7],e[11],e[15]]);
		a[1]=- sarrus([e[4],e[8],e[12],e[6],e[10],e[14],e[7],e[11],e[15]]);
		a[2]=sarrus([e[4],e[8],e[12],e[5],e[9],e[13],e[7],e[11],e[15]]);
		a[3]=- sarrus([e[4],e[8],e[12],e[5],e[9],e[13],e[6],e[10],e[14]]);

		a[4]=- sarrus([e[1],e[9],e[13],e[2],e[10],e[14],e[3],e[11],e[15]]);
		a[5]=sarrus([e[0],e[8],e[12],e[2],e[10],e[14],e[3],e[11],e[15]]);
		a[6]=- sarrus([e[0],e[8],e[12],e[1],e[9],e[13],e[3],e[11],e[15]]);
		a[7]=sarrus([e[0],e[8],e[12],e[1],e[9],e[13],e[2],e[10],e[14]]);

		a[8]=sarrus([e[1],e[5],e[13],e[2],e[6],e[14],e[3],e[7],e[15]]);
		a[9]=- sarrus([e[0],e[4],e[12],e[2],e[6],e[14],e[3],e[7],e[15]]);
		a[10]=sarrus([e[0],e[4],e[12],e[1],e[5],e[13],e[3],e[7],e[15]]);
		a[11]=- sarrus([e[0],e[4],e[12],e[1],e[5],e[13],e[2],e[6],e[14]]);

		a[12]=- sarrus([e[1],e[5],e[9],e[2],e[6],e[10],e[3],e[7],e[11]]);
		a[13]=sarrus([e[0],e[4],e[8],e[2],e[6],e[10],e[3],e[7],e[11]]);
		a[14]=- sarrus([e[0],e[4],e[8],e[1],e[5],e[9],e[3],e[7],e[11]]);
		a[15]=sarrus([e[0],e[4],e[8],e[1],e[5],e[9],e[2],e[6],e[10]]);

		var d:Number=e[0]*a[0]+e[1]*a[1]+e[2]*a[2]+e[3]*a[3];
		//dはdeterminantから
		if (d!=0) {
			entity.rawData = Vector.<Number>([
				a[0]/d,a[4]/d,a[8]/d,a[12]/d,
				a[1]/d,a[5]/d,a[9]/d,a[13]/d,
				a[2]/d,a[6]/d,a[10]/d,a[14]/d,
				a[3]/d,a[7]/d,a[11]/d,a[15]/d]);
		}
		return d != 0;

		function sarrus(c:Array):Number {
			//cはcofactorから
			return c[0]*(c[4]*c[8]-c[5]*c[7])+
			c[1]*(c[5]*c[6]-c[3]*c[8])+
			c[2]*(c[3]*c[7]-c[4]*c[6]);
		}
	}
}
class Util {
	import flash.geom.Matrix3D;
	import flash.geom.Vector3D;
	public static function hikaku(v0:Vector.<Number>,v1:Vector.<Number>):String {
		var _str:String="↓二つのMatrix3Dの要素毎の差\n";
		var _n:int=v0.length;
		for (var i:int=0; i<_n; i++) {
			_str += "["+i+"]:"+(v0[i]-v1[i])+"\n";
		}
		return _str;
	}

	public static function random16():Vector.<Number> {
		var _v:Vector.<Number>=new Vector.<Number>(16,true);
		for (var i:int=0; i<16; i++) {
			_v[i]=Math.random()*200-100;
		}
		return _v;
	}
	public static function random9():Vector.<Number> {
		var _mt:Matrix3D = new Matrix3D();
		var v:Vector.<Vector3D>=new Vector.<Vector3D>  ;
		v[0]=new Vector3D(200*Math.random()-100,200*Math.random()-100,200*Math.random()-100);//平行移動、
		v[1]=new Vector3D(10*Math.random()-5,10*Math.random()-5,10*Math.random()-5);//回転
		v[2]=new Vector3D(100*Math.random(),100*Math.random(),100*Math.random());//拡大 / 縮小
		_mt.recompose(v);
		return _mt.rawData;
	}
}