Quaternion Rotation
Quaternion Rotation by (C)2009 XELF
// Quaternion Rotation by (C)2009 XELF
package {
import flash.display.*;
import flash.text.*;
import flash.events.*;
[SWF(backgroundColor="#FFFFFF", frameRate=60)]
public class FlashTest extends Sprite {
private var Text:TextField = new TextField();
public function FlashTest() {
addEventListener(Event.ENTER_FRAME, Draw);
Text.autoSize = TextFieldAutoSize.LEFT;
addChild(Text);
}
private var viewport:Viewport = new Viewport();
private var t:Number = 0;
private var _rotation:Q = new Q(0, 0, 0, 1);
private function DrawAxis(m:M):void {
var v0:Vector3 = Vector3.Transform(new Vector3(0, 0, 0), m);
var vx:Vector3 = Vector3.Transform(new Vector3(1, 0, 0), m);
var vy:Vector3 = Vector3.Transform(new Vector3(0, 1, 0), m);
var vz:Vector3 = Vector3.Transform(new Vector3(0, 0, 1), m);
viewport.Transform(v0);
viewport.Transform(vx);
viewport.Transform(vy);
viewport.Transform(vz);
graphics.lineStyle(4, 0xff0000);
graphics.moveTo(v0.X, v0.Y);
graphics.lineTo(vx.X, vx.Y);
graphics.lineStyle(4, 0x00ff00);
graphics.moveTo(v0.X, v0.Y);
graphics.lineTo(vy.X, vy.Y);
graphics.lineStyle(4, 0x0000ff);
graphics.moveTo(v0.X, v0.Y);
graphics.lineTo(vz.X, vz.Y);
graphics.lineStyle(4, 0x000000);
graphics.drawCircle(v0.X, v0.Y, 2);
}
private function Draw(e:Event) : void {
graphics.clear();
t += 0.1;
var q:Q = new Q(0.13, 0.21, -0.3, 0);
q.Scale(1.0 / 60 * 0.5);
_rotation.Add(Q.Multiply(_rotation, q));
_rotation.Normalize();
var l:Q = Q.Log(_rotation);
var m:M = M.CreateFromQuaternion(_rotation);
var v:Vector3 = Vector3.Transform(new Vector3(0.3, 0.5, 0.2), m);
var s:Vector3 = Vector3.Rotate(new Vector3(0.3, 0.5, 0.2), _rotation);
Text.text = _rotation + "\n" + l + "\n" + Q.Exp(l) + "\n\n" + m
+ "\nq=" + q
+ "\nv=" + v
+ "\ns=" + s +"\n(v-s)=" + Vector3.Subtract(v, s);
DrawAxis(m);
viewport.Transform(v);
graphics.lineStyle(4, 0x000000);
graphics.drawCircle(v.X, v.Y, 2);
}
}
}
class Q {
public var X:Number;
public var Y:Number;
public var Z:Number;
public var W:Number;
public function Q(x:Number, y:Number, z:Number, w:Number) {
X = x; Y = y; Z = z; W = w;
}
public function Add(value:Q):void {
X += value.X; Y += value.Y; Z += value.Z; W += value.W;
}
public function Subtract(value:Q):void {
X -= value.X; Y -= value.Y; Z -= value.Z; W -= value.W;
}
public static function Subtract(a:Q, b:Q):Q {
return new Q(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
}
public function Scale(value:Number):void {
X *= value; Y *= value; Z *= value; W *= value;
}
public function Negate():void {
X = -X; Y = -Y; Z = -Z; W = -W;
}
public function Conjugate():void {
X = -X; Y = -Y; Z = -Z;
}
public function Invert():void {
var r:Number = 1 / LengthSquared();
X *= -r; Y *= -r; Z *= -r; W *= r;
}
public static function Invert(a:Q):Q {
var result:Q = new Q(a.X, a.Y, a.Z, a.W);
result.Invert();
return result;
}
public function Length():Number {
return Math.sqrt(X * X + Y * Y + Z * Z + W * W);
}
public function VectorPart():Vector3 {
return new Vector3(X, Y, Z);
}
public function LengthSquared():Number {
return X * X + Y * Y + Z * Z + W * W;
}
public static function Multiply(a:Q, b:Q):Q {
return new Q(
a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
a.W * b.Y + a.Y * b.W - a.X * b.Z + a.Z * b.X,
a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
}
public function Normalize():void {
var s:Number = 1 / Length();
X *= s; Y *= s; Z *= s; W *= s;
}
public static function Dot(a:Q, b:Q):Number {
return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
}
public static function Cross(a:Q, b:Q):Q {
return new Q(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X, 0);
}
public static function Log(a:Q):Q {
if (Math.abs(a.W) < 1 - 1e-10) {
var s:Number = Math.acos(a.W) / Math.sqrt(1 - a.W * a.W);
return new Q(a.X * s, a.Y * s, a.Z * s, 0);
}
return new Q(a.X, a.Y, a.Z, 0);
}
public static function Exp(a:Q):Q {
var l:Number = a.X * a.X + a.Y * a.Y + a.Z * a.Z;
if (l > 1e-10) {
l = Math.sqrt(l);
var s:Number = Math.sin(l) / l;
return new Q(a.X * s, a.Y * s, a.Z * s, Math.cos(l));
}
return new Q(a.X, a.Y, a.Z, 1);
}
public function toString():String {
return "" + X + ", " + Y + ", " + Z + ", " + W + " (" + Length() + ")";
}
}
class M {
public var M11:Number;
public var M12:Number;
public var M13:Number;
public var M14:Number;
public var M21:Number;
public var M22:Number;
public var M23:Number;
public var M24:Number;
public var M31:Number;
public var M32:Number;
public var M33:Number;
public var M34:Number;
public var M41:Number;
public var M42:Number;
public var M43:Number;
public var M44:Number;
public static function CreateFromQuaternion(a:Q):M {
var bx:Number = a.X + a.X;
var by:Number = a.Y + a.Y;
var bz:Number = a.Z + a.Z;
var bw:Number = a.W + a.W;
var cx:Number = bx * a.X;
var cy:Number = by * a.Y;
var cz:Number = bz * a.Z;
var cw:Number = bw * a.W;
var xy:Number = bx * a.Y;
var wz:Number = bw * a.Z;
var xz:Number = bx * a.Z;
var wy:Number = bw * a.Y;
var yz:Number = by * a.Z;
var wx:Number = bw * a.X;
var m:M = new M();
m.M11 = 1 - cy - cz;
m.M12 = xy + wz;
m.M13 = xz - wy;
m.M21 = xy - wz;
m.M22 = 1 - cx - cz;
m.M23 = yz + wx;
m.M31 = xz + wy;
m.M32 = yz - wx;
m.M33 = 1 - cx - cy;
m.M14 = 0; m.M24 = 0; m.M34 = 0;
m.M41 = 0; m.M42 = 0; m.M43 = 0; m.M44 = 1;
return m;
}
public function toString():String {
return "["
+ M11 + ", " + M12 + ", " + M13 + ", " + M14 +"]\n["
+ M21 + ", " + M22 + ", " + M23 + ", " + M24 +"]\n["
+ M31 + ", " + M32 + ", " + M33 + ", " + M34 +"]\n["
+ M41 + ", " + M42 + ", " + M43 + ", " + M44 +"]\n";
}
}
class Vector3 {
public var X:Number;
public var Y:Number;
public var Z:Number;
public function Vector3(x:Number, y:Number, z:Number) {
X = x; Y = y; Z = z;
}
public static function Subtract(a:Vector3, b:Vector3):Vector3 {
return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
}
public static function Transform(a:Vector3, m:M):Vector3 {
var r:Number = 1 / (m.M14 + m.M24 + m.M34 + m.M44);
return new Vector3(
(m.M11 * a.X + m.M21 * a.Y + m.M31 * a.Z + m.M41) * r,
(m.M12 * a.X + m.M22 * a.Y + m.M32 * a.Z + m.M42) * r,
(m.M13 * a.X + m.M23 * a.Y + m.M33 * a.Z + m.M43) * r);
}
public static function Rotate(p:Vector3, q:Q):Vector3 {
return Q.Multiply(Q.Multiply(q, new Q(p.X, p.Y, p.Z, 0)), Q.Invert(q)).VectorPart();
}
public function toString():String {
return "" + X + ", " + Y + ", " + Z + "";
}
}
class Vector2 {
public var X:Number;
public var Y:Number;
public function Vector2(x:Number, y:Number) {
X = x; Y = y;
}
public static function Transform(a:Vector2, m:M):Vector2 {
var r:Number = 1 / (m.M14 + m.M24 + m.M34 + m.M44);
return new Vector2(
(m.M11 * a.X + m.M21 * a.Y + m.M41) * r,
(m.M12 * a.X + m.M22 * a.Y + m.M42) * r);
}
public function toString():String {
return "" + X + ", " + Y + "";
}
}
class Viewport {
public var X:Number = 0;
public var Y:Number = 0;
public var Width:Number = 400;
public var Height:Number = 400;
public function Project(value:Vector3):Vector2 {
return new Vector2((value.X + 1) * Width * 0.5 + X, (value.Y + 1) * Height * 0.5 + Y);
}
public function Transform(value:Vector3):void {
value.X = (value.X + 1) * Width * 0.5 + X;
value.Y = (value.Y + 1) * Height * 0.5 + Y;
}
}