

class IKTarget{
	constructor(){
		this.start_pos		= new Vec3();	// World Position start of an IK Chain
		this.end_pos		= new Vec3();	// The End Effector ( Target Position )
		this.axis 			= new Axis();	// Target Axis, Defines Forward and Up mainly.
		this.len_sqr		= 0;			// Distance Squared from Start to End, Faster to check lengths by Squared to avoid using Sqrt to get real lengths.
	}

	/////////////////////////////////////////////////////////////////////
	// GETTERS - SETTERS
	/////////////////////////////////////////////////////////////////////	
		/** Define the target based on a Start and End Position along with
			Up direction or the direction of the bend. */
		from_pos( pA, pB, up_dir ){
			this.start_pos.copy( pA );
			this.end_pos.copy( pB );

			this.len_sqr = this.axis.z.from_sub( pB, pA ).len_sqr();
			this.axis.from_dir( this.axis.z, up_dir );

			return this;
		}

		get_rot( out ){ return Quat.fromAxis( this.axis.x, this.axis.y, this.axis.z ); }


	/////////////////////////////////////////////////////////////////////
	// STATIC
	/////////////////////////////////////////////////////////////////////
		/** Visually see the Target information */
		static debug( d, t, scl=1.0 ){ 
			Axis.debug( d, t.axis, t.startPos, scl );
			d.point( t.start_pos, 6 ).point( t.end_pos, 0 );
			return this;
		}


	///////////////////////////////////////////////////////////////////
	// Single Bone Solvers
	///////////////////////////////////////////////////////////////////
		_aim_bone( chain, pose, wt, out ){
			/*
			The idea is to Aim the root bone in the direction of the target. Originally used a lookAt rotation 
			then correcting it to take in account the bone's points up, not forward.

			Instead, Build a rotation based on axis direction. Start by using target's fwd dir as the bone's up dir.
			To Help keep orientation, use the bone's Bind( or TPose ) world space fwd as a starting point to help get
			the left dir. With UP and Left, do another cross product for fwd to keep the axis orthogonal.

			This aims the limb pretty well at the target. The final step is to twist the limb so its joint (elbow, knee)
			is pointing at the UP dir of the target axis. This helps define how much twisting we need to apply to the bone.
			Arm and Knees tend to have different natural pose. The leg's fwd is fwd but the arm's fwd may be point down or back,
			all depends on how the rigging was setup.

			Since he bone is now aligned to the target, it shares the same Direction axis that we can then easily apply a twist
			rotation. The target's UP is final dir, so we take the lumb's aligning axis's world space dire and simply use 
			Quat.rotateTo( v1, v2 ). This function creates a rotation needed to get from One Vector dir to the other.
			*/
			
			let rot		= Quat.mul( wt.rot, pose.get_local_rot( chain.bones[0] ) ),	// Get World Space Rotation for Bone
				f_dir	= Vec3.transform_quat( Vec3.FORWARD, rot ),					// Get Bone's WS Forward Dir
				l_dir	= Vec3.cross( this.axis.z, f_dir ).norm();					// WS Left Dir

			f_dir.from_cross( l_dir, this.axis.z ).norm();							// Realign forward to keep axis orthogonal for proper rotation

			out.from_axis( l_dir, this.axis.z, f_dir );								// Final World Space Rotation
			if( Quat.dot( out, rot ) < 0 ) out.negate();							// If axis is point in the opposite direction of the bind rot, flip the signs : Thx @gszauer

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Need to apply a twist rotation to aim the bending joint 
			// (elbow,knee) in the direction of the IK Target UP Axis.

			let align_dir;
			switch( chain.ik_align_axis ){ // Arm/Legs have different Axis to align to Twisting.
				case "x": align_dir = l_dir; break;
				case "z": align_dir = f_dir; break;
			}

			// Shortest Twisting Direction
			if( Vec3.dot( align_dir, this.axis.y ) < 0 ) align_dir.invert();

			// Create and apply twist rotation.
			rot.from_unit_vecs( align_dir, this.axis.y );
			out.pmul( rot );

			return out;
		}

