/*
 * Matrix3DとVector3DをPlayer9向けに作ったら
 * こうなるかな、なテスト。20倍とかのバグ周辺はとりあえず無視。
 * 
 * 青いほうがMatrix3Dによる計算結果
 * 赤いほうが互換（を目指した）クラスMtrx3Dによる計算結果
 * 
 * 動作確認が十分でないし、未実装もあるけど、
 * ちょっと整理。
 * 
 * とりあえず、appendRotation、appendScaleあたりは問題ないと思う。
 * 
 * 
 * 要再動作確認
 * decompose
 * 
 * 未完成
 * interpolate
 * 
 * 未実装
 * interpolateTo
 * pointAt
 * 
 * */
package {
	import flash.display.Sprite;
	import flash.events.Event;
	import flash.geom.Matrix3D;
	import flash.geom.Vector3D;
	
	[SWF(backgroundColor="0xCCCCCC")]
	public class Main extends Sprite {
		private var _mtrx3D:Mtrx3D = new Mtrx3D();
		private var _sprite1:Sprite = new Sprite();
		private var _matrix3D:Matrix3D = new Matrix3D();
		private var _sprite2:Sprite = new Sprite();
		
		private var _count:int;
		public function Main() {
			this.addEventListener(Event.ENTER_FRAME, onEnter);
			
			_sprite1.x = 210;
			_sprite1.y = 200;
			this.addChild(_sprite1);
			_sprite2.x = 220;
			_sprite2.y = 200;
			this.addChild(_sprite2);
		}
		private function onEnter(event:Event):void {
			_count++;
			_count = _count%400
			runMtrx3D(_sprite1);
			runMatrix3D(_sprite2);
		}
		
		
		private function runMtrx3D(sprite:Sprite):void {
			if(_count < 100){
				_mtrx3D.appendRotation(3, Vctr3D.Y_AXIS);
				_mtrx3D.appendScale(0.999, 1.005, 1);
				_mtrx3D.appendRotation(1, Vctr3D.X_AXIS);
			}else if(_count < 200){
				_mtrx3D.prependTranslation(0.1, 0, 0.1);
				_mtrx3D.appendRotation(1, Vctr3D.Z_AXIS);
				_mtrx3D.appendScale(1, 0.995, 1.005);
				_mtrx3D.appendRotation( -1, Vctr3D.X_AXIS);
			}else if(_count < 300){
				_mtrx3D.prependRotation(1, Vctr3D.Z_AXIS);
				_mtrx3D.prependTranslation(-0.1, 0, -0.1);
				_mtrx3D.appendScale(1.001, 1, 0.995);
				_mtrx3D.appendTranslation(0, -0.1, 0);
			}else if(_count < 400){
				_mtrx3D.prependRotation(-1, Vctr3D.Z_AXIS);
				_mtrx3D.appendTranslation(0, 0.1, 0);
			}
			var vin:Array = [0, 0, 0, 50, 0, 0, 50, 50, 0, 0, 50, 0];
			var vout:Array = [];
			_mtrx3D.transformVectors(vin, vout);
			
			var n:int = vout.length / 3;
			sprite.graphics.clear();
			sprite.graphics.lineStyle(1, 0xFF0000);
			sprite.graphics.moveTo(vout[0], vout[1]);
			for (var i:int = 1; i < n; i++) {
				sprite.graphics.lineTo(vout[i * 3], vout[i * 3+ 1]);
			}
			sprite.graphics.lineTo(vout[0], vout[1]);
		}
		private function runMatrix3D(sprite:Sprite):void {
			if(_count < 100){
				_matrix3D.appendRotation(3, Vector3D.Y_AXIS);
				_matrix3D.appendScale(0.999, 1.005, 1);
				_matrix3D.appendRotation(1, Vector3D.X_AXIS);
			}else if(_count < 200){
				_matrix3D.prependTranslation(0.1, 0, 0.1);
				_matrix3D.appendRotation(1, Vector3D.Z_AXIS);
				_matrix3D.appendScale(1, 0.995, 1.005);
				_matrix3D.appendRotation( -1, Vector3D.X_AXIS);
			}else if(_count < 300){
				_matrix3D.prependRotation(1, Vector3D.Z_AXIS);
				_matrix3D.prependTranslation(-0.1, 0, -0.1);
				_matrix3D.appendScale(1.001, 1, 0.995);
				_matrix3D.appendTranslation(0, 0.1, 0);
			}else if(_count < 400){
				_matrix3D.prependRotation(-1, Vector3D.Z_AXIS);
				_matrix3D.appendTranslation(0, -0.1, 0);
			}
			var vin:Vector.<Number> = new Vector.<Number>();
			vin = Vector.<Number>([0, 0, 0, 50, 0, 0, 50, 50, 0, 0, 50, 0]);
			var vout:Vector.<Number> = new Vector.<Number>();
			_matrix3D.transformVectors(vin, vout);
			
			var n:int = vout.length / 3;
			sprite.graphics.clear();
			sprite.graphics.lineStyle(1, 0x0000FF);
			sprite.graphics.moveTo(vout[0], vout[1]);
			for (var i:int = 1; i < n; i++) {
				sprite.graphics.lineTo(vout[i * 3], vout[i * 3+ 1]);
			}
			sprite.graphics.lineTo(vout[0], vout[1]);
		}
		
		
	}
}

