3 files changed, 247 insertions, 51 deletions

diff --git a/OpenSim/Region/Environment/Scenes/Scene.cs b/OpenSim/Region/Environment/Scenes/Scene.cs
index 8f3495c..9c0b33c 100644
--- a/OpenSim/Region/Environment/Scenes/Scene.cs
+++ b/OpenSim/Region/Environment/Scenes/Scene.cs
@@ -1067,46 +1067,51 @@ namespace OpenSim.Region.Environment.Scenes
             if (RayTargetID != LLUUID.Zero)
             {
                 SceneObjectPart target = GetSceneObjectPart(RayTargetID);
+                
+                LLVector3 direction = LLVector3.Norm(RayEnd - RayStart);
+                Vector3 AXOrigin = new Vector3(RayStart.X, RayStart.Y, RayStart.Z);
+                Vector3 AXdirection = new Vector3(direction.X, direction.Y, direction.Z);
+
                 if (target != null)
                 {
                     pos = target.AbsolutePosition;
                     //m_log.Info("[OBJECT_REZ]: TargetPos: " + pos.ToString() + ", RayStart: " + RayStart.ToString() + ", RayEnd: " + RayEnd.ToString() + ", Volume: " + Util.GetDistanceTo(RayStart,RayEnd).ToString() + ", mag1: " + Util.GetMagnitude(RayStart).ToString() + ", mag2: " + Util.GetMagnitude(RayEnd).ToString());
-                    //target.Scale.X
-                    if (Math.Abs(target.Scale.X - target.Scale.Y) > 4.5f 
-                        || Math.Abs(target.Scale.Y - target.Scale.Z) > 4.5f 
-                        || Math.Abs(target.Scale.Z - target.Scale.X) > 4.5f)
-                    {
-                        
-                        // for now lets use the old method here as the new method works by using the largest scale vector 
-                        // component as the radius of a sphere and produces wide results if there's a huge difference 
-                        // between the x/y/z vector components
-
-                        // If one scale component is less then .21m, it's likely being used as a thin block and therefore 
-                        // the raytracing would produce a wide result.
-                       
-                      
-                    }
-                    else
+                    
+                    // TODO: Raytrace better here
+                    
+                    //EntityIntersection ei = m_innerScene.GetClosestIntersectingPrim(new Ray(AXOrigin, AXdirection));
+                    Ray NewRay = new Ray(AXOrigin, AXdirection);
+
+                    // Ray Trace against target here
+                    EntityIntersection ei = target.TestIntersectionOBB(NewRay, new Quaternion(1,0,0,0));
+
+                    // Un-comment out the following line to Get Raytrace results printed to the console.
+                    //m_log.Info("[RAYTRACERESULTS]: Hit:" + ei.HitTF.ToString() + " Point: " + ei.ipoint.ToString() + " Normal: " + ei.normal.ToString());
+                    
+                    // If we hit something
+                    if (ei.HitTF)
                     {
-                        // TODO: Raytrace better here
-                        LLVector3 direction = LLVector3.Norm(RayEnd - RayStart);
-                        Vector3 AXOrigin = new Vector3(RayStart.X, RayStart.Y, RayStart.Z);
-                        Vector3 AXdirection = new Vector3(direction.X, direction.Y, direction.Z);
-                        EntityIntersection ei = m_innerScene.GetClosestIntersectingPrim(new Ray(AXOrigin, AXdirection));
-                        //m_log.Info("[RAYTRACERESULTS]: Hit:" + ei.HitTF.ToString() + " Point: " + ei.ipoint.ToString() + " Normal: " + ei.normal.ToString());
-                        
-                        if (ei.HitTF)
-                        {
-                            pos = new LLVector3(ei.ipoint.x, ei.ipoint.y, ei.ipoint.z);
-                        } 
+                        // Set the position to the intersection point
+                        pos = new LLVector3(ei.ipoint.x, ei.ipoint.y, ei.ipoint.z);
+                    } 
                         
-                    }
+                    
                     return pos;
                 }
                 else
                 {
-                    // fall back to our stupid functionality
-                    pos = RayEnd;
+                    // We don't have a target here, so we're going to raytrace all the objects in the scene.
+
+                    EntityIntersection ei = m_innerScene.GetClosestIntersectingPrim(new Ray(AXOrigin, AXdirection));
+
+                    // Un-comment the following line to print the raytrace results to the console.
+                    //m_log.Info("[RAYTRACERESULTS]: Hit:" + ei.HitTF.ToString() + " Point: " + ei.ipoint.ToString() + " Normal: " + ei.normal.ToString());
+
+                    if (ei.HitTF)
+                    {
+                        pos = new LLVector3(ei.ipoint.x, ei.ipoint.y, ei.ipoint.z);
+                    } 
+                    
                     return pos;
                 }
             }
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
index c83027d..1e63ae7 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs
@@ -402,7 +402,9 @@ namespace OpenSim.Region.Environment.Scenes
                         new Quaternion(GroupRotation.W, GroupRotation.X, GroupRotation.Y, GroupRotation.Z);
 
                     // Telling the prim to raytrace.