		aim( chain, bind_pose, pose, wt ){
			let rot = new Quat();

			this._aim_bone( chain, bind_pose, wt, rot );		
			
			//rot.pmul( Quat.invert( wt.rot ) );	// Convert to Bone's Local Space by mul invert of parent bone rotation
			rot.pmul_invert( wt.rot );
			pose.set_bone( chain.bones[ 0 ], rot );
		}

	///////////////////////////////////////////////////////////////////
	// Multi Bone Solvers
	///////////////////////////////////////////////////////////////////

		limb( chain, bind_pose, pose, wt ){
			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Using law of cos SSS, so need the length of all sides of the triangle
			let bone_a 	= bind_pose.bones[ chain.bones[0] ],
				bone_b	= bind_pose.bones[ chain.bones[1] ],
				aLen	= bone_a.len,
				bLen	= bone_b.len,
				cLen	= Math.sqrt( this.len_sqr ),
				wq 		= new Quat(),
				rot 	= new Quat(),	
				tmp		= new Quat(),
				rad;

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// FIRST BONE - Aim then rotate by the angle.
			this._aim_bone( chain, bind_pose, wt, rot );			// Aim the first bone toward the target oriented with the bend direction.
			
			rad	= lawcos_sss( aLen, cLen, bLen );					// Get the Angle between First Bone and Target.
			tmp.from_axis_angle( target.axis.x, -rad );				// Use the Target's X axis for rotation
			rot.pmul( tmp );										// Rotate the the aimed bone by the angle from SSS
			wq.copy( rot );											// Save a Copy as World Rotation before converting to local space

			tmp.from_invert( wt.rot ).mul( rot );					// Convert to Bone's Local Space by mul invert of parent bone rotation

			pose.set_bone( bone_a.idx, tmp );						// Save result to bone.

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// SECOND BONE
			// Need to rotate from Right to Left, So take the angle and subtract it from 180 to rotate from 
			// the other direction. Ex. L->R 70 degrees == R->L 110 degrees
			rad	= Math.PI - lawcos_sss( aLen, bLen, cLen );

			rot .from_mul( wq, bone_b.local.rot )					// Add Bone 2's Local Bind Rotation to Bone 1's new World Rotation.
				.pmul( tmp.from_axis_angle( target.axis.x, rad ) )	// Rotate it by the target's x-axis
				.pmul( tmp.from_invert( wq ) );						// Convert to Bone's Local Space

			pose.set_bone( bone_b.idx, rot );	
		}

