diff --git a/src/world/Avatar.cs b/src/world/Avatar.cs
index f3905cd357..fc1b4a2385 100644
--- a/src/world/Avatar.cs
+++ b/src/world/Avatar.cs
@@ -12,6 +12,8 @@ namespace OpenSim.world
 	public string firstname;
         public string lastname;
 	public OpenSimClient ControllingClient;
+	public uint CurrentKeyMask;
+
 	private libsecondlife.Packets.ObjectUpdatePacket.ObjectDataBlock AvatarTemplate;
 
         public Avatar(OpenSimClient TheClient) {
@@ -21,7 +23,21 @@ namespace OpenSim.world
 	}
 
 	public void update() {
-		base.update();
+		lock(this) {
+			base.update();
+		
+			if((this.CurrentKeyMask & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_AT_POS) != 0) {
+				if((this.velocity.X>230) & (this.velocity.Y>230)) {
+					this.velocity.X=230;
+					this.velocity.Y=230;
+					this.velocity.Z=0;
+				}
+			} else {
+				this.velocity.X=0;
+				this.velocity.Y=0;
+				this.velocity.Z=0;
+			}
+		}
 	}
 
 	private void SetupTemplate(string name)
diff --git a/src/world/Entity.cs b/src/world/Entity.cs
index 92ab2daeb5..0f75c8ae08 100644
--- a/src/world/Entity.cs
+++ b/src/world/Entity.cs
@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 using Axiom.MathLib;
+using libsecondlife;
 using OpenSim.types;
 
 namespace OpenSim.world
@@ -9,8 +10,8 @@ namespace OpenSim.world
     public class Entity
     {
         protected libsecondlife.LLUUID uuid;
-        protected Vector3 position;
-        protected Vector3 velocity;
+	public LLVector3 position;
+	public LLVector3 velocity;
         protected Quaternion rotation;
         protected string name;
         protected List<Entity> children;
@@ -19,8 +20,8 @@ namespace OpenSim.world
         public Entity()
         {
             uuid = new libsecondlife.LLUUID();
-            position = new Vector3();
-            velocity = new Vector3();
+            position = new LLVector3();
+            velocity = new LLVector3();
             rotation = new Quaternion();
             name = "(basic entity)";
             children = new List<Entity>();
@@ -33,6 +34,7 @@ namespace OpenSim.world
             	if(child.needupdate) 
 			child.update();
             }
+	    this.needupdate=false;
         }
 
         public virtual string getName()
diff --git a/src/world/PhysicsEngine.cs b/src/world/PhysicsEngine.cs
index 7585135109..4379aa09ed 100644
--- a/src/world/PhysicsEngine.cs
+++ b/src/world/PhysicsEngine.cs
@@ -18,6 +18,13 @@ namespace OpenSim.world
 	}
  
 	public void DoStuff(World simworld) {
+         foreach (libsecondlife.LLUUID UUID in simworld.Entities.Keys)
+            {
+                if(simworld.Entities[UUID].needupdate) {
+                        simworld.Entities[UUID].position += simworld.Entities[UUID].velocity;
+                }
+
+            }
 	}
     }
 }