From d023c331f813598c3cabab93927a7ab105d6e7f1 Mon Sep 17 00:00:00 2001
From: Teravus Ovares
Date: Thu, 24 Apr 2008 22:26:26 +0000
Subject: * Tuned the llMove2Target PID controller to be more reasonable and
not overshoot the target.
---
OpenSim/Region/Physics/Manager/PhysicsVector.cs | 9 +++++++
OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 32 ++++++++++++++++++-------
2 files changed, 33 insertions(+), 8 deletions(-)
diff --git a/OpenSim/Region/Physics/Manager/PhysicsVector.cs b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
index 4ec943c..adf4715 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsVector.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsVector.cs
@@ -130,6 +130,14 @@ namespace OpenSim.Region.Physics.Manager
return (float) Math.Sqrt(X*X + Y*Y + Z*Z);
}
+ public static float GetDistanceTo(PhysicsVector a, PhysicsVector b)
+ {
+ float dx = a.X - b.X;
+ float dy = a.Y - b.Y;
+ float dz = a.Z - b.Z;
+ return (float) Math.Sqrt(dx * dx + dy * dy + dz * dz);
+ }
+
public static PhysicsVector operator /(PhysicsVector v, float f)
{
return new PhysicsVector(v.X/f, v.Y/f, v.Z/f);
@@ -154,5 +162,6 @@ namespace OpenSim.Region.Physics.Manager
return false;
}
+
}
}
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 947466f..5a5cf59 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1192,12 +1192,27 @@ namespace OpenSim.Region.Physics.OdePlugin
PID_D = 2200.0f;
//PID_P = 900.0f;
}
- PID_D = 1.0f;
- //PID_P = 1.0f;
+ PID_D = 35f;
+ //PID_P = 1.0f;
+ float PID_G = 25;
+
+ if ((m_PIDTau < 1))
+ {
+ PID_G = PID_G / m_PIDTau;
+ }
+
+
+ if ((PID_G - m_PIDTau) <= 0)
+ {
+ PID_G = m_PIDTau + 1;
+ }
//PidStatus = true;
+
+
+
PhysicsVector vec = new PhysicsVector();
d.Vector3 vel = d.BodyGetLinearVel(Body);
@@ -1205,9 +1220,9 @@ namespace OpenSim.Region.Physics.OdePlugin
d.Vector3 pos = d.BodyGetPosition(Body);
_target_velocity =
new PhysicsVector(
- (m_PIDTarget.X - pos.X) / m_PIDTau,
- (m_PIDTarget.Y - pos.Y) / m_PIDTau,
- (m_PIDTarget.Z - pos.Z) / m_PIDTau
+ (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
+ (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
+ (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
);
@@ -1239,8 +1254,8 @@ namespace OpenSim.Region.Physics.OdePlugin
_zeroFlag = false;
// We're flying and colliding with something
- fx = ((_target_velocity.X / m_PIDTau) - vel.X) * (PID_D / 6);
- fy = ((_target_velocity.Y / m_PIDTau) - vel.Y) * (PID_D / 6);
+ fx = ((_target_velocity.X) - vel.X) * (PID_D);
+ fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
@@ -1262,6 +1277,7 @@ namespace OpenSim.Region.Physics.OdePlugin
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
+ enableBodySoft();
d.BodyAddForce(Body, fx, fy, fz);
}
}
@@ -2146,7 +2162,7 @@ namespace OpenSim.Region.Physics.OdePlugin
}
public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } }
public override bool PIDActive { set { m_usePID = value; } }
- public override float PIDTau { set { m_PIDTau = (value * 0.6f); } }
+ public override float PIDTau { set { m_PIDTau = value; } }
private void createAMotor(PhysicsVector axis)
{
--
cgit v1.1