From eb9bf717264083ad76022dff6a2284ad9393ac38 Mon Sep 17 00:00:00 2001 From: John Cochran Date: Tue, 3 Jan 2012 11:38:38 -0600 Subject: Replaced llRot2Euler function. The original function suffered from unexpected results due to rounding errors. An error of only 1 or 2 ulps would cause the code to not detect a singularity at Y rotation +/- PI/2 and take the non-singularity code path. The replacement code does not suffer from wildly inaccurate results at the +/- PI/2 singularity. The check in the code for the singularity isn't strictly needed, but gives more consistent results At the singularity, the X and Z rotations add. The if check simply forces the X rotation to be zero so the entirety of the X+Z rotation is carried by Z. Additionally, the test code has been updated to include test cases that caused the old code to fail. The test algorithm is also updated to perform a more meaningful test. The original code checked if the values against expected values. This could fail at +/- PI rotations since a rotation around an axis by PI causes the identical effect as a rotation by -PI. The new test code checks that the returned angles can be used to recreate a quaternion that causes the same rotation. --- .../Shared/Api/Implementation/LSL_Api.cs | 31 +++++++++------------- 1 file changed, 13 insertions(+), 18 deletions(-) (limited to 'OpenSim/Region/ScriptEngine/Shared/Api/Implementation') diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs index 443e7a5..d6316b2 100644 --- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs +++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs @@ -468,26 +468,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api //Now we start getting into quaternions which means sin/cos, matrices and vectors. ckrinke - // Old implementation of llRot2Euler. Normalization not required as Atan2 function will - // only return values >= -PI (-180 degrees) and <= PI (180 degrees). - + // Using algorithm based off http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/quat_2_euler_paper_ver2-1.pdf + // to avoid issues with singularity and rounding with Y rotation of +/- PI/2 public LSL_Vector llRot2Euler(LSL_Rotation r) { - 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(Math.Atan2(2.0 * (r.x * r.s - r.y * r.z), (-t.x - t.y + t.z + t.s)), - Math.Atan2(n, Math.Sqrt(p)), - 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, 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, Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z)); + LSL_Vector v = new LSL_Vector(0.0, 0.0, 1.0) * r; // Z axis unit vector unaffected by Z rotation component of r. + double m = LSL_Vector.Mag(v); // Just in case v isn't normalized, need magnitude for Asin() operation later. + if (m == 0.0) return new LSL_Vector(); + double x = Math.Atan2(-v.y, v.z); + double sin = v.x / m; + if (sin < -0.999999 || sin > 0.999999) x = 0.0; // Force X rotation to 0 at the singularities. + double y = Math.Asin(sin); + // Rotate X axis unit vector by r and unwind the X and Y rotations leaving only the Z rotation + v = new LSL_Vector(1.0, 0.0, 0.0) * ((r * new LSL_Rotation(Math.Sin(-x / 2.0), 0.0, 0.0, Math.Cos(-x / 2.0))) * new LSL_Rotation(0.0, Math.Sin(-y / 2.0), 0.0, Math.Cos(-y / 2.0))); + double z = Math.Atan2(v.y, v.x); + return new LSL_Vector(x, y, z); } /* From wiki: -- cgit v1.1