Merge remote-tracking branch 'WestLangley/dev-rotations2' into dev

src/cameras/Camera.js

@@ -25,7 +25,7 @@ THREE.Camera.prototype.lookAt = function ( vector ) {
 
 	if ( this.rotationAutoUpdate ) {
 
-		this.rotation.getRotationFromMatrix( this.matrix );
+		this.rotation.setEulerFromRotationMatrix( this.matrix, this.order );
 
 	}
 

src/core/Object3D.js

@@ -61,7 +61,10 @@ THREE.Object3D.prototype = {
 		this.matrix.multiply( matrix, this.matrix );
 
 		this.scale.getScaleFromMatrix( this.matrix );
-		this.rotation.getRotationFromMatrix( this.matrix, this.scale );
+		
+		var mat = new THREE.Matrix4().extractRotation( this.matrix );
+		this.rotation.setEulerFromRotationMatrix( mat, this.eulerOrder );
+		
 		this.position.getPositionFromMatrix( this.matrix );
 
 	},
@@ -99,7 +102,7 @@ THREE.Object3D.prototype = {
 
 		if ( this.rotationAutoUpdate ) {
 
-			this.rotation.getRotationFromMatrix( this.matrix );
+			this.rotation.setEulerFromRotationMatrix( this.matrix, this.eulerOrder );
 
 		}
 

src/core/Quaternion.js

@@ -1,6 +1,7 @@
 /**
  * @author mikael emtinger / http://gomo.se/
  * @author alteredq / http://alteredqualia.com/
+ * @author WestLangley / http://github.com/WestLangley
  */
 
 THREE.Quaternion = function( x, y, z, w ) {
@@ -38,28 +39,79 @@ THREE.Quaternion.prototype = {
 
 	},
 
-	setFromEuler: function ( vector ) {
-
-		var c = Math.PI / 360, // 0.5 * Math.PI / 360, // 0.5 is an optimization
-		x = vector.x * c,
-		y = vector.y * c,
-		z = vector.z * c,
-
-		c1 = Math.cos( y  ),
-		s1 = Math.sin( y  ),
-		c2 = Math.cos( -z ),
-		s2 = Math.sin( -z ),
-		c3 = Math.cos( x  ),
-		s3 = Math.sin( x  ),
-
-		c1c2 = c1 * c2,
-		s1s2 = s1 * s2;
-
-		this.w = c1c2 * c3  - s1s2 * s3;
-	  	this.x = c1c2 * s3  + s1s2 * c3;
-		this.y = s1 * c2 * c3 + c1 * s2 * s3;
-		this.z = c1 * s2 * c3 - s1 * c2 * s3;
-
+	setFromEuler: function ( v, order ) {
+
+		// http://www.mathworks.com/matlabcentral/fileexchange/
+		// 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
+		//	content/SpinCalc.m
+	
+		var _order = order || 'XYZ',
+		
+			c1 = Math.cos( v.x / 2 ),
+			c2 = Math.cos( v.y / 2 ),
+			c3 = Math.cos( v.z / 2 ),
+			s1 = Math.sin( v.x / 2 ),
+			s2 = Math.sin( v.y / 2 ),
+			s3 = Math.sin( v.z / 2 );
+				
+		switch ( _order ) {
+	
+			case 'YXZ':
+	
+				this.x = s1 * c2 * c3 + c1 * s2 * s3;
+				this.y = c1 * s2 * c3 - s1 * c2 * s3;
+				this.z = c1 * c2 * s3 - s1 * s2 * c3;
+				this.w = c1 * c2 * c3 + s1 * s2 * s3;
+	
+				break;
+				
+			case 'ZXY':
+	
+				this.x = s1 * c2 * c3 - c1 * s2 * s3;
+				this.y = c1 * s2 * c3 + s1 * c2 * s3;
+				this.z = c1 * c2 * s3 + s1 * s2 * c3;
+				this.w = c1 * c2 * c3 - s1 * s2 * s3;
+	
+				break;
+				
+			case 'ZYX':
+	
+				this.x = s1 * c2 * c3 - c1 * s2 * s3;
+				this.y = c1 * s2 * c3 + s1 * c2 * s3;
+				this.z = c1 * c2 * s3 - s1 * s2 * c3;
+				this.w = c1 * c2 * c3 + s1 * s2 * s3;
+	
+				break;
+				
+			case 'YZX':
+			
+				this.x = s1 * c2 * c3 + c1 * s2 * s3;
+				this.y = c1 * s2 * c3 + s1 * c2 * s3;
+				this.z = c1 * c2 * s3 - s1 * s2 * c3;
+				this.w = c1 * c2 * c3 - s1 * s2 * s3;
+	
+				break;
+				
+			case 'XZY':
+			
+				this.x = s1 * c2 * c3 - c1 * s2 * s3;
+				this.y = c1 * s2 * c3 - s1 * c2 * s3;
+				this.z = c1 * c2 * s3 + s1 * s2 * c3;
+				this.w = c1 * c2 * c3 + s1 * s2 * s3;
+	
+				break;
+				
+			default: // 'XYZ'
+	
+				this.x = s1 * c2 * c3 + c1 * s2 * s3;
+				this.y = c1 * s2 * c3 - s1 * c2 * s3;
+				this.z = c1 * c2 * s3 + s1 * s2 * c3;
+				this.w = c1 * c2 * c3 - s1 * s2 * s3;
+		
+				break;
+				
+		}
+		
 		return this;
 
 	},
@@ -83,24 +135,57 @@ THREE.Quaternion.prototype = {
 
 	setFromRotationMatrix: function ( m ) {
 
-		// Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
-
-		function copySign( a, b ) {
-
-			return b < 0 ? -Math.abs( a ) : Math.abs( a );
-
+		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
+		
+		// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
+		
+		var te = m.elements,
+			
+			m11 = te[0], m12 = te[4], m13 = te[8],
+			m21 = te[1], m22 = te[5], m23 = te[9],
+			m31 = te[2], m32 = te[6], m33 = te[10],
+			
+			trace = m11 + m22 + m33,
+			s;
+		
+		if( trace > 0 ) {
+		
+			s = 0.5 / Math.sqrt( trace + 1.0 );
+			
+			this.w = 0.25 / s;
+			this.x = ( m32 - m23 ) * s;
+			this.y = ( m13 - m31 ) * s;
+			this.z = ( m21 - m12 ) * s;
+		
+		} else if ( m11 > m22 && m11 > m33 ) {
+		
+			s = 2.0 * Math.sqrt( 1.0 + m11 - m22 - m33 );
+			
+			this.w = (m32 - m23 ) / s;
+			this.x = 0.25 * s;
+			this.y = (m12 + m21 ) / s;
+			this.z = (m13 + m31 ) / s;
+		
+		} else if (m22 > m33) {
+		
+			s = 2.0 * Math.sqrt( 1.0 + m22 - m11 - m33 );
+			
+			this.w = (m13 - m31 ) / s;
+			this.x = (m12 + m21 ) / s;
+			this.y = 0.25 * s;
+			this.z = (m23 + m32 ) / s;
+		
+		} else {
+		
+			s = 2.0 * Math.sqrt( 1.0 + m33 - m11 - m22 );
+			
+			this.w = ( m21 - m12 ) / s;
+			this.x = ( m13 + m31 ) / s;
+			this.y = ( m23 + m32 ) / s;
+			this.z = 0.25 * s;
+		
 		}
-
-		var absQ = Math.pow( m.determinant(), 1.0 / 3.0 );
-		this.w = Math.sqrt( Math.max( 0, absQ + m.elements[0] + m.elements[5] + m.elements[10] ) ) / 2;
-		this.x = Math.sqrt( Math.max( 0, absQ + m.elements[0] - m.elements[5] - m.elements[10] ) ) / 2;
-		this.y = Math.sqrt( Math.max( 0, absQ - m.elements[0] + m.elements[5] - m.elements[10] ) ) / 2;
-		this.z = Math.sqrt( Math.max( 0, absQ - m.elements[0] - m.elements[5] + m.elements[10] ) ) / 2;
-		this.x = copySign( this.x, ( m.elements[6] - m.elements[9] ) );
-		this.y = copySign( this.y, ( m.elements[8] - m.elements[2] ) );
-		this.z = copySign( this.z, ( m.elements[1] - m.elements[4] ) );
-		this.normalize();
-
+	
 		return this;
 
 	},

src/core/Vector3.js

@@ -4,6 +4,7 @@
  * @author philogb / http://blog.thejit.org/
  * @author mikael emtinger / http://gomo.se/
  * @author egraether / http://egraether.com/
+ * @author WestLangley / http://github.com/WestLangley
  */
 
 THREE.Vector3 = function ( x, y, z ) {
@@ -270,89 +271,207 @@ THREE.Vector3.prototype = {
 
 	},
 
-	getRotationFromMatrix: function ( m, scale ) {
-
-		var sx = scale ? scale.x : 1;
-		var sy = scale ? scale.y : 1;
-		var sz = scale ? scale.z : 1;
-
-		var m11 = m.elements[0] / sx, m12 = m.elements[4] / sy, m13 = m.elements[8] / sz;
-		var m21 = m.elements[1] / sx, m22 = m.elements[5] / sy, m23 = m.elements[9] / sz;
-		var m33 = m.elements[10] / sz;
-
-		this.y = Math.asin( m13 );
-
-		var cosY = Math.cos( this.y );
-
-		if ( Math.abs( cosY ) > 0.00001 ) {
-
-			this.x = Math.atan2( - m23 / cosY, m33 / cosY );
-			this.z = Math.atan2( - m12 / cosY, m11 / cosY );
-
-		} else {
-
-			this.x = 0;
-			this.z = Math.atan2( m21, m22 );
-
+	setEulerFromRotationMatrix: function ( m, order ) {
+
+		// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
+	
+		// clamp, to handle numerical problems
+		function clamp( x ) { return Math.min( Math.max( x, -1 ), 1 ) };
+		
+		var _order = order || 'XYZ',
+		
+			te = m.elements,
+			
+			m11 = te[0], m12 = te[4], m13 = te[8],
+			m21 = te[1], m22 = te[5], m23 = te[9],
+			m31 = te[2], m32 = te[6], m33 = te[10];
+	
+		switch ( _order ) {
+	
+			case 'YXZ':
+				
+				this.x = Math.asin( - clamp( m23 ) );
+			
+				if ( Math.abs( m23 ) < 0.99999 ) {
+				
+					this.y = Math.atan2( m13, m33 );
+					this.z = Math.atan2( m21, m22 );
+					
+				} else {
+				
+					this.y = Math.atan2( - m31, m11 );
+					this.z = 0;
+					
+				}
+				
+				break;
+				
+			case 'ZXY':
+	
+				this.x = Math.asin( clamp( m32 ) );
+			
+				if ( Math.abs( m32 ) < 0.99999 ) {
+				
+					this.y = Math.atan2( - m31, m33 );
+					this.z = Math.atan2( - m12, m22 );
+					
+				} else {
+					
+					this.y = 0;
+					this.z = Math.atan2( m13, m11 );
+					
+				}
+				
+				break;
+				
+			case 'ZYX':
+	
+				this.y = Math.asin( - clamp( m31 ) );
+				
+				if ( Math.abs( m31 ) < 0.99999 ) {
+				
+					this.x = Math.atan2( m32, m33 );
+					this.z = Math.atan2( m21, m11 );
+					
+				} else {
+				
+					this.x = 0;
+					this.z = Math.atan2( - m12, m22 );
+					
+				}
+				
+				break;
+				
+			case 'YZX':
+			
+				this.z = Math.asin( clamp( m21 ) );
+			
+				if ( Math.abs( m21 ) < 0.99999 ) {
+			
+					this.x = Math.atan2( - m23, m22 );
+					this.y = Math.atan2( - m31, m11 );
+			
+				} else {
+				
+					this.x = 0;
+					this.y = Math.atan2( m31, m33 );
+			
+				}
+				
+				break;
+				
+			case 'XZY':
+			
+				this.z = Math.asin( - clamp( m12 ) );
+			
+				if ( Math.abs( m12 ) < 0.99999 ) {
+			
+					this.x = Math.atan2( m32, m22 );
+					this.y = Math.atan2( m13, m11 );
+			
+				} else {
+				
+					this.x = Math.atan2( - m13, m33 );
+					this.y = 0;
+			
+				}
+				
+				break;
+				
+			default: // 'XYZ'
+	
+				this.y = Math.asin( clamp( m13 ) );
+			
+				if ( Math.abs( m13 ) < 0.99999 ) {
+			
+					this.x = Math.atan2( - m23, m33 );
+					this.z = Math.atan2( - m12, m11 );
+			
+				} else {
+				
+					this.x = Math.atan2( m21, m22 );
+					this.z = 0;
+			
+				}
+				
+				break;
+				
 		}
-
+		
 		return this;
 
 	},
 
-	/*
-
-	// from http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m
-	// order XYZ
-
-	getEulerXYZFromQuaternion: function ( q ) {
-
-		this.x = Math.atan2( 2 * ( q.x * q.w - q.y * q.z ), ( q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z ) );
-		this.y = Math.asin( 2 *  ( q.x * q.z + q.y * q.w ) );
-		this.z = Math.atan2( 2 * ( q.z * q.w - q.x * q.y ), ( q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z ) );
-
-	},
-
-	// from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
-	// order YZX (assuming heading == y, attitude == z, bank == x)
-
-	getEulerYZXFromQuaternion: function ( q ) {
-
-		var sqw = q.w * q.w;
-		var sqx = q.x * q.x;
-		var sqy = q.y * q.y;
-		var sqz = q.z * q.z;
-		var unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
-		var test = q.x * q.y + q.z * q.w;
-
-		if ( test > 0.499 * unit ) { // singularity at north pole
-
-			this.y = 2 * Math.atan2( q.x, q.w );
-			this.z = Math.PI / 2;
-			this.x = 0;
-
-			return;
-
+	setEulerFromQuaternion: function ( q, order ) {
+
+		// http://www.mathworks.com/matlabcentral/fileexchange/
+		// 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
+		//	content/SpinCalc.m
+	
+		// q is assumed to be normalized
+	
+		// clamp, to handle numerical problems
+		function clamp( x ) { return Math.min( Math.max( x, -1 ), 1 ) };
+	
+		var sqx = q.x * q.x,
+			sqy = q.y * q.y,
+			sqz = q.z * q.z,
+			sqw = q.w * q.w;
+	
+		switch ( order ) {
+	
+			case 'YXZ':
+	
+				this.x = Math.asin(  clamp( 2 * ( q.x * q.w - q.y * q.z ) ) );
+				this.y = Math.atan2( 2 * ( q.x * q.z + q.y * q.w ), ( sqw - sqx - sqy + sqz ) );
+				this.z = Math.atan2( 2 * ( q.x * q.y + q.z * q.w ), ( sqw - sqx + sqy - sqz ) );
+				
+				break;
+	
+			case 'ZXY':
+			
+				this.x = Math.asin(  clamp( 2 * ( q.x * q.w + q.y * q.z ) ) );
+				this.y = Math.atan2( 2 * ( q.y * q.w - q.z * q.x ), ( sqw - sqx - sqy + sqz ) );
+				this.z = Math.atan2( 2 * ( q.z * q.w - q.x * q.y ), ( sqw - sqx + sqy - sqz ) );
+				
+				break;
+			
+			case 'ZYX':
+			
+				this.x = Math.atan2( 2 * ( q.x * q.w + q.z * q.y ), ( sqw - sqx - sqy + sqz ) );
+				this.y = Math.asin(  clamp( 2 * ( q.y * q.w - q.x * q.z ) ) );
+				this.z = Math.atan2( 2 * ( q.x * q.y + q.z * q.w ), ( sqw + sqx - sqy - sqz ) );
+				
+				break;
+	
+			case 'YZX':
+			
+				this.x = Math.atan2( 2 * ( q.x * q.w - q.z * q.y ), ( sqw - sqx + sqy - sqz ) );
+				this.y = Math.atan2( 2 * ( q.y * q.w - q.x * q.z ), ( sqw + sqx - sqy - sqz ) );
+				this.z = Math.asin(  clamp( 2 * ( q.x * q.y + q.z * q.w ) ) );
+				
+				break;
+			
+			case 'XZY':
+			
+				this.x = Math.atan2( 2 * ( q.x * q.w + q.y * q.z ), ( sqw - sqx + sqy - sqz ) );
+				this.y = Math.atan2( 2 * ( q.x * q.z + q.y * q.w ), ( sqw + sqx - sqy - sqz ) );
+				this.z = Math.asin(  clamp( 2 * ( q.z * q.w - q.x * q.y ) ) );
+				
+				break;
+	
+			default: // 'XYZ'
+	
+				this.x = Math.atan2( 2 * ( q.x * q.w - q.y * q.z ), ( sqw - sqx - sqy + sqz ) );
+				this.y = Math.asin(  clamp( 2 * ( q.x * q.z + q.y * q.w ) ) );
+				this.z = Math.atan2( 2 * ( q.z * q.w - q.x * q.y ), ( sqw + sqx - sqy - sqz ) );
+	
 		}
-
-		if ( test < -0.499 * unit ) { // singularity at south pole
-
-			this.y = -2 * Math.atan2( q.x, q.w );
-			this.z = -Math.PI / 2;
-			this.x = 0;
-
-			return;
-
-		}
-
-		this.y = Math.atan2( 2 * q.y * q.w - 2 * q.x * q.z, sqx - sqy - sqz + sqw );
-		this.z = Math.asin( 2 * test / unit );
-		this.x = Math.atan2( 2 * q.x * q.w - 2 * q.y * q.z, -sqx + sqy - sqz + sqw );
+		
+		return this;
 
 	},
 
-	*/
-
 	getScaleFromMatrix: function ( m ) {
 
 		var sx = this.set( m.elements[0], m.elements[1], m.elements[2] ).length();

src/core/Vector4.js

@@ -3,6 +3,7 @@
  * @author philogb / http://blog.thejit.org/
  * @author mikael emtinger / http://gomo.se/
  * @author egraether / http://egraether.com/
+ * @author WestLangley / http://github.com/WestLangley
  */
 
 THREE.Vector4 = function ( x, y, z, w ) {
@@ -169,6 +170,158 @@ THREE.Vector4.prototype = {
 
 		return new THREE.Vector4( this.x, this.y, this.z, this.w );
 
+	},
+
+	setAxisAngleFromQuaternion: function ( q ) {
+
+		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
+		
+		// q is assumed to be normalized
+		
+		this.w = 2 * Math.acos( q.w );
+		
+		var s = Math.sqrt( 1 - q.w * q.w );
+		
+		if ( s < 0.0001 ) {
+		
+			 this.x = 1;
+			 this.y = 0;
+			 this.z = 0;
+		 
+		} else {
+		
+			 this.x = q.x / s;
+			 this.y = q.y / s;
+			 this.z = q.z / s;
+		 
+		}
+	
+		return this;
+
+	},
+
+	setAxisAngleFromRotationMatrix: function ( m ) {
+
+		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
+		
+		// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
+		
+		var angle, x, y, z,		// variables for result
+			epsilon = 0.01,		// margin to allow for rounding errors
+			epsilon2 = 0.1,		// margin to distinguish between 0 and 180 degrees
+		
+			te = m.elements,
+			
+			m11 = te[0], m12 = te[4], m13 = te[8],
+			m21 = te[1], m22 = te[5], m23 = te[9],
+			m31 = te[2], m32 = te[6], m33 = te[10];
+
+		if ( ( Math.abs( m12 - m21 ) < epsilon )
+		  && ( Math.abs( m13 - m31 ) < epsilon )
+		  && ( Math.abs( m23 - m32 ) < epsilon ) ) {
+			
+			// singularity found
+			// first check for identity matrix which must have +1 for all terms
+			// in leading diagonal and zero in other terms
+			
+			if ( ( Math.abs( m12 + m21 ) < epsilon2 )
+			  && ( Math.abs( m13 + m31 ) < epsilon2 )
+			  && ( Math.abs( m23 + m32 ) < epsilon2 )
+			  && ( Math.abs( m11 + m22 + m33 - 3 ) < epsilon2 ) ) {
+				
+				// this singularity is identity matrix so angle = 0
+				
+				this.set( 1, 0, 0, 0 );
+				
+				return this; // zero angle, arbitrary axis
+				
+			}
+			
+			// otherwise this singularity is angle = 180
+			
+			angle = Math.PI;
+			
+			var xx = ( m11 + 1 ) / 2;
+			var yy = ( m22 + 1 ) / 2;
+			var zz = ( m33 + 1 ) / 2;
+			var xy = ( m12 + m21 ) / 4;
+			var xz = ( m13 + m31 ) / 4;
+			var yz = ( m23 + m32 ) / 4;
+			
+			if ( ( xx > yy ) && ( xx > zz ) ) { // m11 is the largest diagonal term
+			
+				if ( xx < epsilon ) {
+				
+					x = 0;
+					y = 0.707106781;
+					z = 0.707106781;
+					
+				} else {
+				
+					x = Math.sqrt( xx );
+					y = xy / x;
+					z = xz / x;
+					
+				}
+				
+			} else if ( yy > zz ) { // m22 is the largest diagonal term
+			
+				if ( yy < epsilon ) {
+				
+					x = 0.707106781;
+					y = 0;
+					z = 0.707106781;
+					
+				} else {
+				
+					y = Math.sqrt( yy );
+					x = xy / y;
+					z = yz / y;
+					
+				}	
+				
+			} else { // m33 is the largest diagonal term so base result on this
+			
+				if ( zz < epsilon ) {
+				
+					x = 0.707106781;
+					y = 0.707106781;
+					z = 0;
+					
+				} else {
+				
+					z = Math.sqrt( zz );
+					x = xz / z;
+					y = yz / z;
+					
+				}
+				
+			}
+			
+			this.set( x, y, z, angle );
+			
+			return this; // return 180 deg rotation
+	
+		}
+		
+		// as we have reached here there are no singularities so we can handle normally
+		
+		var s = Math.sqrt( ( m32 - m23 ) * ( m32 - m23 )
+						 + ( m13 - m31 ) * ( m13 - m31 )
+						 + ( m21 - m12 ) * ( m21 - m12 ) ); // used to normalize
+			
+		if ( Math.abs( s ) < 0.001 ) s = 1; 
+		
+		// prevent divide by zero, should not happen if matrix is orthogonal and should be
+		// caught by singularity test above, but I've left it in just in case
+		
+		this.x = ( m32 - m23 ) / s;
+		this.y = ( m13 - m31 ) / s;
+		this.z = ( m21 - m12 ) / s;
+		this.w = Math.acos( ( m11 + m22 + m33 - 1 ) / 2 );
+	
+		return this;
+
 	}
 
 };

src/extras/helpers/ArrowHelper.js

@@ -1,5 +1,5 @@
 /**
- * @author WestLangley / https://github.com/WestLangley
+ * @author WestLangley / http://github.com/WestLangley
  * @author zz85 / https://github.com/zz85
  *
  * Creates an arrow for visualizing directions
@@ -49,7 +49,7 @@ THREE.ArrowHelper.prototype.setDirection = function ( dir ) {
 
 	this.matrix = new THREE.Matrix4().makeRotationAxis( axis.normalize(), radians );
 
-	this.rotation.getRotationFromMatrix( this.matrix, this.scale );
+	this.rotation.setEulerFromRotationMatrix( this.matrix, this.eulerOrder );
 
 };