From bbaf2fe75e2d1da4b466a3cb8773b7b1a81f2f34 Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Thu, 15 May 2008 19:36:13 +0000 Subject: Committing Xantor's LLEuler3Rot still broken fix patch. Mantis 001235. Thanks Xantor! --- .../ScriptEngine/Common/LSL_BuiltIn_Commands.cs | 141 +++++++-------------- 1 file changed, 44 insertions(+), 97 deletions(-) (limited to 'OpenSim/Region/ScriptEngine') diff --git a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs index abc4bca..d73a47a 100644 --- a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs +++ b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs @@ -294,40 +294,19 @@ namespace OpenSim.Region.ScriptEngine.Common //Now we start getting into quaternions which means sin/cos, matrices and vectors. ckrinke - // Xantor's new llRot2Euler - public LSL_Types.Vector3 llRot2Euler(LSL_Types.Quaternion r) + // Utility function for llRot2Euler + + // normalize an angle between 0 - 2*PI (0 and 360 degrees) + private double NormalizeAngle(double angle) { - m_host.AddScriptLPS(1); - double x, y, z; - double sqw = r.s*r.s; - double sqx = r.x*r.x; - double sqy = r.y*r.y; - double sqz = r.z*r.z; - double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor - double test = r.x*r.y + r.z*r.s; - if (test > 0.499 * unit) // singularity at north pole - { - x = 0; - y = 2 * Math.Atan2(r.x, r.s); - z = Math.PI/2; - return new LSL_Types.Vector3(x, y, z); - } - if (test < -0.499 * unit) // singularity at south pole - { - x = 0; - y = -2 * Math.Atan2(r.x,r.s); - z = -Math.PI/2; - return new LSL_Types.Vector3(x, y, z); - } - x = Math.Atan2(2 * r.x * r.s - 2 * r.y * r.z, -sqx + sqy - sqz + sqw); - y = Math.Atan2(2*r.y*r.s-2*r.x*r.z , sqx - sqy - sqz + sqw); - z = Math.Asin(2*test/unit); - return new LSL_Types.Vector3(x, y, z); + angle = angle % (Math.PI * 2); + if (angle < 0) angle = angle + Math.PI * 2; + return angle; } - - // Old implementation of llRot2Euler - /* + + // Old implementation of llRot2Euler, now normalized + public LSL_Types.Vector3 llRot2Euler(LSL_Types.Quaternion r) { m_host.AddScriptLPS(1); @@ -338,88 +317,56 @@ namespace OpenSim.Region.ScriptEngine.Common double n = 2 * (r.y * r.s + r.x * r.z); double p = m * m - n * n; if (p > 0) - return new LSL_Types.Vector3(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))); + return new LSL_Types.Vector3(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_Types.Vector3(0.0, Math.PI / 2, Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z)); + return new LSL_Types.Vector3(0.0, Math.PI / 2, NormalizeAngle(Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z))); else - return new LSL_Types.Vector3(0.0, -Math.PI / 2, Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z)); + return new LSL_Types.Vector3(0.0, -Math.PI / 2, NormalizeAngle(Math.Atan2((r.z * r.s + r.x * r.y), 0.5 - t.x - t.z))); } - */ - - // Xantor's new llEuler2Rot() + + + // Xantor's newer llEuler2Rot() *try the second* inverted quaternions (-x,-y,-z,w) as LL seems to like + // New and improved, now actually works as described. Prim rotates as expected as does llRot2Euler. + /* From wiki: The Euler angle vector (in radians) is converted to a rotation by doing the rotations around the 3 axes in Z, Y, X order. So llEuler2Rot(<1.0, 2.0, 3.0> * DEG_TO_RAD) generates a rotation by taking the zero rotation, a vector pointing along the X axis, first rotating it 3 degrees around the global Z axis, then rotating the resulting vector 2 degrees around the global Y axis, and finally rotating that 1 degree around the global X axis. */ + public LSL_Types.Quaternion llEuler2Rot(LSL_Types.Vector3 v) { m_host.AddScriptLPS(1); - double x,y,z,s; - - double c1 = Math.Cos(v.y / 2); - double s1 = Math.Sin(v.y / 2); - double c2 = Math.Cos(v.z / 2); - double s2 = Math.Sin(v.z / 2); - double c3 = Math.Cos(v.x / 2); - double s3 = Math.Sin(v.x / 2); - - double c1c2 = c1 * c2; - double s1s2 = s1 * s2; - - s = c1c2 * c3 - s1s2 * s3; - x = c1c2 * s3 + s1s2 * c3; - y = s1 * c2 * c3 + c1 * s2 * s3; - z = c1 * s2 * c3 - s1 * c2 * s3; + double x,y,z,s,s_i; + double cosX = Math.Cos(v.x); + double cosY = Math.Cos(v.y); + double cosZ = Math.Cos(v.z); + double sinX = Math.Sin(v.x); + double sinY = Math.Sin(v.y); + double sinZ = Math.Sin(v.z); + + s = Math.Sqrt( cosY * cosZ - sinX * sinY * sinZ + cosX * cosZ + cosX * cosY + 1.0f) * 0.5f; + if (Math.Abs(s) < 0.00001) // null rotation + { + x = 0.0f; + y = 1.0f; + z = 0.0f; + } + else + { + s_i = 1.0f / (4.0f * s); + x = - ( -sinX * cosY - cosX * sinY * sinZ - sinX * cosZ) * s_i; + y = - ( -cosX * sinY * cosZ + sinX * sinZ - sinY) * s_i; + z = - ( -cosY * sinZ - sinX * sinY * cosZ - cosX * sinZ) * s_i; + } return new LSL_Types.Quaternion(x, y, z, s); } - - - /* - // Old implementation - public LSL_Types.Quaternion llEuler2Rot(LSL_Types.Vector3 v) - { - m_host.AddScriptLPS(1); - //this comes from from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions but is incomplete as of 8/19/07 - float err = 0.00001f; - double ax = Math.Sin(v.x / 2); - double aw = Math.Cos(v.x / 2); - double by = Math.Sin(v.y / 2); - double bw = Math.Cos(v.y / 2); - double cz = Math.Sin(v.z / 2); - double cw = Math.Cos(v.z / 2); - LSL_Types.Quaternion a1 = new LSL_Types.Quaternion(0.0, 0.0, cz, cw); - LSL_Types.Quaternion a2 = new LSL_Types.Quaternion(0.0, by, 0.0, bw); - LSL_Types.Quaternion a3 = new LSL_Types.Quaternion(ax, 0.0, 0.0, aw); - LSL_Types.Quaternion a = (a1 * a2) * a3; - //This multiplication doesnt compile, yet. a = a1 * a2 * a3; - LSL_Types.Quaternion b = new LSL_Types.Quaternion(ax * bw * cw + aw * by * cz, - aw * by * cw - ax * bw * cz, aw * bw * cz + ax * by * cw, - aw * bw * cw - ax * by * cz); - LSL_Types.Quaternion c = new LSL_Types.Quaternion(); - //This addition doesnt compile yet c = a + b; - LSL_Types.Quaternion d = new LSL_Types.Quaternion(); - //This addition doesnt compile yet d = a - b; - if ((Math.Abs(c.x) > err && Math.Abs(d.x) > err) || - (Math.Abs(c.y) > err && Math.Abs(d.y) > err) || - (Math.Abs(c.z) > err && Math.Abs(d.z) > err) || - (Math.Abs(c.s) > err && Math.Abs(d.s) > err)) - { - return b; - //return a new Quaternion that is null until I figure this out - // return b; - // return a; - } - return a; - } - - */ - + public LSL_Types.Quaternion llAxes2Rot(LSL_Types.Vector3 fwd, LSL_Types.Vector3 left, LSL_Types.Vector3 up) { -- cgit v1.1