		three_bone( chain, bind_pose, pose, wt ){
			//------------------------------------
			// TODO, Need a Target Length Limit Check. Possible Idea, use the longest bone as the min length.
			let bone_ratio 	= 0.5;
			let t_ratio		= null;

			let bone_a 	= bind_pose.bones[ chain.bones[0] ],
				bone_b	= bind_pose.bones[ chain.bones[1] ],
				bone_c	= bind_pose.bones[ chain.bones[2] ],
				t_len	= Math.sqrt( this.len_sqr ),
				a_len	= bone_a.len,
				c_len	= bone_c.len,
				ba_len 	= bone_b.len * bone_ratio,
				bc_len	= bone_b.len - ba_len,
				rot 	= new Quat(),
				tmp 	= new Quat(),
				wq		= new Quat(),
				rad;

			// How much of target length to use for the first 2 bones. For the 2nd and 3rd bone we can use 1-ratio.
			// We Take the full length of the first bone and only half of the second bone over total chain length
			t_ratio = t_ratio || ( a_len + ba_len ) / ( a_len + b_len + c_len );
			let ta_len = t_len * t_ratio,
				tb_len = t_len - ta_len;
				
			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Bone A 
			rad	= lawcos_sss( a_len, ta_len, ba_len );		// Get the Angle between First Bone and Target.

			this._aim_bone( chain, bind_pose, wt, rot );	// Aim the first bone toward the target oriented with the bend direction.
			rot.pmul_axis_angle( target.axis.x, -rad );		// Rotate the the aimed bone by the angle from SSS
			wq.copy( rot );									// Save a Copy as World Rotation for later use

			rot.pmul_invert( wt.rot );						// Convert to Bone's Local Space by mul invert of parent bone rotation
			pose.set_bone( bone_a.idx, rot );

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Bone B
			rad = Math.PI - lawcos_sss( a_len, ba_len, ta_len );

			rot .from_mul( wq, bone_b.local.rot )		// Add Bone Local to get its WS rot
				.pmul_axis_angle( this.axis.x, rad );	// Rotate it by the target's x-axis .pmul( tmp.from_axis_angle( this.axis.x, rad ) )

			tmp.from_invert( wq ).mul( rot );			// Convert to Local Space in temp to save WS rot for next bone.
			pose.set_bone( bone_b.idx, tmp );

			wq.copy( rot );								// Save to invert for next bone

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Bone C
			rad = Math.PI - lawcos_sss( c_len, bc_len, tb_len );

			//rot.mul( bone_c.local.rot );
			//	.pmul( tmp.from_axis_angle( target.axis.x, rad ) )	// Rotate it by the target's x-axis
			//	.pmul( tmp.from_invert( wq ) );						// Convert to Bone's Local Space

			rot	.mul( bone_c.local.rot );				// Still contains WS from previous bone, Add next bone's local
				.pmul_axis_angle( target.axis.x, rad )	// Rotate it by the target's x-axis
				.pmul_invert( wq );						// Convert to Bone's Local Space

			pose.set_bone( bone_c.idx, rot );

			//------------------------------------
			// Get the length of the bones, the calculate the ratio length for the bones based on the chain length
			// The 3 bones when placed in a zig-zag pattern creates a Parallelogram shape. We can break the shape down into two triangles
			// By using the ratio of the Target length divided between the 2 triangles, then using the first bone + half of the second bound
			// to solve for the top 2 joiints, then uing the half of the second bone + 3rd bone to solve for the bottom joint.
			// If all bones are equal length,  then we only need to use half of the target length and only test one triangle and use that for
			// both triangles, but if bones are uneven, then we need to solve an angle for each triangle which this function does.		
		}
}

		pmul_axis_angle( axis, angle ){
			let bx		= this[0],	
				by		= this[1],
				bz		= this[2],
				bw		= this[3],

				half	= angle * .5,
				s		= Math.sin( half ),
				ax		= axis[0] * s,	//New Quat based on axis and angle
				ay		= axis[1] * s, 
				az		= axis[2] * s,
				aw		= Math.cos( half );

			// Quat.mul( a, b );
			this[0]	= ax * bw + aw * bx + ay * bz - az * by;
			this[1]	= ay * bw + aw * by + az * bx - ax * bz;
			this[2]	= az * bw + aw * bz + ax * by - ay * bx;
			this[3]	= aw * bw - ax * bx - ay * by - az * bz;
			return this;
		}

		pmul_invert( q ){
			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// q.invert()
			let ax		= this[0],	
				ay		= this[1],
				az		= this[2],
				aw		= this[3],
				dot 	= ax*ax + ay*ay + az*az + aw*aw;

			if( dot == 0 ){
				ax = ay = az = aw = 0;
			}else{
				let dot_inv = 1.0 / dot;
				ax = -ax * dot_inv;
				ay = -ay * dot_inv;
				az = -az * dot_inv;
				aw =  aw * dot_inv;
			}

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Quat.mul( a, b );
			let bx		= this[0],	
				by		= this[1],
				bz		= this[2],
				bw		= this[3],
			this[0]	= ax * bw + aw * bx + ay * bz - az * by;
			this[1]	= ay * bw + aw * by + az * bx - ax * bz;
			this[2]	= az * bw + aw * bz + ax * by - ay * bx;
			this[3]	= aw * bw - ax * bx - ay * by - az * bz;
			return this;
		}

		from_invert( q ) {
			let dot	= q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2;

			// Would be faster to return [0,0,0,0] immediately if dot == 0
			if( dot == 0 ){ this[0] = this[1] = this[2] = this[3] = 0; return this; }

			let dot_inv = 1.0 / dot;
			this[0]	= -q[0] * dot_inv;
			this[1]	= -q[1] * dot_inv;
			this[2]	= -q[2] * dot_inv;
			this[3]	=  q[3] * dot_inv;
			return this;
		}


		function lawcos_sss( aLen, bLen, cLen ){
			// Law of Cosines - SSS : cos(C) = (a^2 + b^2 - c^2) / 2ab
			// The Angle between A and B with C being the opposite length of the angle.
			let v = ( aLen*aLen + bLen*bLen - cLen*cLen ) / ( 2 * aLen * bLen );
			if( v < -1 )		v = -1;	// Clamp to prevent NaN Errors
			else if( v > 1 )	v = 1;
			return Math.acos( v );
		}

		// SINGLE AXIS CONSTRAINT IDEA
		HingeConstraint( axis, min, max ){
			this.axis	= axis;
			this.min 	= min;
			this.max 	= max;
			this.w_min	= Math.cos( min * 0.5 );
			this.w_max	= Math.cos( max * 0.5 );

			apply( q ){
				if( q[3] > this.w_max ) return q.from_axis_angle( this.axis, this.max );
				if( q[3] < this.w_min ) return q.from_axis_angle( this.axis, this.min );

				// How to check for axis??
			}
		}

		getAxisAngle(){
			if(this[3] > 1) this.norm();

			let angle 	= 2 * Math.acos(this[3]),
				s		= Math.sqrt(1 - this[3] * this[3]);

			if(s < 0.001)  return [ 1 , 0 , 0, 0];

			return [ this[0] / s, this[1] / s, this[2] / s, angle ];
		}
		setAxisAngle( axis, angle ){ //AXIS MUST BE NORMALIZED.
			var halfAngle = angle * .5;
			var s = Math.sin(halfAngle);

			this[0] = axis[0] * s;
			this[1] = axis[1] * s;
			this[2] = axis[2] * s;
			this[3] = Math.cos(halfAngle);

			return this;
		}



