Dump OpenSim 0.9.0.1 into it's own branch.

1 files changed, 356 insertions, 0 deletions

diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
new file mode 100644
index 0000000..214205d
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs
@@ -0,0 +1,356 @@
+/*
+ * 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 Umarov 2012
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Runtime.InteropServices;
+using System.Text;
+using OpenSim.Framework;
+using OpenSim.Region.PhysicsModules.SharedBase;
+using OdeAPI;
+using log4net;
+using OpenMetaverse;
+
+namespace OpenSim.Region.PhysicsModule.ubOde
+{
+    /// <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);
+        private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
+
+        private void RotAroundZ(float x, float y, ref Quaternion ori)
+        {
+            double ang = Math.Atan2(y, x);
+            ang *= 0.5d;
+            float s = (float)Math.Sin(ang);
+            float c = (float)Math.Cos(ang);
+
+            ori.X = 0;
+            ori.Y = 0;
+            ori.Z = s;
+            ori.W = c;
+        }
+
+
+        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;
+
+            Vector3 geopos = d.GeomGetPositionOMV(geom);
+            Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
+
+//            Vector3 geopos = actor.Position;
+//            Quaternion geomOri = actor.Orientation;
+
+            Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
+
+            Quaternion ori = Quaternion.Identity;
+
+            Vector3 rayDir = geopos + offset - avCameraPosition;
+
+            float raylen = rayDir.Length();
+            if (raylen < 0.001f)
+            {
+                PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+            float t = 1 / raylen;
+            rayDir.X *= t;
+            rayDir.Y *= t;
+            rayDir.Z *= t;
+
+            raylen += 30f; // focal point may be far
+            List<ContactResult> rayResults;
+
+            rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
+            if (rayResults.Count == 0)
+            {
+/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
+                d.AABB aabb;
+                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);
+*/
+                PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                return;
+            }
+
+            int status = 1;
+
+            offset = rayResults[0].Pos - geopos;
+
+            d.GeomClassID geoclass = d.GeomGetClass(geom);
+
+            if (geoclass == d.GeomClassID.SphereClass)
+            {
+                float r = d.GeomSphereGetRadius(geom);
+
+                offset.Normalize();
+                offset *= r;
+
+                RotAroundZ(offset.X, offset.Y, ref ori);
+
+                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;
+                    }
+                    else if (r > 0.8f && offset.Z > 0.8f * r)
+                    {
+                        status = 3;
+                        avOffset.X = -avOffset.X;
+                        avOffset.Z *= 1.6f;
+                    }
+                }
+
+                offset += avOffset * ori;
+
+                ori = geomInvOri * ori;
+                offset *= geomInvOri;
+
+                PhysicsSitResponse(status, actor.LocalID, offset, ori);
+                return;
+            }
+
+            Vector3 norm = rayResults[0].Normal;
+
+            if (norm.Z < -0.4f)
+            {
+                PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
+                return;
+            }
+
+
+            float SitNormX = -rayDir.X;
+            float SitNormY = -rayDir.Y;
+
+            Vector3 pivot = geopos + offset;
+
+            float edgeNormalX = norm.X;
+            float edgeNormalY = norm.Y;
+            float edgeDirX = -rayDir.X;
+            float edgeDirY = -rayDir.Y;
+            Vector3 edgePos = rayResults[0].Pos;
+            float edgeDist = float.MaxValue;
+
+            bool foundEdge = false;
+
+            if (norm.Z < 0.5f)
+            {
+                float rayDist = 4.0f;
+
+                for (int i = 0; i < 6; i++)
+                {
+                    pivot.X -= 0.01f * norm.X;
+                    pivot.Y -= 0.01f * norm.Y;
+                    pivot.Z -= 0.01f * norm.Z;
+
+                    rayDir.X = -norm.X * norm.Z;
+                    rayDir.Y = -norm.Y * norm.Z;
+                    rayDir.Z = 1.0f - norm.Z * norm.Z;
+                    rayDir.Normalize();
+
+                    rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                    if (rayResults.Count == 0)
+                        break;
+
+                    if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
+                    {
+                        rayDist -= rayResults[0].Depth;
+                        if (rayDist < 0f)
+                            break;
+
+                        pivot = rayResults[0].Pos;
+                        norm = rayResults[0].Normal;
+                        edgeNormalX = norm.X;
+                        edgeNormalY = norm.Y;
+                        edgeDirX = -rayDir.X;
+                        edgeDirY = -rayDir.Y;
+                    }
+                    else
+                    {
+                        foundEdge = true;
+                        edgePos = rayResults[0].Pos;
+                        break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    PhysicsSitResponse(0, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            else if (norm.Z > 0.866f)
+            {
+                float toCamBaseX = avCameraPosition.X - pivot.X;
+                float toCamBaseY = avCameraPosition.Y - pivot.Y;
+                float toCamX = toCamBaseX;
+                float toCamY = toCamBaseY;
+
+                for (int j = 0; j < 4; j++)
+                {
+                    float rayDist = 1.0f;
+                    float curEdgeDist = 0.0f;
+
+                    for (int i = 0; i < 3; i++)
+                    {
+                        pivot.Z -= 0.01f;
+                        rayDir.X = toCamX;
+                        rayDir.Y = toCamY;
+                        rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
+                        rayDir.Normalize();
+
+                        rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
+                        if (rayResults.Count == 0)
+                            break;
+
+                        curEdgeDist += rayResults[0].Depth;
+
+                        if (rayResults[0].Normal.Z > 0.5f)
+                        {
+                            rayDist -= rayResults[0].Depth;
+                            if (rayDist < 0f)
+                                break;
+
+                            pivot = rayResults[0].Pos;
+                            norm = rayResults[0].Normal;
+                        }
+                        else
+                        {
+                            foundEdge = true;
+                            if (curEdgeDist < edgeDist)
+                            {
+                                edgeDist = curEdgeDist;
+                                edgeNormalX = rayResults[0].Normal.X;
+                                edgeNormalY = rayResults[0].Normal.Y;
+                                edgeDirX = rayDir.X;
+                                edgeDirY = rayDir.Y;
+                                edgePos = rayResults[0].Pos;
+                            }
+                            break;
+                        }
+                    }
+                    if (foundEdge && edgeDist < 0.2f)
+                        break;
+
+                    pivot = geopos + offset;
+
+                    switch (j)
+                    {
+                        case 0:
+                            toCamX = -toCamBaseY;
+                            toCamY = toCamBaseX;
+                            break;
+                        case 1:
+                            toCamX = toCamBaseY;
+                            toCamY = -toCamBaseX;
+                            break;
+                        case 2:
+                            toCamX = -toCamBaseX;
+                            toCamY = -toCamBaseY;
+                            break;
+                        default:
+                            break;
+                    }
+                }
+
+                if (!foundEdge)
+                {
+                    avOffset.X = -avOffset.X;
+                    avOffset.Z *= 1.6f;
+
+                    RotAroundZ(SitNormX, SitNormY, ref ori);
+
+                    offset += avOffset * ori;
+
+                    ori = geomInvOri * ori;
+                    offset *= geomInvOri;
+
+                    PhysicsSitResponse(3, actor.LocalID, offset, ori);
+                    return;
+                }
+                avOffset.X *= 0.5f;
+            }
+
+            SitNormX = edgeNormalX;
+            SitNormY = edgeNormalY;
+            if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
+            {
+                SitNormX = -SitNormX;
+                SitNormY = -SitNormY;
+            }
+
+            RotAroundZ(SitNormX, SitNormY, ref ori);
+
+            offset = edgePos + avOffset * ori;
+            offset -= geopos;
+
+            ori = geomInvOri * ori;
+            offset *= geomInvOri;
+
+            PhysicsSitResponse(1, actor.LocalID, offset, ori);
+            return;
+        }
+    }
+}