2 files changed, 88 insertions, 61 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 4504e18..5d0baf3 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -227,8 +227,6 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         public bool LandAtTarget { get; private set; }
 
-        private bool m_followCamAuto;
-
         private int m_movementUpdateCount;
         private const int NumMovementsBetweenRayCast = 5;
 
@@ -357,6 +355,9 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         protected Vector3 m_lastCameraPosition;
 
+        private Vector4 m_lastCameraCollisionPlane = new Vector4(0f, 0f, 0f, 1);
+        private bool m_doingCamRayCast = false;
+
         public Vector3 CameraPosition { get; set; }
 
         public Quaternion CameraRotation
@@ -1407,36 +1408,40 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="collisionPoint"></param>
         /// <param name="localid"></param>
         /// <param name="distance"></param>
-        public void RayCastCameraCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 pNormal)
+        /// 
+
+        private void UpdateCameraCollisionPlane(Vector4 plane)
         {
-            const float POSITION_TOLERANCE = 0.02f;
-            const float VELOCITY_TOLERANCE = 0.02f;
-            const float ROTATION_TOLERANCE = 0.02f;
+            if (m_lastCameraCollisionPlane != plane)
+            {
+                m_lastCameraCollisionPlane = plane;
+                ControllingClient.SendCameraConstraint(plane);
+            }
+        }
 
-            if (m_followCamAuto)
+        public void RayCastCameraCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 pNormal)
+        {
+            m_doingCamRayCast = false;
+            if (hitYN && localid != LocalId)
             {
-                if (hitYN && localid != LocalId)
-                {
+                CameraConstraintActive = true;
+                pNormal.X = (float)Math.Round(pNormal.X, 2);
+                pNormal.Y = (float)Math.Round(pNormal.Y, 2);
+                pNormal.Z = (float)Math.Round(pNormal.Z, 2);
+                pNormal.Normalize();
+                collisionPoint.X = (float)Math.Round(collisionPoint.X, 1);
+                collisionPoint.Y = (float)Math.Round(collisionPoint.Y, 1);
+                collisionPoint.Z = (float)Math.Round(collisionPoint.Z, 1);
+
+                Vector4 plane = new Vector4(pNormal.X, pNormal.Y, pNormal.Z, Vector3.Dot(collisionPoint, pNormal));
+                UpdateCameraCollisionPlane(plane);
+            }
+            else
+            {
+                Vector4 plane = new Vector4(0.9f, 0.0f, 0.361f, -9000f); // not right...
+                UpdateCameraCollisionPlane(plane);
 
-                    CameraConstraintActive = true;
-                    //m_log.DebugFormat("[RAYCASTRESULT]: {0}, {1}, {2}, {3}", hitYN, collisionPoint, localid, distance);
-                    
-                    Vector3 normal = Vector3.Normalize(new Vector3(0f, 0f, collisionPoint.Z) - collisionPoint);
-                    ControllingClient.SendCameraConstraint(new Vector4(normal.X, normal.Y, normal.Z, -1 * Vector3.Distance(new Vector3(0,0,collisionPoint.Z),collisionPoint)));
-                }
-                else
-                {
-                    if (!m_pos.ApproxEquals(m_lastPosition, POSITION_TOLERANCE) ||
-                        !Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) ||
-                        !Rotation.ApproxEquals(m_lastRotation, ROTATION_TOLERANCE))
-                    {
-                        if (CameraConstraintActive)
-                        {
-                            ControllingClient.SendCameraConstraint(new Vector4(0f, 0.5f, 0.9f, -3000f));
-                            CameraConstraintActive = false;
-                        }
-                    }
-                }
+                CameraConstraintActive = false;
             }
         }
 
@@ -1511,12 +1516,6 @@ namespace OpenSim.Region.Framework.Scenes
             // DrawDistance = agentData.Far;
             DrawDistance = Scene.DefaultDrawDistance;
 
-            // Check if Client has camera in 'follow cam' or 'build' mode.
-            Vector3 camdif = (Vector3.One * Rotation - Vector3.One * CameraRotation);
-
-            m_followCamAuto = ((CameraUpAxis.Z > 0.959f && CameraUpAxis.Z < 0.98f)
-               && (Math.Abs(camdif.X) < 0.4f && Math.Abs(camdif.Y) < 0.4f)) ? true : false;
-
             m_mouseLook = (flags & AgentManager.ControlFlags.AGENT_CONTROL_MOUSELOOK) != 0;
             m_leftButtonDown = (flags & AgentManager.ControlFlags.AGENT_CONTROL_LBUTTON_DOWN) != 0;
 
@@ -1536,26 +1535,37 @@ namespace OpenSim.Region.Framework.Scenes
                 StandUp();
             }
 
-            //m_log.DebugFormat("[FollowCam]: {0}", m_followCamAuto);
             // Raycast from the avatar's head to the camera to see if there's anything blocking the view
