22 files changed, 1305 insertions, 695 deletions

diff --git a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
index 2e86315..f835e56 100644
--- a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
@@ -4845,7 +4845,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                     if (part != null && part != part.ParentGroup.RootPart)
                     {
                         position = part.OffsetPosition + presence.OffsetPosition * part.RotationOffset;
-                        rotation = presence.Rotation * part.RotationOffset;
+                        rotation = part.RotationOffset * presence.Rotation;
                     }
                 }
 
@@ -4974,7 +4974,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                 if (part != null && part != part.ParentGroup.RootPart)
                 {
                     offsetPosition = part.OffsetPosition + data.OffsetPosition * part.RotationOffset;
-                    rotation = data.Rotation * part.RotationOffset;
+                    rotation = part.RotationOffset * data.Rotation;
                     parentID = part.ParentGroup.RootPart.LocalId;
                 }
             }
diff --git a/OpenSim/Region/CoreModules/Avatar/AvatarFactory/AvatarFactoryModule.cs b/OpenSim/Region/CoreModules/Avatar/AvatarFactory/AvatarFactoryModule.cs
index b0cee03..2bebd30 100644
--- a/OpenSim/Region/CoreModules/Avatar/AvatarFactory/AvatarFactoryModule.cs
+++ b/OpenSim/Region/CoreModules/Avatar/AvatarFactory/AvatarFactoryModule.cs
@@ -551,12 +551,17 @@ namespace OpenSim.Region.CoreModules.Avatar.AvatarFactory
         /// <param name="client"></param>
         private void Client_OnRequestWearables(IClientAPI client)
         {
-            // m_log.DebugFormat("[AVFACTORY]: Client_OnRequestWearables called for {0} ({1})", client.Name, client.AgentId);
-            ScenePresence sp = m_scene.GetScenePresence(client.AgentId);
-            if (sp != null)
-                client.SendWearables(sp.Appearance.Wearables, sp.Appearance.Serial++);
-            else
-                m_log.WarnFormat("[AVFACTORY]: Client_OnRequestWearables unable to find presence for {0}", client.AgentId);
+            Util.FireAndForget(delegate(object x)
+            {
+                Thread.Sleep(4000);
+
+                // m_log.DebugFormat("[AVFACTORY]: Client_OnRequestWearables called for {0} ({1})", client.Name, client.AgentId);
+                ScenePresence sp = m_scene.GetScenePresence(client.AgentId);
+                if (sp != null)
+                    client.SendWearables(sp.Appearance.Wearables, sp.Appearance.Serial++);
+                else
+                    m_log.WarnFormat("[AVFACTORY]: Client_OnRequestWearables unable to find presence for {0}", client.AgentId);
+            });
         }
         
         /// <summary>
diff --git a/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs b/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs
index 2498705..60a8f86 100644
--- a/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs
+++ b/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs
@@ -1746,6 +1746,12 @@ namespace OpenSim.Region.CoreModules.Framework.EntityTransfer
                         if (grp.RootPart.PhysActor != null)
                         {
                             grp.RootPart.PhysActor.CrossingFailure();
+                            if (grp.RootPart.KeyframeMotion != null)
+                            {
+                                grp.RootPart.Velocity = Vector3.Zero;
+                                grp.RootPart.KeyframeMotion.CrossingFailure();
+                                grp.SendGroupRootTerseUpdate();
+                            }
                         }
                     }
 
