/src/away3d/core/math/Vector3DUtils.as

  1package away3d.core.math
  2{
  3	import flash.geom.*;
  4	
  5	/**
  6	 * Vector3DUtils provides additional Vector3D math functions.
  7	 */
  8	public class Vector3DUtils
  9	{
 10		private static const MathPI:Number = Math.PI;
 11		
 12		/**
 13		 * Returns the angle in radians made between the 3d number obejct and the given <code>Vector3D</code> object.
 14		 *
 15		 * @param    w                The first 3d number object to use in the calculation.
 16		 * @param    q                The first 3d number object to use in the calculation.
 17		 * @return                    An angle in radians representing the angle between the two <code>Vector3D</code> objects.
 18		 */
 19		public static function getAngle(w:Vector3D, q:Vector3D):Number
 20		{
 21			return Math.acos(w.dotProduct(q)/(w.length*q.length));
 22		}
 23		
 24		/**
 25		 * Returns a <code>Vector3D</code> object with the euler angles represented by the 3x3 matrix rotation of the given <code>Matrix3D</code> object.
 26		 *
 27		 * @param    m1    The 3d matrix object to use in the calculation.
 28		 * @return        A 3d vector representing the euler angles extracted from the 3d matrix.
 29		 */
 30		public static function matrix2euler(m1:Matrix3D):Vector3D
 31		{
 32			var m2:Matrix3D = new Matrix3D();
 33			var result:Vector3D = new Vector3D();
 34			var raw:Vector.<Number> = Matrix3DUtils.RAW_DATA_CONTAINER;
 35			m1.copyRawDataTo(raw);
 36			
 37			// Extract the first angle, rotationX
 38			result.x = -Math.atan2(raw[uint(6)], raw[uint(10)]); // rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
 39			
 40			// Remove the rotationX rotation from m2, so that the remaining
 41			// rotation, m2 is only around two axes, and gimbal lock cannot occur.
 42			m2.appendRotation(result.x*180/MathPI, new Vector3D(1, 0, 0));
 43			m2.append(m1);
 44			
 45			m2.copyRawDataTo(raw);
 46			
 47			// Extract the other two angles, rot.y and rot.z, from m2.
 48			var cy:Number = Math.sqrt(raw[uint(0)]*raw[uint(0)] + raw[uint(1)]*raw[uint(1)]); // T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]);
 49			
 50			result.y = Math.atan2(-raw[uint(2)], cy); // rot.y = Math<T>::atan2 (-N[0][2], cy);
 51			result.z = Math.atan2(-raw[uint(4)], raw[uint(5)]); //rot.z = Math<T>::atan2 (-N[1][0], N[1][1]);
 52			
 53			// Fix angles
 54			if (Math.round(result.z/MathPI) == 1) {
 55				if (result.y > 0)
 56					result.y = -(result.y - MathPI);
 57				else
 58					result.y = -(result.y + MathPI);
 59				
 60				result.z -= MathPI;
 61				
 62				if (result.x > 0)
 63					result.x -= MathPI;
 64				else
 65					result.x += MathPI;
 66			} else if (Math.round(result.z/MathPI) == -1) {
 67				if (result.y > 0)
 68					result.y = -(result.y - MathPI);
 69				else
 70					result.y = -(result.y + MathPI);
 71				
 72				result.z += MathPI;
 73				
 74				if (result.x > 0)
 75					result.x -= MathPI;
 76				else
 77					result.x += MathPI;
 78			} else if (Math.round(result.x/MathPI) == 1) {
 79				if (result.y > 0)
 80					result.y = -(result.y - MathPI);
 81				else
 82					result.y = -(result.y + MathPI);
 83				
 84				result.x -= MathPI;
 85				
 86				if (result.z > 0)
 87					result.z -= MathPI;
 88				else
 89					result.z += MathPI;
 90			} else if (Math.round(result.x/MathPI) == -1) {
 91				if (result.y > 0)
 92					result.y = -(result.y - MathPI);
 93				else
 94					result.y = -(result.y + MathPI);
 95				
 96				result.x += MathPI;
 97				
 98				if (result.z > 0)
 99					result.z -= MathPI;
100				else
101					result.z += MathPI;
102			}
103			
104			return result;
105		}
106		
107		/**
108		 * Returns a <code>Vector3D</code> object containing the euler angles represented by the given <code>Quaternion</code> object.
109		 *
110		 * @param    quaternion    The quaternion object to use in the calculation.
111		 * @return                A 3d vector representing the euler angles extracted from the quaternion.
112		 */
113		
114		public static function quaternion2euler(quarternion:Quaternion):Vector3D
115		{
116			var result:Vector3D = new Vector3D();
117			
118			var test:Number = quarternion.x*quarternion.y + quarternion.z*quarternion.w;
119			if (test > 0.499) { // singularity at north pole
120				result.x = 2*Math.atan2(quarternion.x, quarternion.w);
121				result.y = Math.PI/2;
122				result.z = 0;
123				return result;
124			}
125			if (test < -0.499) { // singularity at south pole
126				result.x = -2*Math.atan2(quarternion.x, quarternion.w);
127				result.y = -Math.PI/2;
128				result.z = 0;
129				return result;
130			}
131			
132			var sqx:Number = quarternion.x*quarternion.x;
133			var sqy:Number = quarternion.y*quarternion.y;
134			var sqz:Number = quarternion.z*quarternion.z;
135			
136			result.x = Math.atan2(2*quarternion.y*quarternion.w - 2*quarternion.x*quarternion.z, 1 - 2*sqy - 2*sqz);
137			result.y = Math.asin(2*test);
138			result.z = Math.atan2(2*quarternion.x*quarternion.w - 2*quarternion.y*quarternion.z, 1 - 2*sqx - 2*sqz);
139			
140			return result;
141		}
142		
143		/**
144		 * Returns a <code>Vector3D</code> object containing the scale values represented by the given <code>Matrix3D</code> object.
145		 *
146		 * @param    m    The 3d matrix object to use in the calculation.
147		 * @return        A 3d vector representing the axis scale values extracted from the 3d matrix.
148		 */
149		public static function matrix2scale(m:Matrix3D):Vector3D
150		{
151			var result:Vector3D = new Vector3D();
152			var raw:Vector.<Number> = Matrix3DUtils.RAW_DATA_CONTAINER;
153			m.copyRawDataTo(raw);
154			
155			result.x = Math.sqrt(raw[uint(0)]*raw[uint(0)] + raw[uint(1)]*raw[uint(1)] + raw[uint(2)]*raw[uint(2)]);
156			result.y = Math.sqrt(raw[uint(4)]*raw[uint(4)] + raw[uint(5)]*raw[uint(5)] + raw[uint(6)]*raw[uint(6)]);
157			result.z = Math.sqrt(raw[uint(8)]*raw[uint(8)] + raw[uint(9)]*raw[uint(9)] + raw[uint(10)]*raw[uint(10)]);
158			
159			return result;
160		}
161		
162		public static function rotatePoint(aPoint:Vector3D, rotation:Vector3D):Vector3D
163		{
164			if (rotation.x != 0 || rotation.y != 0 || rotation.z != 0) {
165				
166				var x1:Number;
167				var y1:Number;
168				
169				var rad:Number = MathConsts.DEGREES_TO_RADIANS;
170				var rotx:Number = rotation.x*rad;
171				var roty:Number = rotation.y*rad;
172				var rotz:Number = rotation.z*rad;
173				
174				var sinx:Number = Math.sin(rotx);
175				var cosx:Number = Math.cos(rotx);
176				var siny:Number = Math.sin(roty);
177				var cosy:Number = Math.cos(roty);
178				var sinz:Number = Math.sin(rotz);
179				var cosz:Number = Math.cos(rotz);
180				
181				var x:Number = aPoint.x;
182				var y:Number = aPoint.y;
183				var z:Number = aPoint.z;
184				
185				y1 = y;
186				y = y1*cosx + z* -sinx;
187				z = y1*sinx + z*cosx;
188				
189				x1 = x;
190				x = x1*cosy + z*siny;
191				z = x1* -siny + z*cosy;
192				
193				x1 = x;
194				x = x1*cosz + y* -sinz;
195				y = x1*sinz + y*cosz;
196				
197				aPoint.x = x;
198				aPoint.y = y;
199				aPoint.z = z;
200			}
201			
202			return aPoint;
203		}
204		
205		public static function subdivide(startVal:Vector3D, endVal:Vector3D, numSegments:int):Vector.<Vector3D>
206		{
207			var points:Vector.<Vector3D> = new Vector.<Vector3D>();
208			var numPoints:int = 0;
209			var stepx:Number = (endVal.x - startVal.x)/numSegments;
210			var stepy:Number = (endVal.y - startVal.y)/numSegments;
211			var stepz:Number = (endVal.z - startVal.z)/numSegments;
212			
213			var step:int = 1;
214			var scalestep:Vector3D;
215			
216			while (step < numSegments) {
217				scalestep = new Vector3D();
218				scalestep.x = startVal.x + (stepx*step);
219				scalestep.y = startVal.y + (stepy*step);
220				scalestep.z = startVal.z + (stepz*step);
221				points[numPoints++] = scalestep;
222				
223				step++;
224			}
225			
226			points[numPoints] = endVal;
227			
228			return points;
229		}
230	}
231}