class Mtrx3D {
	private var _rawData:Array = [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1];
	
	//コンストラクタ
	public function Mtrx3D(v:Array = null) {
		if (v) {
			_rawData = v.concat();
		}
	}
	
	//プロパティ
	public function get determinant():Number{
		//余因子(cofactor)を作る
		var d:Number = _rawData[0]*sarrus([_rawData[5],_rawData[9],_rawData[13],_rawData[6],_rawData[10],_rawData[14],_rawData[7],_rawData[11],_rawData[15]]);
		//dはdeterminantから
		d -= _rawData[1]*sarrus([_rawData[4],_rawData[8],_rawData[12],_rawData[6],_rawData[10],_rawData[14],_rawData[7],_rawData[11],_rawData[15]]);
		d += _rawData[2]*sarrus([_rawData[4],_rawData[8],_rawData[12],_rawData[5],_rawData[9],_rawData[13],_rawData[7],_rawData[11],_rawData[15]]);
		d -= _rawData[3]*sarrus([_rawData[4],_rawData[8],_rawData[12],_rawData[5],_rawData[9],_rawData[13],_rawData[6],_rawData[10],_rawData[14]]);
		d -= ((_rawData[4]-_rawData[8])*_rawData[1]+(_rawData[9]-_rawData[5])*_rawData[0])*_rawData[11]*_rawData[14];
		return -d;
		function sarrus(c:Array):Number{
			//cはcofactorから
			//サルス(Sarrus)の公式(関－サルスの公式)
			return c[0]*c[4]*c[8]+c[3]*c[7]*c[2]+c[6]*c[1]*c[5]
			-c[6]*c[4]*c[2]-c[3]*c[1]*c[8]-c[0]*c[7]*c[5];
		}
	}
	public function get position():Vctr3D{
		return new Vctr3D(_rawData[12],_rawData[13],_rawData[14]);
	}
	public function set position(v:Vctr3D):void{
		_rawData[12] = v.x;
		_rawData[13] = v.y;
		_rawData[14] = v.z;
	}
	public function get rawData():Array { return _rawData };
	public function set rawData(value:Array):void {
		_rawData = value.concat();
	}
	
	
	//メソッド
	public function append(lhs:Mtrx3D):void {
		_rawData = matrix44Calculat(_rawData,lhs.rawData);
	}
	public function appendRotation(degrees:Number,axis:Vctr3D,pivotPoint:Vctr3D=null):void {
		if(!pivotPoint){
			pivotPoint = new Vctr3D(0, 0, 0);
		}
		var tempAxis:Vctr3D = axis.clone();

//AXIS_ANGLE to QUATERNION
		var degreesPIper360:Number = degrees / 360 * Math.PI;
		var w:Number = Math.cos(degreesPIper360);
		var x:Number = Math.sin(degreesPIper360) * tempAxis.x;
		var y:Number = Math.sin(degreesPIper360) * tempAxis.y;
		var z:Number = Math.sin(degreesPIper360) * tempAxis.z;
		
		//rawData from QUATERNION
		var p:Array = rawDataFromQuaternion(x, y, z, w);
		
		//Matrix * entity
		_rawData = matrix44Calculat(_rawData, p);
		appendTranslation(pivotPoint.x,pivotPoint.y,pivotPoint.z);
		
	}
	public function appendScale(xScale:Number,yScale:Number,zScale:Number):void {
		_rawData[0]*=xScale;
		_rawData[1]*=yScale;
		_rawData[2]*=zScale;
		_rawData[4]*=xScale;
		_rawData[5]*=yScale;
		_rawData[6]*=zScale;
		_rawData[8]*=xScale;
		_rawData[9]*=yScale;
		_rawData[10]*=zScale;
		_rawData[12]*=xScale;
		_rawData[13]*=yScale;
		_rawData[14]*=zScale;
	}
	public function appendTranslation(x:Number, y:Number, z:Number):void{
		_rawData[12] += x;
		_rawData[13] += y;
		_rawData[14] += z;
	}
	