class Chain{
	constructor( ik_align_axis="z" ){		
		this.bones			= [];			// Maybe Just Hold Bone Indexes, Will always Need Pose to Reference Info
		this.cnt			= 0;			// How many Bones in the chain
		this.len 			= 0;			// Chain Length
		this.len_sqr		= 0;			// Chain Length Squared, Cached for Checks without SQRT

		this.ik_align_axis	= ik_align_axis;
	}

	/////////////////////////////////////////////////////////////////////
	//
	/////////////////////////////////////////////////////////////////////
}

//#####################################################################
class Solver{
	///////////////////////////////////////////////////////////////////
	// Single Bone Solvers
	///////////////////////////////////////////////////////////////////

		static _aim_bone( chain, target, wt, out ){
			/*
			The idea is to Aim the root bone in the direction of the target. Originally used a lookAt rotation 
			then correcting it to take in account the bone's points up, not forward.

			Instead, Build a rotation based on axis direction. Start by using target's fwd dir as the bone's up dir.
			To Help keep orientation, use the bone's Bind( or TPose ) world space fwd as a starting point to help get
			the left dir. With UP and Left, do another cross product for fwd to keep the axis orthogonal.

			This aims the limb pretty well at the target. The final step is to twist the limb so its joint (elbow, knee)
			is pointing at the UP dir of the target axis. This helps define how much twisting we need to apply to the bone.
			Arm and Knees tend to have different natural pose. The leg's fwd is fwd but the arm's fwd may be point down or back,
			all depends on how the rigging was setup.

			Since he bone is now aligned to the target, it shares the same Direction axis that we can then easily apply a twist
			rotation. The target's UP is final dir, so we take the lumb's aligning axis's world space dire and simply use 
			Quat.rotateTo( v1, v2 ). This function creates a rotation needed to get from One Vector dir to the other.
			*/
			
			let rot = Quat.mul( wt.rot, chain.bind[ 0 ].rot ),	// Get World Space Rotation for Bone
				up	= target.axis.z,							// Main Direction, Point Bone up toward axis forward.
				fwd	= Vec3.transformQuat( Vec3.FORWARD, rot ),	// Get Bone's World Space Forward Direction.
				lft	= Vec3.cross( up, fwd ).norm();		// Figure out World Space Left Direction
			
			Vec3.cross( lft, up, fwd ).norm();				// Realign forward to keep axis orthogonal for proper rotation

			Quat.fromAxis( lft, up, fwd, out );					// Final World Space Rotation

			if( Quat.dot( out, rot ) < 0 ) out.negate();		// If axis is point in the opposite direction of the bind rot, flip the signs : Thx @gszauer

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Need to apply a twist rotation to aim the bending joint 
			// (elbow,knee) in the direction of the IK Target UP Axis.

			let alignDir;
			switch( chain.ikAlignAxis ){ // Arm/Legs have different Axis to align to Twisting.
				case "x": alignDir = lft.clone(); break;
				case "z": alignDir = fwd.clone(); break;
			}

			// Shortest Twisting Direction
			if( Vec3.dot( alignDir, target.axis.y ) < 0 ) alignDir.invert();

			// Create and apply twist rotation.
			let q = Quat.rotationTo( alignDir, target.axis.y ); 
			//rad = Vec3.angle( alignDir, target.axis.y ),		// Angle between
			out.pmul( q );

			return out;
		}