diff --git a/OpenSim/Region/CoreModules/Framework/InventoryAccess/InventoryAccessModule.cs b/OpenSim/Region/CoreModules/Framework/InventoryAccess/InventoryAccessModule.cs
index 6c4c63f..192d55f 100644
--- a/OpenSim/Region/CoreModules/Framework/InventoryAccess/InventoryAccessModule.cs
+++ b/OpenSim/Region/CoreModules/Framework/InventoryAccess/InventoryAccessModule.cs
@@ -355,6 +355,12 @@ namespace OpenSim.Region.CoreModules.Framework.InventoryAccess
 
             foreach (SceneObjectGroup objectGroup in objlist)
             {
+                if (objectGroup.RootPart.KeyframeMotion != null)
+                    objectGroup.RootPart.KeyframeMotion.Stop();
+                objectGroup.RootPart.SetForce(Vector3.Zero);
+                objectGroup.RootPart.SetAngularImpulse(Vector3.Zero, false);
+                objectGroup.RootPart.KeyframeMotion = null;
+
                 Vector3 inventoryStoredPosition = new Vector3
                             (((objectGroup.AbsolutePosition.X > (int)Constants.RegionSize)
                                   ? 250
diff --git a/OpenSim/Region/Framework/Scenes/KeyframeMotion.cs b/OpenSim/Region/Framework/Scenes/KeyframeMotion.cs
new file mode 100644
index 0000000..b7b0d27
--- /dev/null
+++ b/OpenSim/Region/Framework/Scenes/KeyframeMotion.cs
@@ -0,0 +1,422 @@
+// Proprietary code of Avination Virtual Limited
+// (c) 2012 Melanie Thielker
+//
+
+using System;
+using System.Timers;
+using System.Collections;
+using System.Collections.Generic;
+using System.IO;
+using System.Diagnostics;
+using System.Reflection;
+using System.Threading;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.Physics.Manager;
+using OpenSim.Region.Framework.Scenes.Serialization;
+using System.Runtime.Serialization.Formatters.Binary;
+using System.Runtime.Serialization;
+using Timer = System.Timers.Timer;
+using log4net;
+
+namespace OpenSim.Region.Framework.Scenes
+{
+    [Serializable]
+    public class KeyframeMotion
+    {
+        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+
+        public enum PlayMode : int
+        {
+            Forward = 0,
+            Reverse = 1,
+            Loop = 2,
+            PingPong = 3
+        };
+
+        [Flags]
+        public enum DataFormat : int
+        {
+            Translation = 1,
+            Rotation = 2
+        }
+
+        [Serializable]
+        public struct Keyframe
+        {
+            public Vector3? Position;
+            public Quaternion? Rotation;
+            public Quaternion StartRotation;
+            public int TimeMS;
+            public int TimeTotal;
+            public Vector3 AngularVelocity;
+        };
+
+        private Vector3 m_basePosition;
+        private Quaternion m_baseRotation;
+        private Vector3 m_serializedPosition;
+
+        private Keyframe m_currentFrame;
+        private List<Keyframe> m_frames = new List<Keyframe>();
+
+        private Keyframe[] m_keyframes;
+
+        [NonSerialized()]
+        protected Timer m_timer = new Timer();
+
+        [NonSerialized()]
+        private SceneObjectGroup m_group;
+
+        private PlayMode m_mode = PlayMode.Forward;
+        private DataFormat m_data = DataFormat.Translation | DataFormat.Rotation;
+
+        private bool m_running = false;
+        [NonSerialized()]
+        private bool m_selected = false;
+
+        private int m_iterations = 0;
+
+        private const double timerInterval = 50.0;
+
+        public DataFormat Data
+        {
+            get { return m_data; }
+        }
+
+        public bool Selected
+        {
+            set
+            {
+                if (value)
+                {
+                    // Once we're let go, recompute positions
+                    if (m_selected)
+                        UpdateSceneObject(m_group);
+                }
+                else
+                {
+                    // Save selection position in case we get moved
+                    if (!m_selected)
+                        m_serializedPosition = m_group.AbsolutePosition;
+                }
+                m_selected = value; }
+        }
+
+        public static KeyframeMotion FromData(SceneObjectGroup grp, Byte[] data)
+        {
+            MemoryStream ms = new MemoryStream(data);
+
+            BinaryFormatter fmt = new BinaryFormatter();
+
+            KeyframeMotion newMotion = (KeyframeMotion)fmt.Deserialize(ms);
+
+            // This will be started when position is updated
+            newMotion.m_timer = new Timer();
+            newMotion.m_timer.Interval = (int)timerInterval;
+            newMotion.m_timer.AutoReset = true;
+            newMotion.m_timer.Elapsed += newMotion.OnTimer;
+
+            return newMotion;
+        }
+
+        public void UpdateSceneObject(SceneObjectGroup grp)
+        {
+            m_group = grp;
+            Vector3 offset = grp.AbsolutePosition - m_serializedPosition;
+
+            m_basePosition += offset;
+            m_currentFrame.Position += offset;
+            for (int i = 0 ; i < m_frames.Count ; i++)
+            {
+                Keyframe k = m_frames[i];
+                k.Position += offset;
+                m_frames[i] = k;
+            }
+
+            if (m_running)
+                Start();
+        }
+
+        public KeyframeMotion(SceneObjectGroup grp, PlayMode mode, DataFormat data)
+        {
+            m_mode = mode;
+            m_data = data;
+
+            m_group = grp;
+            m_basePosition = grp.AbsolutePosition;
+            m_baseRotation = grp.GroupRotation;
+
+            m_timer.Interval = (int)timerInterval;
+            m_timer.AutoReset = true;
+            m_timer.Elapsed += OnTimer;
+        }
+
+        public void SetKeyframes(Keyframe[] frames)
+        {
+            m_keyframes = frames;
+        }
+
+        public void Start()
+        {
+            if (m_keyframes.Length > 0)
+                m_timer.Start();
+            m_running = true;
+        }
+
+        public void Stop()
+        {
+            // Failed object creation
+            if (m_timer == null)
+                return;
+            m_timer.Stop();
+
+            m_basePosition = m_group.AbsolutePosition;
+            m_baseRotation = m_group.GroupRotation;
+
+            m_group.RootPart.Velocity = Vector3.Zero;
+            m_group.RootPart.UpdateAngularVelocity(Vector3.Zero);
+            m_group.SendGroupRootTerseUpdate();
+
+            m_frames.Clear();
+            m_running = false;
+        }
+
+        public void Pause()
+        {
+            m_group.RootPart.Velocity = Vector3.Zero;
+            m_group.RootPart.UpdateAngularVelocity(Vector3.Zero);
+            m_group.SendGroupRootTerseUpdate();
+
+            m_timer.Stop();
+            m_running = false;
+        }
+
+        private void GetNextList()
+        {
+            m_frames.Clear();
+            Vector3 pos = m_basePosition;
+            Quaternion rot = m_baseRotation;
+
+            if (m_mode == PlayMode.Loop || m_mode == PlayMode.PingPong || m_iterations == 0)
+            {
+                int direction = 1;
+                if (m_mode == PlayMode.Reverse || ((m_mode == PlayMode.PingPong) && ((m_iterations & 1) != 0)))
+                    direction = -1;
+
+                int start = 0;
+                int end = m_keyframes.Length;
+//                if (m_mode == PlayMode.PingPong && m_keyframes.Length > 1)
+//                    end = m_keyframes.Length - 1;
+
+                if (direction < 0)
+                {
+                    start = m_keyframes.Length - 1;
+                    end = -1;
+//                    if (m_mode == PlayMode.PingPong && m_keyframes.Length > 1)
+//                        end = 0;
+                }
+
+                for (int i = start; i != end ; i += direction)
+                {
+                    Keyframe k = m_keyframes[i];
+
+                    if (k.Position.HasValue)
+                        k.Position = (k.Position * direction) + pos;
+                    else
+                        k.Position = pos;
+
+                    k.StartRotation = rot;
+                    if (k.Rotation.HasValue)
+                    {
+                        if (direction == -1)
+                            k.Rotation = Quaternion.Conjugate((Quaternion)k.Rotation);
+                        k.Rotation = rot * k.Rotation;
+                    }
+                    else
+                    {
+                        k.Rotation = rot;
+                    }
+
+                    float angle = 0;
+
+                    float aa = k.StartRotation.X * k.StartRotation.X + k.StartRotation.Y * k.StartRotation.Y + k.StartRotation.Z * k.StartRotation.Z + k.StartRotation.W * k.StartRotation.W;
+                    float bb = ((Quaternion)k.Rotation).X * ((Quaternion)k.Rotation).X + ((Quaternion)k.Rotation).Y * ((Quaternion)k.Rotation).Y + ((Quaternion)k.Rotation).Z * ((Quaternion)k.Rotation).Z + ((Quaternion)k.Rotation).W * ((Quaternion)k.Rotation).W;
+                    float aa_bb = aa * bb;
+
+                    if (aa_bb == 0)
+                    {
+                        angle = 0;
+                    }
+                    else
+                    {
+                        float ab = k.StartRotation.X * ((Quaternion)k.Rotation).X +
+                                   k.StartRotation.Y * ((Quaternion)k.Rotation).Y +
+                                   k.StartRotation.Z * ((Quaternion)k.Rotation).Z +
+                                   k.StartRotation.W * ((Quaternion)k.Rotation).W;
+                        float q = (ab * ab) / aa_bb;
+
+                        if (q > 1.0f)
+                        {
+                            angle = 0;
+                        }
+                        else
+                        {
+                            angle = (float)Math.Acos(2 * q - 1);
+                        }
+                    }
+
+                    k.AngularVelocity = (new Vector3(0, 0, 1) * (Quaternion)k.Rotation) * (angle / (k.TimeMS / 1000));
+                    k.TimeTotal = k.TimeMS;
+
+                    m_frames.Add(k);
+
+                    pos = (Vector3)k.Position;
+                    rot = (Quaternion)k.Rotation;
+                }
+
+                m_basePosition = pos;
+                m_baseRotation = rot;
+
+                m_iterations++;
+            }
+        }
+
+        protected void OnTimer(object sender, ElapsedEventArgs e)
+        {
+            if (m_frames.Count == 0)
+            {
+                GetNextList();
+
+                if (m_frames.Count == 0)
+                {
+                    Stop();
+                    return;
+                }
+
+                m_currentFrame = m_frames[0];
+            }
+
+            if (m_selected)
+            {
+                if (m_group.RootPart.Velocity != Vector3.Zero)
+                {
+                    m_group.RootPart.Velocity = Vector3.Zero;
+                    m_group.SendGroupRootTerseUpdate();
+                }
+                return;
+            }
+
+            // Do the frame processing
+            double steps = (double)m_currentFrame.TimeMS / timerInterval;
+            float complete = ((float)m_currentFrame.TimeTotal - (float)m_currentFrame.TimeMS) / (float)m_currentFrame.TimeTotal;
+
+            if (steps <= 1.0)
+            {
+                m_currentFrame.TimeMS = 0;
+
+                m_group.AbsolutePosition = (Vector3)m_currentFrame.Position;
+                m_group.UpdateGroupRotationR((Quaternion)m_currentFrame.Rotation);
+            }
+            else
+            {
+                Vector3 v = (Vector3)m_currentFrame.Position - m_group.AbsolutePosition;
+                Vector3 motionThisFrame = v / (float)steps;
+                v = v * 1000 / m_currentFrame.TimeMS;
+
+                bool update = false;
+
+                if (Vector3.Mag(motionThisFrame) >= 0.05f)
+                {
+                    m_group.AbsolutePosition += motionThisFrame;
+                    m_group.RootPart.Velocity = v;
+                    update = true;
+                }
+
+                if ((Quaternion)m_currentFrame.Rotation != m_group.GroupRotation)
+                {
+                    Quaternion current = m_group.GroupRotation;
+
+                    Quaternion step = Quaternion.Slerp(m_currentFrame.StartRotation, (Quaternion)m_currentFrame.Rotation, complete);
+
+                    float angle = 0;
+
+                    float aa = current.X * current.X + current.Y * current.Y + current.Z * current.Z + current.W * current.W;
+                    float bb = step.X * step.X + step.Y * step.Y + step.Z * step.Z + step.W * step.W;
+                    float aa_bb = aa * bb;
+
+                    if (aa_bb == 0)
+                    {
+                        angle = 0;
+                    }
+                    else
+                    {
+                        float ab = current.X * step.X +
+                                   current.Y * step.Y +
+                                   current.Z * step.Z +
+                                   current.W * step.W;
+                        float q = (ab * ab) / aa_bb;
+
+                        if (q > 1.0f)
+                        {
+                            angle = 0;
+                        }
+                        else
+                        {
+                            angle = (float)Math.Acos(2 * q - 1);
+                        }
+                    }
+
+                    if (angle > 0.01f)
+                    {
+                        m_group.UpdateGroupRotationR(step);
+                        //m_group.RootPart.UpdateAngularVelocity(m_currentFrame.AngularVelocity / 2);
+                        update = true;
+                    }
+                }
+
+                if (update)
+                    m_group.SendGroupRootTerseUpdate();
+            }
+
+            m_currentFrame.TimeMS -= (int)timerInterval;
+
+            if (m_currentFrame.TimeMS <= 0)
+            {
+                m_group.RootPart.Velocity = Vector3.Zero;
+                m_group.RootPart.UpdateAngularVelocity(Vector3.Zero);
+                m_group.SendGroupRootTerseUpdate();
+
+                m_frames.RemoveAt(0);
+                if (m_frames.Count > 0)
+                    m_currentFrame = m_frames[0];
+            }
+        }
+
+        public Byte[] Serialize()
+        {
+            MemoryStream ms = new MemoryStream();
+            m_timer.Stop();
+
+            BinaryFormatter fmt = new BinaryFormatter();
+            SceneObjectGroup tmp = m_group;
+            m_group = null;
+            m_serializedPosition = tmp.AbsolutePosition;
+            fmt.Serialize(ms, this);
+            m_group = tmp;
+            return ms.ToArray();
+        }
+
+        public void CrossingFailure()
+        {
+            // The serialization has stopped the timer, so let's wait a moment
+            // then retry the crossing. We'll get back here if it fails.
+            Util.FireAndForget(delegate (object x)
+            {
+                Thread.Sleep(60000);
+                if (m_running)
+                    m_timer.Start();
+            });
+        }
+    }
+}
diff --git a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
index bf2e775..b006045 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.PacketHandlers.cs
@@ -138,12 +138,12 @@ namespace OpenSim.Region.Framework.Scenes
             {
                 SceneObjectGroup sog = part.ParentGroup;
                 sog.SendPropertiesToClient(remoteClient);
-                sog.IsSelected = true;
 
                 // A prim is only tainted if it's allowed to be edited by the person clicking it.
                 if (Permissions.CanEditObject(sog.UUID, remoteClient.AgentId)
                     || Permissions.CanMoveObject(sog.UUID, remoteClient.AgentId))
                 {
+                    sog.IsSelected = true;
                     EventManager.TriggerParcelPrimCountTainted();
                 }
             }
@@ -215,7 +215,9 @@ namespace OpenSim.Region.Framework.Scenes
             // handled by group, but by prim. Legacy cruft.
             // TODO: Make selection flagging per prim!
             //
-            part.ParentGroup.IsSelected = false;
+            if (Permissions.CanEditObject(part.ParentGroup.UUID, remoteClient.AgentId)
+                || Permissions.CanMoveObject(part.ParentGroup.UUID, remoteClient.AgentId))
+                part.ParentGroup.IsSelected = false;
             
             if (part.ParentGroup.IsAttachment)
                 isAttachment = true;
diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs
index 4324cc0..e675c73 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.cs
@@ -2411,6 +2411,8 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (newPosition != Vector3.Zero)
                 newObject.RootPart.GroupPosition = newPosition;
+            if (newObject.RootPart.KeyframeMotion != null)
+                newObject.RootPart.KeyframeMotion.UpdateSceneObject(newObject);
 
             if (!AddSceneObject(newObject))
             {
diff --git a/OpenSim/Region/Framework/Scenes/SceneGraph.cs b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
index 5a7f124..a320601 100644
--- a/OpenSim/Region/Framework/Scenes/SceneGraph.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneGraph.cs
@@ -1731,6 +1731,12 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="childPrims"></param>
         protected internal void LinkObjects(SceneObjectPart root, List<SceneObjectPart> children)
         {
+            if (root.KeyframeMotion != null)
+            {
+                root.KeyframeMotion.Stop();
+                root.KeyframeMotion = null;
+            }
+
             SceneObjectGroup parentGroup = root.ParentGroup;
             if (parentGroup == null) return;
 
@@ -1823,6 +1829,11 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     if (part != null)
                     {
+                        if (part.KeyframeMotion != null)
+                        {
+                            part.KeyframeMotion.Stop();
+                            part.KeyframeMotion = null;
+                        }
                         if (part.ParentGroup.PrimCount != 1) // Skip single
                         {
                             if (part.LinkNum < 2) // Root
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index cf8637f..c9ea8e4 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -582,7 +582,7 @@ namespace OpenSim.Region.Framework.Scenes
                     foreach (ScenePresence av in m_linkedAvatars)
                     {
                         SceneObjectPart p = m_scene.GetSceneObjectPart(av.ParentID);
-                        if (m_parts.TryGetValue(p.UUID, out p))
+                        if (p != null && m_parts.TryGetValue(p.UUID, out p))
                         {
                             Vector3 offset = p.GetWorldPosition() - av.ParentPosition;
                             av.AbsolutePosition += offset;
@@ -720,6 +720,8 @@ namespace OpenSim.Region.Framework.Scenes
                             child.PhysActor.Selected = value;
                     }
                 }
+                if (RootPart.KeyframeMotion != null)
+                    RootPart.KeyframeMotion.Selected = value;
             }
         }
 
@@ -890,6 +892,10 @@ namespace OpenSim.Region.Framework.Scenes
             ApplyPhysics();
 
             if (RootPart.PhysActor != null)
+                RootPart.Force = RootPart.Force;
+            if (RootPart.PhysActor != null)
+                RootPart.Torque = RootPart.Torque;
+            if (RootPart.PhysActor != null)
                 RootPart.Buoyancy = RootPart.Buoyancy;
 
             // Don't trigger the update here - otherwise some client issues occur when multiple updates are scheduled
@@ -1829,6 +1835,11 @@ namespace OpenSim.Region.Framework.Scenes
 
                         backup_group.ForEachPart(delegate(SceneObjectPart part) 
                         { 
+                            if (part.KeyframeMotion != null)
+                            {
+                                part.KeyframeMotion = KeyframeMotion.FromData(backup_group, part.KeyframeMotion.Serialize());
+                                part.KeyframeMotion.UpdateSceneObject(this);
+                            }
                             part.Inventory.ProcessInventoryBackup(datastore); 
                         });
 
@@ -1978,10 +1989,18 @@ namespace OpenSim.Region.Framework.Scenes
         public void CopyRootPart(SceneObjectPart part, UUID cAgentID, UUID cGroupID, bool userExposed)
         {
             SetRootPart(part.Copy(m_scene.AllocateLocalId(), OwnerID, GroupID, 0, userExposed));
+            if (userExposed)
+                RootPart.Velocity = Vector3.Zero; // In case source is moving
         }
 
         public void ScriptSetPhysicsStatus(bool usePhysics)
         {
+            if (usePhysics)
+            {
+                if (RootPart.KeyframeMotion != null)
+                    RootPart.KeyframeMotion.Stop();
+                RootPart.KeyframeMotion = null;
+            }
             UpdatePrimFlags(RootPart.LocalId, usePhysics, IsTemporary, IsPhantom, IsVolumeDetect);
         }
 
@@ -2045,30 +2064,9 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        public void setAngularImpulse(Vector3 impulse)
-        {
-            if (RootPart.PhysActor != null)
-            {
-                if (!IsAttachment)
-                {
-                    RootPart.PhysActor.Torque = impulse;
-                    m_scene.PhysicsScene.AddPhysicsActorTaint(RootPart.PhysActor);
-                }
-            }
-        }
-
         public Vector3 GetTorque()
         {
-            if (RootPart.PhysActor != null)
-            {
-                if (!IsAttachment)
-                {
-                    Vector3 torque = RootPart.PhysActor.Torque;
-                    return torque;
-                }
-            }
-
-            return Vector3.Zero;
+            return RootPart.Torque;
         }
 
          // This is used by both Double-Click Auto-Pilot and llMoveToTarget() in an attached object
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index dd9431b..1c72b10 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -294,6 +294,8 @@ namespace OpenSim.Region.Framework.Scenes
         protected Vector3 m_lastAngularVelocity;
         protected int m_lastTerseSent;
         protected float m_buoyancy = 0.0f;
+        protected Vector3 m_force;
+        protected Vector3 m_torque;
         
         /// <summary>
         /// Stores media texture data
@@ -313,6 +315,14 @@ namespace OpenSim.Region.Framework.Scenes
 
         private SOPVehicle m_vehicle = null;
 
+        private KeyframeMotion m_keyframeMotion = null;
+
+        public KeyframeMotion KeyframeMotion
+        {
+            get; set;
+        }
+
+
         #endregion Fields
 
 //        ~SceneObjectPart()
@@ -906,7 +916,7 @@ namespace OpenSim.Region.Framework.Scenes
             get
             {
                 PhysicsActor actor = PhysActor;
-                if ((actor != null) && actor.IsPhysical)
+                if ((actor != null) && actor.IsPhysical && ParentGroup.RootPart == this)
                 {
                     m_angularVelocity = actor.RotationalVelocity;
                 }
@@ -1302,14 +1312,69 @@ namespace OpenSim.Region.Framework.Scenes
 
         public float Buoyancy
         {
-            get { return m_buoyancy; }
+            get
+            {
+                if (ParentGroup.RootPart == this)
+                    return m_buoyancy;
+
+                return ParentGroup.RootPart.Buoyancy;
+            }
             set
             {
+                if (ParentGroup != null && ParentGroup.RootPart != null && ParentGroup.RootPart != this)
+                {
+                    ParentGroup.RootPart.Buoyancy = value;
+                    return;
+                }
                 m_buoyancy = value;
                 if (PhysActor != null)
-                {
                     PhysActor.Buoyancy = value;
+            }
+        }
+
+        public Vector3 Force
+        {
+            get
+            {
+                if (ParentGroup.RootPart == this)
+                    return m_force;
+
+                return ParentGroup.RootPart.Force;
+            }
+
+            set
+            {
+                if (ParentGroup != null && ParentGroup.RootPart != null && ParentGroup.RootPart != this)
+                {
+                    ParentGroup.RootPart.Force = value;
+                    return;
+                }
+                m_force = value;
+                if (PhysActor != null)
+                    PhysActor.Force = value;
+            }
+        }
+
+        public Vector3 Torque
+        {
+            get
+            {
+                if (ParentGroup.RootPart == this)
+                    return m_torque;
+                
+                return ParentGroup.RootPart.Torque;
+            }
+
+            set
+            {
+                if (ParentGroup != null && ParentGroup.RootPart != null && ParentGroup.RootPart != this)
+                {
+                    ParentGroup.RootPart.Torque = value;
+                    return;
                 }
+                m_torque = value;
+                if (PhysActor != null)
+                    PhysActor.Torque = value;
             }
         }
 
@@ -1488,20 +1553,24 @@ namespace OpenSim.Region.Framework.Scenes
         /// </summary>
         /// <param name="impulsei">Vector force</param>
         /// <param name="localGlobalTF">true for the local frame, false for the global frame</param>
-        public void SetAngularImpulse(Vector3 impulsei, bool localGlobalTF)
+        
+        // this is actualy Set Torque.. keeping naming so not to edit lslapi also
+        public void SetAngularImpulse(Vector3 torquei, bool localGlobalTF)
         {
-            Vector3 impulse = impulsei;
+            Vector3 torque = torquei;
 
             if (localGlobalTF)
             {
+/*
                 Quaternion grot = GetWorldRotation();
                 Quaternion AXgrot = grot;
                 Vector3 AXimpulsei = impulsei;
                 Vector3 newimpulse = AXimpulsei * AXgrot;
-                impulse = newimpulse;
+ */
+                torque *= GetWorldRotation();
             }
 
-            ParentGroup.setAngularImpulse(impulse);
+            Torque = torque;
         }
 
         /// <summary>
@@ -1571,14 +1640,22 @@ namespace OpenSim.Region.Framework.Scenes
 
                         DoPhysicsPropertyUpdate(RigidBody, true);
                         PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0);
+                     
+                        if (!building)
+                            PhysActor.Building = false;
 
                         Velocity = velocity;
                         AngularVelocity = rotationalVelocity;
                         PhysActor.Velocity = velocity;
                         PhysActor.RotationalVelocity = rotationalVelocity;
 
-                                                if (!building)
-                            PhysActor.Building = false;
+                        // if not vehicle and root part apply force and torque
+                        if ((m_vehicle == null || m_vehicle.Type == Vehicle.TYPE_NONE)
+                                && LocalId == ParentGroup.RootPart.LocalId)
+                        {
+                            PhysActor.Force = Force;
+                            PhysActor.Torque = Torque;
+                        }
                     }
                 }
             }
@@ -1816,7 +1893,8 @@ namespace OpenSim.Region.Framework.Scenes
 
                             Velocity = new Vector3(0, 0, 0);
                             Acceleration = new Vector3(0, 0, 0);
-                            AngularVelocity = new Vector3(0, 0, 0);
+                            if (ParentGroup.RootPart == this)
+                                AngularVelocity = new Vector3(0, 0, 0);
 
                             PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate;
                             PhysActor.OnOutOfBounds -= PhysicsOutOfBounds;
@@ -1840,7 +1918,8 @@ namespace OpenSim.Region.Framework.Scenes
                             // velocity-vector.
                             Velocity = new Vector3(0, 0, 0);
                             Acceleration = new Vector3(0, 0, 0);
-                            AngularVelocity = new Vector3(0, 0, 0);
+                            if (ParentGroup.RootPart == this)
+                                AngularVelocity = new Vector3(0, 0, 0);
                             //RotationalVelocity = new Vector3(0, 0, 0);
                         }
 
@@ -1855,6 +1934,9 @@ namespace OpenSim.Region.Framework.Scenes
                         {
                             if (UsePhysics)
                             {
+                                if (ParentGroup.RootPart.KeyframeMotion != null)
+                                    ParentGroup.RootPart.KeyframeMotion.Stop();
+                                ParentGroup.RootPart.KeyframeMotion = null;
                                 ParentGroup.Scene.AddPhysicalPrim(1);
 
                                 PhysActor.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate;
@@ -2002,10 +2084,7 @@ namespace OpenSim.Region.Framework.Scenes
 
         public Vector3 GetForce()
         {
-            if (PhysActor != null)
-                return PhysActor.Force;
-            else
-                return Vector3.Zero;
+                return Force;
         }
 
         /// <summary>
@@ -3150,10 +3229,13 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void SetBuoyancy(float fvalue)
         {
+            Buoyancy = fvalue;
+/*            
             if (PhysActor != null)
             {
                 PhysActor.Buoyancy = fvalue;
             }
+ */
         }
 
         public void SetDieAtEdge(bool p)
@@ -3181,10 +3263,13 @@ namespace OpenSim.Region.Framework.Scenes
 
         public void SetForce(Vector3 force)
         {
+            Force = force;
+/*
             if (PhysActor != null)
             {
                 PhysActor.Force = force;
             }
+ */
         }
 
         public SOPVehicle sopVehicle
diff --git a/OpenSim/Region/Framework/Scenes/Serialization/SceneObjectSerializer.cs b/OpenSim/Region/Framework/Scenes/Serialization/SceneObjectSerializer.cs
index 72a0ec3..51a3320 100644
--- a/OpenSim/Region/Framework/Scenes/Serialization/SceneObjectSerializer.cs
+++ b/OpenSim/Region/Framework/Scenes/Serialization/SceneObjectSerializer.cs
@@ -244,6 +244,12 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
                     sr.Close();
                 }
 
+                XmlNodeList keymotion = doc.GetElementsByTagName("KeyframeMotion");
+                if (keymotion.Count > 0)
+                    sceneObject.RootPart.KeyframeMotion = KeyframeMotion.FromData(sceneObject, Convert.FromBase64String(keymotion[0].InnerText));
+                else
+                    sceneObject.RootPart.KeyframeMotion = null;
+
                 // Script state may, or may not, exist. Not having any, is NOT
                 // ever a problem.
                 sceneObject.LoadScriptState(doc);
@@ -349,6 +355,8 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
             m_SOPXmlProcessors.Add("PayPrice4", ProcessPayPrice4);
 
             m_SOPXmlProcessors.Add("Buoyancy", ProcessBuoyancy);
+            m_SOPXmlProcessors.Add("Force", ProcessForce);
+            m_SOPXmlProcessors.Add("Torque", ProcessTorque);
             m_SOPXmlProcessors.Add("VolumeDetectActive", ProcessVolumeDetectActive);
 
             //Ubit comented until proper testing
@@ -595,7 +603,6 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
                 obj.sopVehicle = _vehicle;
         }
                    
-
         private static void ProcessShape(SceneObjectPart obj, XmlTextReader reader)
         {
             List<string> errorNodeNames;
@@ -762,7 +769,16 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
 
         private static void ProcessBuoyancy(SceneObjectPart obj, XmlTextReader reader)
         {
-            obj.Buoyancy = (int)reader.ReadElementContentAsFloat("Buoyancy", String.Empty);
+            obj.Buoyancy = (float)reader.ReadElementContentAsFloat("Buoyancy", String.Empty);
+        }
+
+        private static void ProcessForce(SceneObjectPart obj, XmlTextReader reader)
+        {
+            obj.Force = Util.ReadVector(reader, "Force");
+        }
+        private static void ProcessTorque(SceneObjectPart obj, XmlTextReader reader)
+        {
+            obj.Torque = Util.ReadVector(reader, "Torque");
         }
 
         private static void ProcessVolumeDetectActive(SceneObjectPart obj, XmlTextReader reader)
@@ -1157,6 +1173,16 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
             });
 
             writer.WriteEndElement();
+
+            if (sog.RootPart.KeyframeMotion != null)
+            {
+                Byte[] data = sog.RootPart.KeyframeMotion.Serialize();
+
+                writer.WriteStartElement(String.Empty, "KeyframeMotion", String.Empty);
+                writer.WriteBase64(data, 0, data.Length);
+                writer.WriteEndElement();
+            }
+
             writer.WriteEndElement();
         }
 