	public function clone():Mtrx3D {
		return new Mtrx3D(_rawData);
	}
	
	public function decompose(orientationStyle:String = "eulerAngles"):Array{
		var e:Array = _rawData.concat();
		
		//prependScale的処理
		var vec:Array = matrix3dToEulerAnglePrepend(e);
		
		if(orientationStyle != "eulerAngles"){
			vec = matrix3dToQuaternion(e,vec[2],orientationStyle);
		}
		return vec;
	}
	
	private function matrix3dToQuaternion(e:Array,scale:Vctr3D,orientationStyle:String):Array{
		if(scale.x > 0){
			e[0]/=scale.x;
			e[4]/=scale.x;
			e[8]/=scale.x;
		}
		if(scale.y > 0){
			e[1]/=scale.y;
			e[5]/=scale.y;
			e[9]/=scale.y;
		}
		if(scale.z > 0){
			e[2]/=scale.z;
			e[6]/=scale.z;
			e[10]/=scale.z;
		}
		var w:Number;
		var x:Number;
		var y:Number;
		var z:Number;
		var _ar:Array = new Array(e[0]+e[5]+e[10],e[0]-e[5]-e[10],e[5]-e[0]-e[10],e[10]-e[0]-e[5]);
		var biggestIndex:int = _ar.sort(Array.NUMERIC|Array.RETURNINDEXEDARRAY|Array.DESCENDING)[0];
		var biggestVal:Number = Math.sqrt(_ar[biggestIndex]+1)*0.5;
		var mult:Number = 0.25/biggestVal;
		switch (biggestIndex) {
			case 0:
				w = biggestVal;
				x = (e[6]-e[9])*mult;
				y = (e[8]-e[2])*mult;
				z = (e[1]-e[4])*mult;
				break;
			case 1:
				x = biggestVal;
				w = (e[6]-e[9])*mult;
				y = (e[4]+e[1])*mult;
				z = (e[2]+e[8])*mult;
				break;
			case 2:
				y = biggestVal;
				w = (e[8]-e[2])*mult;
				x = (e[4]+e[1])*mult;
				z = (e[9]+e[6])*mult;
				break;
			case 3:
				z = biggestVal;
				w = (e[1]-e[4])*mult;
				x = (e[2]+e[8])*mult;
				y = (e[9]+e[6])*mult;
				break;
		}

if(orientationStyle == "axisAngle"){
			if(Math.sin(Math.acos(w)) != 0){
				x = x/Math.sin(Math.acos(w));
				y = y/Math.sin(Math.acos(w));
				z = z/Math.sin(Math.acos(w));
				w = 2*Math.acos(w);
			}else{
				x = y = z= w = 0;
			}
		}
		return [new Vctr3D(e[12],e[13],e[14]),new Vctr3D(x,y,z,w),scale];
	}

private function matrix3dToEulerAnglePrepend(e:Array):Array{
		var _z:Number = Math.atan2(e[1],e[0]);
		var sz:Number = Math.sin(_z);
		var cz:Number = Math.cos(_z);

var _y:Number;
		if(Math.abs(cz) > 0.7){
			_y = Math.atan2(-e[2],e[0]/cz);
		}else{
			_y = Math.atan2(-e[2],e[1]/sz);
		}
		var sy:Number = Math.sin(_y);
		var cy:Number = Math.cos(_y);

var _x:Number;
		if(Math.abs(cz) > 0.7){
			_x = Math.atan2((sy*sz-e[9]*cy/e[10]),cz);
		}else{
			_x = Math.atan2((e[8]*cy/e[10]-sy*cz)/sz,1);
		}
        
		//_x = Math.atan2((sy*sz-e[9]*cy/e[10])/cz,1);
		//_x = Math.atan2((e[8]*cy/e[10]-sy*cz)/sz,1);

var sx:Number = Math.sin(_x);
		var cx:Number = Math.cos(_x);

var scale_x:Number = -e[2]/sy;
		var scale_y:Number = e[6]/(sx*cy);
		var scale_z:Number = e[10]/(cx*cy);
		return [new Vctr3D(e[12], e[13], e[14]), new Vctr3D(_x, _y, _z), new Vctr3D(scale_x, scale_y, scale_z)];
		
	}

public function deltaTransformVector(v:Vctr3D):Vctr3D {
		var e:Array = _rawData;
		return new Vctr3D((e[0]*v.x+e[4]*v.y+e[8]*v.z),(e[1]*v.x+e[5]*v.y+e[9]*v.z),(e[2]*v.x+e[6]*v.y+e[10]*v.z),(e[3]*v.x+e[7]*v.y+e[11]*v.z));
	}
	public function identity():void {
		_rawData=[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1];
	}
	public function interpolate(thisMat:Mtrx3D,toMat:Mtrx3D,percent:Number):Mtrx3D{
		var v0:Vctr3D = thisMat.decompose("quaternion")[1];
		var v1:Vctr3D = toMat.decompose("quaternion")[1];
		var cosOmega:Number = v0.w*v1.w + v0.x*v1.x + v0.y*v1.y + v0.z*v1.z;
		if(cosOmega < 0){
			v1.x = -v1.x;
			v1.y = -v1.y;
			v1.z = -v1.z;
			v1.w = -v1.w;
			cosOmega = -cosOmega;
		}
		var k0:Number;
		var k1:Number;
		if(cosOmega > 0.9999){
			k0 = 1 - percent;
			k1 = percent;
		}else{
			var sinOmega:Number = Math.sqrt(1 - cosOmega*cosOmega);
			var omega:Number = Math.atan2(sinOmega,cosOmega);
			var oneOverSinOmega:Number = 1/sinOmega;
			k0 = Math.sin((1-percent)*omega)*oneOverSinOmega;
			k1 = Math.sin(percent*omega)*oneOverSinOmega;
		}
		var scale_x:Number = thisMat.decompose("quaternion")[2].x*(1-percent) + toMat.decompose("quaternion")[2].x*percent;
		var scale_y:Number = thisMat.decompose("quaternion")[2].y*(1-percent) + toMat.decompose("quaternion")[2].y*percent;
		var scale_z:Number = thisMat.decompose("quaternion")[2].z*(1-percent) + toMat.decompose("quaternion")[2].z*percent;

var tx:Number = thisMat.decompose("quaternion")[0].x*(1-percent) + toMat.decompose("quaternion")[0].x*percent;
		var ty:Number = thisMat.decompose("quaternion")[0].y*(1-percent) + toMat.decompose("quaternion")[0].y*percent;
		var tz:Number = thisMat.decompose("quaternion")[0].z*(1-percent) + toMat.decompose("quaternion")[0].z*percent;

var x:Number = v0.x*k0+v1.x*k1;
		var y:Number = v0.y*k0+v1.y*k1;
		var z:Number = v0.z*k0+v1.z*k1;
		var w:Number = v0.w * k0 + v1.w * k1;
		var _q:Array = rawDataFromQuaternion(x, y, z, w);
		_q[12] = tx;
		_q[13] = ty;
		_q[14] = tz;
		
		//var v:Vctr3D = new Vctr3D(v0.x*k0+v1.x*k1,v0.y*k0+v1.y*k1,v0.z*k0+v1.z*k1,v0.w*k0+v1.w*k1);

//var txyz:Vector3D = new Vector3D(tx,ty,tz);
		//var m:Matrix3D=new Matrix3D();
		//m.recompose(Vector.<Vector3D>([txyz,v,new Vector3D(scale_x,scale_y,scale_z)]),"quaternion");
		//trace(m.rawData);
		return new Mtrx3D(_q);
	}
	public function interpolateTo():* {
		//未実装
	}	
	