-                    EntityIntersection inter = part.TestIntersection(hRay, parentrotation);
+                    //EntityIntersection inter = part.TestIntersection(hRay, parentrotation);
+
+                    EntityIntersection inter = part.TestIntersectionOBB(hRay, parentrotation);
 
                     // This may need to be updated to the maximum draw distance possible..  
                     // We might (and probably will) be checking for prim creation from other sims
diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
index 51a633e..ddfa332 100644
--- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs
@@ -933,29 +933,60 @@ namespace OpenSim.Region.Environment.Scenes
 
             return returnresult;
         }
-        public EntityIntersection TestIntersectionOABB(Ray iray, Quaternion parentrot)
+
+        public double GetDistanceTo(Vector3 a, Vector3 b)
+        {
+            float dx = a.x - b.x;
+            float dy = a.y - b.y;
+            float dz = a.z - b.z;
+            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
+        }
+
+        public EntityIntersection TestIntersectionOBB(Ray iray, Quaternion parentrot)
         {
             // In this case we're using a rectangular prism, which has 6 faces and therefore 6 planes
             // This breaks down into the ray---> plane equation.
             // TODO: Change to take shape into account
             Vector3[] vertexes = new Vector3[8];
-            
-            Vector3[] FaceA = new Vector3[6];
-            Vector3[] FaceB = new Vector3[6];
-            Vector3[] FaceC = new Vector3[6];
-            Vector3[] FaceD = new Vector3[6];
 
-            Vector3[] normals = new Vector3[6];
+            float[] distance = new float[6];
+            Vector3[] FaceA = new Vector3[6]; // vertex A for Facei
+            Vector3[] FaceB = new Vector3[6]; // vertex B for Facei
+            Vector3[] FaceC = new Vector3[6]; // vertex C for Facei
+            Vector3[] FaceD = new Vector3[6]; // vertex D for Facei
+
+            Vector3[] normals = new Vector3[6]; // Normal for Facei
 
-            Vector3 AmBa = new Vector3(0, 0, 0);
-            Vector3 AmBb = new Vector3(0, 0, 0);
+            Vector3 AmBa = new Vector3(0, 0, 0); // Vertex A - Vertex B
+            Vector3 AmBb = new Vector3(0, 0, 0); // Vertex B - Vertex C
+            Vector3 cross = new Vector3();
 
             LLVector3 pos = GetWorldPosition();
             LLQuaternion rot = GetWorldRotation();
+
+            // Variables prefixed with AX are Axiom.Math copies of the LL variety.
+
             Quaternion AXrot = new Quaternion(rot.W,rot.X,rot.Y,rot.Z);
+            AXrot.Normalize();
+
+            Vector3 AXpos = new Vector3(pos.X, pos.Y, pos.Z);
+
+            // tScale is the offset to derive the vertex based on the scale.
+            // it's different for each vertex because we've got to rotate it 
+            // to get the world position of the vertex to produce the Oriented Bounding Box
+
+            Vector3 tScale = new Vector3(); 
 
+            Vector3 AXscale = new Vector3(m_shape.Scale.X * 0.5f, m_shape.Scale.Y * 0.5f, m_shape.Scale.Z * 0.5f);
+
+            //Vector3 pScale = (AXscale) - (AXrot.Inverse() * (AXscale));
+            //Vector3 nScale = (AXscale * -1) - (AXrot.Inverse() * (AXscale * -1));
             
+            // rScale is the rotated offset to find a vertex based on the scale and the world rotation.
+            Vector3 rScale = new Vector3();
+
             // Get Vertexes for Faces Stick them into ABCD for each Face
+            // Form: Face<vertex>[face] that corresponds to the below diagram
             #region ABCD Face Vertex Map Comment Diagram
             //                   A _________ B
             //                    |         |
@@ -987,64 +1018,222 @@ namespace OpenSim.Region.Environment.Scenes
             //                    |_________|
             //                   A           B   
             #endregion
-            vertexes[0] = (AXrot * new Vector3((pos.X - m_shape.Scale.X),(pos.Y - m_shape.Scale.Y),(pos.Z + m_shape.Scale.Z)));
+
+            #region Plane Decomposition of Oriented Bounding Box
+            tScale = new Vector3(AXscale.x, -AXscale.y, AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[0] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+           // vertexes[0].x = pos.X + vertexes[0].x;
+            //vertexes[0].y = pos.Y + vertexes[0].y;
+            //vertexes[0].z = pos.Z + vertexes[0].z;
 
             FaceA[0] = vertexes[0];
             FaceA[3] = vertexes[0];
             FaceA[4] = vertexes[0];
 
-            vertexes[1] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z)));
+            tScale = AXscale;
+            rScale = ((AXrot * tScale));
+            vertexes[1] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+           // vertexes[1].x = pos.X + vertexes[1].x;
+           // vertexes[1].y = pos.Y + vertexes[1].y;
+            //vertexes[1].z = pos.Z + vertexes[1].z;
 
             FaceB[0] = vertexes[1];
             FaceA[1] = vertexes[1];
             FaceC[4] = vertexes[1];
 