		static aim( chain, target, pose, wt ){
			let rot = new Quat();

			this._aim_bone( chain, target, wt, rot );		

			rot.pmul( Quat.invert( wt.rot ) );	// Convert to Bone's Local Space by mul invert of parent bone rotation

			if( pose )	pose.updateBone( chain.idx[ 0 ], rot );
			else		chain.updateBone( 0, rot );
		}


	///////////////////////////////////////////////////////////////////
	// Multi Bone Solvers
	///////////////////////////////////////////////////////////////////

		static limb( chain, target, pose, wt ){
			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// Using law of cos SSS, so need the length of all sides of the triangle
			let aLen	= chain.lens[ 0 ],
				bLen	= chain.lens[ 1 ],
				cLen	= Math.sqrt( target.distanceSqr ),
				wq 		= new Quat(),
				rot 	= new Quat(),	
				tmp		= new Quat(),
				rad;

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// FIRST BONE - Aim then rotate by the angle.
			this._aim_bone( chain, target, wt, rot );				// Aim the first bone toward the target oriented with the bend direction.
			
			rad	= Maths.lawcos_sss( aLen, cLen, bLen );				// Get the Angle between First Bone and Target.
			tmp.setAxisAngle( target.axis.x, -rad );				// Use the Target's X axis for rotation
			rot.pmul( tmp );										// Rotate the the aimed bone by the angle from SSS
			wq.copy( rot );											// Save a Copy as World Rotation before converting to local space

			rot.pmul( wt.rot.invert( tmp ) );						// Convert to Bone's Local Space by mul invert of parent bone rotation

			if( pose )	pose.updateBone( chain.idx[ 0 ], rot );		// Save result to bone.
			else		chain.updateBone( 0, rot );

			//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
			// SECOND BONE
			// Need to rotate from Right to Left, So take the angle and subtract it from 180 to rotate from 
			// the other direction. Ex. L->R 70 degrees == R->L 110 degrees
			rad	= Math.PI - Maths.lawcos_sss( aLen, bLen, cLen );

			Quat.mul( wq, chain.bind[ 1 ].rot, rot ) 				// Add Bone 2's Local Bind Rotation to Bone 1's new World Rotation.
				.pmul( tmp.setAxisAngle( target.axis.x, rad ) )		// Rotate it by the target's x-axis
				.pmul( wq.invert( tmp ) );							// Convert to Bone's Local Space

			if( pose )	pose.updateBone( chain.idx[ 1 ], rot );
			else		chain.updateBone( 1, rot );
		}

}