	public function invert():Boolean {
		var e:Array = _rawData;
		var a:Array=new Array(16);
		//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) {
			_rawData = [
				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]);
		}
	}
	public function pointAt():* {
		//未実装
	}	
	public function prepend(rhs:Mtrx3D):void {
		_rawData = matrix44Calculat(rhs.rawData,_rawData);
	}
	public function prependRotation(degrees:Number,axis:Vctr3D,pivotPoint:Vctr3D=null):void {
		if(!pivotPoint){
			pivotPoint = new Vctr3D(0, 0, 0);
		}
		var tempAxis:Vctr3D = axis.clone();
		tempAxis.normalize();
		//AXIS_ANGLE to QUATERNION
		var degreesPIper360:Number = degrees / 360 * Math.PI;
		var w:Number = Math.cos(degreesPIper360);
		var x:Number = Math.sin(degreesPIper360) * tempAxis.x;
		var y:Number = Math.sin(degreesPIper360) * tempAxis.y;
		var z:Number = Math.sin(degreesPIper360) * tempAxis.z;
		
		//rawData from QUATERNION
		var p:Array = rawDataFromQuaternion(x, y, z, w);
		
		//Matrix * entity
		_rawData = matrix44Calculat(p, _rawData);
		appendTranslation(pivotPoint.x,pivotPoint.y,pivotPoint.z);
	}
	public function prependScale(xScale:Number,yScale:Number,zScale:Number):void {
		_rawData[0]*=xScale;
		_rawData[1]*=xScale;
		_rawData[2]*=xScale;
		_rawData[3]*=xScale;
		_rawData[4]*=yScale;
		_rawData[5]*=yScale;
		_rawData[6]*=yScale;
		_rawData[7]*=yScale;
		_rawData[8]*=zScale;
		_rawData[9]*=zScale;
		_rawData[10]*=zScale;
		_rawData[11]*=zScale;
	}
	
	public function prependTranslation(x:Number, y:Number, z:Number):void{
		_rawData[12] += _rawData[0]*x+_rawData[4]*y+_rawData[8]*z;
		_rawData[13] += _rawData[1]*x+_rawData[5]*y+_rawData[9]*z;
		_rawData[14] += _rawData[2]*x+_rawData[6]*y+_rawData[10]*z;
		_rawData[15] += _rawData[3]*x+_rawData[7]*y+_rawData[11]*z;
	}
	public function recompose(components:Array, orientationStyle:String = "eulerAngles"):Boolean {
		var scale:Array=new Array(16);
		//prependScale的な乗算
		scale[0] = scale[1] = scale[2] = components[2].x;
		scale[4] = scale[5] = scale[6] = components[2].y;
		scale[8] = scale[9] = scale[10] = components[2].z;
		
		var v:Array=new Array(16);
		if (orientationStyle=="eulerAngles") {
			//eulerAngles to Matrix3D
			var cx:Number=Math.cos(components[1].x);
			var cy:Number=Math.cos(components[1].y);
			var cz:Number=Math.cos(components[1].z);
			var sx:Number=Math.sin(components[1].x);
			var sy:Number=Math.sin(components[1].y);
			var sz:Number=Math.sin(components[1].z);
			v[0]=cy*cz*scale[0];
			v[1]=cy*sz*scale[1];
			v[2]=- sy*scale[2];
			v[3]=0;
			v[4] = (sx*sy*cz-cx*sz)*scale[4];
			v[5] = (sx*sy*sz+cx*cz)*scale[5];
			v[6]=sx*cy*scale[6];
			v[7]=0;
			v[8] = (cx*sy*cz+sx*sz)*scale[8];
			v[9] = (cx*sy*sz-sx*cz)*scale[9];
			v[10]=cx*cy*scale[10];
			v[11]=0;
			v[12]=components[0].x;
			v[13]=components[0].y;
			v[14]=components[0].z;
			v[15]=1;
		} else {
			//"quaternion" to Matrix3D
			var x:Number=components[1].x;
			var y:Number=components[1].y;
			var z:Number=components[1].z;
			var w:Number=components[1].w;
			if (orientationStyle=="axisAngle") {
				//"axisAngle" to Matrix3D
				x*=Math.sin(w/2);
				y*=Math.sin(w/2);
				z*=Math.sin(w/2);
				w=Math.cos(w/2);
			}
			v[0] = (1-2*y*y-2*z*z)*scale[0];
			v[1] = (2*x*y+2*w*z)*scale[1];
			v[2] = (2*x*z-2*w*y)*scale[2];
			v[3]=0;
			v[4] = (2*x*y-2*w*z)*scale[4];
			v[5] = (1-2*x*x-2*z*z)*scale[5];
			v[6] = (2*y*z+2*w*x)*scale[6];
			v[7]=0;
			v[8] = (2*x*z+2*w*y)*scale[8];
			v[9] = (2*y*z-2*w*x)*scale[9];
			v[10] = (1-2*x*x-2*y*y)*scale[10];
			v[11]=0;
			v[12]=components[0].x;
			v[13]=components[0].y;
			v[14]=components[0].z;
			v[15]=1;
		}
		//v[0],v[4],v[8]が三つとも0だと
		//ArgumentError: Error #2188: 行マトリックスが無効です。マトリックスは反転可能である必要があります。
		//というエラーでMatrix3Dを作れないので、設定可能な最小値を入れている。
		//0.000000000000000000000000000000000000000000001;
		//これは1e-45と書ける。
		//ただしx,y,z三つとも1e-45には設定できないので、1e-15にしてる。
		if(components[2].x == 0){v[0] = 1e-15};
		if(components[2].y == 0){v[5] = 1e-15};
		if(components[2].z == 0){v[10] = 1e-15};
		_rawData = v;
		//helpには拡大 / 縮小エレメントのいずれかが 0 の場合は、false を返します。
		//とあるがtrueしか返さないっぽい、、
		return !(components[2].x == 0 || components[2].y == 0 || components[2].y == 0)
	}
	public function transformVector(v:Vctr3D):Vctr3D{
		var tempV:Vctr3D = new Vctr3D();
		tempV.x = _rawData[0]*v.x+_rawData[4]*v.y+_rawData[8]*v.z+_rawData[12];
		tempV.y = _rawData[1]*v.x+_rawData[5]*v.y+_rawData[9]*v.z+_rawData[13];
		tempV.z = _rawData[2]*v.x+_rawData[6]*v.y+_rawData[10]*v.z+_rawData[14];
		tempV.w = _rawData[3]*v.x+_rawData[7]*v.y+_rawData[11]*v.z+_rawData[15];
		return tempV;
	}
	
	public function transformVectors(vin:Array, vout:Array):void {
		var e:Array = _rawData;
		var n:int=vin.length;
		var temp:Array = new Array(n);
		for (var i:int=0; i<n; i+=3) {
			temp[i] = e[0]*vin[i]+e[4]*vin[i+1]+e[8]*vin[i+2]+e[12];
			temp[i+1] = e[1]*vin[i]+e[5]*vin[i+1]+e[9]*vin[i+2]+e[13];
			temp[i+2] = e[2]*vin[i]+e[6]*vin[i+1]+e[10]*vin[i+2]+e[14];
		}
		for (var j:int=0; j<n; j++) {
			vout[j] = temp[j];
		}
	}
	public function transpose():void {
		_rawData = [
			_rawData[0],_rawData[4],_rawData[8],_rawData[12],
			_rawData[1],_rawData[5],_rawData[9],_rawData[13],
			_rawData[2],_rawData[6],_rawData[10],_rawData[14],
			_rawData[3],_rawData[7],_rawData[11],_rawData[15]
		]
	}
	
	
	
	//private function
	private function matrix44Calculat(e:Array,p:Array):Array {
		var pe:Array = new Array();
		
		pe[0] = p[0]*e[0]+p[4]*e[1]+p[8]*e[2]+p[12]*e[3];
		pe[1] = p[1]*e[0]+p[5]*e[1]+p[9]*e[2]+p[13]*e[3];
		pe[2] = p[2]*e[0]+p[6]*e[1]+p[10]*e[2]+p[14]*e[3];
		pe[3] = p[3]*e[0]+p[7]*e[1]+p[11]*e[2]+p[15]*e[3];
		
		pe[4] = p[0]*e[4]+p[4]*e[5]+p[8]*e[6]+p[12]*e[7];
		pe[5] = p[1]*e[4]+p[5]*e[5]+p[9]*e[6]+p[13]*e[7];
		pe[6] = p[2]*e[4]+p[6]*e[5]+p[10]*e[6]+p[14]*e[7];
		pe[7] = p[3]*e[4]+p[7]*e[5]+p[11]*e[6]+p[15]*e[7];
		
		pe[8] = p[0]*e[8]+p[4]*e[9]+p[8]*e[10]+p[12]*e[11];
		pe[9] = p[1]*e[8]+p[5]*e[9]+p[9]*e[10]+p[13]*e[11];
		pe[10] = p[2]*e[8]+p[6]*e[9]+p[10]*e[10]+p[14]*e[11];
		pe[11] = p[3]*e[8]+p[7]*e[9]+p[11]*e[10]+p[15]*e[11];
		
		pe[12] = p[0]*e[12]+p[4]*e[13]+p[8]*e[14]+p[12]*e[15];
		pe[13] = p[1]*e[12]+p[5]*e[13]+p[9]*e[14]+p[13]*e[15];
		pe[14] = p[2]*e[12]+p[6]*e[13]+p[10]*e[14]+p[14]*e[15];
		pe[15] = p[3]*e[12]+p[7]*e[13]+p[11]*e[14]+p[15]*e[15];
		
		return pe;
	}
	private function rawDataFromQuaternion(x:Number,y:Number,z:Number,w:Number):Array {
		var p:Array = new Array(16);
		p[0] = (w*w+x*x-y*y-z*z);
		p[1] = 2*(y*x+w*z);
		p[2] = 2*(z*x-w*y);
		p[3] = 0;
		p[4] = 2*(y*x-w*z);
		p[5] = (w*w-x*x+y*y-z*z);
		p[6] = 2*(w*x+z*y);
		p[7] = 0;
		p[8] = 2*(z*x+w*y);
		p[9] = 2*(z*y-w*x);
		p[10] = (w*w-x*x-y*y+z*z);
		p[11] = 0;
		p[12] = 0;
		p[13] = 0;
		p[14] = 0;
		p[15] = 1;
		return p;
	}
	
}