-            if ((m_movementUpdateCount % NumMovementsBetweenRayCast) == 0 && m_scene.PhysicsScene.SupportsRayCast())
+            // this exclude checks may not be complete
+
+            if (!m_doingCamRayCast && !m_mouseLook && m_scene.PhysicsScene.SupportsRayCast() && ParentID == 0)
             {
-                if (m_followCamAuto)
+                Vector3 posAdjusted = AbsolutePosition;
+                posAdjusted.Z += 0.5f * Appearance.AvatarSize.Z - 0.5f;
+
+                Vector3 tocam = CameraPosition - posAdjusted;
+                tocam.X = (float)Math.Round(tocam.X, 1);
+                tocam.Y = (float)Math.Round(tocam.Y, 1);
+                tocam.Z = (float)Math.Round(tocam.Z, 1);
+                
+                float distTocamlen = tocam.Length();
+                if (distTocamlen > 0.3f)
                 {
-                    //                    Vector3 posAdjusted = m_pos + HEAD_ADJUSTMENT;
-                    //                    m_scene.PhysicsScene.RaycastWorld(m_pos, Vector3.Normalize(CameraPosition - posAdjusted), Vector3.Distance(CameraPosition, posAdjusted) + 0.3f, RayCastCameraCallback);
-                    
-                    Vector3 posAdjusted = AbsolutePosition + HEAD_ADJUSTMENT;
-                    Vector3 distTocam = CameraPosition - posAdjusted;
-                    float distTocamlen = distTocam.Length();
-                    if (distTocamlen > 0)
-                    {
-                        distTocam *= 1.0f / distTocamlen;
-                        m_scene.PhysicsScene.RaycastWorld(posAdjusted, distTocam, distTocamlen + 0.3f, RayCastCameraCallback);
-                    }
-                    
+                    tocam *= (1.0f / distTocamlen);
+                    posAdjusted.X = (float)Math.Round(posAdjusted.X, 1);
+                    posAdjusted.Y = (float)Math.Round(posAdjusted.Y, 1);
+                    posAdjusted.Z = (float)Math.Round(posAdjusted.Z, 1);
+
+                    m_doingCamRayCast = true;
+                    m_scene.PhysicsScene.RaycastWorld(posAdjusted, tocam, distTocamlen + 1.0f, RayCastCameraCallback);
                 }
             }
+            else if (CameraConstraintActive && (m_mouseLook || ParentID != 0) )
+            {
+                Vector4 plane = new Vector4(0.9f, 0.0f, 0.361f, -10000f); // not right...
+                UpdateCameraCollisionPlane(plane);
+                CameraConstraintActive = false;
+            }
 
             uint flagsForScripts = (uint)flags;
             flags = RemoveIgnoredControls(flags, IgnoredControls);
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
index 6e9281b..7fe3109 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -173,8 +173,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.GeomRaySetClosestHit(ray, closestHit);
 
                         if (req.callbackMethod is RaycastCallback)
+                        {
                             // if we only want one get only one per Collision pair saving memory
                             CurrentRayFilter |= RayFilterFlags.ClosestHit;
+                            d.GeomRaySetClosestHit(ray, 1);
+                        }
+                        else
+                            d.GeomRaySetClosestHit(ray, closestHit);
                     }
 
                     if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
@@ -555,10 +560,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     ContactResult collisionresult = new ContactResult();
                     collisionresult.ConsumerID = ID;
-                    collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
                     collisionresult.Depth = curcontact.depth;
-                    collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
-                                                         curcontact.normal.Z);
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
                     lock (m_contactResults)
                     {
                         m_contactResults.Add(collisionresult);
@@ -581,10 +589,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (curcontact.depth < collisionresult.Depth)
                     {
-                        collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
                         collisionresult.Depth = curcontact.depth;
-                        collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
-                                                             curcontact.normal.Z);
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
                     }
                 }
 
@@ -699,10 +710,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     ContactResult collisionresult = new ContactResult();
                     collisionresult.ConsumerID = ID;
-                    collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
+                    collisionresult.Pos.X = curcontact.pos.X;
+                    collisionresult.Pos.Y = curcontact.pos.Y;
+                    collisionresult.Pos.Z = curcontact.pos.Z;
                     collisionresult.Depth = curcontact.depth;
-                    collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
-                                                         curcontact.normal.Z);
+                    collisionresult.Normal.X = curcontact.normal.X;
+                    collisionresult.Normal.Y = curcontact.normal.Y;
+                    collisionresult.Normal.Z = curcontact.normal.Z;
                     lock (m_contactResults)
                     {
                         m_contactResults.Add(collisionresult);
@@ -725,10 +739,13 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (curcontact.depth < collisionresult.Depth)
                     {
-                        collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
+                        collisionresult.Pos.X = curcontact.pos.X;
+                        collisionresult.Pos.Y = curcontact.pos.Y;
+                        collisionresult.Pos.Z = curcontact.pos.Z;
                         collisionresult.Depth = curcontact.depth;
-                        collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
-                                                             curcontact.normal.Z);
+                        collisionresult.Normal.X = curcontact.normal.X;
+                        collisionresult.Normal.Y = curcontact.normal.Y;
+                        collisionresult.Normal.Z = curcontact.normal.Z;
                     }
                 }