14 files changed, 674 insertions, 35 deletions
diff --git a/bin/PhysX-wrapper.dll b/bin/PhysX-wrapper.dll
new file mode 100644
index 0000000..73d8b91
--- /dev/null
+++ b/bin/PhysX-wrapper.dll
Binary files differ
diff --git a/bin/PhysX_Wrapper_Dotnet.dll b/bin/PhysX_Wrapper_Dotnet.dll
new file mode 100644
index 0000000..1e97cb5
--- /dev/null
+++ b/bin/PhysX_Wrapper_Dotnet.dll
Binary files differ
diff --git a/src/Main.cs b/src/Main.cs
index 600ce38..3103ea3 100644
--- a/src/Main.cs
+++ b/src/Main.cs
@@ -71,6 +71,7 @@ namespace OpenSim
                private PhysicsManager physManager;
                private System.Timers.Timer timer1 = new System.Timers.Timer();
                private string ConfigDll = "SimConfig.dll";
+                private string _physicsEngine = "PhysX";
                public bool sandbox = false;
                public bool loginserver = false;
                
@@ -93,6 +94,11 @@ namespace OpenSim
                                {
                                        sim.loginserver = true;
                                }
+                                if(args[i] == "-realphysx")
+                                {
+                                        sim._physicsEngine = "RealPhysX";
+                                        OpenSim.world.Avatar.PhysicsEngineFlying = true;
+                                }
                        }
                        
                        OpenSim_Main.gridServers = new Grid();
@@ -148,7 +154,7 @@ namespace OpenSim
                        this.physManager = new PhysicsSystem.PhysicsManager();
                        this.physManager.LoadPlugins();
                        ServerConsole.MainConsole.Instance.WriteLine("Main.cs:Startup() - Starting up messaging system");
-                        local_world.PhysScene = this.physManager.GetPhysicsScene("PhysX"); //should be reading from the config file what physics engine to use
+                        local_world.PhysScene = this.physManager.GetPhysicsScene(this._physicsEngine); //should be reading from the config file what physics engine to use
                        local_world.PhysScene.SetTerrain(local_world.LandMap);
        
                        OpenSim_Main.gridServers.AssetServer.SetServerInfo(OpenSim_Main.cfg.AssetURL, OpenSim_Main.cfg.AssetSendKey);
diff --git a/src/OpenSimClient.cs b/src/OpenSimClient.cs
index 50d8c78..dd698e4 100644
--- a/src/OpenSimClient.cs
+++ b/src/OpenSimClient.cs
@@ -135,8 +135,8 @@ namespace OpenSim
                                                {
                                                        if(ent.localid == multipleupdate.ObjectData[ i ].ObjectLocalID)
                                                        {
-                                                                ent.position = pos;
+                                                                ((OpenSim.world.Primitive)ent).UpdatePosition( pos);
-                                                                ((OpenSim.world.Primitive)ent).UpdateFlag = true;
+                                                                
                                                        }
                                                }
                                                
@@ -191,6 +191,7 @@ namespace OpenSim
                                        OpenSim_Main.local_world.Entities.Remove(this.AgentID);
                                }
                                //need to do other cleaning up here too
+                                OpenSim_Main.sim.ClientThreads.Remove(this.userEP);
                                this.ClientThread.Abort();
                                break;
                        case PacketType.ChatFromViewer:
@@ -222,7 +223,7 @@ namespace OpenSim
                                {
                                        foreach (Packet packet in NeedAck.Values)
                                        {
-                                                if (now - packet.TickCount > RESEND_TIMEOUT)
+                                                if ((now - packet.TickCount > RESEND_TIMEOUT) && (!packet.Header.Resent))
                                                {
                                                        ServerConsole.MainConsole.Instance.WriteLine("Resending " + packet.Type.ToString() + " packet, " +
                                                         (now - packet.TickCount) + "ms have passed");
diff --git a/src/Second-server.sln b/src/Second-server.sln
index a460b2a..17c807f 100644
--- a/src/Second-server.sln
+++ b/src/Second-server.sln
@@ -19,6 +19,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "ServerConsole", "ServerCons
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Db4LocalStorage", "LocalStorage\Db4LocalStorage\Db4LocalStorage.csproj", "{74784F23-B0FD-484C-82C1-96C0215733DC}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RealPhysXplugin", "physics\RealPhysX\RealPhysXplugin\RealPhysXplugin.csproj", "{56C1D214-F389-4228-921A-0A3A0712C159}"
+EndProject
 Global
        GlobalSection(SolutionConfigurationPlatforms) = preSolution
                Debug|.NET 1.1 = Debug|.NET 1.1
@@ -99,6 +101,14 @@ Global
                {74784F23-B0FD-484C-82C1-96C0215733DC}.Release|.NET 1.1.ActiveCfg = Release|.NET 1.1
                {74784F23-B0FD-484C-82C1-96C0215733DC}.Release|Any CPU.Build.0 = Release|Any CPU
                {74784F23-B0FD-484C-82C1-96C0215733DC}.Release|Any CPU.ActiveCfg = Release|Any CPU
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Debug|.NET 1.1.Build.0 = Debug|.NET 1.1
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Debug|.NET 1.1.ActiveCfg = Debug|.NET 1.1
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Debug|Any CPU.Build.0 = Debug|Any CPU
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Release|.NET 1.1.Build.0 = Release|.NET 1.1
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Release|.NET 1.1.ActiveCfg = Release|.NET 1.1
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Release|Any CPU.Build.0 = Release|Any CPU
+                {56C1D214-F389-4228-921A-0A3A0712C159}.Release|Any CPU.ActiveCfg = Release|Any CPU
        EndGlobalSection
        GlobalSection(SolutionProperties) = preSolution
                HideSolutionNode = FALSE
diff --git a/src/physics/PhysicsManager.cs b/src/physics/PhysicsManager.cs
index 0986501..e5d5bd6 100644
--- a/src/physics/PhysicsManager.cs
+++ b/src/physics/PhysicsManager.cs
@@ -110,6 +110,8 @@ namespace PhysicsSystem
        {
                public abstract PhysicsActor AddAvatar(PhysicsVector position);
                
+                public abstract PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size);
+                
                public abstract void Simulate(float timeStep);
                
                public abstract void GetResults();
@@ -140,6 +142,11 @@ namespace PhysicsSystem
                {
                        get;
                }
+                public abstract bool Flying
+                {
+                        get;
+                        set;
+                }
                
                public abstract void AddForce(PhysicsVector force);
                
diff --git a/src/physics/RealPhysX/RealPhysXplugin/AssemblyInfo.cs b/src/physics/RealPhysX/RealPhysXplugin/AssemblyInfo.cs
new file mode 100644
index 0000000..913aae7
--- /dev/null
+++ b/src/physics/RealPhysX/RealPhysXplugin/AssemblyInfo.cs
@@ -0,0 +1,31 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+// Information about this assembly is defined by the following
+// attributes.
+//
+// change them to the information which is associated with the assembly
+// you compile.
+[assembly: AssemblyTitle("RealPhysXplugin")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("RealPhysXplugin")]
+[assembly: AssemblyCopyright("")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+// This sets the default COM visibility of types in the assembly to invisible.
+// If you need to expose a type to COM, use [ComVisible(true)] on that type.
+[assembly: ComVisible(false)]
+// The assembly version has following format :
+//
+// Major.Minor.Build.Revision
+//
+// You can specify all values by your own or you can build default build and revision
+// numbers with the '*' character (the default):
+[assembly: AssemblyVersion("1.0.*")]
diff --git a/src/physics/RealPhysX/RealPhysXplugin/RealPhysX.cs b/src/physics/RealPhysX/RealPhysXplugin/RealPhysX.cs
new file mode 100644
index 0000000..9576a40
--- /dev/null
+++ b/src/physics/RealPhysX/RealPhysXplugin/RealPhysX.cs
@@ -0,0 +1,361 @@
+/*
+* Copyright (c) OpenSim project, http://sim.opensecondlife.org/
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*     * Redistributions of source code must retain the above copyright
+*       notice, this list of conditions and the following disclaimer.
+*     * Redistributions in binary form must reproduce the above copyright
+*       notice, this list of conditions and the following disclaimer in the
+*       documentation and/or other materials provided with the distribution.
+*     * Neither the name of the <organization> nor the
+*       names of its contributors may be used to endorse or promote products
+*       derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
+* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+* 
+*/
+/*
+* Copyright (c) OpenSim project, http://sim.opensecondlife.org/
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*     * Redistributions of source code must retain the above copyright
+*       notice, this list of conditions and the following disclaimer.
+*     * Redistributions in binary form must reproduce the above copyright
+*       notice, this list of conditions and the following disclaimer in the
+*       documentation and/or other materials provided with the distribution.
+*     * Neither the name of the <organization> nor the
+*       names of its contributors may be used to endorse or promote products
+*       derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY <copyright holder> ``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
+* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+* 
+*/
+using System;
+using System.Collections.Generic;
+using PhysicsSystem;
+using PhysXWrapper;
+namespace PhysXplugin
+{
+        /// <summary>
+        /// Will be the PhysX plugin but for now will be a very basic physics engine
+        /// </summary>
+        public class PhysXPlugin : IPhysicsPlugin
+        {
+                private PhysXScene _mScene;
+                
+                public PhysXPlugin()
+                {
+                        
+                }
+                
+                public bool Init()
+                {
+                        return true;
+                }
+                
+                public PhysicsScene GetScene()
+                {
+                        if(_mScene == null)
+                        {
+                                _mScene = new PhysXScene();
+                        }
+                        return(_mScene);
+                }
+                
+                public string GetName()
+                {
+                        return("RealPhysX");
+                }
+                
+                public void Dispose()
+                {
+                        
+                }
+        }
+        
+        public class PhysXScene :PhysicsScene
+        {
+                private List<PhysXCharacter> _characters = new List<PhysXCharacter>();
+                private List<PhysXPrim> _prims = new List<PhysXPrim>();
+                private float[] _heightMap;
+                private NxPhysicsSDK mySdk;
+                private NxScene scene;
+                
+                public PhysXScene()
+                {
+                        mySdk = NxPhysicsSDK.CreateSDK();
+                        scene = mySdk.CreateScene();
+                
+                }
+                
+                public override PhysicsActor AddAvatar(PhysicsVector position)
+                {
+                        Vec3 pos = new Vec3();
+                        pos.X = position.X;
+                        pos.Y = position.Y;
+                        pos.Z = position.Z;
+                        PhysXCharacter act = new PhysXCharacter( scene.AddCharacter(pos));
+                        act.Position = position;
+                        _characters.Add(act);
+                        return act;
+                }
+                
+                public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size)
+                {
+                        Vec3 pos = new Vec3();
+                        pos.X = position.X;
+                        pos.Y = position.Y;
+                        pos.Z = position.Z;
+                        Vec3 siz = new Vec3();
+                        siz.X = size.X;
+                        siz.Y = size.Y;
+                        siz.Z = size.Z;
+                        PhysXPrim act = new PhysXPrim( scene.AddNewBox(pos, siz));
+                        _prims.Add(act);
+                        return act;
+                }
+                public override void Simulate(float timeStep)
+                {
+                        foreach (PhysXCharacter actor in _characters)
+                        {
+                                actor.Move(timeStep);
+                        }
+                        scene.Simulate(timeStep);
+                        scene.FetchResults();
+                        scene.UpdateControllers();
+                        
+                        foreach (PhysXCharacter actor in _characters)
+                        {
+                                actor.UpdatePosition();
+                        }
+                                
+                }
+                
+                public override void GetResults()
+                {
+                
+                }
+                
+                public override bool IsThreaded
+                {
+                        get
+                        {
+                                return(false); // for now we won't be multithreaded
+                        }
+                }
+                
+                public override void SetTerrain(float[] heightMap)
+                {
+                        this._heightMap = heightMap;
+                        this.scene.AddTerrain(heightMap);
+                }
+        }
+        
+        public  class PhysXCharacter : PhysicsActor
+        {
+                private PhysicsVector _position;
+                private PhysicsVector _velocity;
+                private PhysicsVector _acceleration;
+                private NxCharacter _character;
+                private bool flying;
+                
+                public PhysXCharacter(NxCharacter character)
+                {
+                        _velocity = new PhysicsVector();
+                        _position = new PhysicsVector();
+                        _acceleration = new PhysicsVector();
+                        _character = character;
+                }
+                
+                public override bool Flying
+                {
+                        get
+                        {
+                                return flying;
+                        }
+                        set
+                        {
+                                flying = value;
+                        }
+                }
+                
+                public override PhysicsVector Position
+                {
+                        get
+                        {
+                                return _position;
+                        }
+                        set
+                        {
+                                _position = value;
+                        }
+                }
+                
+                public override PhysicsVector Velocity
+                {
+                        get
+                        {
+                                return _velocity;
+                        }
+                        set
+                        {
+                                _velocity = value;
+                        }
+                }
+                
+                public override PhysicsVector Acceleration
+                {
+                        get
+                        {
+                                return _acceleration;
+                        }
+                        
+                }
+                public void SetAcceleration (PhysicsVector accel)
+                {
+                        this._acceleration = accel;
+                }
+                
+                public override void AddForce(PhysicsVector force)
+                {
+                        
+                }
+                
+                public override void SetMomentum(PhysicsVector momentum)
+                {
+                        
+                }
+                
+                public void Move(float timeStep)
+                {
+                        Vec3 vec = new Vec3();
+                        vec.X = this._velocity.X * timeStep;
+                        vec.Y = this._velocity.Y * timeStep;
+                        if(flying)
+                        {
+                                vec.Z = ( this._velocity.Z) * timeStep;
+                        }
+                        else
+                        {
+                                vec.Z = (-9.8f + this._velocity.Z) * timeStep;
+                        }
+                        this._character.Move(vec);
+                }
+                
+                public void UpdatePosition()
+                {
+                        Vec3 vec = this._character.Position;
+                        this._position.X = vec.X;
+                        this._position.Y = vec.Y;
+                        this._position.Z = vec.Z;
+                }
+        }
+        
+        public  class PhysXPrim : PhysicsActor
+        {
+                private PhysicsVector _position;
+                private PhysicsVector _velocity;
+                private PhysicsVector _acceleration;
+                private NxActor _prim;
+                
+                public PhysXPrim(NxActor prim)
+                {
+                        _velocity = new PhysicsVector();
+                        _position = new PhysicsVector();
+                        _acceleration = new PhysicsVector();
+                        _prim = prim;
+                }
+                public override bool Flying
+                {
+                        get
+                        {
+                                return false; //no flying prims for you
+                        }
+                        set
+                        {
+                                
+                        }
+                }
+                public override PhysicsVector Position
+                {
+                        get
+                        {
+                                PhysicsVector pos = new PhysicsVector();
+                                Vec3 vec = this._prim.Position;
+                                pos.X = vec.X;
+                                pos.Y = vec.Y;
+                                pos.Z = vec.Z;
+                                return pos;
+                                
+                        }
+                        set
+                        {
+                                PhysicsVector vec = value;
+                                Vec3 pos = new Vec3();
+                                pos.X = vec.X;
+                                pos.Y = vec.Y;
+                                pos.Z = vec.Z;
+                                this._prim.Position = pos;
+                        }
+                }
+                
+                public override PhysicsVector Velocity
+                {
+                        get
+                        {
+                                return _velocity;
+                        }
+                        set
+                        {
+                                _velocity = value;
+                        }
+                }
+                
+                public override PhysicsVector Acceleration
+                {
+                        get
+                        {
+                                return _acceleration;
+                        }
+                        
+                }
+                public void SetAcceleration (PhysicsVector accel)
+                {
+                        this._acceleration = accel;
+                }
+                
+                public override void AddForce(PhysicsVector force)
+                {
+                        
+                }
+                
+                public override void SetMomentum(PhysicsVector momentum)
+                {
+                        
+                }
+                
+                
+        }
+}
diff --git a/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.csproj b/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.csproj
new file mode 100644
index 0000000..ae7bb79
--- /dev/null
+++ b/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.csproj
@@ -0,0 +1,45 @@
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <OutputType>Library</OutputType>
+    <RootNamespace>RealPhysXplugin</RootNamespace>
+    <AssemblyName>RealPhysXplugin</AssemblyName>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProjectGuid>{56C1D214-F389-4228-921A-0A3A0712C159}</ProjectGuid>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
+    <OutputPath>bin\Debug\</OutputPath>
+    <Optimize>False</Optimize>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <DebugSymbols>True</DebugSymbols>
+    <DebugType>Full</DebugType>
+    <CheckForOverflowUnderflow>True</CheckForOverflowUnderflow>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
+    <OutputPath>bin\Release\</OutputPath>
+    <Optimize>True</Optimize>
+    <DefineConstants>TRACE</DefineConstants>
+    <DebugSymbols>False</DebugSymbols>
+    <DebugType>None</DebugType>
+    <CheckForOverflowUnderflow>False</CheckForOverflowUnderflow>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+    <Reference Include="System.Xml" />
+    <Reference Include="PhysX_Wrapper_Dotnet">
+      <HintPath>..\..\..\..\bin\PhysX_Wrapper_Dotnet.dll</HintPath>
+      <SpecificVersion>False</SpecificVersion>
+    </Reference>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="RealPhysX.cs" />
+    <Compile Include="AssemblyInfo.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\PhysicsManager.csproj">
+      <Project>{3C86A846-7977-4EE7-A8DC-DD487FA5DC2B}</Project>
+      <Name>PhysicsManager</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.Targets" />
+</Project>
+\ No newline at end of file
diff --git a/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.sln b/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.sln
new file mode 100644
index 0000000..34f87e6
--- /dev/null
+++ b/src/physics/RealPhysX/RealPhysXplugin/RealPhysXplugin.sln
@@ -0,0 +1,7 @@
+Microsoft Visual Studio Solution File, Format Version 9.00
+# SharpDevelop 2.1.0.2017
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RealPhysXplugin", "RealPhysXplugin.csproj", "{56C1D214-F389-4228-921A-0A3A0712C159}"
+EndProject
+Global
+EndGlobal
diff --git a/src/physics/plugins/PhysXplugin.cs b/src/physics/plugins/PhysXplugin.cs
index 58a82fb..8c09dc8 100644
--- a/src/physics/plugins/PhysXplugin.cs
+++ b/src/physics/plugins/PhysXplugin.cs
@@ -85,14 +85,30 @@ namespace PhysXplugin
                        return act;
                }
                
+                public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size)
+                {
+                        return null;
+                }
+                
                public override void Simulate(float timeStep)
                {
                        foreach (PhysXActor actor in _actors)
                        {
-                                actor.Position.X = actor.Position.X + actor.Velocity.X * timeStep;
+                                actor.Position.X = actor.Position.X + (actor.Velocity.X * timeStep);
-                                actor.Position.Y = actor.Position.Y + actor.Velocity.Y * timeStep;
+                                actor.Position.Y = actor.Position.Y + (actor.Velocity.Y * timeStep);
-                                actor.Position.Z = actor.Position.Z + actor.Velocity.Z * timeStep;
+                                actor.Position.Z = actor.Position.Z + (actor.Velocity.Z * timeStep);
-                                actor.Position.Z = _heightMap[(int)actor.Position.Y * 256 + (int)actor.Position.X]+1;
+                                /*if(actor.Flying)
+                                {
+                                        actor.Position.Z = actor.Position.Z + (actor.Velocity.Z * timeStep);
+                                }
+                                else
+                                {
+                                        actor.Position.Z = actor.Position.Z + ((-9.8f + actor.Velocity.Z) * timeStep);
+                                }
+                                if(actor.Position.Z < (_heightMap[(int)actor.Position.Y * 256 + (int)actor.Position.X]+1))
+                                {*/
+                                        actor.Position.Z = _heightMap[(int)actor.Position.Y * 256 + (int)actor.Position.X]+1;
+                                //}
                                if(actor.Position.X<0)
                                {
                                        actor.Position.X = 0;
@@ -140,7 +156,7 @@ namespace PhysXplugin
                private PhysicsVector _position;
                private PhysicsVector _velocity;
                private PhysicsVector _acceleration;
-                
+                private bool flying;
                public PhysXActor()
                {
                        _velocity = new PhysicsVector();
@@ -148,6 +164,18 @@ namespace PhysXplugin
                        _acceleration = new PhysicsVector();
                }
                
+                public override bool Flying
+                {
+                        get
+                        {
+                                return false;
+                        }
+                        set
+                        {
+                                flying= value;
+                        }
+                }
+                
                public override PhysicsVector Position
                {
                        get
diff --git a/src/world/Avatar.cs b/src/world/Avatar.cs
index c09c008..facfeee 100644
--- a/src/world/Avatar.cs
+++ b/src/world/Avatar.cs
@@ -11,15 +11,17 @@ namespace OpenSim.world
 {
    public class Avatar : Entity
    {
+        public static bool PhysicsEngineFlying;
        public string firstname;
        public string lastname;
        public OpenSimClient ControllingClient;
        private PhysicsActor _physActor;
        private static libsecondlife.Packets.ObjectUpdatePacket.ObjectDataBlock AvatarTemplate;
                private bool updateflag;
-                private bool walking;
+                private byte movementflag;
                private List<NewForce> forcesList = new List<NewForce>();
                private short _updateCount;
+                private Axiom.MathLib.Quaternion bodyRot;
                
        public Avatar(OpenSimClient TheClient) {
                ServerConsole.MainConsole.Instance.WriteLine("Avatar.cs - Loading details from grid (DUMMY)");
@@ -77,14 +79,14 @@ namespace OpenSim.world
                        }
                        
                        updateflag =false;
-                        this._updateCount = 0;
+                        //this._updateCount = 0;
                }
                else
                {
-                        if(walking)
+                        //if((movementflag & 1) !=0)
-                        {
+                        //{
                                _updateCount++;
-                                if(_updateCount>3)
+                                if(( (!PhysicsEngineFlying) && (_updateCount>3)) || (_updateCount>0))
                                {
                                        //It has been a while since last update was sent so lets send one.
                                        ImprovedTerseObjectUpdatePacket.ObjectDataBlock terseBlock = CreateTerseBlock();
@@ -98,7 +100,7 @@ namespace OpenSim.world
                                        }
                                        _updateCount = 0;
                                }
-                        }
+                        //}
                }
        }
@@ -249,36 +251,111 @@ namespace OpenSim.world
                }
        
        public void HandleUpdate(AgentUpdatePacket pack) {
-           if(((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_AT_POS) !=0) {
+                if(((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_FLY) !=0)
-                        if(!walking)
+                {
+                        this._physActor.Flying = true;
+                }
+                else
+                {
+                        this._physActor.Flying = false;
+                }
+                if(((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_AT_POS) !=0) {
+                        Axiom.MathLib.Quaternion q = new Axiom.MathLib.Quaternion(pack.AgentData.BodyRotation.W, pack.AgentData.BodyRotation.X, pack.AgentData.BodyRotation.Y, pack.AgentData.BodyRotation.Z);
+                        if(((movementflag & 1) ==0) || (q!= this.bodyRot))
                        {
-                                //we should add a new force to the list 
+                                //we should add a new force to the list
                                // but for now we will deal with velocities
                                NewForce newVelocity = new NewForce();
                                Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(1, 0, 0);
-                                Axiom.MathLib.Quaternion q = new Axiom.MathLib.Quaternion(pack.AgentData.BodyRotation.W, pack.AgentData.BodyRotation.X, pack.AgentData.BodyRotation.Y, pack.AgentData.BodyRotation.Z);
                                Axiom.MathLib.Vector3 direc = q * v3;
                                direc.Normalize();
                                
                                //work out velocity for sim physics system
                                direc = direc * ((0.03f) * 128f);
+                                if(this._physActor.Flying)
+                                        direc *=2;
+                                
+                                newVelocity.X = direc.x;
+                                newVelocity.Y = direc.y;
+                                newVelocity.Z = direc.z;
+                                this.forcesList.Add(newVelocity);
+                                movementflag = 1;
+                                this.bodyRot = q;
+                        }
+                }
+                else if((((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_UP_POS) !=0) &&(PhysicsEngineFlying)) {
+                        if(((movementflag & 2) ==0) && this._physActor.Flying)
+                        {
+                                //we should add a new force to the list
+                                // but for now we will deal with velocities
+                                NewForce newVelocity = new NewForce();
+                                Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(0, 0, 1);
+                                Axiom.MathLib.Vector3 direc = v3;
+                                direc.Normalize();
+                                
+                                //work out velocity for sim physics system
+                                direc = direc * ((0.03f) * 128f *2);
+                                newVelocity.X = direc.x;
+                                newVelocity.Y = direc.y;
+                                newVelocity.Z = direc.z;
+                                this.forcesList.Add(newVelocity);
+                                movementflag = 2;
+                        }
+                }
+                else if((((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_UP_NEG) !=0) && (PhysicsEngineFlying)) {
+                        if(((movementflag & 4) ==0) && this._physActor.Flying)
+                        {
+                                //we should add a new force to the list
+                                // but for now we will deal with velocities
+                                NewForce newVelocity = new NewForce();
+                                Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(0, 0, -1);
+                                //Axiom.MathLib.Quaternion q = new Axiom.MathLib.Quaternion(pack.AgentData.BodyRotation.W, pack.AgentData.BodyRotation.X, pack.AgentData.BodyRotation.Y, pack.AgentData.BodyRotation.Z);
+                                Axiom.MathLib.Vector3 direc = v3;
+                                direc.Normalize();
+                                
+                                //work out velocity for sim physics system
+                                direc = direc * ((0.03f) * 128f *2);
+                                newVelocity.X = direc.x;
+                                newVelocity.Y = direc.y;
+                                newVelocity.Z = direc.z;
+                                this.forcesList.Add(newVelocity);
+                                movementflag = 4;
+                        }
+                }
+                else if(((uint)pack.AgentData.ControlFlags & (uint)MainAvatar.AgentUpdateFlags.AGENT_CONTROL_AT_NEG) !=0) {
+                        Axiom.MathLib.Quaternion q = new Axiom.MathLib.Quaternion(pack.AgentData.BodyRotation.W, pack.AgentData.BodyRotation.X, pack.AgentData.BodyRotation.Y, pack.AgentData.BodyRotation.Z);
+                        if(((movementflag & 8) ==0) || (q!= this.bodyRot))
+                        {
+                                //we should add a new force to the list
+                                // but for now we will deal with velocities
+                                NewForce newVelocity = new NewForce();
+                                Axiom.MathLib.Vector3 v3 = new Axiom.MathLib.Vector3(-1, 0, 0);
+                                Axiom.MathLib.Vector3 direc = q * v3;
+                                direc.Normalize();
+                                
+                                //work out velocity for sim physics system
+                                direc = direc * ((0.03f) * 128f);
+                                if(this._physActor.Flying)
+                                        direc *=2;
+                                
                                newVelocity.X = direc.x;
                                newVelocity.Y = direc.y;
                                newVelocity.Z = direc.z;
                                this.forcesList.Add(newVelocity);
-                                walking=true;
+                                movementflag = 8;
+                                this.bodyRot = q;
                        }
                }
                else
                {
-                        if(walking)
+                        if((movementflag) !=0)
                        {
                                NewForce newVelocity = new NewForce();
                                newVelocity.X = 0;
                                newVelocity.Y = 0;
                                newVelocity.Z = 0;
                                this.forcesList.Add(newVelocity);
-                                walking = false;
+                                movementflag = 0;
                        }
                }
        }
diff --git a/src/world/Primitive.cs b/src/world/Primitive.cs
index 0d7d951..6be33ef 100644
--- a/src/world/Primitive.cs
+++ b/src/world/Primitive.cs
@@ -5,6 +5,7 @@ using OpenSim.types;
 using libsecondlife;
 using libsecondlife.Packets;
 using GridInterfaces;
+using PhysicsSystem;
 namespace OpenSim.world
 {
@@ -16,8 +17,21 @@ namespace OpenSim.world
        protected bool newPrimFlag;
        protected bool updateFlag;
        protected bool dirtyFlag;
-        protected ObjectUpdatePacket OurPacket;
+        private ObjectUpdatePacket OurPacket;
+        private PhysicsActor _physActor;
+        private bool physicsEnabled;
       
+        public bool PhysicsEnabled
+        {
+                get
+                {
+                        return physicsEnabled;
+                }
+                set
+                {
+                        physicsEnabled = value;
+                }
+        }
        public bool UpdateFlag
        {
                get
@@ -41,6 +55,14 @@ namespace OpenSim.world
                        return this.primData.Scale;
                }
        }
+        public PhysicsActor PhysActor
+        {
+                set
+                {
+                        this._physActor = value;
+                }
+        }
+        
        public Primitive()
        {
            mesh_cutbegin = 0.0f;
@@ -61,6 +83,16 @@ namespace OpenSim.world
            return mesh;
        }
        
+        public void UpdatePosition( LLVector3 pos)
+        {
+                this.position = pos;
+                if(this._physActor != null && this.physicsEnabled)
+                {
+                        this._physActor.Position = new PhysicsVector(pos.X, pos.Y, pos.Z);
+                }
+                this.updateFlag = true;
+        }
+        
        public override void update()
        {
                if(this.newPrimFlag)
@@ -89,13 +121,39 @@ namespace OpenSim.world
                        }
                        this.dirtyFlag = false;
                }
-                
+                else
+                {
+                        if(this._physActor != null && this.physicsEnabled) 
+                        {
+                                ImprovedTerseObjectUpdatePacket terse = new ImprovedTerseObjectUpdatePacket();
+                                terse.RegionData.RegionHandle = OpenSim_Main.cfg.RegionHandle; // FIXME
+                                terse.RegionData.TimeDilation = 64096;
+                                terse.ObjectData = new ImprovedTerseObjectUpdatePacket.ObjectDataBlock[1];
+                                terse.ObjectData[0] = this.CreateImprovedBlock();
+                                foreach(OpenSimClient client in OpenSim_Main.sim.ClientThreads.Values) {
+                                        client.OutPacket(terse);
+                                }
+                        }
+                }
        }
        
        public void UpdateClient(OpenSimClient RemoteClient)
        {
-                byte[] pb = this.position.GetBytes();
+                
+                LLVector3 lPos; 
+                if( this._physActor != null && this.physicsEnabled)
+                {
+                        PhysicsVector pPos = this._physActor.Position; 
+                                lPos = new LLVector3( pPos.X, pPos.Y, pPos.Z);
+                }
+                else
+                {
+                        lPos = this.position;
+                }
+                        byte[] pb = lPos.GetBytes();
                Array.Copy(pb, 0, OurPacket.ObjectData[0].ObjectData, 0, pb.Length);
+                
+                // OurPacket should be update with the follwing in updateShape() rather than having to do it here
                OurPacket.ObjectData[0].OwnerID = this.primData.OwnerID;
                OurPacket.ObjectData[0].PCode = this.primData.PCode;
                OurPacket.ObjectData[0].PathBegin = this.primData.PathBegin;
@@ -137,7 +195,6 @@ namespace OpenSim.world
                this.primData.PathCurve = addPacket.PathCurve;
                this.primData.ProfileCurve = addPacket.ProfileCurve;
                this.primData.ProfileHollow = addPacket.ProfileHollow;
-                
                this.primData.PathRadiusOffset = addPacket.PathRadiusOffset;
                this.primData.PathRevolutions = addPacket.PathRevolutions;
                this.primData.PathTaperX = addPacket.PathTaperX;
@@ -145,7 +202,6 @@ namespace OpenSim.world
                this.primData.PathTwist = addPacket.PathTwist;
                this.primData.PathTwistBegin =addPacket.PathTwistBegin;
                this.dirtyFlag = true;
-                
        }
        
        public void CreateFromPacket( ObjectAddPacket addPacket, LLUUID agentID, uint localID)
@@ -162,7 +218,6 @@ namespace OpenSim.world
                objupdate.ObjectData[0].ExtraParams = new byte[1];
                objupdate.ObjectData[0].MediaURL = new byte[0];
                objupdate.ObjectData[0].NameValue = new byte[0];
-                objupdate.ObjectData[0].PSBlock = new byte[0];
                objupdate.ObjectData[0].Text = new byte[0];
                objupdate.ObjectData[0].TextColor = new byte[4];
                objupdate.ObjectData[0].JointAxisOrAnchor = new LLVector3(0,0,0);
@@ -232,7 +287,6 @@ namespace OpenSim.world
                objupdate.ObjectData[0].ExtraParams = new byte[1];
                objupdate.ObjectData[0].MediaURL = new byte[0];
                objupdate.ObjectData[0].NameValue = new byte[0];
-                objupdate.ObjectData[0].PSBlock = new byte[0];
                objupdate.ObjectData[0].Text = new byte[0];
                objupdate.ObjectData[0].TextColor = new byte[4];
                objupdate.ObjectData[0].JointAxisOrAnchor = new LLVector3(0,0,0);
@@ -291,7 +345,6 @@ namespace OpenSim.world
                uint ID = this.localid;
                byte[] bytes = new byte[60];
                        
-                        
                        int i = 0;
                        ImprovedTerseObjectUpdatePacket.ObjectDataBlock dat = new ImprovedTerseObjectUpdatePacket.ObjectDataBlock();
                        dat.TextureEntry = this.OurPacket.ObjectData[0].TextureEntry;
@@ -302,8 +355,18 @@ namespace OpenSim.world
                        bytes[i++] = (byte)((ID >> 24) % 256);
                        bytes[i++]= 0;
                        bytes[i++]= 0;
+                        
-                        byte[] pb = this.position.GetBytes();
+                        LLVector3 lPos; 
+                if( this._physActor != null && this.physicsEnabled)
+                {
+                        PhysicsVector pPos = this._physActor.Position; 
+                                lPos = new LLVector3( pPos.X, pPos.Y, pPos.Z);
+                }
+                else
+                {
+                        lPos = this.position;
+                }
+                        byte[] pb = lPos.GetBytes();
                        Array.Copy(pb, 0, bytes, i, pb.Length);
                        i += 12;
                        ushort ac = 32767;
diff --git a/src/world/World.cs b/src/world/World.cs
index fb78819..e1c84bc 100644
--- a/src/world/World.cs
+++ b/src/world/World.cs
@@ -176,9 +176,8 @@ namespace OpenSim.world
                ServerConsole.MainConsole.Instance.WriteLine("World.cs:AddViewerAgent() - Adding new avatar to world");
                ServerConsole.MainConsole.Instance.WriteLine("World.cs:AddViewerAgent() - Starting RegionHandshake ");
                NewAvatar.SendRegionHandshake(this);
-                
+                PhysicsVector pVec = new PhysicsVector(NewAvatar.position.X, NewAvatar.position.Y, NewAvatar.position.Z);
-                NewAvatar.PhysActor = this.phyScene.AddAvatar(new PhysicsVector(NewAvatar.position.X, NewAvatar.position.Y, NewAvatar.position.Z));
+                NewAvatar.PhysActor = this.phyScene.AddAvatar(pVec);
-                //this.Update();                // will work for now, but needs to be optimised so we don't update everything in the sim for each new user
                this.Entities.Add(AgentClient.AgentID, NewAvatar);
        }
        
@@ -187,6 +186,10 @@ namespace OpenSim.world
                ServerConsole.MainConsole.Instance.WriteLine("World.cs: AddNewPrim() - Creating new prim");
                Primitive prim = new Primitive();
                prim.CreateFromPacket(addPacket, AgentClient.AgentID, this._primCount);
+                PhysicsVector pVec = new PhysicsVector(prim.position.X, prim.position.Y, prim.position.Z);
+                PhysicsVector pSize = new PhysicsVector( 0.25f, 0.25f, 0.25f);
+                //prim.PhysActor = this.phyScene.AddPrim(pVec, pSize );
+                //prim.PhysicsEnabled = true;
                this.Entities.Add(prim.uuid, prim);
                this._primCount++;
        }