static threeBone(chain, target, pose){
	//------------------------------------
	//If the scale is near or over 1, do full extenction of chain.
	if(target.scale > 0.9999){ IKChainSolver.fullDirection( chain, target, pose, true ); return; }


	//------------------------------------
	let chainPosition 	= TNode.getWorldPosition( chain.links[0] ),
		scale			= target.scale;

	if(!scale){
		scale = chainPosition.length( target.position ) / chain.length;

		//Check scale, if over length of chain, then straighten it out.
		if(scale > 0.9999){ IKChainSolver.fullDirection(chain, target, pose); return; }
	}
	
	//------------------------------------
	// TODO, Need a Target Length Limit Check. Possible Idea, use the longest bone as the min length.


	//------------------------------------
	// Get the length of the bones, the calculate the ratio length for the bones based on the chain length
	// The 3 bones when placed in a zig-zag pattern creates a Parallelogram shape. We can break the shape down into two triangles
	// By using the ratio of the Target length divided between the 2 triangles, then using the first bone + half of the second bound
	// to solve for the top 2 joiints, then uing the half of the second bone + 3rd bone to solve for the bottom joint.
	// If all bones are equal length,  then we only need to use half of the target length and only test one triangle and use that for
	// both triangles, but if bones are uneven, then we need to solve an angle for each triangle which this function does.

	let b0Len = chain.links[0].com.Bone.length,
		b1Len = chain.links[1].com.Bone.length,
		b2Len = chain.links[2].com.Bone.length,
			// How much of target length to use for the first 2 bones
			// For the 2nd and 3rd bone we can use 1-ratio.
		ratio = ( b0Len + b1Len * 0.5 ) / ( b0Len + b1Len + b2Len ),

		targetLength = chain.length * scale;

	//------------------------------------
	// Law of Cosines - SSS
	// cos(C) = (a^2 + b^2 - c^2) / 2ab
	// Angle for B0 B1
	let bLen	= b1Len * 0.5,
		aLen	= b0Len,
		cLen	= targetLength * ratio,
		n 		= Maths.clamp( (aLen*aLen + bLen*bLen - cLen*cLen) / (2 * aLen * bLen), -1, 1 ), // Prevent NaN Error
		angleA	= Math.PI - Math.acos( n );

	// Angle for B1 B2
	aLen	= b2Len;
	cLen	= targetLength * (1 - ratio);
	n		= Maths.clamp( (aLen*aLen + bLen*bLen - cLen*cLen) / (2 * aLen * bLen), -1, 1 );  // Prevent NaN Error
	let angleB	= Math.PI - Math.acos( n );


	//------------------------------------
	//Apply the angle
	IKChainSolver.aimDirection(chain, target, pose); //Align root bone in the direction of target z axis

	pose.links[1].rotation.setAxisAngle(Vec3.LEFT, angleA);		//.pmul( q.setAxisAngle(Vec3.LEFT, angleA) );
	pose.links[1].useRotation = true;

	pose.links[2].rotation.setAxisAngle(Vec3.LEFT, -angleB);	//.pmul( q.setAxisAngle(Vec3.LEFT, -angleB) );
	pose.links[2].useRotation = true;


	//------------------------------------
	//The end position usually doesn't line up with the target, so try to align it
	let endPosition = IKChainSolver.chainEndPosition( chain, pose, chainPosition );	// world position of end of chain
	IKChainSolver.alignFwdRotation(chain, target, pose, endPosition.sub(chainPosition).normalize());
}



//#####################################################################
class Axis{
	constructor(){
		this.x = new Vec3( Vec3.LEFT );
		this.y = new Vec3( Vec3.UP );
		this.z = new Vec3( Vec3.FORWARD );
	}

	////////////////////////////////////////////////////////////////////
	// SETTERS / GETTERS
	////////////////////////////////////////////////////////////////////
		//Passing in Vectors.
		set( x, y, z, do_norm = false ){
			this.x.copy( x );
			this.y.copy( y );
			this.z.copy( z );

			if(do_norm){
				this.x.norm();
				this.y.norm();
				this.z.norm();
			}
			return this;
		}

