aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
-rw-r--r--OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs141
1 files changed, 44 insertions, 97 deletions
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
294 294
295 //Now we start getting into quaternions which means sin/cos, matrices and vectors. ckrinke 295 //Now we start getting into quaternions which means sin/cos, matrices and vectors. ckrinke
296 296
297 // Xantor's new llRot2Euler 297 // Utility function for llRot2Euler
298 public LSL_Types.Vector3 llRot2Euler(LSL_Types.Quaternion r) 298
299 // normalize an angle between 0 - 2*PI (0 and 360 degrees)
300 private double NormalizeAngle(double angle)
299 { 301 {
300 m_host.AddScriptLPS(1); 302 angle = angle % (Math.PI * 2);
301 double x, y, z; 303 if (angle < 0) angle = angle + Math.PI * 2;
302 double sqw = r.s*r.s; 304 return angle;
303 double sqx = r.x*r.x;
304 double sqy = r.y*r.y;
305 double sqz = r.z*r.z;
306 double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
307 double test = r.x*r.y + r.z*r.s;
308 if (test > 0.499 * unit) // singularity at north pole
309 {
310 x = 0;
311 y = 2 * Math.Atan2(r.x, r.s);
312 z = Math.PI/2;
313 return new LSL_Types.Vector3(x, y, z);
314 }
315 if (test < -0.499 * unit) // singularity at south pole
316 {
317 x = 0;
318 y = -2 * Math.Atan2(r.x,r.s);
319 z = -Math.PI/2;
320 return new LSL_Types.Vector3(x, y, z);
321 }
322 x = Math.Atan2(2 * r.x * r.s - 2 * r.y * r.z, -sqx + sqy - sqz + sqw);
323 y = Math.Atan2(2*r.y*r.s-2*r.x*r.z , sqx - sqy - sqz + sqw);
324 z = Math.Asin(2*test/unit);
325 return new LSL_Types.Vector3(x, y, z);
326 } 305 }
327
328 306
329 // Old implementation of llRot2Euler 307
330 /* 308 // Old implementation of llRot2Euler, now normalized
309
331 public LSL_Types.Vector3 llRot2Euler(LSL_Types.Quaternion r) 310 public LSL_Types.Vector3 llRot2Euler(LSL_Types.Quaternion r)
332 { 311 {
333 m_host.AddScriptLPS(1); 312 m_host.AddScriptLPS(1);
@@ -338,88 +317,56 @@ namespace OpenSim.Region.ScriptEngine.Common
338 double n = 2 * (r.y * r.s + r.x * r.z); 317 double n = 2 * (r.y * r.s + r.x * r.z);
339 double p = m * m - n * n; 318 double p = m * m - n * n;
340 if (p > 0) 319 if (p > 0)
341 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)), 320 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))),
342 Math.Atan2(n, Math.Sqrt(p)), 321 NormalizeAngle(Math.Atan2(n, Math.Sqrt(p))),
343 Math.Atan2(2.0 * (r.z * r.s - r.x * r.y), (t.x - t.y - t.z + t.s))); 322 NormalizeAngle(Math.Atan2(2.0 * (r.z * r.s - r.x * r.y), (t.x - t.y - t.z + t.s))));
344 else if (n > 0) 323 else if (n > 0)
345 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)); 324 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)));
346 else 325 else
347 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)); 326 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)));
348 } 327 }
349 */ 328
350 329
351 // Xantor's new llEuler2Rot() 330 // Xantor's newer llEuler2Rot() *try the second* inverted quaternions (-x,-y,-z,w) as LL seems to like
331 // New and improved, now actually works as described. Prim rotates as expected as does llRot2Euler.
332
352 /* From wiki: 333 /* From wiki:
353 The Euler angle vector (in radians) is converted to a rotation by doing the rotations around the 3 axes 334 The Euler angle vector (in radians) is converted to a rotation by doing the rotations around the 3 axes
354 in Z, Y, X order. So llEuler2Rot(<1.0, 2.0, 3.0> * DEG_TO_RAD) generates a rotation by taking the zero rotation, 335 in Z, Y, X order. So llEuler2Rot(<1.0, 2.0, 3.0> * DEG_TO_RAD) generates a rotation by taking the zero rotation,
355 a vector pointing along the X axis, first rotating it 3 degrees around the global Z axis, then rotating the resulting 336 a vector pointing along the X axis, first rotating it 3 degrees around the global Z axis, then rotating the resulting
356 vector 2 degrees around the global Y axis, and finally rotating that 1 degree around the global X axis. 337 vector 2 degrees around the global Y axis, and finally rotating that 1 degree around the global X axis.
357 */ 338 */
339
358 public LSL_Types.Quaternion llEuler2Rot(LSL_Types.Vector3 v) 340 public LSL_Types.Quaternion llEuler2Rot(LSL_Types.Vector3 v)
359 { 341 {
360 m_host.AddScriptLPS(1); 342 m_host.AddScriptLPS(1);
361 343
362 double x,y,z,s; 344 double x,y,z,s,s_i;
363
364 double c1 = Math.Cos(v.y / 2);
365 double s1 = Math.Sin(v.y / 2);
366 double c2 = Math.Cos(v.z / 2);
367 double s2 = Math.Sin(v.z / 2);
368 double c3 = Math.Cos(v.x / 2);
369 double s3 = Math.Sin(v.x / 2);
370
371 double c1c2 = c1 * c2;
372 double s1s2 = s1 * s2;
373
374 s = c1c2 * c3 - s1s2 * s3;
375 x = c1c2 * s3 + s1s2 * c3;
376 y = s1 * c2 * c3 + c1 * s2 * s3;
377 z = c1 * s2 * c3 - s1 * c2 * s3;
378 345
346 double cosX = Math.Cos(v.x);
347 double cosY = Math.Cos(v.y);
348 double cosZ = Math.Cos(v.z);
349 double sinX = Math.Sin(v.x);
350 double sinY = Math.Sin(v.y);
351 double sinZ = Math.Sin(v.z);
352
353 s = Math.Sqrt( cosY * cosZ - sinX * sinY * sinZ + cosX * cosZ + cosX * cosY + 1.0f) * 0.5f;
354 if (Math.Abs(s) < 0.00001) // null rotation
355 {
356 x = 0.0f;
357 y = 1.0f;
358 z = 0.0f;
359 }
360 else
361 {
362 s_i = 1.0f / (4.0f * s);
363 x = - ( -sinX * cosY - cosX * sinY * sinZ - sinX * cosZ) * s_i;
364 y = - ( -cosX * sinY * cosZ + sinX * sinZ - sinY) * s_i;
365 z = - ( -cosY * sinZ - sinX * sinY * cosZ - cosX * sinZ) * s_i;
366 }
379 return new LSL_Types.Quaternion(x, y, z, s); 367 return new LSL_Types.Quaternion(x, y, z, s);
380 } 368 }
381 369
382
383 /*
384 // Old implementation
385 public LSL_Types.Quaternion llEuler2Rot(LSL_Types.Vector3 v)
386 {
387 m_host.AddScriptLPS(1);
388 //this comes from from http://lslwiki.net/lslwiki/wakka.php?wakka=LibraryRotationFunctions but is incomplete as of 8/19/07
389 float err = 0.00001f;
390 double ax = Math.Sin(v.x / 2);
391 double aw = Math.Cos(v.x / 2);
392 double by = Math.Sin(v.y / 2);
393 double bw = Math.Cos(v.y / 2);
394 double cz = Math.Sin(v.z / 2);
395 double cw = Math.Cos(v.z / 2);
396 LSL_Types.Quaternion a1 = new LSL_Types.Quaternion(0.0, 0.0, cz, cw);
397 LSL_Types.Quaternion a2 = new LSL_Types.Quaternion(0.0, by, 0.0, bw);
398 LSL_Types.Quaternion a3 = new LSL_Types.Quaternion(ax, 0.0, 0.0, aw);
399 LSL_Types.Quaternion a = (a1 * a2) * a3;
400 //This multiplication doesnt compile, yet. a = a1 * a2 * a3;
401 LSL_Types.Quaternion b = new LSL_Types.Quaternion(ax * bw * cw + aw * by * cz,
402 aw * by * cw - ax * bw * cz, aw * bw * cz + ax * by * cw,
403 aw * bw * cw - ax * by * cz);
404 LSL_Types.Quaternion c = new LSL_Types.Quaternion();
405 //This addition doesnt compile yet c = a + b;
406 LSL_Types.Quaternion d = new LSL_Types.Quaternion();
407 //This addition doesnt compile yet d = a - b;
408 if ((Math.Abs(c.x) > err && Math.Abs(d.x) > err) ||
409 (Math.Abs(c.y) > err && Math.Abs(d.y) > err) ||
410 (Math.Abs(c.z) > err && Math.Abs(d.z) > err) ||
411 (Math.Abs(c.s) > err && Math.Abs(d.s) > err))
412 {
413 return b;
414 //return a new Quaternion that is null until I figure this out
415 // return b;
416 // return a;
417 }
418 return a;
419 }
420
421 */
422
423 370
424 public LSL_Types.Quaternion llAxes2Rot(LSL_Types.Vector3 fwd, LSL_Types.Vector3 left, LSL_Types.Vector3 up) 371 public LSL_Types.Quaternion llAxes2Rot(LSL_Types.Vector3 fwd, LSL_Types.Vector3 left, LSL_Types.Vector3 up)
425 { 372 {