diff options
author | dahlia | 2010-06-28 17:33:47 -0700 |
---|---|---|
committer | dahlia | 2010-06-28 17:33:47 -0700 |
commit | 621195e4f182d475b087049f4489e4c9187dd474 (patch) | |
tree | dc23773d9686c0a49df144435c7c7198e6a9987c /OpenSim | |
parent | One more bug fix in FriendsModule: GrantRights. One of the caches was not bei... (diff) | |
download | opensim-SC_OLD-621195e4f182d475b087049f4489e4c9187dd474.zip opensim-SC_OLD-621195e4f182d475b087049f4489e4c9187dd474.tar.gz opensim-SC_OLD-621195e4f182d475b087049f4489e4c9187dd474.tar.bz2 opensim-SC_OLD-621195e4f182d475b087049f4489e4c9187dd474.tar.xz |
Thanks Micheil Merlin for Mantis #4784: [PATCH] llRotBetween does not return correct rotations for a 180 degree angle between vectors
Signed-off-by: dahlia <dahliaTrimble@gmail.removeme.com>
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs | 89 |
1 files changed, 71 insertions, 18 deletions
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs index 1feb153..dc34e1c 100644 --- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs +++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs | |||
@@ -703,24 +703,77 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api | |||
703 | 703 | ||
704 | public LSL_Rotation llRotBetween(LSL_Vector a, LSL_Vector b) | 704 | public LSL_Rotation llRotBetween(LSL_Vector a, LSL_Vector b) |
705 | { | 705 | { |
706 | //A and B should both be normalized | 706 | //A and B should both be normalized |
707 | m_host.AddScriptLPS(1); | 707 | m_host.AddScriptLPS(1); |
708 | double dotProduct = LSL_Vector.Dot(a, b); | 708 | LSL_Rotation rotBetween; |
709 | LSL_Vector crossProduct = LSL_Vector.Cross(a, b); | 709 | // Check for zero vectors. If either is zero, return zero rotation. Otherwise, |
710 | double magProduct = LSL_Vector.Mag(a) * LSL_Vector.Mag(b); | 710 | // continue calculation. |
711 | double angle = Math.Acos(dotProduct / magProduct); | 711 | if (a == new LSL_Vector(0.0f, 0.0f, 0.0f) || b == new LSL_Vector(0.0f, 0.0f, 0.0f)) |
712 | LSL_Vector axis = LSL_Vector.Norm(crossProduct); | 712 | { |
713 | double s = Math.Sin(angle / 2); | 713 | rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f); |
714 | 714 | } | |
715 | double x = axis.x * s; | 715 | else |
716 | double y = axis.y * s; | 716 | { |
717 | double z = axis.z * s; | 717 | a = LSL_Vector.Norm(a); |
718 | double w = Math.Cos(angle / 2); | 718 | b = LSL_Vector.Norm(b); |
719 | 719 | double dotProduct = LSL_Vector.Dot(a, b); | |
720 | if (Double.IsNaN(x) || Double.IsNaN(y) || Double.IsNaN(z) || Double.IsNaN(w)) | 720 | // There are two degenerate cases possible. These are for vectors 180 or |
721 | return new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f); | 721 | // 0 degrees apart. These have to be detected and handled individually. |
722 | 722 | // | |
723 | return new LSL_Rotation((float)x, (float)y, (float)z, (float)w); | 723 | // Check for vectors 180 degrees apart. |
724 | // A dot product of -1 would mean the angle between vectors is 180 degrees. | ||
725 | if (dotProduct < -0.9999999f) | ||
726 | { | ||
727 | // First assume X axis is orthogonal to the vectors. | ||
728 | LSL_Vector orthoVector = new LSL_Vector(1.0f, 0.0f, 0.0f); | ||
729 | orthoVector = orthoVector - a * (a.x / LSL_Vector.Dot(a, a)); | ||
730 | // Check for near zero vector. A very small non-zero number here will create | ||
731 | // a rotation in an undesired direction. | ||
732 | if (LSL_Vector.Mag(orthoVector) > 0.0001) | ||
733 | { | ||
734 | rotBetween = new LSL_Rotation(orthoVector.x, orthoVector.y, orthoVector.z, 0.0f); | ||
735 | } | ||
736 | // If the magnitude of the vector was near zero, then assume the X axis is not | ||
737 | // orthogonal and use the Z axis instead. | ||
738 | else | ||
739 | { | ||
740 | // Set 180 z rotation. | ||
741 | rotBetween = new LSL_Rotation(0.0f, 0.0f, 1.0f, 0.0f); | ||
742 | } | ||
743 | } | ||
744 | // Check for parallel vectors. | ||
745 | // A dot product of 1 would mean the angle between vectors is 0 degrees. | ||
746 | else if (dotProduct > 0.9999999f) | ||
747 | { | ||
748 | // Set zero rotation. | ||
749 | rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f); | ||
750 | } | ||
751 | else | ||
752 | { | ||
753 | // All special checks have been performed so get the axis of rotation. | ||
754 | LSL_Vector crossProduct = LSL_Vector.Cross(a, b); | ||
755 | // Quarternion s value is the length of the unit vector + dot product. | ||
756 | double qs = 1.0 + dotProduct; | ||
757 | rotBetween = new LSL_Rotation(crossProduct.x, crossProduct.y, crossProduct.z, qs); | ||
758 | // Normalize the rotation. | ||
759 | double mag = LSL_Rotation.Mag(rotBetween); | ||
760 | // We shouldn't have to worry about a divide by zero here. The qs value will be | ||
761 | // non-zero because we already know if we're here, then the dotProduct is not -1 so | ||
762 | // qs will not be zero. Also, we've already handled the input vectors being zero so the | ||
763 | // crossProduct vector should also not be zero. | ||
764 | rotBetween.x = rotBetween.x / mag; | ||
765 | rotBetween.y = rotBetween.y / mag; | ||
766 | rotBetween.z = rotBetween.z / mag; | ||
767 | rotBetween.s = rotBetween.s / mag; | ||
768 | // Check for undefined values and set zero rotation if any found. This code might not actually be required | ||
769 | // any longer since zero vectors are checked for at the top. | ||
770 | if (Double.IsNaN(rotBetween.x) || Double.IsNaN(rotBetween.y) || Double.IsNaN(rotBetween.z) || Double.IsNaN(rotBetween.s)) | ||
771 | { | ||
772 | rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f); | ||
773 | } | ||
774 | } | ||
775 | } | ||
776 | return rotBetween; | ||
724 | } | 777 | } |
725 | 778 | ||
726 | public void llWhisper(int channelID, string text) | 779 | public void llWhisper(int channelID, string text) |