From a21112cceedfc93840b935feae4ad8725c4afb48 Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Tue, 25 Mar 2008 03:36:31 +0000 Subject: [PATCH] * Adds llMoveToTarget and llStopMoveToTarget support to the ODEPlugin. * It doesn't generate at_target events, because they don't exist yet in the script engine. * The Tau is different, however, compatible with scripts I tested. * Not perfect... but pretty good. --- .../Environment/Scenes/SceneObjectGroup.cs | 21 +++ .../Environment/Scenes/SceneObjectPart.cs | 18 +++ .../BasicPhysicsPlugin/BasicPhysicsPlugin.cs | 3 + .../Physics/BulletXPlugin/BulletXPlugin.cs | 3 + .../Region/Physics/Manager/PhysicsActor.cs | 9 ++ .../Region/Physics/OdePlugin/ODECharacter.cs | 3 + OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 124 +++++++++++++++++- OpenSim/Region/Physics/POSPlugin/POSPlugin.cs | 6 + .../Region/Physics/PhysXPlugin/PhysXPlugin.cs | 6 + .../Common/LSL_BuiltIn_Commands.cs | 6 +- 10 files changed, 193 insertions(+), 6 deletions(-) diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs index 28758747d7..2f2a1bec12 100644 --- a/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Environment/Scenes/SceneObjectGroup.cs @@ -694,6 +694,25 @@ namespace OpenSim.Region.Environment.Scenes } } + public void moveToTarget(LLVector3 target, float tau) + { + SceneObjectPart rootpart = m_rootPart; + if (rootpart != null) + { + rootpart.PhysActor.PIDTarget = new PhysicsVector(target.X, target.Y, target.Z); + rootpart.PhysActor.PIDTau = tau; + rootpart.PhysActor.PIDActive = true; + } + } + public void stopMoveToTarget() + { + SceneObjectPart rootpart = m_rootPart; + if (rootpart != null) + { + rootpart.PhysActor.PIDActive = false; + } + } + public void SetRootPartOwner(SceneObjectPart part, LLUUID cAgentID, LLUUID cGroupID) { part.LastOwnerID = part.OwnerID; @@ -2019,5 +2038,7 @@ namespace OpenSim.Region.Environment.Scenes } } } + + } } diff --git a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs index 48177dc5f1..cbefc191f7 100644 --- a/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Environment/Scenes/SceneObjectPart.cs @@ -586,6 +586,24 @@ namespace OpenSim.Region.Environment.Scenes } } + public void MoveToTarget(LLVector3 target, float tau) + { + if (tau > 0) + { + m_parentGroup.moveToTarget(target, tau); + } + else + { + StopMoveToTarget(); + } + + } + + public void StopMoveToTarget() + { + m_parentGroup.stopMoveToTarget(); + } + public void TriggerScriptChangedEvent(Changed val) { if (m_parentGroup != null) diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs index 8459fbcb12..16ec66e4e9 100644 --- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs +++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs @@ -390,5 +390,8 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin public override void CrossingFailure() { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } } diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs index f100ab0fe9..bc777e2f7d 100644 --- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs +++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs @@ -992,6 +992,9 @@ namespace OpenSim.Region.Physics.BulletXPlugin { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } /// diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index c63e1f7d33..b40635caef 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -180,6 +180,10 @@ namespace OpenSim.Region.Physics.Manager public abstract PhysicsVector RotationalVelocity { get; set; } public abstract bool Kinematic { get; set; } public abstract float Buoyancy { get; set; } + public abstract PhysicsVector PIDTarget { set;} + public abstract bool PIDActive { set;} + public abstract float PIDTau { set; } + public abstract void AddForce(PhysicsVector force); public abstract void SetMomentum(PhysicsVector momentum); @@ -343,6 +347,7 @@ namespace OpenSim.Region.Physics.Manager { } + public override void AddForce(PhysicsVector force) { } @@ -353,6 +358,10 @@ namespace OpenSim.Region.Physics.Manager set { return; } } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + public override void SetMomentum(PhysicsVector momentum) { } diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index 30f0b062dc..b870a77a75 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -852,5 +852,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } } diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index ddadc98bf7..43a727274f 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -46,11 +46,17 @@ namespace OpenSim.Region.Physics.OdePlugin private PhysicsVector m_rotationalVelocity; private PhysicsVector _size; private PhysicsVector _acceleration; + private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); private Quaternion _orientation; private PhysicsVector m_taintposition; private PhysicsVector m_taintsize; private PhysicsVector m_taintVelocity = PhysicsVector.Zero; private Quaternion m_taintrot; + + private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0); + private float m_PIDTau = 0f; + private bool m_usePID = false; + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom | CollisionCategories.Space | CollisionCategories.Body @@ -1079,31 +1085,138 @@ namespace OpenSim.Region.Physics.OdePlugin public void Move(float timestep) { + float fx = 0; + float fy = 0; + float fz = 0; if (IsPhysical && Body != (IntPtr)0 && !m_isSelected) { + float PID_D = 2200.0f; + float PID_P = 900.0f; + + float m_mass = CalculateMass(); + + fz = 0f; //m_log.Info(m_collisionFlags.ToString()); + + + + if (m_buoyancy != 0) { - float buoyancy = 0f; + if (m_buoyancy > 0) { - buoyancy = ((9.8f * m_buoyancy) * m_mass); + fz = ((9.8f * m_buoyancy) * m_mass); //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (9.8f * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); } else { - buoyancy = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass)); + fz = (-1 * ((9.8f * (-1 * m_buoyancy)) * m_mass)); } - d.BodyAddForce(Body, 0, 0, buoyancy); + } + + if (m_usePID) + { + + // If we're using the PID controller, then we have no gravity + fz = ((9.8f) * this.Mass ); + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + if (System.Environment.OSVersion.Platform == PlatformID.Unix) + { + PID_D = 3200.0f; + PID_P = 1400.0f; + } + else + { + PID_D = 2200.0f; + PID_P = 900.0f; + } + PID_D = 1.0f; + PID_P = 1.0f; + + + //PidStatus = true; + + PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + + 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 + ); + + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + + } + else + { + + _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); + + + + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + + } + + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + d.BodyAddForce(Body, fx, fy, fz); + } } else { + _zeroPosition = d.BodyGetPosition(Body); return; } } @@ -1952,5 +2065,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override void SetMomentum(PhysicsVector momentum) { } + 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); } } } } diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs index 57fcbe49ab..12b2d8fa01 100644 --- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs +++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs @@ -518,6 +518,9 @@ namespace OpenSim.Region.Physics.POSPlugin { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } public class POSPrim : PhysicsActor @@ -711,5 +714,8 @@ namespace OpenSim.Region.Physics.POSPlugin public override void CrossingFailure() { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } } diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs index 3541b5c2b5..2d00bdbd83 100644 --- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs +++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs @@ -433,6 +433,9 @@ namespace OpenSim.Region.Physics.PhysXPlugin { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } @@ -650,5 +653,8 @@ namespace OpenSim.Region.Physics.PhysXPlugin public override void CrossingFailure() { } + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } } } diff --git a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs index 82d67d4fbd..73d901d6c9 100644 --- a/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs +++ b/OpenSim/Region/ScriptEngine/Common/LSL_BuiltIn_Commands.cs @@ -1097,13 +1097,15 @@ namespace OpenSim.Region.ScriptEngine.Common public void llMoveToTarget(LSL_Types.Vector3 target, double tau) { m_host.AddScriptLPS(1); - NotImplemented("llMoveToTarget"); + m_host.MoveToTarget(new LLVector3((float)target.x, (float)target.y, (float)target.z), (float)tau); + //NotImplemented("llMoveToTarget"); } public void llStopMoveToTarget() { m_host.AddScriptLPS(1); - NotImplemented("llStopMoveToTarget"); + m_host.StopMoveToTarget(); + //NotImplemented("llStopMoveToTarget"); } public void llApplyImpulse(LSL_Types.Vector3 force, int local)