package
{
import flash.geom.Vector3D;
import flash.geom.Matrix3D;
/**
* クォータニオンクラス
*/
public class Quaternion
{
public const tol:Number = 0.0000000001; // float type tolerance
public var n:Number; // number (scalar) part
public var v:Vector3D; // vector part: v.x, v.y, v.z
public function Quaternion(...args)
{
v = new Vector3D();
switch(args.length) {
case 0:
n = 0;
v.x = 0;
v.y = 0;
v.z = 0;
break;
case 4:
var e0:Number = args[0];
var e1:Number = args[1];
var e2:Number = args[2];
var e3:Number = args[3];
n = e0;
v.x = e1;
v.y = e2;
v.z = e3;
break;
}
}
public function Magnitude():Number
{
return Number(Math.sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z));
}
public function GetVector():Vector3D
{
return new Vector3D(v.x, v.y, v.z);
}
public function GetScalar():Number
{
return n;
}
public function addEqual(q:Quaternion):Quaternion
{
n += q.n;
v.x += q.v.x;
v.y += q.v.y;
v.z += q.v.z;
return this;
}
public function subtractEqual(q:Quaternion):Quaternion
{
n -= q.n;
v.x -= q.v.x;
v.y -= q.v.y;
v.z -= q.v.z;
return this;
}
public function multiplyEqual(s:Number):Quaternion
{
n *= s;
v.x *= s;
v.y *= s;
v.z *= s;
return this;
}
public function divideEqual(s:Number):Quaternion
{
n /= s;
v.x /= s;
v.y /= s;
v.z /= s;
return this;
}
public function Add(q1:Quaternion , q2:Quaternion ):Quaternion
{
return new Quaternion( q1.n + q2.n,
q1.v.x + q2.v.x,
q1.v.y + q2.v.y,
q1.v.z + q2.v.z);
}
public function subtract(q1:Quaternion, q2:Quaternion):Quaternion
{
return new Quaternion( q1.n - q2.n,
q1.v.x - q2.v.x,
q1.v.y - q2.v.y,
q1.v.z - q2.v.z);
}
public function multiply(obj_1:Object, obj_2:Object):Quaternion
{
var q1:Quaternion;
var q2:Quaternion;
var q:Quaternion;
var s:Number;
var v:Vector3D;
if (obj_1 is Quaternion && obj_2 is Quaternion) {
q1 = Quaternion(obj_1);
q2 = Quaternion(obj_2);
return new Quaternion( q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z,
q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y,
q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z,
q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x);
}
else if (obj_1 is Quaternion && obj_2 is Number) {
q = Quaternion(obj_1);
s = Number(obj_2);
return new Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
}
else if (obj_1 is Number && obj_2 is Quaternion) {
s = Number(obj_1);
q = Quaternion(obj_2);
return new Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
}
else if (obj_1 is Quaternion && obj_2 is Vector3D) {
q = Quaternion(obj_1);
v = Vector3D(obj_2);
return new Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
q.n * v.x + q.v.y * v.z - q.v.z * v.y,
q.n * v.y + q.v.z * v.x - q.v.x * v.z,
q.n * v.z + q.v.x * v.y - q.v.y * v.x);
}
else if (obj_1 is Vector3D && obj_2 is Quaternion) {
v = Vector3D(obj_1);
q = Quaternion(obj_2);
return new Quaternion( -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
q.n * v.x + q.v.z * v.y - q.v.y * v.z,
q.n * v.y + q.v.x * v.z - q.v.z * v.x,
q.n * v.z + q.v.y * v.x - q.v.x * v.y);
}
return null;
}
public function divide(q:Quaternion, s:Number):Quaternion
{
return new Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
}
public function conjugate():Quaternion
{
return new Quaternion(n, -v.x, -v.y, -v.z);
}
public function QGetAngle(q:Quaternion):Number
{
return Number(2 * Math.acos(q.n));
}
public function QGetAxis(q:Quaternion ):Vector3D
{
var v:Vector3D = new Vector3D;
var m:Number;
v = q.GetVector();
m = v.length;
if (m <= tol)
return v;
else
v.x = v.x / m;
v.y = v.y / m;
v.z = v.z / m;
return v;
}
public function QRotate(q1:Quaternion, q2:Quaternion):Quaternion
{
var q:Quaternion;
var conjugateQ:Quaternion; // 共役
conjugateQ = q2.conjugate();
q = q.multiply(q1, q2);
q = q.multiply(q, conjugateQ);
return q;
}
public function QVRotate(q:Quaternion, v:Vector3D):Vector3D
{
var t:Quaternion;
var conjugateQ:Quaternion; // 共役
conjugateQ = q.conjugate();
//t = q * v * (~q);
t = t.multiply(q, v);
t = t.multiply(q, conjugateQ);
return t.GetVector();
}
public static function MakeQFromEulerAngles(x:Number, y:Number, z:Number):Quaternion
{
var q:Quaternion;
var roll:Number = DegreesToRadians(x);
var pitch:Number = DegreesToRadians(y);
var yaw:Number = DegreesToRadians(z);
var cyaw:Number, cpitch:Number, croll:Number, syaw:Number, spitch:Number, sroll:Number;
var cyawcpitch:Number, syawspitch:Number, cyawspitch:Number, syawcpitch:Number;
cyaw = Math.cos(0.5 * yaw);
cpitch = Math.cos(0.5 * pitch);
croll = Math.cos(0.5 * roll);
syaw = Math.sin(0.5 * yaw);
spitch = Math.sin(0.5 * pitch);
sroll = Math.sin(0.5 * roll);
cyawcpitch = cyaw * cpitch;
syawspitch = syaw * spitch;
cyawspitch = cyaw * spitch;
syawcpitch = syaw * cpitch;
q.n = Number(cyawcpitch * croll + syawspitch * sroll);
q.v.x = Number(cyawcpitch * sroll - syawspitch * croll);
q.v.y = Number(cyawspitch * croll + syawcpitch * sroll);
q.v.z = Number(syawcpitch * croll - cyawspitch * sroll);
return q;
}
public function MakeEulerAnglesFromQ(q:Quaternion):Vector3D
{
var r11:Number, r21:Number, r31:Number, r32:Number, r33:Number, r12:Number, r13:Number;
var q00:Number, q11:Number, q22:Number, q33:Number;
var tmp:Number;
var u:Vector3D;
q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;
r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
r33 = q00 - q11 - q22 + q33;
tmp = Math.abs(r31);
if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
u.x = RadiansToDegrees(0.0); //roll
u.y = RadiansToDegrees(Number( -(Math.PI / 2) * r31 / tmp)); // pitch
u.z = RadiansToDegrees(Number(Math.atan2( -r12, -r31 * r13))); // yaw
return u;
}
u.x = RadiansToDegrees(Number(Math.atan2(r32, r33))); // roll
u.y = RadiansToDegrees(Number(Math.asin( -r31))); // pitch
u.z = RadiansToDegrees(Number(Math.atan2(r21, r11))); // yaw
return u;
}
public static function MakeMatrixFromQuaternion(q:Quaternion):Matrix3D
{
var q00:Number, q11:Number, q22:Number, q33:Number;
var m:Matrix3D = new Matrix3D();
q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;
m.rawData[0] = q00 + q11 - q22 - q33;
m.rawData[4] = 2 * (q.v.x * q.v.y + q.n * q.v.z);
m.rawData[8] = 2 * (q.v.x * q.v.z - q.n * q.v.y);
m.rawData[1] = 2 * (q.v.x * q.v.y - q.n * q.v.z);
m.rawData[5] = q00 - q11 + q22 - q33;
m.rawData[9] = 2 * (q.v.y * q.v.z + q.n * q.v.x);
m.rawData[2] = 2 * (q.v.x * q.v.z + q.n * q.v.y);
m.rawData[6] = 2 * (q.v.y * q.v.z - q.n * q.v.x);
m.rawData[9] = q00 - q11 - q22 + q33;
return m;
}
public static function DegreesToRadians(deg:Number):Number
{
return deg * Math.PI / 180.0;
}
public function RadiansToDegrees(rad:Number):Number
{
return rad * 180.0 / Math.PI;
}
}
}
