/* ----------------------------------------------------------------------------- クォータニオン(4元数)関数集 ファイル名 : quat.as as.Ver : AS 1.0 Flash 5 以降 製作 : Hakuhin (C) 2006/08/17 ----------------------------------------------------------------------------- */ /* ----------------------------------------------------------------------------- -------------------------------------- クォータニオンの型 -------------------------------------- quat = { x : 0, // 虚数 y : 0, // 虚数 z : 0, // 虚数 w : 0 // 実数 }; -------------------------------------- // クォータニオン用関数 --------------------------------------------------- function QuatCreate(x,y,z,w); // クォータニオン生成 function QuatClone(q); // クォータニオン複製 function QuatIdentity(q); // 単位クォータニオン化 function QuatAdd(q1,q2); // 足し算 function QuatSub(q1,q2); // 引き算 function QuatMul(q1,q2); // 掛け算 function QuatScale(q,s); // 実数掛け算 function QuatNorm(q); // 絶対値(ノルム) |q| function QuatNormSqr(q); // 絶対値の2乗 |q|~2 // 回転クォータニオン ----------------------------------------------------- function QuatRotate(vec,rot); // 回転クォータニオン作成 function QuatRotatePref(vec,rot,m); // 回転クォータニオン * 指定クォータニオン function QuatRotatePost(vec,rot,m); // 指定クォータニオン * 回転クォータニオン // 射影変換 --------------------------------------------------------------- function QuatTransformVector(vec,q); // ベクトルの射影変換 // コンバート関数 --------------------------------------------------------- function QuatConvertFromMatrix(m); // 行列からクォータニオンに変換 function QuatConvertToMatrix(q); // クォータニオンから行列に変換 function QuatRotateConvertToMatrix(q); // クォータニオンから行列に変換 (回転成分のみ) // 補間 --------------------------------------------------------- function QuatSlerp(q1,q2,d); // 球面線形補間 ----------------------------------------------------------------------------- */ /* ------------------------------------ クォータニオン生成 ------------------------------------ */ function QuatCreate(x,y,z,w){ return {x:x,y:y,z:z,w:w}; } /* ------------------------------------ クォータニオン複製 ------------------------------------ */ function QuatClone(q){ return { w : q.w, x : q.x, y : q.y, z : q.z } } /* ------------------------------------ 単位クォータニオン化 ------------------------------------ */ function QuatIdentity(q){ q.x = 0; q.y = 0; q.z = 0; q.w = 1; return q; } /* ------------------------------------ クォータニオン同士の足し算 ------------------------------------ */ function QuatAdd(q1,q2){ return { w : q1.w + q2.w, x : q1.x + q2.x, y : q1.y + q2.y, z : q1.z + q2.z } } /* ------------------------------------ クォータニオン同士の引き算 ------------------------------------ */ function QuatSub(q1,q2){ return { w : q1.w - q2.w, x : q1.x - q2.x, y : q1.y - q2.y, z : q1.z - q2.z } } /* ------------------------------------ クォータニオン同士の掛け算 ------------------------------------ */ function QuatMul(q1,q2){ return { w : q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z, x : q1.y * q2.z - q1.z * q2.y + q1.w * q2.x + q1.x * q2.w, y : q1.z * q2.x - q1.x * q2.z + q1.w * q2.y + q1.y * q2.w, z : q1.x * q2.y - q1.y * q2.x + q1.w * q2.z + q1.z * q2.w } } /* ------------------------------------ クォータニオン実数の掛け算 ------------------------------------ */ function QuatScale(q,s){ return { w : q.w * s, x : q.x * s, y : q.y * s, z : q.z * s } } /* ------------------------------------ クォータニオンの絶対値(ノルム) |q| ------------------------------------ */ function QuatNorm(q){ return Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); } /* ------------------------------------ クォータニオンの絶対値の2乗 |q|*|q| ------------------------------------ */ function QuatNormSqr(q){ var n = Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); return n * n; } /* ------------------------------------ 回転クォータニオンを作成 ------------------------------------ */ function QuatRotate (v,r){ var rad = r * (Math.PI / 180.0) / 2.0; var s = Math.sin(rad); return { w : Math.cos(rad), x : v.x * s, y : v.y * s, z : v.z * s } } /* ------------------------------------ (回転クォータニオン * 指定クォータニオン) ------------------------------------ */ function QuatRotatePref (v,r,q){ var rad = r * (Math.PI / 180.0) / 2.0; var s = Math.sin(rad); var qw = Math.cos(rad); var qx = v.x * s; var qy = v.y * s; var qz = v.z * s; return { w : q.w * qw - q.x * qx - q.y * qy - q.z * qz, x : q.y * qz - q.z * qy + q.w * qx + q.x * qw, y : q.z * qx - q.x * qz + q.w * qy + q.y * qw, z : q.x * qy - q.y * qx + q.w * qz + q.z * qw } } /* ------------------------------------ (指定クォータニオン * 回転クォータニオン) ------------------------------------ */ function QuatRotatePost (v,r,q){ var rad = r * (Math.PI / 180.0) / 2.0; var s = Math.sin(rad); var qw = Math.cos(rad); var qx = v.x * s; var qy = v.y * s; var qz = v.z * s; return { w : qw * q.w - qx * q.x - qy * q.y - qz * q.z, x : qy * q.z - qz * q.y + qw * q.x + qx * q.w, y : qz * q.x - qx * q.z + qw * q.y + qy * q.w, z : qx * q.y - qy * q.x + qw * q.z + qz * q.w } } /* ------------------------------------ ベクトルの射影変換 ------------------------------------ */ function QuatTransformVector(v,q){ var w =-q.x * v.x - q.y * v.y - q.z * v.z; var x = q.y * v.z - q.z * v.y + q.w * v.x; var y = q.z * v.x - q.x * v.z + q.w * v.y; var z = q.x * v.y - q.y * v.x + q.w * v.z; return { x : y * -q.z + z * q.y - w * q.x + x * q.w, y : z * -q.x + x * q.z - w * q.y + y * q.w, z : x * -q.y + y * q.x - w * q.z + z * q.w } } /* ------------------------------------ 回転行列からクォータニオンに変換 ------------------------------------ */ function QuatConvertFromMatrix(m){ var s; var tr = m._00 + m._11 + m._22 + 1.0; if (tr >= 1.0) { s = 0.5 / Math.sqrt(tr); return { w : 0.25 / s, x : (m._12 - m._21) * s, y : (m._20 - m._02) * s, z : (m._01 - m._10) * s } } else { var max; if(m._11 > m._22) max = m._11; else max = m._22; if (max < m._00) { s = Math.sqrt(m._00 - (m._11 + m._22) + 1.0); var x = s * 0.5; s = 0.5 / s; return { x : x, y : (m._01 + m._10) * s, z : (m._20 + m._02) * s, w : (m._12 - m._21) * s } } else if (max == m._11) { s = Math.sqrt(m._11 - (m._22 + m._00) + 1.0); var y = s * 0.5; s = 0.5 / s; return { x : (m._01 + m._10) * s, y : y, z : (m._12 + m._21) * s, w : (m._20 - m._02) * s } } else { s = Math.sqrt(m._22 - (m._00 + m._11) + 1.0); var z = s * 0.5; s = 0.5 / s; return { x : (m._20 + m._02) * s, y : (m._12 + m._21) * s, z : z, w : (m._01 - m._10) * s } } } } /* ------------------------------------ クォータニオンから行列に変換 ------------------------------------ */ function QuatConvertToMatrix(q){ var sx = q.x * q.x; var sy = q.y * q.y; var sz = q.z * q.z; var cx = q.y * q.z; var cy = q.x * q.z; var cz = q.x * q.y; var wx = q.w * q.x; var wy = q.w * q.y; var wz = q.w * q.z; return { _00 : 1.0 - 2.0 * (sy + sz), _01 : 2.0 * (cz + wz), _02 : 2.0 * (cy - wy), _10 : 2.0 * (cz - wz), _11 : 1.0 - 2.0 * (sx + sz), _12 : 2.0 * (cx + wx), _20 : 2.0 * (cy + wy), _21 : 2.0 * (cx - wx), _22 : 1.0 - 2.0 * (sx + sy), _30 : 0.0, _31 : 0.0, _32 : 0.0 }; } /* ------------------------------------ クォータニオンから行列に変換(回線成分のみ) ------------------------------------ */ function QuatRotateConvertToMatrix(q){ var s = Math.sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z); s = 2.0 / (s * s); var vx = q.x * s; var vy = q.y * s; var vz = q.z * s; var wx = vx * q.w; var wy = vy * q.w; var wz = vz * q.w; var sx = q.x * vx; var sy = q.y * vy; var sz = q.z * vz; var cx = q.y * vz; var cy = q.z * vx; var cz = q.x * vy; return { _00 : 1.0 - sy - sz, _01 : cz + wz, _02 : cy - wy, _10 : cz - wz, _11 : 1.0 - sx - sz, _12 : cx + wx, _20 : cy + wy, _21 : cx - wx, _22 : 1.0 - sx - sy, _30 : 0.0, _31 : 0.0, _32 : 0.0 }; } /* ------------------------------------ 球面線形補間 ------------------------------------ */ function QuatSlerp(q1,q2,d){ var qr = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; var ss = 1.0 - (qr * qr); if (ss == 0.0){ return { x : q1.x, y : q1.y, z : q1.z, w : q1.w }; }else{ var ph = Math.acos(qr); if(qr < 0.0 && ph > Math.PI / 2.0){ var s1,s2; qr = - q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; ph = Math.acos(qr); s1 = Math.sin(ph * (1.0 - d)) / Math.sin(ph); s2 = Math.sin(ph * d ) / Math.sin(ph); return { x : q1.x * s1 - q2.x * s2, y : q1.y * s1 - q2.y * s2, z : q1.z * s1 - q2.z * s2, w : q1.w * s1 - q2.w * s2 }; }else{ var s1,s2; s1 = Math.sin(ph * (1.0 - d)) / Math.sin(ph); s2 = Math.sin(ph * d ) / Math.sin(ph); return { x : q1.x * s1 + q2.x * s2, y : q1.y * s1 + q2.y * s2, z : q1.z * s1 + q2.z * s2, w : q1.w * s1 + q2.w * s2 } } } }