class Vctr3D{
	public var w:Number;
	public var x:Number;
	public var y:Number;
	public var z:Number;
	public static const X_AXIS:Vctr3D=new Vctr3D(1,0,0);
	public static const Y_AXIS:Vctr3D=new Vctr3D(0,1,0);
	public static const Z_AXIS:Vctr3D=new Vctr3D(0,0,1);
	function Vctr3D(x:Number=0.,y:Number=0.,z:Number=0.,w:Number=0.) {
		this.w=w;
		this.x=x;
		this.y=y;
		this.z=z;
	}
	public function get length():Number {
		return Math.sqrt(x*x+y*y+z*z);
	}
	public function get lengthSquared():Number {
		return x*x+y*y+z*z;
	}
	public function add(a:Vctr3D):Vctr3D {
		return new Vctr3D(a.x+x,a.y+y,a.z+z);
	}
	public static function angleBetween(a:Vctr3D,b:Vctr3D):Number {
		return Math.acos(a.x*b.x+a.y*b.y+a.z*b.z/Math.sqrt(a.x*a.x+a.y*a.y+a.z*a.z)*Math.sqrt(b.x*b.x+b.y*b.y+b.z*b.z));
	}
	public function clone():Vctr3D {
		return new Vctr3D(x,y,z,w);
	}
	public function crossProduct(a:Vctr3D):Vctr3D {
		return new Vctr3D(y*a.z-z*a.y,z*a.x-x*a.z,x*a.y-y*a.x,1);
	}
	public function decrementBy(a:Vctr3D):void {
		x-=a.x;
		y-=a.y;
		z-=a.z;
	}
	public static function distance(pt1:Vctr3D,pt2:Vctr3D):Number {
		return Math.sqrt(pt1.x-pt2.x*pt1.x-pt2.x+pt1.y-pt2.y*pt1.y-pt2.y+pt1.z-pt2.z*pt1.z-pt2.z);
	}
	public function dotProduct(a:Vctr3D):Number {
		return x*a.x+y*a.y+z*a.z;
	}
	public function equals(toCompare:Vctr3D,allFour:Boolean=false):Boolean {
		return x==toCompare.x&&y==toCompare.y&&z==toCompare.z&&! allFour||w==toCompare.w;
	}
	public function incrementBy(a:Vctr3D):void {
		x+=a.x;
		y+=a.y;
		z+=a.z;
	}
	public function nearEquals(toCompare:Vctr3D,tolerance:Number,allFour:Boolean=false):Boolean {
		return Math.abs(x-toCompare.x)<tolerance&&Math.abs(y-toCompare.y)<tolerance&&Math.abs(z-toCompare.z)<tolerance&&! allFour||Math.abs(w-toCompare.w)<tolerance;
	}
	public function negate():void {
		x=- x;
		y=- y;
		z=- z;
	}
	public function normalize():Number {
		var _len:Number=Math.sqrt(x*x+y*y+z*z);
		x/=_len;
		y/=_len;
		z/=_len;
		return _len;
	}
	public function project():void {
		x/=w;
		y/=w;
		z/=w;
	}
	public function scaleBy(s:Number):void {
		x*=s;
		y*=s;
		z*=s;
	}
	public function subtract(a:Vctr3D):Vctr3D {
		return new Vctr3D(x-a.x,y-a.y,z-a.z);
	}
	public function toString():String {
		return "Vctr3D("+x+", "+y+", "+z+")";
	}
	// おまけ
	public function isHiddenCrossProduct(a:Vctr3D):Boolean {
		return x*a.y-y*a.x>0;
	}
}

Mtrx3DとVctr3Dのつくり途中