OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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);

avinationmerge
Tom Grimshaw 2010-06-17 04:15:07 -07:00
parent b9c40320f1
commit d1c8b083b8
1 changed files with 25 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
View File

@ -502,25 +502,33 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
return remainder; return remainder;
} }
// Old implementation of llRot2Euler, now normalized public LSL_Vector llRot2Euler(LSL_Rotation q1)
public LSL_Vector llRot2Euler(LSL_Rotation r)
{ {
m_host.AddScriptLPS(1); m_host.AddScriptLPS(1);
//This implementation is from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions. ckrinke LSL_Vector eul = new LSL_Vector();
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); double sqw = q1.s*q1.s;
if (m == 0) return new LSL_Vector(); double sqx = q1.x*q1.x;
double n = 2 * (r.y * r.s + r.x * r.z); double sqy = q1.z*q1.z;
double p = m * m - n * n; double sqz = q1.y*q1.y;
if (p > 0) double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
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))), double test = q1.x*q1.z + q1.y*q1.s;
NormalizeAngle(Math.Atan2(n, Math.Sqrt(p))), if (test > 0.4999*unit) { // singularity at north pole
NormalizeAngle(Math.Atan2(2.0 * (r.z * r.s - r.x * r.y), (t.x - t.y - t.z + t.s)))); eul.z = 2 * Math.Atan2(q1.x,q1.s);
else if (n > 0) eul.y = Math.PI/2;
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))); eul.x = 0;
else return eul;
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))); }
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: /* From wiki: