slerp test

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

package {
	import com.actionscriptbible.Example;
	
	/**
	 * slerp test
	 */
	public class SlerpTest extends Example {
		
		public function SlerpTest () {
			var a:Vertex = new Vertex;
			a.x = 1; a.y = 0; a.z = 0;
			var b:Vertex = new Vertex;
			b.x = 3; b.y = 0; b.z = 0;
			var c:Vertex = a.interpolate (b, 0.5);
			trace (c.x, c.y, c.z); // 2,0,0

			b.x = -3;
			c = a.interpolate (b, 0.5);
			trace (c.x, c.y, c.z); // 0,?,? (length = 2)

			b.x = 0; b.y = 1;
			c = a.interpolate (b, 0.5);
			trace (c.x, c.y, c.z); // .7071,.7071,0

			c = a.interpolate (b, 1.0);
			trace (c.x, c.y, c.z); // 0,1,0
		}
	}
}

class Vertex {
	public var x:Number;
	public var y:Number;
	public var z:Number;

	public function interpolate (other:Vertex, t:Number):Vertex {
		var v:Vertex = new Vertex;
		slerp (this.x, this.y, this.z, other.x, other.y, other.z, t);
		v.x = sx; v.y = sy; v.z = sz;
		return v;
	}

	private var cx:Number;
	private var cy:Number;
	private var cz:Number;
	private function cross (x1:Number, y1:Number, z1:Number, x2:Number, y2:Number, z2:Number):void {
		cx = y1 * z2 - z1 * y2;
		cy = z1 * x2 - x1 * z2;
		cz = x1 * y2 - y1 * x2;
	}

	private var sx:Number;
	private var sy:Number;
	private var sz:Number;
	private function slerp (x1:Number, y1:Number, z1:Number, x2:Number, y2:Number, z2:Number, t:Number):void {
		// projection plane
		var ax:Number, ay:Number, az:Number, bx:Number, by:Number, bz:Number;
		cross (x1, y1, z1, x2, y2, z2);
		var d:Number = cx * cx + cy * cy + cz * cz;
		if (d == 0) {
			ax = 1; ay = 0; az = 0;
			if ((x1 != 0) || (y1 != 0)) {
				bx = 0; by = 1; bz = 0;
			} else {
				bx = 0; by = 0; bz = 1;
			}
		} else {
			if ((cx != 0) || (cy != 0)) {
				ax = -cy; ay = +cx; az = 0;
			} else {
				ax = -cz; ay = 0; az = +cx;
			}
			cross (cx, cy, cz, ax, ay, az);
			bx = cx; by = cy; bz = cz;
		}

		// lengths
		var La:Number = Math.sqrt (ax * ax + ay * ay + az * az);
		var Lb:Number = Math.sqrt (bx * bx + by * by + bz * bz);

		// projection
		var a1:Number = (ax * x1 + ay * y1 + az * z1) / La;
		var b1:Number = (bx * x1 + by * y1 + bz * z1) / Lb;
		var a2:Number = (ax * x2 + ay * y2 + az * z2) / La;
		var b2:Number = (bx * x2 + by * y2 + bz * z2) / Lb;

		// lengths (should be mostly equal)
		var L1:Number = Math.sqrt (a1 * a1 + b1 * b1);
		var L2:Number = Math.sqrt (a2 * a2 + b2 * b2);

		// angles
		var p1:Number = Math.atan2 (a1, b1); if (p1 < 0) p1 += 2 * Math.PI;
		var p2:Number = Math.atan2 (a2, b2); if (p2 < 0) p2 += 2 * Math.PI;

		// interpolate
		var pt:Number = p1 * (1 - t) + p2 * t;
		d = p1 - p2; if ((d > Math.PI) || (d < -Math.PI)) pt += Math.PI;

		var Lt:Number = L1 * (1 - t) + L2 * t;

		// back to vector, 2D
		var at:Number = Lt * Math.sin (pt);
		var bt:Number = Lt * Math.cos (pt);

		// finally, 3D
		sx = ax * at + bx * bt;
		sy = ay * at + by * bt;
		sz = az * at + bz * bt;
	}
}