		//Create axis based on a Quaternion
		to_quat( out ){ return (out || new Quat()).from_axis( this.x, this.y, this.z ); }
		from_quat( q ){
			// Same code for Quat to Matrix 3 conversion
			let x = q[0], y = q[1], z = q[2], w = q[3],
				x2 = x + x,
				y2 = y + y,
				z2 = z + z,
				xx = x * x2,
				yx = y * x2,
				yy = y * y2,
				zx = z * x2,
				zy = z * y2,
				zz = z * z2,
				wx = w * x2,
				wy = w * y2,
				wz = w * z2;

			this.x.set( 1 - yy - zz,	yx + wz,		zx - wy );
			this.y.set( yx - wz,		1 - xx - zz,	zy + wx );
			this.z.set( zx + wy,		zy - wx,		1 - xx - yy );
			return this;
		}

		//Create an axis based on a looking direction
		/*
		lookAt( v ){
			//If the vector is pretty much forward, just do set default axis.
			var test = Vec3.nearZero( v );
			if(test.x == 0 && test.y == 0){
				this.z.copy( Vec3.FORWARD );
				this.y.copy( Vec3.UP );
				this.x.copy( Vec3.LEFT );
				return this;
			}

			Vec3.norm(v, this.z);	//Direction is the forward vector
			let dot = Vec3.dot(Vec3.LEFT, this.z);

			// If dot product is in the negative, need to use BACK (0,0,-1) 
			// to get the correct up direction, else up will point down.
			if(dot >= 0)	Vec3.cross(Vec3.FORWARD, this.z, this.y).normalize();
			else			Vec3.cross(Vec3.BACK, this.z, this.y).normalize();

			Vec3.cross(this.z, this.y, this.x).normalize();
			return this;
		}
		*/

		from_dir( fwd, up ){
			this.z.copy( fwd ).norm();
			this.x.from_cross( up, this.z ).norm();
			this.y.from_cross( this.z, this.x ).norm();			
			return this;
		}


	////////////////////////////////////////////////////////////////////
	//  
	////////////////////////////////////////////////////////////////////

		// Axis is pretty much a Rotation Matrix, so easy to apply rotation to a vector.
		transform_vec3( v, out ){
			let x = v[0], y = v[1], z = v[2];
			
			out = out || new Vec3();
			out[0] = x * this.x[0] + y * this.y[0] + z * this.z[0];
			out[1] = x * this.x[1] + y * this.y[1] + z * this.z[1];
			out[2] = x * this.x[2] + y * this.y[2] + z * this.z[2];
			return out;
		}

		rotate( rad, axis = "x", out = null ){
			out = out || this;

			let sin = Math.sin(rad),
				cos = Math.cos(rad),
				x, y, z;

			switch(axis){
				case "y": //..........................
					x = this.x[0];	z = this.x[2];
					this.x[0]	= z*sin + x*cos; //x
					this.x[2]	= z*cos - x*sin; //z

					x = this.z[0];	z = this.z[2];
					this.z[0]	= z*sin + x*cos; //x
					this.z[2]	= z*cos - x*sin; //z
				break;
				case "x": //..........................
					y = this.y[1];	z = this.y[2];
					this.y[1]	= y*cos - z*sin; //y
					this.y[2]	= y*sin + z*cos; //z

					y = this.z[1];	z = this.z[2];
					this.z[1]	= y*cos - z*sin; //y
					this.z[2]	= y*sin + z*cos; //z
				break;
				case "z": //..........................
					x = this.x[0];	y = this.x[1];
					this.x[0]	= x*cos - y*sin; //x
					this.x[1]	= x*sin + y*cos; //y

					x = this.y[0];	y = this.y[1];
					this.y[0]	= x*cos - y*sin; //x
					this.y[1]	= x*sin + y*cos; //y
				break;
			}

			return out;
		}



	////////////////////////////////////////////////////////////////////
	// STATIC
	////////////////////////////////////////////////////////////////////
		
		static debug( o, axis, origin=null, scl=1.0 ){
			origin = origin || Vec3.ZERO;

			var v = new Vec3();
			v.copy( axis.z ).scale( scl ).add( origin );
			o.line( origin, v, 1 );

			v.copy( axis.y ).scale( scl ).add( origin );
			o.line( origin, v, 2 );

			v.copy( axis.x ).scale( scl ).add( origin );
			o.line(origin, v, 0);
		}
}