3 files changed, 244 insertions, 176 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index e026ffd..7ff163b 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -2103,12 +2103,29 @@ namespace OpenSim.Region.Framework.Scenes
             }
             else
             {
-                if (Util.GetDistanceTo(AbsolutePosition, pos) <= 10)
-                {
+//                if (Util.GetDistanceTo(AbsolutePosition, pos) <= 10)
+//                {
 //                    m_log.DebugFormat(
 //                        "[SCENE PRESENCE]: Sitting {0} on {1} {2} because sit target is unset and within 10m",
 //                        Name, part.Name, part.LocalId);
 
+                if (m_scene.PhysicsScene != null &&
+                    part.PhysActor != null &&
+                    Util.GetDistanceTo(AbsolutePosition, pos) <= 30)
+                {
+
+                    Vector3 camdif = CameraPosition - part.AbsolutePosition;
+                    camdif.Normalize();
+
+//                    m_log.InfoFormat("sit {0} {1}", offset.ToString(), camdif.ToString());
+
+                    if (m_scene.PhysicsScene.SitAvatar(part.PhysActor, AbsolutePosition, CameraPosition, offset, new Vector3(0.35f, 0, 0.65f), PhysicsSitResponse) != 0)
+                        return;
+                }
+
+                if (Util.GetDistanceTo(AbsolutePosition, pos) <= 10)
+                {
+
                     AbsolutePosition = pos + new Vector3(0.0f, 0.0f, m_sitAvatarHeight);
                     canSit = true;
                 }
@@ -2196,197 +2213,54 @@ namespace OpenSim.Region.Framework.Scenes
             SendSitResponse(targetID, offset, Quaternion.Identity);
         }
 
-        /*
-        public void SitRayCastAvatarPosition(SceneObjectPart part)
+        public void PhysicsSitResponse(int status, uint partID, Vector3 offset, Quaternion Orientation)
         {
-            Vector3 EndRayCastPosition = part.AbsolutePosition + m_requestedSitOffset;
-            Vector3 StartRayCastPosition = AbsolutePosition;
-            Vector3 direction = Vector3.Normalize(EndRayCastPosition - StartRayCastPosition);
-            float distance = Vector3.Distance(EndRayCastPosition, StartRayCastPosition);
-            m_scene.PhysicsScene.RaycastWorld(StartRayCastPosition, direction, distance, SitRayCastAvatarPositionResponse);
-        }
 
-        public void SitRayCastAvatarPositionResponse(bool hitYN, Vector3 collisionPoint, uint localid, float pdistance, Vector3 normal)
-        {
-            SceneObjectPart part =  FindNextAvailableSitTarget(m_requestedSitTargetUUID);
-            if (part != null)
-            {
-                if (hitYN)
-                {
-                    if (collisionPoint.ApproxEquals(m_requestedSitOffset + part.AbsolutePosition, 0.2f))
-                    {
-                        SitRaycastFindEdge(collisionPoint, normal);
-                        m_log.DebugFormat("[SIT]: Raycast Avatar Position succeeded at point: {0}, normal:{1}", collisionPoint, normal);
-                    }
-                    else
-                    {
-                        SitRayCastAvatarPositionCameraZ(part);
-                    }
-                }
-                else
-                {
-                    SitRayCastAvatarPositionCameraZ(part);
-                }
-            }
-            else
+            if (status < 0)
             {
                 ControllingClient.SendAlertMessage("Sit position no longer exists");
-                m_requestedSitTargetUUID = UUID.Zero;
-                m_requestedSitTargetID = 0;
-                m_requestedSitOffset = Vector3.Zero;
+                return;
             }
 
-        }
-
-        public void SitRayCastAvatarPositionCameraZ(SceneObjectPart part)
-        {
-            // Next, try to raycast from the camera Z position
-            Vector3 EndRayCastPosition = part.AbsolutePosition + m_requestedSitOffset;
-            Vector3 StartRayCastPosition = AbsolutePosition; StartRayCastPosition.Z = CameraPosition.Z;
-            Vector3 direction = Vector3.Normalize(EndRayCastPosition - StartRayCastPosition);
-            float distance = Vector3.Distance(EndRayCastPosition, StartRayCastPosition);
-            m_scene.PhysicsScene.RaycastWorld(StartRayCastPosition, direction, distance, SitRayCastAvatarPositionCameraZResponse);
-        }
+            if (status == 0)
+                return;
 
-        public void SitRayCastAvatarPositionCameraZResponse(bool hitYN, Vector3 collisionPoint, uint localid, float pdistance, Vector3 normal)
-        {
-            SceneObjectPart part = FindNextAvailableSitTarget(m_requestedSitTargetUUID);
-            if (part != null)
+            SceneObjectPart part = m_scene.GetSceneObjectPart(partID);
+            if (part == null || part.ParentGroup.IsAttachment)
             {
-                if (hitYN)
-                {
-                    if (collisionPoint.ApproxEquals(m_requestedSitOffset + part.AbsolutePosition, 0.2f))
-                    {
-                        SitRaycastFindEdge(collisionPoint, normal);
-                        m_log.DebugFormat("[SIT]: Raycast Avatar Position + CameraZ succeeded at point: {0}, normal:{1}", collisionPoint, normal);
-                    }
-                    else
-                    {
-                        SitRayCastCameraPosition(part);
-                    }
-                }
-                else
-                {
-                    SitRayCastCameraPosition(part);
-                }
-            }
-            else
-            {
-                ControllingClient.SendAlertMessage("Sit position no longer exists");
-                m_requestedSitTargetUUID = UUID.Zero;
-                m_requestedSitTargetID = 0;
-                m_requestedSitOffset = Vector3.Zero;
+                return;
             }
 
-        }
+//            m_log.InfoFormat("physsit {0} {1}", offset.ToString(),Orientation.ToString());
 
-        public void SitRayCastCameraPosition(SceneObjectPart part)
-        {
-            // Next, try to raycast from the camera position
-            Vector3 EndRayCastPosition = part.AbsolutePosition + m_requestedSitOffset;
-            Vector3 StartRayCastPosition = CameraPosition;
-            Vector3 direction = Vector3.Normalize(EndRayCastPosition - StartRayCastPosition);
-            float distance = Vector3.Distance(EndRayCastPosition, StartRayCastPosition);
-            m_scene.PhysicsScene.RaycastWorld(StartRayCastPosition, direction, distance, SitRayCastCameraPositionResponse);
-        }
+            part.AddSittingAvatar(UUID);
 
-        public void SitRayCastCameraPositionResponse(bool hitYN, Vector3 collisionPoint, uint localid, float pdistance, Vector3 normal)
-        {
-            SceneObjectPart part = FindNextAvailableSitTarget(m_requestedSitTargetUUID);
-            if (part != null)
-            {
-                if (hitYN)
-                {
-                    if (collisionPoint.ApproxEquals(m_requestedSitOffset + part.AbsolutePosition, 0.2f))
-                    {
-                        SitRaycastFindEdge(collisionPoint, normal);
-                        m_log.DebugFormat("[SIT]: Raycast Camera Position succeeded at point: {0}, normal:{1}", collisionPoint, normal);
-                    }
-                    else
-                    {
-                        SitRayHorizontal(part);
-                    }
-                }
-                else
-                {
-                    SitRayHorizontal(part);
-                }
-            }
-            else
-            {
-                ControllingClient.SendAlertMessage("Sit position no longer exists");
-                m_requestedSitTargetUUID = UUID.Zero;
-                m_requestedSitTargetID = 0;
-                m_requestedSitOffset = Vector3.Zero;
-            }
+            Vector3 cameraAtOffset = part.GetCameraAtOffset();
+            Vector3 cameraEyeOffset = part.GetCameraEyeOffset();
+            bool forceMouselook = part.GetForceMouselook();
 
-        }
+            ControllingClient.SendSitResponse(
+                part.UUID, offset, Orientation, false, cameraAtOffset, cameraEyeOffset, forceMouselook);
 
-        public void SitRayHorizontal(SceneObjectPart part)
-        {
-            // Next, try to raycast from the avatar position to fwd
-            Vector3 EndRayCastPosition = part.AbsolutePosition + m_requestedSitOffset;
-            Vector3 StartRayCastPosition = CameraPosition;
-            Vector3 direction = Vector3.Normalize(EndRayCastPosition - StartRayCastPosition);
-            float distance = Vector3.Distance(EndRayCastPosition, StartRayCastPosition);
-            m_scene.PhysicsScene.RaycastWorld(StartRayCastPosition, direction, distance, SitRayCastHorizontalResponse);
-        }
+            part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK);
 
-        public void SitRayCastHorizontalResponse(bool hitYN, Vector3 collisionPoint, uint localid, float pdistance, Vector3 normal)
-        {
-            SceneObjectPart part = FindNextAvailableSitTarget(m_requestedSitTargetUUID);
-            if (part != null)
-            {
-                if (hitYN)
-                {
-                    if (collisionPoint.ApproxEquals(m_requestedSitOffset + part.AbsolutePosition, 0.2f))
-                    {
-                        SitRaycastFindEdge(collisionPoint, normal);
-                        m_log.DebugFormat("[SIT]: Raycast Horizontal Position succeeded at point: {0}, normal:{1}", collisionPoint, normal);
-                        // Next, try to raycast from the camera position
-                        Vector3 EndRayCastPosition = part.AbsolutePosition + m_requestedSitOffset;
-                        Vector3 StartRayCastPosition = CameraPosition;
-                        Vector3 direction = Vector3.Normalize(EndRayCastPosition - StartRayCastPosition);
-                        float distance = Vector3.Distance(EndRayCastPosition, StartRayCastPosition);
-                        //m_scene.PhysicsScene.RaycastWorld(StartRayCastPosition, direction, distance, SitRayCastResponseAvatarPosition);
-                    }
-                    else
-                    {
-                        ControllingClient.SendAlertMessage("Sit position not accessable.");
-                        m_requestedSitTargetUUID = UUID.Zero;
-                        m_requestedSitTargetID = 0;
-                        m_requestedSitOffset = Vector3.Zero;
-                    }
-                }
-                else
-                {
-                    ControllingClient.SendAlertMessage("Sit position not accessable.");
-                    m_requestedSitTargetUUID = UUID.Zero;
-                    m_requestedSitTargetID = 0;
-                    m_requestedSitOffset = Vector3.Zero;
-                }
-            }
-            else
-            {
-                ControllingClient.SendAlertMessage("Sit position no longer exists");
-                m_requestedSitTargetUUID = UUID.Zero;
-                m_requestedSitTargetID = 0;
-                m_requestedSitOffset = Vector3.Zero;
-            }
+            // assuming no autopilot in use
+            Velocity = Vector3.Zero;
+            RemoveFromPhysicalScene();
 
-        }
+            Rotation = Orientation;
+            m_pos = offset;
 
-        private void SitRaycastFindEdge(Vector3 collisionPoint, Vector3 collisionNormal)
-        {
-            int i = 0;
-            //throw new NotImplementedException();
-            //m_requestedSitTargetUUID = UUID.Zero;
-            //m_requestedSitTargetID = 0;
-            //m_requestedSitOffset = Vector3.Zero;
+            m_requestedSitTargetID = 0; // invalidate the viewer sit comand for now
+            part.ParentGroup.AddAvatar(UUID);
 
-            SendSitResponse(ControllingClient, m_requestedSitTargetUUID, collisionPoint - m_requestedSitOffset, Quaternion.Identity);
+            ParentPart = part;
+            ParentID = part.LocalId;
+
+            Animator.TrySetMovementAnimation("SIT");
+            SendAvatarDataToAllAgents();
         }
-        */
+
 
         public void HandleAgentSit(IClientAPI remoteClient, UUID agentID)
         {
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
new file mode 100644
index 0000000..225bff8
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+// Ubit 2012
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.Physics.OdePlugin
+{
+    /// <summary>
+    /// </summary>
+    public class ODESitAvatar
+    {
+        private OdeScene m_scene;
+        private ODERayCastRequestManager m_raymanager;
+
+        public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
+        {
+            m_scene = pScene;
+            m_raymanager = raymanager;
+        }
+
+        private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
+
+        public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
+        {
+            if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+            IntPtr geom = ((OdePrim)actor).prim_geom;
+
+            d.Vector3 dtmp = d.GeomGetPosition(geom);
+            Vector3 geopos;
+            geopos.X = dtmp.X;
+            geopos.Y = dtmp.Y;
+            geopos.Z = dtmp.Z;
+
+
+            d.AABB aabb;
+            Quaternion ori;
+            d.Quaternion qtmp;
+            d.GeomCopyQuaternion(geom, out qtmp);
+            Quaternion geomOri;
+            geomOri.X = qtmp.X;
+            geomOri.Y = qtmp.Y;
+            geomOri.Z = qtmp.Z;
+            geomOri.W = qtmp.W;
+            Quaternion geomInvOri;
+            geomInvOri.X = -qtmp.X;
+            geomInvOri.Y = -qtmp.Y;
+            geomInvOri.Z = -qtmp.Z;
+            geomInvOri.W = qtmp.W;
+
+            Vector3 target = geopos + offset;
+            Vector3 rayDir = target - avCameraPosition;
+            float raylen = rayDir.Length();
+            float t = 1 / raylen;
+            rayDir.X *= t;
+            rayDir.Y *= t;
+            rayDir.Z *= t;
+
+            raylen += 0.5f;
+            List<ContactResult> rayResults;
+
+            rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir , raylen, 1);
+            if (rayResults.Count == 0 || rayResults[0].ConsumerID != actor.LocalID)
+            {
+                d.GeomGetAABB(geom,out aabb);
+                offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
+                ori = geomInvOri;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(1, actor.LocalID, offset, ori);
+                return;
+            }
+
+            offset = rayResults[0].Pos - geopos;
+            double ang;
+            float s;
+            float c;
+
+            d.GeomClassID geoclass = d.GeomGetClass(geom);
+
+            if (geoclass == d.GeomClassID.SphereClass)
+            {
+                float r = d.GeomSphereGetRadius(geom);              
+
+                offset.Normalize();
+                offset *= r;
+
+                ang = Math.Atan2(offset.Y, offset.X);
+                ang *= 0.5d;
+                s = (float)Math.Sin(ang);
+                c = (float)Math.Cos(ang);
+
+                ori = new Quaternion(0, 0, s, c);
+
+                if (r < 0.4f)
+                {
+                    offset = new Vector3(0, 0, r);
+                }
+                else if (offset.Z < 0.4f)
+                {
+                    t = offset.Z;
+                    float rsq = r * r;
+
+                    t = 1.0f / (rsq - t * t);
+                    offset.X *= t;
+                    offset.Y *= t;
+                    offset.Z = 0.4f;
+                    t = rsq - 0.16f;
+                    offset.X *= t;
+                    offset.Y *= t;
+                }
+
+                offset += avOffset * ori;
+
+                ori = geomInvOri * ori;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(1, actor.LocalID, offset, ori);
+                return;
+            }
+
+            Vector3 norm = rayResults[0].Normal;
+
+            if (norm.Z < 0)
+            {
+                PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+            ang = Math.Atan2(-rayDir.Y, -rayDir.X);
+            ang *= 0.5d;
+            s = (float)Math.Sin(ang);
+            c = (float)Math.Cos(ang);
+
+            ori = new Quaternion(0, 0, s, c);
+
+            offset += avOffset * ori;
+
+            ori = geomInvOri * ori;
+            offset *= geomInvOri;
+
+            PhysicsSitResponse(1, actor.LocalID, offset, ori);
+            return;
+        }
+    }
+}
\ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 15eb01f..fbf2f0d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -362,8 +362,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             nearCallback = near;
 
-            m_rayCastManager = new ODERayCastRequestManager(this);
-            
+            m_rayCastManager = new ODERayCastRequestManager(this);         
 
             lock (OdeLock)
                 {
@@ -2711,5 +2710,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             return new List<ContactResult>();
         }
+
+        public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
+        {
+            Util.FireAndForget( delegate
+            {
+                ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
+                if(sitAvatar != null)
+                    sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
+            });
+            return 1;
+        }
+
     }
 }