-            vertexes[2] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z)));
+            tScale = new Vector3(AXscale.x, -AXscale.y, -AXscale.z);
+            rScale = ((AXrot * tScale));
+
+            vertexes[2] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+            //vertexes[2].x = pos.X + vertexes[2].x;
+            //vertexes[2].y = pos.Y + vertexes[2].y;
+            //vertexes[2].z = pos.Z + vertexes[2].z;
 
             FaceC[0] = vertexes[2];
             FaceC[3] = vertexes[2];
             FaceC[5] = vertexes[2];
 
-            vertexes[3] = (AXrot * new Vector3((pos.X - m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z)));
+            tScale = new Vector3(AXscale.x, AXscale.y, -AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[3] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+            //vertexes[3].x = pos.X + vertexes[3].x;
+           // vertexes[3].y = pos.Y + vertexes[3].y;
+           // vertexes[3].z = pos.Z + vertexes[3].z;
 
             FaceD[0] = vertexes[3];
             FaceC[1] = vertexes[3];
             FaceA[5] = vertexes[3];
 
-            vertexes[4] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z)));
+            tScale = new Vector3(-AXscale.x, AXscale.y, AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[4] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+           // vertexes[4].x = pos.X + vertexes[4].x;
+           // vertexes[4].y = pos.Y + vertexes[4].y;
+           // vertexes[4].z = pos.Z + vertexes[4].z;
 
             FaceB[1] = vertexes[4];
             FaceA[2] = vertexes[4];
             FaceD[4] = vertexes[4];
 
-            vertexes[5] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y + m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z)));
+            tScale = new Vector3(-AXscale.x, AXscale.y, -AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[5] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+           // vertexes[5].x = pos.X + vertexes[5].x;
+           // vertexes[5].y = pos.Y + vertexes[5].y;
+           // vertexes[5].z = pos.Z + vertexes[5].z;
 
             FaceD[1] = vertexes[5];
             FaceC[2] = vertexes[5];
             FaceB[5] = vertexes[5];
 
-            vertexes[6] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z + m_shape.Scale.Z)));
+            tScale = new Vector3(-AXscale.x, -AXscale.y, AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[6] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+           // vertexes[6].x = pos.X + vertexes[6].x;
+           // vertexes[6].y = pos.Y + vertexes[6].y;
+           // vertexes[6].z = pos.Z + vertexes[6].z;
 
             FaceB[2] = vertexes[6];
             FaceB[3] = vertexes[6];
             FaceB[4] = vertexes[6];
 
-            vertexes[7] = (AXrot * new Vector3((pos.X + m_shape.Scale.X), (pos.Y - m_shape.Scale.Y), (pos.Z - m_shape.Scale.Z)));
+            tScale = new Vector3(-AXscale.x, -AXscale.y, -AXscale.z);
+            rScale = ((AXrot * tScale));
+            vertexes[7] = (new Vector3((pos.X + rScale.x), (pos.Y + rScale.y), (pos.Z + rScale.z)));
+
+           // vertexes[7].x = pos.X + vertexes[7].x;
+           // vertexes[7].y = pos.Y + vertexes[7].y;
+           // vertexes[7].z = pos.Z + vertexes[7].z;
 
             FaceD[2] = vertexes[7];
             FaceD[3] = vertexes[7];
             FaceD[5] = vertexes[7];
+            #endregion
 
             // Get our plane normals
             for (int i = 0; i < 6; i++)
             {
-                AmBa = FaceB[i] - FaceA[i];
-                AmBb = FaceC[i] - FaceA[i];
-                normals[i] = AmBa.Cross(AmBb);
+                //m_log.Info("[FACECALCULATION]: FaceA[" + i + "]=" + FaceA[i] + " FaceB[" + i + "]=" + FaceB[i] + " FaceC[" + i + "]=" + FaceC[i] + " FaceD[" + i + "]=" + FaceD[i]);
+                
+                // Our Plane direction
+                AmBa = FaceA[i] - FaceB[i];
+                AmBb = FaceB[i] - FaceC[i];
+                
+                cross = AmBb.Cross(AmBa);
+
+                // normalize the cross product to get the normal.
+                normals[i] = cross / cross.Length; 
+                
+                //m_log.Info("[NORMALS]: normals[ " + i + "]" + normals[i].ToString());
+                //distance[i] = (normals[i].x * AmBa.x + normals[i].y * AmBa.y + normals[i].z * AmBa.z) * -1;
             }
 
             EntityIntersection returnresult = new EntityIntersection();
+            
+            returnresult.distance = 1024;
+            float c = 0;
+            float a = 0;
+            float d = 0;
+            Vector3 q = new Vector3();
+
+            #region OBB Version 2 Experiment
+            //float fmin = 999999;
+            //float fmax = -999999;
+            //float s = 0;
+
+            //for (int i=0;i<6;i++)
+            //{
+                //s = iray.Direction.Dot(normals[i]);
+                //d = normals[i].Dot(FaceB[i]);
+
+                //if (s == 0)
+                //{
+                    //if (iray.Origin.Dot(normals[i]) > d)
+                    //{
+                        //return returnresult;
+                    //}
+                   // else 
+                    //{
+                        //continue;
+                    //}
+                //}
+                //a = (d - iray.Origin.Dot(normals[i])) / s;
+                //if ( iray.Direction.Dot(normals[i]) < 0)
+                //{
+                    //if (a > fmax)
+                    //{
+                        //if (a > fmin)
+                        //{
+                            //return returnresult;
+                        //}
+                        //fmax = a;
+                    //}
+
+                //}
+                //else 
+                //{
+                    //if (a < fmin) 
+                    //{
+                        //if (a < 0 || a < fmax)
+                        //{
+                            //return returnresult;
+                        //}
+                        //fmin = a;
+                    //}
+                //}
+            //}
+            //if (fmax > 0)
+            //    a= fmax;
+            //else
+           //     a=fmin;
+
+            //q = iray.Origin + a * iray.Direction;
+            #endregion
 
+            // Loop over faces (6 of them)
+            for (int i = 0; i < 6; i++)
+            {
+                AmBa = FaceA[i] - FaceB[i];
+                AmBb = FaceB[i] - FaceC[i];
+                d = normals[i].Dot(FaceB[i]);
+                c = iray.Direction.Dot(normals[i]);
+                if (c == 0)
+                    continue;
+
+                a = (d - iray.Origin.Dot(normals[i])) / c;
+
+                if (a < 0)
+                    continue;
+
+                // If the normal is pointing outside the object
+                if (iray.Direction.Dot(normals[i]) < 0)
+                {
+                    
+                    q = iray.Origin + a * iray.Direction;
+
+                    // Is this the closest hit to the object's origin?
+                    //float distance2 = (float)GetDistanceTo(q, iray.Origin);
+                    float distance2 = (float)GetDistanceTo(q, AXpos);
+
+                    if (distance2 < returnresult.distance)
+                    {
+                        returnresult.distance = distance2;
+                        returnresult.HitTF = true;
+                        returnresult.ipoint = q;
+                        //m_log.Info("[POINT]: " + q.ToString());
+                        returnresult.normal = 1;
+
+                    }
+                }
+
+            }
             return returnresult;
         }