From add7abc1de2fce8db4c6d01cc4b5305bafa4bd87 Mon Sep 17 00:00:00 2001
From: Kitto Flora
Date: Fri, 7 May 2010 14:12:07 -0400
Subject: Fix Mouse+WASD makes Av rise; Fix PREJUMP.

---
 OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs | 30 +++++++++++++++-------
 1 file changed, 21 insertions(+), 9 deletions(-)

(limited to 'OpenSim/Region/Physics/ChOdePlugin')

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
index 7a5093b..38c38b6 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -142,7 +142,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         // unique UUID of this character object
         public UUID m_uuid;
         public bool bad = false;
-
+        
         public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
         {
             m_uuid = UUID.Random();
@@ -892,7 +892,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             // If the PID Controller isn't active then we set our force
             // calculating base velocity to the current position
-
+                
             if (Body == IntPtr.Zero)
                 return;
 
@@ -972,8 +972,17 @@ namespace OpenSim.Region.Physics.OdePlugin
                     // Prim to avatar collisions
 
                     d.Vector3 pos = d.BodyGetPosition(Body);
-                    vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
-                    vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
+                    float errX = _zeroPosition.X - pos.X;
+                    float errY = _zeroPosition.Y - pos.Y;
+                    if( (Math.Abs(errX) > 0.1f) || (Math.Abs(errY) > 0.1f) )
+                    {
+                    	vec.X = (_target_velocity.X - vel.X) * (PID_D) + (errX) * (PID_P * 2);
+                    	vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (errY) * (PID_P * 2);
+                    }
+                    else
+                    {	// close, jump to lateral destination
+                    	d.BodySetPosition(Body, _zeroPosition.X, _zeroPosition.Y, pos.Z);
+                    }
                     if (flying)
                     {
                         vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
@@ -1056,11 +1065,14 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             if (vec.IsFinite())
             {
-                doForce(vec);
-                if (!_zeroFlag)
-                {
-                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
-                }
+            	if (!vec.ApproxEquals(Vector3.Zero, 0.01f))
+            	{
+	                doForce(vec);
+	                if (!_zeroFlag)
+	                {
+	                  AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
+	                }
+	            }
             }
             else
             {
-- 
cgit v1.1