public Vector3 llRot2Euler(IScriptInstance script, Quaternion rot)
{
lsl_rotation quat = rot;
// This implementation is from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions
lsl_rotation t = new lsl_rotation(quat.x * quat.x, quat.y * quat.y, quat.z * quat.z, quat.s * quat.s);
double m = (t.x + t.y + t.z + t.s);
if (m == 0.0) return lsl_vector.Zero;
double n = 2 * (quat.y * quat.s + quat.x * quat.z);
double p = m * m - n * n;
if (p > 0.0)
{
return new lsl_vector(
NormalizeAngle(Math.Atan2(2.0 * (quat.x * quat.s - quat.y * quat.z), (-t.x - t.y + t.z + t.s))),
NormalizeAngle(Math.Atan2(n, Math.Sqrt(p))),
NormalizeAngle(Math.Atan2(2.0 * (quat.z * quat.s - quat.x * quat.y), (t.x - t.y - t.z + t.s))));
}
else if (n > 0.0)
{
return new lsl_vector(
0.0,
Math.PI * 0.5,
NormalizeAngle(Math.Atan2((quat.z * quat.s + quat.x * quat.y), 0.5 - t.x - t.z)));
}
else
{
return new lsl_vector(
0.0,
-Math.PI * 0.5,
NormalizeAngle(Math.Atan2((quat.z * quat.s + quat.x * quat.y), 0.5 - t.x - t.z)));
}
}
public lsl_rotation(lsl_rotation Quat)
{
x = (float)Quat.x;
y = (float)Quat.y;
z = (float)Quat.z;
s = (float)Quat.s;
if (x == 0 && y == 0 && z == 0 && s == 0)
s = 1;
}
public static double Mag(lsl_rotation q)
{
return Math.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.s * q.s);
}
// Vector-Rotation Math
public static lsl_vector operator *(lsl_vector v, lsl_rotation r)
{
lsl_rotation vq = new lsl_rotation(v.x, v.y, v.z, 0);
lsl_rotation nq = new lsl_rotation(-r.x, -r.y, -r.z, r.s);
// adapted for operator * computing "b * a"
lsl_rotation result = nq * (vq * r);
return new lsl_vector(result.x, result.y, result.z);
}