@@ -1256,6 +1282,10 @@ namespace OpenSim.Region.Framework.Scenes.Serialization
             writer.WriteElementString("PayPrice4", sop.PayPrice[4].ToString());
 
             writer.WriteElementString("Buoyancy", sop.Buoyancy.ToString());
+
+            WriteVector(writer, "Force", sop.Force);
+            WriteVector(writer, "Torque", sop.Torque);
+
             writer.WriteElementString("VolumeDetectActive", sop.VolumeDetectActive.ToString().ToLower());
 
             //Ubit comented until proper testing
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
index ae46c97..78e9b29 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
@@ -99,8 +99,11 @@ namespace OpenSim.Region.OptionalModules.World.NPC
             // Delete existing sp attachments
             scene.AttachmentsModule.DeleteAttachmentsFromScene(sp, false);
 
+            AvatarAppearance app = new AvatarAppearance(appearance, true);
+            sp.Appearance = app;
+
             // Set new sp appearance. Also sends to clients.
-            scene.RequestModuleInterface<IAvatarFactoryModule>().SetAppearance(sp, new AvatarAppearance(appearance, true));
+            scene.RequestModuleInterface<IAvatarFactoryModule>().SetAppearance(sp, app);
             
             // Rez needed sp attachments
             scene.AttachmentsModule.RezAttachments(sp);
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 80c1277..0a4ebe4 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -704,6 +704,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_isphysical)
                 m_targetSpace = _parent_scene.space;
 
+            _triMeshData = IntPtr.Zero;
+
             m_primName = primName;
             m_taintserial = null;
             m_taintadd = true;
@@ -773,6 +775,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_targetSpace = _parent_scene.space;
             }
 
+            _triMeshData = IntPtr.Zero;
+
             m_taintserial = null;
             m_primName = primName;
             m_taintadd = true;
@@ -1762,7 +1766,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
 
-        public void setMesh(OdeScene parent_scene, IMesh mesh)
+        public bool setMesh(OdeScene parent_scene, IMesh mesh)
         {
             // This sleeper is there to moderate how long it takes between
             // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
@@ -1785,24 +1789,48 @@ namespace OpenSim.Region.Physics.OdePlugin
                     disableBody();
                 }
             }
+
+// do it on caller instead
+/*
+            if (_triMeshData != IntPtr.Zero)
+            {
+                d.GeomTriMeshDataDestroy(_triMeshData);
+                _triMeshData = IntPtr.Zero;
+            }
+*/
             IntPtr vertices, indices;
             int vertexCount, indexCount;
             int vertexStride, triStride;
             mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
             mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
 
+            // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid
             mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
+
+            if (vertexCount == 0 || indexCount == 0)
+            {
+                m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z);
+                _size.X = 0.05f;
+                _size.Y = 0.05f;
+                _size.Z = 0.05f;
+                return false;
+            }
+
+/*
             if (m_MeshToTriMeshMap.ContainsKey(mesh))
             {
                 _triMeshData = m_MeshToTriMeshMap[mesh];
             }
             else
+*/
+
+
             {
                 _triMeshData = d.GeomTriMeshDataCreate();
 
                 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
                 d.GeomTriMeshDataPreprocess(_triMeshData);
-                m_MeshToTriMeshMap[mesh] = _triMeshData;
+//                m_MeshToTriMeshMap[mesh] = _triMeshData;
             }
 
             _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -1810,13 +1838,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 //    if (prim_geom == IntPtr.Zero)  // setGeom takes care of phys engine recreate and prim_geom pointer 
                 //    {
-                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                //                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
+                SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
                 //    }
             }
-            catch (AccessViolationException)
+            catch (Exception e)
             {
-                m_log.Error("[PHYSICS]: MESH LOCKED");
-                return;
+                m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message);
+
+                if (_triMeshData != IntPtr.Zero)
+                {
+                    d.GeomTriMeshDataDestroy(_triMeshData);
+                    _triMeshData = IntPtr.Zero;
+                }
+                _size.X = 0.05f;
+                _size.Y = 0.05f;
+                _size.Z = 0.05f;
+                return false;
             }
 
 
@@ -1828,6 +1866,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             //     enableBody();
             // }
+            return true;
         }
 
         public void ProcessTaints(float timestep) //=============================================================================
@@ -1837,6 +1876,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 changeadd(timestep);
             }
 
+            if (m_taintremove)
+                return;
+
             if (prim_geom != IntPtr.Zero)
             {
                 if (!_position.ApproxEquals(m_taintposition, 0f))
@@ -2286,75 +2328,62 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
         {
+            bool gottrimesh = false;
+
+            if (_triMeshData != IntPtr.Zero)
+            {
+                d.GeomTriMeshDataDestroy(_triMeshData);
+                _triMeshData = IntPtr.Zero;
+            }
+
             if (_mesh != null)                                  // Special - make mesh
             {
-                setMesh(_parent_scene, _mesh);
+                gottrimesh = setMesh(_parent_scene, _mesh);
             }
-            else                                                // not a mesh
+
+            if (!gottrimesh)    // not a mesh
             {
-                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)           // special profile??
+                IntPtr geo = IntPtr.Zero;
+
+                if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
+                    && _size.X == _size.Y && _size.X == _size.Z)
                 {
-                    if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)                                 // Equi-size
+                    // its a sphere
+                    _parent_scene.waitForSpaceUnlock(m_targetSpace);
+                    try
                     {
-                        if (((_size.X / 2f) > 0f))                                                                      // Has size
-                        {
-                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                            try
-                            {
-                                SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
-                            }
-                            catch (AccessViolationException)
-                            {
-                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                                ode.dunlock(_parent_scene.world);
-                                return;
-                            }
-                        }
-                        else
-                        {
-                            _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                            try
-                            {
-                                SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
-                            }
-                            catch (AccessViolationException)
-                            {
-                                m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                                ode.dunlock(_parent_scene.world);
-                                return;
-                            }
-                        }
+                        geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
                     }
-                    else                                                                                                // not equi-size
+                    catch (Exception e)
                     {
-                        _parent_scene.waitForSpaceUnlock(m_targetSpace);
-                        try
-                        {
-                            SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
-                        }
-                        catch (AccessViolationException)
-                        {
-                            m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
-                            ode.dunlock(_parent_scene.world);
-                            return;
-                        }
+                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
+                        geo = IntPtr.Zero;
+                        ode.dunlock(_parent_scene.world);
                     }
                 }
-
-                else                                                                                    // not special profile
+                else // make it a box
                 {
                     _parent_scene.waitForSpaceUnlock(m_targetSpace);
                     try
                     {
-                        SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
+                        geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
                     }
-                    catch (AccessViolationException)
+                    catch (Exception e)
                     {
-                        m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
+                        m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message);
+                        geo = IntPtr.Zero;
                         ode.dunlock(_parent_scene.world);
-                        return;
                     }
                 }
+
+                if (geo == IntPtr.Zero)
+                {
+                    m_taintremove = true;
+                    _parent_scene.AddPhysicsActorTaint(this);
+                    return;
+                }
+
+                SetGeom(geo);
             }
         }
 
@@ -2372,18 +2401,17 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (_parent_scene.needsMeshing(_pbs))
                 {
-                    // Don't need to re-enable body..   it's done in SetMesh
                     try
                     {
-                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
+                        _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true);
                     }
                     catch
                     {
                         //Don't continuously try to mesh prims when meshing has failed
                         m_meshfailed = true;
+                        _mesh = null;
+                        m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                     }
-                    // createmesh returns null when it's a shape that isn't a cube.
-                    // m_log.Debug(m_localID);
                 }
             }
 
@@ -2630,17 +2658,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                 try
                 {
                     if (_parent_scene.needsMeshing(_pbs))
-                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
+                        mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                 }
                 catch
                 {
                     m_meshfailed = true;
+                    mesh = null;
+                    m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                 }
 
                 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
                 CreateGeom(m_targetSpace, mesh);
-
-
             }
             else
             {
@@ -2732,18 +2760,23 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 // Don't need to re-enable body..   it's done in SetMesh
                 float meshlod = _parent_scene.meshSculptLOD;
+                IMesh mesh;
 
                 if (IsPhysical)
                     meshlod = _parent_scene.MeshSculptphysicalLOD;
                 try
                 {
-                    IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
-                    CreateGeom(m_targetSpace, mesh);
+                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true);
                 }
                 catch
                 {
+                    mesh = null;
                     m_meshfailed = true;
+                    m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z);
                 }
+
+                CreateGeom(m_targetSpace, mesh);
+
                 // createmesh returns null when it doesn't mesh.
             }
             else
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index 61fb2d0..7a1e671 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -1772,7 +1772,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             IMesh mesh = null;
 
             if (needsMeshing(pbs))
-                mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
+                mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
 
             result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid);
 
@@ -2174,6 +2174,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                     {
                         prim.ResetTaints();
 
+                        try
+                        {
+                            if (prim._triMeshData != IntPtr.Zero)
+                            {
+                                d.GeomTriMeshDataDestroy(prim._triMeshData);
+                                prim._triMeshData = IntPtr.Zero;
+                            }
+                        }
+                        catch { };
+
                         if (prim.IsPhysical)
                         {
                             prim.disableBody();
@@ -2185,7 +2195,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 prim.IsPhysical = false;
                             }
 
-
                         }
                         // we don't want to remove the main space
 
@@ -2505,7 +2514,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
 
             // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
-            if (!forceSimplePrimMeshing)
+            if (!forceSimplePrimMeshing && !pbs.SculptEntry)
             {
                 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
                     || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
@@ -2528,6 +2537,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            if (forceSimplePrimMeshing)
+                return true;
+
             if (pbs.ProfileHollow != 0)
                 iPropertiesNotSupportedDefault++;
 
@@ -2592,6 +2604,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
             }
 
+            if (pbs.SculptEntry && meshSculptedPrim)
+                iPropertiesNotSupportedDefault++;
 
             if (iPropertiesNotSupportedDefault == 0)
             {
@@ -3443,8 +3457,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             int heightmapWidth = regionsize + 2;                                // ODE map size 257 x 257 (Meters) (1 extra 
             int heightmapHeight = regionsize + 2;
             
-            int heightmapWidthSamples = (int)regionsize + 3;            // Sample file size, 258 x 258 samples
-            int heightmapHeightSamples = (int)regionsize + 3;
+            int heightmapWidthSamples = (int)regionsize + 2;            // Sample file size, 258 x 258 samples
+            int heightmapHeightSamples = (int)regionsize + 2;
             
             // Array of height samples for ODE 
             float[] _heightmap;
@@ -3481,7 +3495,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                 // Output x = 0  1  2  3  ..... 255  256  257      258 total out
                     float val= heightMap[(yy * regionsize) + xx];  // input from heightMap,  <0-255 * 256> <0-255>
                     if (val < minele) val = minele;
-                    _heightmap[x * (heightmapHeightSamples) + y] = val; // samples output to _heightmap,  <0-257 * 258> <0-257>
+                    _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap,  <0-257 * 258> <0-257>
                     hfmin = (val < hfmin) ? val : hfmin;
                     hfmax = (val > hfmax) ? val : hfmax;
                 }
@@ -3531,8 +3545,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                 d.GeomSetRotation(GroundGeom, ref R);
                 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
-                // having nsamples = size + 1  center is actually at size/2
-                d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)), (pOffset.Y + (regionsize * 0.5f)), 0);
                 IntPtr testGround = IntPtr.Zero;
                 if (RegionTerrain.TryGetValue(pOffset, out testGround))
                 {
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index 80218e7..c9d0909 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -83,7 +83,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
         private float m_linearMotorDecayTimescale = 120;
         private float m_linearMotorTimescale = 1000;
-        private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
         private Vector3 m_linearMotorOffset = Vector3.Zero;
 
         //Angular properties
@@ -91,7 +90,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_angularMotorTimescale = 1000;                      // motor angular velocity ramp up rate
         private float m_angularMotorDecayTimescale = 120;                 // motor angular velocity decay rate
         private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);      // body angular velocity  decay rate
-        private Vector3 m_lastAngularVelocity = Vector3.Zero;           // what was last applied to body
 
         //Deflection properties
         private float m_angularDeflectionEfficiency = 0;
@@ -102,7 +100,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         //Banking properties
         private float m_bankingEfficiency = 0;
         private float m_bankingMix = 0;
-        private float m_bankingTimescale = 0;
+        private float m_bankingTimescale = 1000;
 
         //Hover and Buoyancy properties
         private float m_VhoverHeight = 0f;
@@ -117,9 +115,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 1000f;        // Timescale > 300  means no vert attractor.
 
-        // auxiliar 
-        private Vector3 m_dir = Vector3.Zero;                           // velocity applied to body
 
+        // auxiliar
         private float m_lmEfect = 0;                                            // current linear motor eficiency
         private float m_amEfect = 0;                                            // current angular motor eficiency
 
@@ -130,6 +127,82 @@ namespace OpenSim.Region.Physics.OdePlugin
             _pParentScene = rootPrim._parent_scene;
         }
 
+
+        public void DoSetVehicle(VehicleData vd)
+        {
+
+            float timestep = _pParentScene.ODE_STEPSIZE;
+            float invtimestep = 1.0f / timestep;
+
+            m_type = vd.m_type;
+            m_flags = vd.m_flags;
+
+            // Linear properties
+            m_linearMotorDirection = vd.m_linearMotorDirection;
+
+            m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
+            if (m_linearFrictionTimescale.X < timestep) m_linearFrictionTimescale.X = timestep;
+            if (m_linearFrictionTimescale.Y < timestep) m_linearFrictionTimescale.Y = timestep;
+            if (m_linearFrictionTimescale.Z < timestep) m_linearFrictionTimescale.Z = timestep;
+
+            m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
+            if (m_linearMotorDecayTimescale < 0.5f) m_linearMotorDecayTimescale = 0.5f;
+            m_linearMotorDecayTimescale *= invtimestep;
+
+            m_linearMotorTimescale = vd.m_linearMotorTimescale;
+            if (m_linearMotorTimescale < timestep) m_linearMotorTimescale = timestep;
+
+            m_linearMotorOffset = vd.m_linearMotorOffset;
+
+            //Angular properties
+            m_angularMotorDirection = vd.m_angularMotorDirection;
+            m_angularMotorTimescale = vd.m_angularMotorTimescale;
+            if (m_angularMotorTimescale < timestep) m_angularMotorTimescale = timestep;
+
+            m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
+            if (m_angularMotorDecayTimescale < 0.5f) m_angularMotorDecayTimescale = 0.5f;
+            m_angularMotorDecayTimescale *= invtimestep;
+
+            m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
+            if (m_angularFrictionTimescale.X < timestep) m_angularFrictionTimescale.X = timestep;
+            if (m_angularFrictionTimescale.Y < timestep) m_angularFrictionTimescale.Y = timestep;
+            if (m_angularFrictionTimescale.Z < timestep) m_angularFrictionTimescale.Z = timestep;
+
+            //Deflection properties
+            m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
+            m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
+            if (m_angularDeflectionTimescale < timestep) m_angularDeflectionTimescale = timestep;
+
+            m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
+            m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
+            if (m_linearDeflectionTimescale < timestep) m_linearDeflectionTimescale = timestep;
+
+            //Banking properties
+            m_bankingEfficiency = vd.m_bankingEfficiency;
+            m_bankingMix = vd.m_bankingMix;
+            m_bankingTimescale = vd.m_bankingTimescale;
+            if (m_bankingTimescale < timestep) m_bankingTimescale = timestep;
+
+            //Hover and Buoyancy properties
+            m_VhoverHeight = vd.m_VhoverHeight;
+            m_VhoverEfficiency = vd.m_VhoverEfficiency;
+            m_VhoverTimescale = vd.m_VhoverTimescale;
+            if (m_VhoverTimescale < timestep) m_VhoverTimescale = timestep;
+
+            m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
+
+            //Attractor properties
+            m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
+            m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
+            if (m_verticalAttractionTimescale < timestep) m_verticalAttractionTimescale = timestep;
+
+            // Axis
+            m_referenceFrame = vd.m_referenceFrame;
+
+            m_lmEfect = 0;
+            m_amEfect = 0;
+        }
+
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             float len;
@@ -231,6 +304,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
                     m_amEfect = 1.0f; // turn it on
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     if (pValue < timestep) pValue = timestep;
@@ -242,6 +318,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -273,6 +352,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 12.566f)
                         m_angularMotorDirection *= (12.566f / len);
                     m_amEfect = 1.0f; // turn it on
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     if (pValue.X < timestep) pValue.X = timestep;
@@ -286,6 +368,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     if (len > 30.0f)
                         m_linearMotorDirection *= (30.0f / len);
                     m_lmEfect = 1.0f; // turn it on
+                    if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
+                            && !rootPrim.m_isSelected && !rootPrim.m_disabled)
+                        d.BodyEnable(rootPrim.Body);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -347,12 +432,23 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
                     m_linearMotorTimescale = 1000;
-                    m_linearMotorDecayTimescale = 120 * invtimestep;
+                    m_linearMotorDecayTimescale = 120;
                     m_angularMotorTimescale = 1000;
-                    m_angularMotorDecayTimescale = 1000 * invtimestep;
+                    m_angularMotorDecayTimescale = 1000;
                     m_VhoverHeight = 0;
+                    m_VhoverEfficiency = 1;
                     m_VhoverTimescale = 1000;
                     m_VehicleBuoyancy = 0;
+                    m_linearDeflectionEfficiency = 0;
+                    m_linearDeflectionTimescale = 1000;
+                    m_angularDeflectionEfficiency = 0;
+                    m_angularDeflectionTimescale = 1000;
+                    m_bankingEfficiency = 0;
+                    m_bankingMix = 1;
+                    m_bankingTimescale = 1000;
+                    m_verticalAttractionEfficiency = 0;
+                    m_verticalAttractionTimescale = 1000;
+
                     m_flags = (VehicleFlag)0;
                     break;
 
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 3b7f562..0ccdbc0 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -111,7 +111,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                                                         | CollisionCategories.Body
                                                         | CollisionCategories.Character
                                                         );
-        private bool m_collidesLand = true;
+//        private bool m_collidesLand = true;
         private bool m_collidesWater;
         public bool m_returnCollisions;
 
@@ -122,7 +122,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private CollisionCategories m_collisionFlags = m_default_collisionFlags;
 
         public bool m_disabled;
-        public bool m_taintselected;
+
 
         public uint m_localID;
 
@@ -142,20 +142,19 @@ namespace OpenSim.Region.Physics.OdePlugin
         private List<OdePrim> childrenPrim = new List<OdePrim>();
 
         private bool m_iscolliding;
-        private bool m_wascolliding;
-        private bool m_isSelected;
+
+        public bool m_isSelected;
+        private bool m_delaySelect;
+        private bool m_lastdoneSelected;
+        public bool m_outbounds;
 
         internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
 
         private bool m_throttleUpdates;
         private int throttleCounter;
-        public int m_interpenetrationcount;
         public float m_collisionscore;
         int m_colliderfilter = 0;
-        public int m_roundsUnderMotionThreshold;
-        private int m_crossingfailures;
 
-        public bool outofBounds;
         private float m_density = 10.000006836f; // Aluminum g/cm3;
 
         public bool _zeroFlag;
@@ -166,12 +165,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         private Vector3 _target_velocity;
 
         public Vector3 primOOBsize; // prim real dimensions from mesh 
-        public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb
+        public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
         public float primOOBradiusSQ;
         public d.Mass primdMass; // prim inertia information on it's own referencial
         float primMass; // prim own mass
         float _mass; // object mass acording to case
-        public d.Mass objectpMass; // object last computed inertia
         private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
 
         public int givefakepos = 0;
@@ -182,9 +180,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         public int m_eventsubscription;
         private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
 
-        private IntPtr m_linkJoint = IntPtr.Zero;
-        private IntPtr _linkJointGroup = IntPtr.Zero;
-
         public volatile bool childPrim;
 
         public ODEDynamics m_vehicle;
@@ -264,7 +259,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             set
             {
                 if (value)
-                    m_isSelected = value;
+                    m_isSelected = value; // if true set imediatly to stop moves etc
                 AddChange(changes.Selected, value);
             }
         }
@@ -298,13 +293,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     m_iscolliding = false;
                 else
                     m_iscolliding = true;
-
-                if (m_wascolliding != m_iscolliding)
-                {
-                    if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero)
-                        d.BodyEnable(Body);
-                    m_wascolliding = m_iscolliding;
-                }
             }
         }
 
@@ -665,19 +653,21 @@ namespace OpenSim.Region.Physics.OdePlugin
             strVehicleQuatParam fp = new strVehicleQuatParam();
             fp.param = param;
             fp.value = value;
-            AddChange(changes.VehicleVectorParam, fp);
+            AddChange(changes.VehicleRotationParam, fp);
         }
 
         public override void VehicleFlags(int param, bool value)
         {
-            if (m_vehicle == null)
-                return;
             strVehicleBoolParam bp = new strVehicleBoolParam();
             bp.param = param;
             bp.value = value;
             AddChange(changes.VehicleFlags, bp);
         }
 
+        public override void SetVehicle(object vdata)
+        {
+            AddChange(changes.SetVehicle, vdata);
+        }
         public void SetAcceleration(Vector3 accel)
         {
             _acceleration = accel;
@@ -710,8 +700,30 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public override void CrossingFailure()
         {
-            m_crossingfailures++;
-            changeDisable(false);
+            if (m_outbounds)
+            {
+                _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
+                _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
+                _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
+
+                m_lastposition = _position;
+                _velocity.X = 0;
+                _velocity.Y = 0;
+                _velocity.Z = 0;
+
+                m_lastVelocity = _velocity;
+                if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+                    m_vehicle.Stop();
+
+                if(Body != IntPtr.Zero)
+                    d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+                if (prim_geom != IntPtr.Zero)
+                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+
+                m_outbounds = false;
+                changeDisable(false);
+                base.RequestPhysicsterseUpdate();
+            }
         }
 
         public override void SetMomentum(Vector3 momentum)
@@ -865,12 +877,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             m_force = Vector3.Zero;
 
             m_iscolliding = false;
-            m_wascolliding = false;
             m_colliderfilter = 0;
 
             hasOOBoffsetFromMesh = false;
             _triMeshData = IntPtr.Zero;
 
+            m_lastdoneSelected = false;
+            m_isSelected = false;
+            m_delaySelect = false;
 
             primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
             primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
@@ -885,8 +899,6 @@ namespace OpenSim.Region.Physics.OdePlugin
         private void resetCollisionAccounting()
         {
             m_collisionscore = 0;
-            m_interpenetrationcount = 0;
-            m_disabled = false;
         }
 
         private void createAMotor(Vector3 axis)
@@ -926,9 +938,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             curr.W = dcur.W;
             Vector3 ax;
 
-            const int StopERP = 7;
-            const int StopCFM = 8;
-
             int i = 0;
             int j = 0;
             if (axis.X == 0)
@@ -943,10 +952,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
                 i++;
-                j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set
+                j = 256; // move to next axis set
             }
 
             if (axis.Y == 0)
@@ -960,8 +969,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
                 i++;
                 j += 256;
             }
@@ -977,8 +986,8 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
                 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
-                d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
+                d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
             }
         }
 
@@ -1186,24 +1195,10 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void enableBodySoft()
         {
-            if (!childPrim)
+            if (!childPrim && !m_isSelected)
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    if (m_targetSpace != _parent_scene.ActiveSpace)
-                    {
-                        m_targetSpace = _parent_scene.ActiveSpace;
-
-                        foreach (OdePrim prm in childrenPrim)
-                        {
-                            if (prm.prim_geom != IntPtr.Zero)
-                            {
-                                d.SpaceAdd(m_targetSpace, prm.prim_geom);
-                                prm.m_targetSpace = m_targetSpace;
-                            }
-                        }
-                        d.SpaceAdd(m_targetSpace, prim_geom);
-                    }
                     d.GeomEnable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomEnable(prm.prim_geom);
@@ -1211,6 +1206,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.BodyEnable(Body);
                 }
             }
+            m_disabled = false;
             resetCollisionAccounting(); // this sets m_disable to false
         }
 
@@ -1221,19 +1217,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    if (m_targetSpace == _parent_scene.ActiveSpace)
-                    {
-                        foreach (OdePrim prm in childrenPrim)
-                        {
-                            if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero)
-                            {
-                                d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
-                                prm.m_targetSpace = IntPtr.Zero;
-                            }
-                        }
-                        d.SpaceRemove(m_targetSpace, prim_geom);
-                        m_targetSpace = IntPtr.Zero;
-                    }
                     d.GeomDisable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomDisable(prm.prim_geom);
@@ -1369,9 +1352,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetMass(Body, ref objdmass);
             _mass = objdmass.mass;
 
-            m_collisionCategories |= CollisionCategories.Body;
-            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
-
             // disconnect from world gravity so we can apply buoyancy
             d.BodySetGravityMode(Body, false);
 
@@ -1379,16 +1359,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
             //            d.BodySetLinearDampingThreshold(Body, 0.01f);
             //            d.BodySetAngularDampingThreshold(Body, 0.001f);
-            d.BodySetDamping(Body, .001f, .0002f);
+            d.BodySetDamping(Body, .002f, .002f);
 
+            m_collisionCategories |= CollisionCategories.Body;
+            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
             d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
             d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
-
-            m_interpenetrationcount = 0;
             m_collisionscore = 0;
 
-            m_disabled = false;
-
             if (m_targetSpace != _parent_scene.ActiveSpace)
             {
                 if (m_targetSpace != IntPtr.Zero)
@@ -1416,6 +1394,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                     d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                     d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
+                    prm.m_collisionscore = 0;
 
                     if (prm.m_targetSpace != _parent_scene.ActiveSpace)
                     {
@@ -1428,10 +1407,11 @@ namespace OpenSim.Region.Physics.OdePlugin
                         prm.m_targetSpace = _parent_scene.ActiveSpace;
                         d.SpaceAdd(m_targetSpace, prm.prim_geom);
                     }
-                    d.GeomEnable(prm.prim_geom);
+
+                    if (m_isSelected || m_disabled)
+                        d.GeomDisable(prm.prim_geom);
+
                     prm.m_disabled = false;
-                    prm.m_interpenetrationcount = 0;
-                    prm.m_collisionscore = 0;
                     _parent_scene.addActivePrim(prm);
                 }
             }
@@ -1442,8 +1422,12 @@ namespace OpenSim.Region.Physics.OdePlugin
                 createAMotor(m_angularlock);
             }
 
-            d.GeomEnable(prim_geom);
-            m_disabled = false;
+            if (m_isSelected || m_disabled)
+            {
+                d.GeomDisable(prim_geom);
+                d.BodyDisable(Body);
+            }
+
             _parent_scene.addActivePrim(this);
         }
 
@@ -1484,12 +1468,16 @@ namespace OpenSim.Region.Physics.OdePlugin
                             prm.m_collisionscore = 0;
                         }
                     }
+                    if (Amotor != IntPtr.Zero)
+                    {
+                        d.JointDestroy(Amotor);
+                        Amotor = IntPtr.Zero;
+                    }
                     d.BodyDestroy(Body);
                 }
                 Body = IntPtr.Zero;
             }
             _mass = primMass;
-            m_disabled = true;
             m_collisionscore = 0;
         }
 
@@ -2115,49 +2103,72 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.BodySetTorque(Body, 0f, 0f, 0f);
                 d.BodySetLinearVel(Body, 0f, 0f, 0f);
                 d.BodySetAngularVel(Body, 0f, 0f, 0f);
-
             }
         }
 
         private void changeSelectedStatus(bool newval)
         {
+            if (m_lastdoneSelected == newval)
+                return;
+
+            m_lastdoneSelected = newval;
+            DoSelectedStatus(newval);
+        }
+
+        private void CheckDelaySelect()
+        {
+            if (m_delaySelect)
+            {
+                DoSelectedStatus(m_isSelected);
+            }
+        }
+
+        private void DoSelectedStatus(bool newval)
+        {
             m_isSelected = newval;
             Stop();
 
             if (newval)
             {
-                m_collisionCategories = CollisionCategories.Selected;
-                m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
+                if (!childPrim && Body != IntPtr.Zero)
+                    d.BodyDisable(Body);
 
-                if (prim_geom != IntPtr.Zero)
+                if (m_delaySelect)
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    if (!childPrim)
+                    {
+                        foreach (OdePrim prm in childrenPrim)
+                        {
+                            d.GeomDisable(prm.prim_geom);
+                            prm.m_delaySelect = false;
+                        }
+                    }
+                    d.GeomDisable(prim_geom);
+                    m_delaySelect = false;
+                }
+                else
+                {
+                    m_delaySelect = true;
                 }
-
-                disableBodySoft();
             }
             else
             {
-                m_collisionCategories = CollisionCategories.Geom;
-
-                if (m_isphysical)
-                    m_collisionCategories |= CollisionCategories.Body;
-
-                m_collisionFlags = m_default_collisionFlags;
-
-                if (m_collidesLand)
-                    m_collisionFlags |= CollisionCategories.Land;
-                if (m_collidesWater)
-                    m_collisionFlags |= CollisionCategories.Water;
+                if (!childPrim && Body != IntPtr.Zero && !m_disabled)
+                    d.BodyEnable(Body);
 
-                if (prim_geom != IntPtr.Zero)
+                if (!childPrim)
                 {
-                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
-                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
+                    foreach (OdePrim prm in childrenPrim)
+                    {
+                        if(!prm.m_disabled)
+                            d.GeomEnable(prm.prim_geom);
+                        prm.m_delaySelect = false;
+                    }
                 }
+                if(!m_disabled)
+                    d.GeomEnable(prim_geom);
 
-                enableBodySoft();
+                m_delaySelect = false;
             }
 
             resetCollisionAccounting();
@@ -2165,6 +2176,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePosition(Vector3 newPos)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim)  // inertia is messed, must rebuild
@@ -2207,6 +2219,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changeOrientation(Quaternion newOri)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim)  // inertia is messed, must rebuild
@@ -2258,6 +2271,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
         {
+            CheckDelaySelect();
             if (m_isphysical)
             {
                 if (childPrim && m_building)  // inertia is messed, must rebuild
@@ -2342,6 +2356,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changePhysicsStatus(bool NewStatus)
         {
+            CheckDelaySelect();
+
             m_isphysical = NewStatus;
 
             if (!childPrim)
@@ -2384,6 +2400,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         private void changeprimsizeshape()
         {
+            CheckDelaySelect();
+
             OdePrim parent = (OdePrim)_parent;
 
             bool chp = childPrim;
@@ -2508,7 +2526,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 }
 
                 m_collisionscore = 0;
-                m_interpenetrationcount = 0;
             }
         }
 
@@ -2528,7 +2545,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                 }
                 m_collisionscore = 0;
-                m_interpenetrationcount = 0;
             }
         }
 
@@ -2565,6 +2581,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             else
             {
                 m_building = false;
+                CheckDelaySelect();
                 if (!childPrim)
                     MakeBody();
             }
@@ -2575,18 +2592,26 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
-        private void changeVehicleType(int value)
+       public void changeSetVehicle(VehicleData vdata)
         {
             if (m_vehicle == null)
+                m_vehicle = new ODEDynamics(this);
+            m_vehicle.DoSetVehicle(vdata);
+        }
+        private void changeVehicleType(int value)
+        {
+            if (value == (int)Vehicle.TYPE_NONE)
             {
-                if (value != (int)Vehicle.TYPE_NONE)
-                {
-                    m_vehicle = new ODEDynamics(this);
-                    m_vehicle.ProcessTypeChange((Vehicle)value);
-                }
+                if (m_vehicle != null)
+                    m_vehicle = null;
             }
             else
+            {
+                if (m_vehicle == null)
+                    m_vehicle = new ODEDynamics(this);
+
                 m_vehicle.ProcessTypeChange((Vehicle)value);
+            }
         }
 
         private void changeVehicleFloatParam(strVehicleFloatParam fp)
@@ -2595,8 +2620,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 return;
 
             m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleVectorParam(strVehicleVectorParam vp)
@@ -2604,8 +2627,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleRotationParam(strVehicleQuatParam qp)
@@ -2613,8 +2634,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         private void changeVehicleFlags(strVehicleBoolParam bp)
@@ -2622,8 +2641,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_vehicle == null)
                 return;
             m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
-            if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
-                d.BodyEnable(Body);
         }
 
         #endregion
@@ -2849,41 +2866,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (Body != IntPtr.Zero)
                 {
-                    if (m_crossingfailures != 0 && m_crossingfailures < 5)
-                    {
-                        _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f);
-                        _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f);
-                        _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
-
-                        float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y);
-                        if (_position.Z < tmp)
-                            _position.Z = tmp + 0.2f;
-
-                        m_lastposition = _position;
-                        m_lastorientation = _orientation;
-                        _velocity.X = 0;
-                        _velocity.Y = 0;
-                        _velocity.Z = 0;
-
-                        m_lastVelocity = _velocity;
-                        m_rotationalVelocity = _velocity;
-                        if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
-                            m_vehicle.Stop();
-
-                        m_crossingfailures = 0; // do this only once
-                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
-                        d.BodySetAngularVel(Body, 0, 0, 0);
-                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-                        enableBodySoft();
-                        base.RequestPhysicsterseUpdate();
-                        return;
-                    }
-
-                    else if (m_crossingfailures != 0)
-                    {
-                        return;
-                    }
-
                     Vector3 pv = Vector3.Zero;
                     bool lastZeroFlag = _zeroFlag;
 
@@ -2899,24 +2881,21 @@ namespace OpenSim.Region.Physics.OdePlugin
                         // we can't let it keeping moving and having colisions
                         // since it can be stucked between something like terrain and edge
                         // so lets stop and disable it until something else kicks it
-                        if (m_crossingfailures == 0)
-                        {
 
-                            _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f);
-                            _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f);
-                            _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
+                        _position.X = Util.Clip(lpos.X, -0.2f, _parent_scene.WorldExtents.X + 0.2f);
+                        _position.Y = Util.Clip(lpos.Y, -0.2f, _parent_scene.WorldExtents.Y + 0.2f);
+                        _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
 
-                            m_lastposition = _position;
-                            m_lastorientation = _orientation;
+                        m_lastposition = _position;
+//                        m_lastorientation = _orientation;
 
-                            d.BodySetLinearVel(Body, 0, 0, 0); // stop it
-                            d.BodySetAngularVel(Body, 0, 0, 0);
-                            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
-                            disableBodySoft(); // stop collisions
-                            m_crossingfailures++; // do this only once
-                            base.RequestPhysicsterseUpdate();
+                        d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+//                        d.BodySetAngularVel(Body, 0, 0, 0);
+                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+                        disableBodySoft(); // stop collisions
+                        m_outbounds = true;
+                        base.RequestPhysicsterseUpdate();
                             return;
-                        }
                     }
 
                     if (lpos.Z < -100 || lpos.Z > 100000f)
@@ -3159,6 +3138,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     else
                         ChildRemove(this, false);
 
+                    m_vehicle = null;
                     RemoveGeom();
                     m_targetSpace = IntPtr.Zero;
                     if (m_eventsubscription > 0)
@@ -3273,6 +3253,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                     changeVehicleRotationParam((strVehicleQuatParam) arg);
                     break;
 
