From d1c8b083b8d44417d67fb401646eaa7215ff0851 Mon Sep 17 00:00:00 2001 From: Tom Grimshaw Date: Thu, 17 Jun 2010 04:15:07 -0700 Subject: [PATCH] Fix llRot2Euler with a mathematically sound implementation. The only difference between this an SL is that SL resolves a figure of negative PI into PI, and does the equivalent in the reverse (llEuler2Rot, -1.0 becomes 1.0); --- .../Shared/Api/Implementation/LSL_Api.cs | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs index 877c3cba08..fe8c70ee57 100644 --- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs +++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs @@ -502,25 +502,33 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api return remainder; } - // Old implementation of llRot2Euler, now normalized - - public LSL_Vector llRot2Euler(LSL_Rotation r) + public LSL_Vector llRot2Euler(LSL_Rotation q1) { m_host.AddScriptLPS(1); - //This implementation is from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions. ckrinke - LSL_Rotation t = new LSL_Rotation(r.x * r.x, r.y * r.y, r.z * r.z, r.s * r.s); - double m = (t.x + t.y + t.z + t.s); - if (m == 0) return new LSL_Vector(); - double n = 2 * (r.y * r.s + r.x * r.z); - double p = m * m - n * n; - if (p > 0) - return new LSL_Vector(NormalizeAngle(Math.Atan2(2.0 * (r.x * r.s - r.y * r.z), (-t.x - t.y + t.z + t.s))), - NormalizeAngle(Math.Atan2(n, Math.Sqrt(p))), - NormalizeAngle(Math.Atan2(2.0 * (r.z * r.s - r.x * r.y), (t.x - t.y - t.z + t.s)))); - else if (n > 0) - return new LSL_Vector(0.0, Math.PI * 0.5, NormalizeAngle(Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z))); - else - return new LSL_Vector(0.0, -Math.PI * 0.5, NormalizeAngle(Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z))); + LSL_Vector eul = new LSL_Vector(); + + double sqw = q1.s*q1.s; + double sqx = q1.x*q1.x; + double sqy = q1.z*q1.z; + double sqz = q1.y*q1.y; + double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor + double test = q1.x*q1.z + q1.y*q1.s; + if (test > 0.4999*unit) { // singularity at north pole + eul.z = 2 * Math.Atan2(q1.x,q1.s); + eul.y = Math.PI/2; + eul.x = 0; + return eul; + } + if (test < -0.4999*unit) { // singularity at south pole + eul.z = -2 * Math.Atan2(q1.x,q1.s); + eul.y = -Math.PI/2; + eul.x = 0; + return eul; + } + eul.z = Math.Atan2(2*q1.z*q1.s-2*q1.x*q1.y , sqx - sqy - sqz + sqw); + eul.y = Math.Asin(2*test/unit); + eul.x = Math.Atan2(2*q1.x*q1.s-2*q1.z*q1.y , -sqx + sqy - sqz + sqw); + return eul; } /* From wiki: