12 files changed, 94 insertions, 54 deletions

diff --git a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
index 85f9d68..5da0ca1 100644
--- a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
@@ -5195,8 +5195,9 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             {
                 ScenePresence presence = (ScenePresence)entity;
 
-//                m_log.DebugFormat(
-//                    "[LLCLIENTVIEW]: Sending terse update to {0} with position {1} in {2}", Name, presence.OffsetPosition, m_scene.Name);
+//                m_log.DebugFormat( 
+//                    "[LLCLIENTVIEW]: Sending terse update to {0} with pos {1}, vel {2} in {3}", 
+//                    Name, presence.OffsetPosition, presence.Velocity, m_scene.Name);
 
                 attachPoint = presence.State;
                 collisionPlane = presence.CollisionPlane;
@@ -5333,13 +5334,13 @@ namespace OpenSim.Region.ClientStack.LindenUDP
         protected ObjectUpdatePacket.ObjectDataBlock CreateAvatarUpdateBlock(ScenePresence data)
         {
 //            m_log.DebugFormat(
-//                "[LLCLIENTVIEW]: Sending full update to {0} with position {1} in {2}", Name, data.OffsetPosition, m_scene.Name);
+//                "[LLCLIENTVIEW]: Sending full update to {0} with pos {1}, vel {2} in {3}", Name, data.OffsetPosition, data.Velocity, m_scene.Name);
 
             byte[] objectData = new byte[76];
 
             data.CollisionPlane.ToBytes(objectData, 0);
             data.OffsetPosition.ToBytes(objectData, 16);
-//            data.Velocity.ToBytes(objectData, 28);
+            data.Velocity.ToBytes(objectData, 28);
 //            data.Acceleration.ToBytes(objectData, 40);
 
             // Whilst not in mouselook, an avatar will transmit only the Z rotation as this is the only axis
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 5a35aff..966152a 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -636,6 +636,11 @@ namespace OpenSim.Region.Framework.Scenes
 
             set
             {
+//                Util.PrintCallStack();
+//                m_log.DebugFormat(
+//                    "[SCENE PRESENCE]: In {0} set velocity of {1} to {2}",
+//                    Scene.RegionInfo.RegionName, Name, value);  
+
                 if (PhysicsActor != null)
                 {
                     try
@@ -648,11 +653,7 @@ namespace OpenSim.Region.Framework.Scenes
                     }
                 }
 
-                m_velocity = value;
-
-//                m_log.DebugFormat(
-//                    "[SCENE PRESENCE]: In {0} set velocity of {1} to {2}",
-//                    Scene.RegionInfo.RegionName, Name, m_velocity);
+                m_velocity = value;                                            
             }
         }
 /*
@@ -1185,15 +1186,23 @@ namespace OpenSim.Region.Framework.Scenes
                 }
                 AbsolutePosition = pos;
 
+//                m_log.DebugFormat(
+//                    "Set pos {0}, vel {1} in {1} to {2} from input position of {3} on MakeRootAgent", 
+//                    Name, Scene.Name, AbsolutePosition, pos);
+//
                 if (m_teleportFlags == TeleportFlags.Default)
                 {
-                    Vector3 vel = Velocity;
                     AddToPhysicalScene(isFlying);
-                    if (PhysicsActor != null)
-                        PhysicsActor.SetMomentum(vel);
+//
+//                        Console.WriteLine(
+//                            "Set velocity of {0} in {1} to {2} from input velocity of {3} on MakeRootAgent", 
+//                            Name, Scene.Name, PhysicsActor.Velocity, vel);
+//                    }
                 }
                 else
+                {
                     AddToPhysicalScene(isFlying);
+                }
 
                 // XXX: This is to trigger any secondary teleport needed for a megaregion when the user has teleported to a 
                 // location outside the 'root region' (the south-west 256x256 corner).  This is the earlist we can do it
@@ -1211,6 +1220,7 @@ namespace OpenSim.Region.Framework.Scenes
                     Flying = false;
                 }
             }
+
             // Don't send an animation pack here, since on a region crossing this will sometimes cause a flying 
             // avatar to return to the standing position in mid-air.  On login it looks like this is being sent
             // elsewhere anyway
@@ -1275,7 +1285,6 @@ namespace OpenSim.Region.Framework.Scenes
             // If we don't reset the movement flag here, an avatar that crosses to a neighbouring sim and returns will
             // stall on the border crossing since the existing child agent will still have the last movement
             // recorded, which stops the input from being processed.
-
             MovementFlag = 0;
 
             m_scene.EventManager.TriggerOnMakeRootAgent(this);
@@ -3668,10 +3677,7 @@ namespace OpenSim.Region.Framework.Scenes
             // Compute the avatar position in the next physics tick.
             // If the avatar will be crossing, we force the crossing to happen now
             //     in the hope that this will make the avatar movement smoother when crossing.
-            float timeStep = 0.1f;
-            pos2.X = pos2.X + (vel.X * timeStep);
-            pos2.Y = pos2.Y + (vel.Y * timeStep);
-            pos2.Z = pos2.Z + (vel.Z * timeStep);
+            pos2 += vel * 0.1f;
 
             if (m_scene.PositionIsInCurrentRegion(pos2))
                 return;
@@ -3682,9 +3688,11 @@ namespace OpenSim.Region.Framework.Scenes
             // Disconnect from the current region
             bool isFlying = Flying;
             RemoveFromPhysicalScene();
+
             // pos2 is the forcasted position so make that the 'current' position so the crossing
             //    code will move us into the newly addressed region.
             m_pos = pos2;
+
             if (CrossToNewRegion())
             {
                 AddToPhysicalScene(isFlying);
@@ -4116,19 +4124,15 @@ namespace OpenSim.Region.Framework.Scenes
             if (Appearance.AvatarHeight == 0)
 //                Appearance.SetHeight();
                 Appearance.SetSize(new Vector3(0.45f,0.6f,1.9f));
-
-            PhysicsScene scene = m_scene.PhysicsScene;
-
-            Vector3 pVec = AbsolutePosition;
-
+                    
 /*
             PhysicsActor = scene.AddAvatar(
                 LocalId, Firstname + "." + Lastname, pVec,
                 new Vector3(0.45f, 0.6f, Appearance.AvatarHeight), isFlying);
 */
 
-            PhysicsActor = scene.AddAvatar(
-                LocalId, Firstname + "." + Lastname, pVec,
+            PhysicsActor = m_scene.PhysicsScene.AddAvatar(
+                LocalId, Firstname + "." + Lastname, AbsolutePosition, Velocity,
                 Appearance.AvatarBoxSize, isFlying);
 
             //PhysicsActor.OnRequestTerseUpdate += SendTerseUpdateToAllClients;
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
index f53adcb..06a205e 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsScene.cs
@@ -80,10 +80,11 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
             return prim;
         }
 
-        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
         {
             BasicActor act = new BasicActor(size);
             act.Position = position;
+            act.Velocity = velocity;
             act.Flying = isFlying;
             _actors.Add(act);
             return act;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
index 14518e9..8e998ba 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
@@ -30,6 +30,7 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 
+using OpenSim.Framework;
 using OpenSim.Region.Physics.Manager;
 
 using OMV = OpenMetaverse;
@@ -109,6 +110,10 @@ public class BSActorAvatarMove : BSActor
         {
             if (m_velocityMotor != null)
             {
+//                    if (targ == OMV.Vector3.Zero)
+//                        Util.PrintCallStack();
+//
+//                    Console.WriteLine("SetVelocityAndTarget, {0} {1}", vel, targ);
                 m_velocityMotor.Reset();
                 m_velocityMotor.SetTarget(targ);
                 m_velocityMotor.SetCurrent(vel);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 4c54f9f..f29784a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -64,15 +64,25 @@ public sealed class BSCharacter : BSPhysObject
     private bool _usePID;
     private float _PIDTau;
 
-    public BSCharacter(uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, bool isFlying)
+//        public override OMV.Vector3 RawVelocity 
+//        { get { return base.RawVelocity; } 
+//            set { 
+//                if (value != base.RawVelocity)
+//                    Util.PrintCallStack();
+//                Console.WriteLine("Set rawvel to {0}", value);
+//                base.RawVelocity = value; }
+//        }
+
+    public BSCharacter(
+            uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 vel, OMV.Vector3 size, bool isFlying)
             : base(parent_scene, localID, avName, "BSCharacter")
     {
         _physicsActorType = (int)ActorTypes.Agent;
-        RawPosition = pos;
+        RawPosition = pos;        
 
         _flying = isFlying;
         RawOrientation = OMV.Quaternion.Identity;
-        RawVelocity = OMV.Vector3.Zero;
+        RawVelocity = vel;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
         Density = BSParam.AvatarDensity;
@@ -89,13 +99,15 @@ public sealed class BSCharacter : BSPhysObject
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
 
-        DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5},pos={6}",
-                            LocalID, _size, Scale, Density, _avatarVolume, RawMass, pos);
+        DetailLog(
+            "{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5},pos={6},vel={7}",
+            LocalID, _size, Scale, Density, _avatarVolume, RawMass, pos, vel);
 
         // do actual creation in taint time
         PhysScene.TaintedObject(LocalID, "BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
+
             // New body and shape into PhysBody and PhysShape
             PhysScene.Shapes.GetBodyAndShape(true, PhysScene.World, this);
 
@@ -142,6 +154,7 @@ public sealed class BSCharacter : BSPhysObject
             m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false);
 
         ForceVelocity = RawVelocity;
+        TargetVelocity = RawVelocity;
 
         // This will enable or disable the flying buoyancy of the avatar.
         // Needs to be reset especially when an avatar is recreated after crossing a region boundry.
@@ -256,7 +269,6 @@ public sealed class BSCharacter : BSPhysObject
     // Called at taint time!
     public override void ZeroMotion(bool inTaintTime)
     {
-        RawVelocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
@@ -267,6 +279,7 @@ public sealed class BSCharacter : BSPhysObject
                 PhysScene.PE.ClearAllForces(PhysBody);
         });
     }
+
     public override void ZeroAngularMotion(bool inTaintTime)
     {
         _rotationalVelocity = OMV.Vector3.Zero;
@@ -441,32 +454,40 @@ public sealed class BSCharacter : BSPhysObject
         get { return RawVelocity; }
         set {
             RawVelocity = value;
-            // m_log.DebugFormat("{0}: set velocity = {1}", LogHeader, RawVelocity);
+                OMV.Vector3 vel = RawVelocity;
+
+            DetailLog("{0}: set Velocity = {1}", LogHeader, value);
+
             PhysScene.TaintedObject(LocalID, "BSCharacter.setVelocity", delegate()
             {
                 if (m_moveActor != null)
-                    m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, true /* inTaintTime */);
+                        m_moveActor.SetVelocityAndTarget(vel, vel, true /* inTaintTime */);
 
-                DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, RawVelocity);
-                ForceVelocity = RawVelocity;
+                    m_log.DebugFormat("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, vel);
+                    ForceVelocity = vel;
             });
         }
     }
+
     public override OMV.Vector3 ForceVelocity {
         get { return RawVelocity; }
         set {
             PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity");
+//                Util.PrintCallStack();
+            DetailLog("{0}: set ForceVelocity = {1}", LogHeader, value);
 
             RawVelocity = value;
             PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity);
             PhysScene.PE.Activate(PhysBody, true);
         }
     }
+
     public override OMV.Vector3 Torque {
         get { return RawTorque; }
         set { RawTorque = value;
         }
     }
+
     public override float CollisionScore {
         get { return _collisionScore; }
         set { _collisionScore = value;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index f059322..e4d8df8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -228,7 +228,7 @@ public abstract class BSPhysObject : PhysicsActor
     public virtual OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
-    public OMV.Vector3 RawVelocity { get; set; }
+    public virtual OMV.Vector3 RawVelocity { get; set; }
     public abstract OMV.Vector3 ForceVelocity { get; set; }
 
     public OMV.Vector3 RawForce { get; set; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 0f79a10..414bc92 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -461,19 +461,19 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     #region Prim and Avatar addition and removal
 
-    public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+    public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
     {
         m_log.ErrorFormat("{0}: CALL TO AddAvatar in BSScene. NOT IMPLEMENTED", LogHeader);
         return null;
     }
 
-    public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, bool isFlying)
+    public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
     {
         // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName);
 
         if (!m_initialized) return null;
 
-        BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying);
+        BSCharacter actor = new BSCharacter(localID, avName, this, position, velocity, size, isFlying);
         lock (PhysObjects)
             PhysObjects.Add(localID, actor);
 
diff --git a/OpenSim/Region/Physics/Manager/NullPhysicsScene.cs b/OpenSim/Region/Physics/Manager/NullPhysicsScene.cs
index 1ccf46d..b52f1f6 100644
--- a/OpenSim/Region/Physics/Manager/NullPhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/NullPhysicsScene.cs
@@ -45,7 +45,8 @@ namespace OpenSim.Region.Physics.Manager
             // Does nothing right now
         }
 
-        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        public override PhysicsActor AddAvatar(
+            string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
         {
             m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddAvatar({0})", position);
             return PhysicsActor.Null;
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 71ad795..9cdedbf 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -142,10 +142,12 @@ namespace OpenSim.Region.Physics.Manager
         /// </summary>
         /// <param name="avName"></param>
         /// <param name="position"></param>
+        /// <param name="velocity"></param>
         /// <param name="size"></param>
         /// <param name="isFlying"></param>
         /// <returns></returns>
-        public abstract PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying);
+        public abstract PhysicsActor AddAvatar(
+            string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying);
 
         /// <summary>
         /// Add an avatar
@@ -153,13 +155,18 @@ namespace OpenSim.Region.Physics.Manager
         /// <param name="localID"></param>
         /// <param name="avName"></param>
         /// <param name="position"></param>
+        /// <param name="velocity"></param>
         /// <param name="size"></param>
         /// <param name="isFlying"></param>
         /// <returns></returns>
-        public virtual PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, bool isFlying)
+        public virtual PhysicsActor AddAvatar(
+            uint localID, string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
         {
-            PhysicsActor ret = AddAvatar(avName, position, size, isFlying);
-            if (ret != null) ret.LocalID = localID;
+            PhysicsActor ret = AddAvatar(avName, position, velocity, size, isFlying);
+
+            if (ret != null) 
+                ret.LocalID = localID;
+
             return ret;
         }
 
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 67503df..05eaf2a 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -167,6 +167,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="avName"></param>
         /// <param name="parent_scene"></param>
         /// <param name="pos"></param>
+        /// <param name="vel"></param>
         /// <param name="size"></param>
         /// <param name="pid_d"></param>
         /// <param name="pid_p"></param>
@@ -178,7 +179,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="walk_divisor"></param>
         /// <param name="rundivisor"></param>
         public OdeCharacter(
-            String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p,
+            String avName, OdeScene parent_scene, Vector3 pos, Vector3 vel, Vector3 size, float pid_d, float pid_p,
             float capsule_radius, float tensor, float density,
             float walk_divisor, float rundivisor)
         {
@@ -210,6 +211,9 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
             }
 
+            _velocity = vel;
+            m_taintTargetVelocity = vel;
+
             _parent_scene = parent_scene;
 
             PID_D = pid_d;
diff --git a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
index 6d7f079..5953557 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdeScene.cs
@@ -1969,16 +1969,11 @@ namespace OpenSim.Region.Physics.OdePlugin
 
         #region Add/Remove Entities
 
-        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
-        {
-            Vector3 pos;
-            pos.X = position.X;
-            pos.Y = position.Y;
-            pos.Z = position.Z;
-            
+        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
+        {                       
             OdeCharacter newAv
                 = new OdeCharacter(
-                    avName, this, pos, size, avPIDD, avPIDP,
+                    avName, this, position, velocity, size, avPIDD, avPIDP,
                     avCapRadius, avStandupTensor, avDensity,
                     avMovementDivisorWalk, avMovementDivisorRun);
 
diff --git a/OpenSim/Region/Physics/POSPlugin/POSScene.cs b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
index d30d482..080c6ab 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSScene.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSScene.cs
@@ -58,7 +58,8 @@ namespace OpenSim.Region.Physics.POSPlugin
         {
         }
 
-        public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
+        public override PhysicsActor AddAvatar(
+            string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
         {
             POSCharacter act = new POSCharacter();
             act.Position = position;