+                case changes.SetVehicle:
+                    changeSetVehicle((VehicleData) arg);
+                    break;
                 case changes.Null:
                     donullchange();
                     break;
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
index e62746e..2b6bc59 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -1,4 +1,3 @@
-
 /*
  * based on:
  * Ode.NET - .NET bindings for ODE
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 56f3786..6e4c373 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         VehicleVectorParam,
         VehicleRotationParam,
         VehicleFlags,
+        SetVehicle,
 
         Null             //keep this last used do dim the methods array. does nothing but pulsing the prim
     }
@@ -166,8 +167,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         
         float frictionMovementMult = 0.3f;
 
-        float TerrainBounce = 0.3f;
-        float TerrainFriction = 0.3f;
+        float TerrainBounce = 0.1f;
+        float TerrainFriction = 0.1f;
 
         public float AvatarBounce = 0.3f;
         public float AvatarFriction = 0;// 0.9f * 0.5f;
@@ -989,145 +990,62 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="timeStep"></param>
         private void collision_optimized()
         {
-//        _perloopContact.Clear();
-// clear characts IsColliding until we do it some other way
-
             lock (_characters)
                 {
-                foreach (OdeCharacter chr in _characters)
+                try
+                {
+                    foreach (OdeCharacter chr in _characters)
                     {
-                    // this are odd checks  if they are needed something is wrong elsewhere
-                    // keep for now
-                    if (chr == null)
-                        continue;
-
-                    if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
-                        continue;
-
-                    chr.IsColliding = false;
-                    //                    chr.CollidingGround = false; not done here
-                    chr.CollidingObj = false;
+                        if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
+                            continue;
+
+                        chr.IsColliding = false;
+                        //                    chr.CollidingGround = false; not done here
+                        chr.CollidingObj = false;
+                        // do colisions with static space
+                        d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
                     }
                 }
-
-            // now let ode do its job
-            // colide active things amoung them
-
-            int st = Util.EnvironmentTickCount();
-            int ta;
-            int ts;
-            try
-                {
-                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
-                }
-            catch (AccessViolationException)
+                catch (AccessViolationException)
                 {
-                m_log.Warn("[PHYSICS]: Unable to Active space collide");
+                    m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
                 }
-            ta = Util.EnvironmentTickCountSubtract(st);
-            // then active things with static enviroment
-            try
+
+            }
+
+            // collide active prims with static enviroment
+            lock (_activeprims)
+            {
+                try
                 {
-                d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback);
+                    foreach (OdePrim prm in _activeprims)
+                    {
+                        if (d.BodyIsEnabled(prm.Body))
+                            d.SpaceCollide2(StaticSpace, prm.prim_geom, IntPtr.Zero, nearCallback);
+                    }
                 }
-            catch (AccessViolationException)
+                catch (AccessViolationException)
                 {
-                m_log.Warn("[PHYSICS]: Unable to Active to static space collide");
+                    m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
                 }
-            ts = Util.EnvironmentTickCountSubtract(st);
-//            _perloopContact.Clear();
-        }
-
-        #endregion
-
-
-        public float GetTerrainHeightAtXY(float x, float y)
-        {
-            // assumes 1m size grid and constante size square regions
-            // region offset in mega position
-
-            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
-
-            IntPtr heightFieldGeom = IntPtr.Zero;
-
-            // get region map
-            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
-                return 0f;
-
-            if (heightFieldGeom == IntPtr.Zero)
-                return 0f;
-
-            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
-                return 0f;
-
-            // TerrainHeightField for ODE as offset 1m
-            x += 1f - offsetX;
-            y += 1f - offsetY;
-
-            // make position fit into array
-            if (x < 0)
-                x = 0;
-            if (y < 0)
-                y = 0;
-
-            // integer indexs
-            int ix;
-            int iy;
-            //  interpolators offset
-            float dx;
-            float dy;
-
-            int regsize = (int)Constants.RegionSize + 2; // map size see setterrain
-
-            // we  still have square fixed size regions
-            // also flip x and y because of how map is done for ODE fliped axis
-            // so ix,iy,dx and dy are inter exchanged
-            if (x < regsize - 1)
-            {
-                iy = (int)x;
-                dy = x - (float)iy;
-            }
-            else // out world use external height
-            {
-                iy = regsize - 1;
-                dy = 0;
             }
-            if (y < regsize - 1)
+
+            // finally colide active things amoung them
+            try
             {
-                ix = (int)y;
-                dx = y - (float)ix;
+                d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
             }
-            else
+            catch (AccessViolationException)
             {
-                ix = regsize - 1;
-                dx = 0;
+                m_log.Warn("[PHYSICS]: Unable to collide in Active space");
             }
 
-            float h0;
-            float h1;
-            float h2;
-
-            iy *= regsize;
-            iy += ix; // all indexes have iy + ix
+//            _perloopContact.Clear();
+        }
 
-            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+        #endregion
 
-            if ((dx + dy) <= 1.0f)
-            {
-                h0 = ((float)heights[iy]); // 0,0 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
-                h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
-            }
-            else
-            {
-                h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
-                h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
-                h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
-            }
 
-            return h0 + h1 + h2;
-        }
 
         /// <summary>
         /// Add actor to the list that should receive collision events in the simulate loop.
@@ -1835,273 +1753,94 @@ namespace OpenSim.Region.Physics.OdePlugin
             get { return (false); }
         }
 
-        #region ODE Specific Terrain Fixes
-        public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
+        public float GetTerrainHeightAtXY(float x, float y)
         {
-            float[] returnarr = new float[262144];
-            float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
+            // region offset in mega position
 
-            // Filling out the array into its multi-dimensional components
-            for (int y = 0; y < WorldExtents.Y; y++)
-            {
-                for (int x = 0; x < WorldExtents.X; x++)
-                {
-                    resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
-                }
-            }
+            int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
+            int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
 
-            // Resize using Nearest Neighbour
-
-            // This particular way is quick but it only works on a multiple of the original
-
-            // The idea behind this method can be described with the following diagrams
-            // second pass and third pass happen in the same loop really..  just separated
-            // them to show what this does.
-
-            // First Pass
-            // ResultArr:
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-
-            // Second Pass
-            // ResultArr2:
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-
-            // Third pass fills in the blanks
-            // ResultArr2:
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-
-            // X,Y = .
-            // X+1,y = ^
-            // X,Y+1 = *
-            // X+1,Y+1 = #
-
-            // Filling in like this;
-            // .*
-            // ^#
-            // 1st .
-            // 2nd *
-            // 3rd ^
-            // 4th #
-            // on single loop.
-
-            float[,] resultarr2 = new float[512, 512];
-            for (int y = 0; y < WorldExtents.Y; y++)
-            {
-                for (int x = 0; x < WorldExtents.X; x++)
-                {
-                    resultarr2[y * 2, x * 2] = resultarr[y, x];
+            IntPtr heightFieldGeom = IntPtr.Zero;
 
-                    if (y < WorldExtents.Y)
-                    {
-                        resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
-                    }
-                    if (x < WorldExtents.X)
-                    {
-                        resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
-                    }
-                    if (x < WorldExtents.X && y < WorldExtents.Y)
-                    {
-                        resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
-                    }
-                }
-            }
+            // get region map
+            if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
+                return 0f;
 
-            //Flatten out the array
-            int i = 0;
-            for (int y = 0; y < 512; y++)
-            {
-                for (int x = 0; x < 512; x++)
-                {
-                    if (resultarr2[y, x] <= 0)
-                        returnarr[i] = 0.0000001f;
-                    else
-                        returnarr[i] = resultarr2[y, x];
+            if (heightFieldGeom == IntPtr.Zero)
+                return 0f;
 
-                    i++;
-                }
-            }
+            if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
+                return 0f;
 
-            return returnarr;
-        }
+            // TerrainHeightField for ODE as offset 1m
+            x += 1f - offsetX;
+            y += 1f - offsetY;
 
-        public float[] ResizeTerrain512Interpolation(float[] heightMap)
-        {
-            float[] returnarr = new float[262144];
-            float[,] resultarr = new float[512,512];
+            // make position fit into array
+            if (x < 0)
+                x = 0;
+            if (y < 0)
+                y = 0;
+
+            // integer indexs
+            int ix;
+            int iy;
+            //  interpolators offset
+            float dx;
+            float dy;
+
+            int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
 
-            // Filling out the array into its multi-dimensional components
-            for (int y = 0; y < 256; y++)
+            // we  still have square fixed size regions
+            // also flip x and y because of how map is done for ODE fliped axis
+            // so ix,iy,dx and dy are inter exchanged
+            if (x < regsize - 1)
             {
-                for (int x = 0; x < 256; x++)
-                {
-                    resultarr[y, x] = heightMap[y * 256 + x];
-                }
+                iy = (int)x;
+                dy = x - (float)iy;
             }
-
-            // Resize using interpolation
-
-            // This particular way is quick but it only works on a multiple of the original
-
-            // The idea behind this method can be described with the following diagrams
-            // second pass and third pass happen in the same loop really..  just separated
-            // them to show what this does.
-
-            // First Pass
-            // ResultArr:
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-            // 1,1,1,1,1,1
-
-            // Second Pass
-            // ResultArr2:
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-            // ,,,,,,,,,,
-            // 1,,1,,1,,1,,1,,1,
-
-            // Third pass fills in the blanks
-            // ResultArr2:
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-            // 1,1,1,1,1,1,1,1,1,1,1,1
-
-            // X,Y = .
-            // X+1,y = ^
-            // X,Y+1 = *
-            // X+1,Y+1 = #
-
-            // Filling in like this;
-            // .*
-            // ^#
-            // 1st .
-            // 2nd *
-            // 3rd ^
-            // 4th #
-            // on single loop.
-
-            float[,] resultarr2 = new float[512,512];
-            for (int y = 0; y < (int)Constants.RegionSize; y++)
+            else // out world use external height
             {
-                for (int x = 0; x < (int)Constants.RegionSize; x++)
-                {
-                    resultarr2[y*2, x*2] = resultarr[y, x];
-
-                    if (y < (int)Constants.RegionSize)
-                    {
-                        if (y + 1 < (int)Constants.RegionSize)
-                        {
-                            if (x + 1 < (int)Constants.RegionSize)
-                            {
-                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                            }
-                            else
-                            {
-                                resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
-                            }
-                        }
-                        else
-                        {
-                            resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
-                        }
-                    }
-                    if (x < (int)Constants.RegionSize)
-                    {
-                        if (x + 1 < (int)Constants.RegionSize)
-                        {
-                            if (y + 1 < (int)Constants.RegionSize)
-                            {
-                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                               resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                            }
-                            else
-                            {
-                                resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
-                            }
-                        }
-                        else
-                        {
-                            resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
-                        }
-                    }
-                    if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
-                    {
-                        if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
-                        {
-                            resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
-                                                                 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
-                        }
-                        else
-                        {
-                            resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
-                        }
-                    }
-                }
+                iy = regsize - 1;
+                dy = 0;
             }
-            //Flatten out the array
-            int i = 0;
-            for (int y = 0; y < 512; y++)
+            if (y < regsize - 1)
             {
-                for (int x = 0; x < 512; x++)
-                {
-                    if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
-                    {
-                        m_log.Warn("[PHYSICS]: Non finite heightfield element detected.  Setting it to 0");
-                        resultarr2[y, x] = 0;
-                    }
-                    returnarr[i] = resultarr2[y, x];
-                    i++;
-                }
+                ix = (int)y;
+                dx = y - (float)ix;
+            }
+            else
+            {
+                ix = regsize - 1;
+                dx = 0;
             }
 
-            return returnarr;
-        }
+            float h0;
+            float h1;
+            float h2;
 
-        #endregion
+            iy *= regsize;
+            iy += ix; // all indexes have iy + ix
 
+            float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
+
+            if ((dx + dy) <= 1.0f)
+            {
+                h0 = ((float)heights[iy]); // 0,0 vertice
+                h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
+                h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
+            }
+            else
+            {
+                h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
+                h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
+                h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
+            }
+
+            return h0 + h1 + h2;
+        }
         public override void SetTerrain(float[] heightMap)
         {
             if (m_worldOffset != Vector3.Zero && m_parentScene != null)
@@ -2124,48 +1863,47 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         public void SetTerrain(float[] heightMap, Vector3 pOffset)
             {
+            // assumes 1m size grid and constante size square regions
+            // needs to know about sims around in future
 
             float[] _heightmap;
-            _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
 
             uint heightmapWidth = Constants.RegionSize + 2;
             uint heightmapHeight = Constants.RegionSize + 2;
 
-            uint heightmapWidthSamples;
+            uint heightmapWidthSamples = heightmapWidth + 1;
+            uint heightmapHeightSamples = heightmapHeight + 1;
 
-            uint heightmapHeightSamples;
- 
-            heightmapWidthSamples = (uint)Constants.RegionSize + 2;
-            heightmapHeightSamples = (uint)Constants.RegionSize + 2;
+            _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
 
             const float scale = 1.0f;
             const float offset = 0.0f;
             const float thickness = 10f;
             const int wrap = 0;
 
-            int regionsize = (int) Constants.RegionSize + 2;
+            uint regionsize = Constants.RegionSize;
  
             float hfmin = float.MaxValue;
             float hfmax = float.MinValue;
             float val;
-            int xx;
-            int yy;
+            uint xx;
+            uint yy;
 
-            int maxXXYY = regionsize - 3;
+            uint maxXXYY = regionsize - 1;
             // flipping map adding one margin all around so things don't fall in edges
 
-            int xt = 0;
+            uint xt = 0;
             xx = 0;
 
-            for (int x = 0; x < heightmapWidthSamples; x++)
+            for (uint x = 0; x < heightmapWidthSamples; x++)
             {
                 if (x > 1 && xx < maxXXYY)
                     xx++;
                 yy = 0;
-                for (int y = 0; y < heightmapHeightSamples; y++)
+                for (uint y = 0; y < heightmapHeightSamples; y++)
                 {
                     if (y > 1 && y < maxXXYY)
-                        yy += (int)Constants.RegionSize;
+                        yy += regionsize;
 
                     val = heightMap[yy + xx];
                     _heightmap[xt + y] = val;
@@ -2176,8 +1914,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                         hfmax = val;
 
                 }
-
-                xt += regionsize;
+                xt += heightmapHeightSamples;
             }
             lock (OdeLock)
             {
@@ -2230,11 +1967,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
                 d.GeomSetRotation(GroundGeom, ref R);
                 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0);
-                IntPtr testGround = IntPtr.Zero;
-                if (RegionTerrain.TryGetValue(pOffset, out testGround))
-                {
-                    RegionTerrain.Remove(pOffset);
-                }
                 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
 //                TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
                 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 8708b99..4366626 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -2484,13 +2484,13 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         public void llApplyRotationalImpulse(LSL_Vector force, int local)
         {
             m_host.AddScriptLPS(1);
-            m_host.ApplyAngularImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z), local != 0);
+            m_host.ParentGroup.RootPart.ApplyAngularImpulse(new Vector3((float)force.x, (float)force.y, (float)force.z), local != 0);
         }
 
         public void llSetTorque(LSL_Vector torque, int local)
         {
             m_host.AddScriptLPS(1);
-            m_host.SetAngularImpulse(new Vector3((float)torque.x, (float)torque.y, (float)torque.z), local != 0);
+            m_host.ParentGroup.RootPart.SetAngularImpulse(new Vector3((float)torque.x, (float)torque.y, (float)torque.z), local != 0);
         }
 
         public LSL_Vector llGetTorque()
@@ -4657,6 +4657,28 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             ScriptSleep(5000);
         }
 
+        public void llTeleportAgent(string agent, string simname, LSL_Vector pos, LSL_Vector lookAt)
+        {
+            m_host.AddScriptLPS(1);
+            UUID agentId = new UUID();
+            if (UUID.TryParse(agent, out agentId))
+            {
+                ScenePresence presence = World.GetScenePresence(agentId);
+                if (presence != null)
+                {
+                    // agent must not be a god
+                    if (presence.UserLevel >= 200) return;
+
+                    // agent must be over the owners land
+                    if (m_host.OwnerID == World.LandChannel.GetLandObject(
+                            presence.AbsolutePosition.X, presence.AbsolutePosition.Y).LandData.OwnerID)
+                    {
+                        World.RequestTeleportLocation(presence.ControllingClient, simname, new Vector3((float)pos.x, (float)pos.y, (float)pos.z), new Vector3((float)lookAt.x, (float)lookAt.y, (float)lookAt.z), (uint)TeleportFlags.ViaLocation);
+                    }
+                }
+            }
+        }
+
         public void llTextBox(string agent, string message, int chatChannel)
         {
             IDialogModule dm = World.RequestModuleInterface<IDialogModule>();
@@ -11980,6 +12002,144 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         }
 
         #endregion
+
+        public void llSetKeyframedMotion(LSL_List frames, LSL_List options)
+        {
+            SceneObjectGroup group = m_host.ParentGroup;
+
+            if (group.RootPart.PhysActor != null && group.RootPart.PhysActor.IsPhysical)
+                return;
+            if (group.IsAttachment)
+                return;
+
+            if (frames.Data.Length > 0) // We are getting a new motion
+            {
+                if (group.RootPart.KeyframeMotion != null)
+                    group.RootPart.KeyframeMotion.Stop();
+                group.RootPart.KeyframeMotion = null;
+
+                int idx = 0;
+
+                KeyframeMotion.PlayMode mode = KeyframeMotion.PlayMode.Forward;
+                KeyframeMotion.DataFormat data = KeyframeMotion.DataFormat.Translation | KeyframeMotion.DataFormat.Rotation;
+
+                while (idx < options.Data.Length)
+                {
+                    int option = (int)options.GetLSLIntegerItem(idx++);
+                    int remain = options.Data.Length - idx;
+
+                    switch (option)
+                    {
+                        case ScriptBaseClass.KFM_MODE:
+                            if (remain < 1)
+                                break;
+                            int modeval = (int)options.GetLSLIntegerItem(idx++);
+                            switch(modeval)
+                            {
+                                case ScriptBaseClass.KFM_FORWARD:
+                                    mode = KeyframeMotion.PlayMode.Forward;
+                                    break;
+                                case ScriptBaseClass.KFM_REVERSE:
+                                    mode = KeyframeMotion.PlayMode.Reverse;
+                                    break;
+                                case ScriptBaseClass.KFM_LOOP:
+                                    mode = KeyframeMotion.PlayMode.Loop;
+                                    break;
+                                case ScriptBaseClass.KFM_PING_PONG:
+                                    mode = KeyframeMotion.PlayMode.PingPong;
+                                    break;
+                            }
+                            break;
+                        case ScriptBaseClass.KFM_DATA:
+                            if (remain < 1)
+                                break;
+                            int dataval = (int)options.GetLSLIntegerItem(idx++);
+                            data = (KeyframeMotion.DataFormat)dataval;
+                            break;
+                    }
+                }
+
+                group.RootPart.KeyframeMotion = new KeyframeMotion(group, mode, data);
+
+                idx = 0;
+
+                int elemLength = 2;
+                if (data == (KeyframeMotion.DataFormat.Translation | KeyframeMotion.DataFormat.Rotation))
+                    elemLength = 3;
+
+                List<KeyframeMotion.Keyframe> keyframes = new List<KeyframeMotion.Keyframe>();
+                while (idx < frames.Data.Length)
+                {
+                    int remain = frames.Data.Length - idx;
+
+                    if (remain < elemLength)
+                        break;
+
+                    KeyframeMotion.Keyframe frame = new KeyframeMotion.Keyframe();
+                    frame.Position = null;
+                    frame.Rotation = null;
+
+                    if ((data & KeyframeMotion.DataFormat.Translation) != 0)
+                    {
+                        LSL_Types.Vector3 tempv = frames.GetVector3Item(idx++);
+                        frame.Position = new Vector3((float)tempv.x, (float)tempv.y, (float)tempv.z);
+                    }
+                    if ((data & KeyframeMotion.DataFormat.Rotation) != 0)
+                    {
+                        LSL_Types.Quaternion tempq = frames.GetQuaternionItem(idx++);
+                        frame.Rotation = new Quaternion((float)tempq.x, (float)tempq.y, (float)tempq.z, (float)tempq.s);
+                    }
+
+                    float tempf = (float)frames.GetLSLFloatItem(idx++);
+                    frame.TimeMS = (int)(tempf * 1000.0f);
+
+                    keyframes.Add(frame);
+                }
+
+                group.RootPart.KeyframeMotion.SetKeyframes(keyframes.ToArray());
+                group.RootPart.KeyframeMotion.Start();
+            }
+            else
+            {
+                if (group.RootPart.KeyframeMotion == null)
+                    return;
+
+                if (options.Data.Length == 0)
+                {
+                    group.RootPart.KeyframeMotion.Stop();
+                    return;
+                }
+                    
+                int code = (int)options.GetLSLIntegerItem(0);
+
+                int idx = 0;
+
+                while (idx < options.Data.Length)
+                {
+                    int option = (int)options.GetLSLIntegerItem(idx++);
+                    int remain = options.Data.Length - idx;
+
+                    switch (option)
+                    {
+                        case ScriptBaseClass.KFM_COMMAND:
+                            int cmd = (int)options.GetLSLIntegerItem(idx++);
+                            switch (cmd)
+                            {
+                                case ScriptBaseClass.KFM_CMD_PLAY:
+                                    group.RootPart.KeyframeMotion.Start();
+                                    break;
+                                case ScriptBaseClass.KFM_CMD_STOP:
+                                    group.RootPart.KeyframeMotion.Stop();
+                                    break;
+                                case ScriptBaseClass.KFM_CMD_PAUSE:
+                                    group.RootPart.KeyframeMotion.Pause();
+                                    break;
+                            }
+                            break;
+                    }
+                }
+            }
+        }
     }
 
     public class NotecardCache
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
index 55444dc..8d97a7c 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
@@ -399,6 +399,7 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api.Interfaces
               void llTargetOmega(LSL_Vector axis, double spinrate, double gain);
               void llTargetRemove(int number);
               void llTeleportAgentHome(string agent);
+              void llTeleportAgent(string agent, string simname, LSL_Vector pos, LSL_Vector lookAt);
               void llTextBox(string avatar, string message, int chat_channel);
         LSL_String llToLower(string source);
         LSL_String llToUpper(string source);
@@ -419,5 +420,6 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api.Interfaces
 
               void SetPrimitiveParamsEx(LSL_Key prim, LSL_List rules);
           LSL_List GetLinkPrimitiveParamsEx(LSL_Key prim, LSL_List rules);
+              void llSetKeyframedMotion(LSL_List frames, LSL_List options);
     }
 }
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
index 3d0e5cb..a5e160d 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Constants.cs
@@ -641,5 +641,18 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
         public static readonly LSLInteger RCERR_UNKNOWN = -1;        
         public static readonly LSLInteger RCERR_SIM_PERF_LOW = -2;
         public static readonly LSLInteger RCERR_CAST_TIME_EXCEEDED = 3;
+
+        public const int KFM_MODE = 1;
+        public const int KFM_LOOP = 1;
+        public const int KFM_REVERSE = 3;
+        public const int KFM_FORWARD = 0;
+        public const int KFM_PING_PONG = 2;
+        public const int KFM_DATA = 2;
+        public const int KFM_TRANSLATION = 2;
+        public const int KFM_ROTATION = 1;
+        public const int KFM_COMMAND = 0;
+        public const int KFM_CMD_PLAY = 0;
+        public const int KFM_CMD_STOP = 1;
+        public const int KFM_CMD_PAUSE = 2;
     }
 }
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
index 5f9f0b9..a8d1ddb 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
@@ -1820,6 +1820,11 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
             m_LSL_Functions.llTargetRemove(number);
         }
 
+        public void llTeleportAgent(string agent, string simname, LSL_Vector pos, LSL_Vector lookAt)
+        {
+            m_LSL_Functions.llTeleportAgent(agent, simname, pos, lookAt);
+        }
+
         public void llTeleportAgentHome(string agent)
         {
             m_LSL_Functions.llTeleportAgentHome(agent);
@@ -1937,7 +1942,12 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
 
         public LSL_Integer llGetLinkNumberOfSides(LSL_Integer link)
         {
-            return  m_LSL_Functions.llGetLinkNumberOfSides(link);
+            return m_LSL_Functions.llGetLinkNumberOfSides(link);
+        }
+
+        public void llSetKeyframedMotion(LSL_List frames, LSL_List options)
+        {
+            m_LSL_Functions.llSetKeyframedMotion(frames, options);
         }
     }
 }