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;
}
// 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: