diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs index 75ff24ed0b..bdf4bc0b01 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs @@ -50,7 +50,8 @@ public class BSActorMoveToTarget : BSActor // BSActor.isActive public override bool isActive { - get { return Enabled; } + // MoveToTarget only works on physical prims + get { return Enabled && m_controllingPrim.IsPhysicallyActive; } } // Release any connections and resources used by the actor. @@ -102,16 +103,28 @@ public class BSActorMoveToTarget : BSActor // We're taking over after this. m_controllingPrim.ZeroMotion(true); - m_targetMotor = new BSVMotor("BSActorMoveToTargget.Activate", - m_controllingPrim.MoveToTargetTau, // timeScale - BSMotor.Infinite, // decay time scale - 1f // efficiency + /* Someday use the PID controller + m_targetMotor = new BSPIDVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString()); + m_targetMotor.TimeScale = m_controllingPrim.MoveToTargetTau; + m_targetMotor.Efficiency = 1f; + */ + m_targetMotor = new BSVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString(), + m_controllingPrim.MoveToTargetTau, // timeScale + BSMotor.Infinite, // decay time scale + 1f // efficiency ); m_targetMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages. m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget); m_targetMotor.SetCurrent(m_controllingPrim.RawPosition); - m_physicsScene.BeforeStep += Mover; + // m_physicsScene.BeforeStep += Mover; + m_physicsScene.BeforeStep += Mover2; + } + else + { + // If already allocated, make sure the target and other paramters are current + m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget); + m_targetMotor.SetCurrent(m_controllingPrim.RawPosition); } } @@ -119,12 +132,16 @@ public class BSActorMoveToTarget : BSActor { if (m_targetMotor != null) { - m_physicsScene.BeforeStep -= Mover; + // m_physicsScene.BeforeStep -= Mover; + m_physicsScene.BeforeStep -= Mover2; m_targetMotor = null; } } - // Called just before the simulation step. Update the vertical position for hoverness. + // Origional mover that set the objects position to move to the target. + // The problem was that gravity would keep trying to push the object down so + // the overall downward velocity would increase to infinity. + // Called just before the simulation step. private void Mover(float timeStep) { // Don't do hovering while the object is selected. @@ -142,6 +159,7 @@ public class BSActorMoveToTarget : BSActor m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,zeroMovement,movePos={1},pos={2},mass={3}", m_controllingPrim.LocalID, movePosition, m_controllingPrim.RawPosition, m_controllingPrim.Mass); m_controllingPrim.ForcePosition = m_targetMotor.TargetValue; + m_controllingPrim.ForceVelocity = OMV.Vector3.Zero; // Setting the position does not cause the physics engine to generate a property update. Force it. m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); } @@ -151,7 +169,51 @@ public class BSActorMoveToTarget : BSActor // Setting the position does not cause the physics engine to generate a property update. Force it. m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); } - m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", m_controllingPrim.LocalID, origPosition, movePosition); + m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", + m_controllingPrim.LocalID, origPosition, movePosition); + } + + // Version of mover that applies forces to move the physical object to the target. + // Also overcomes gravity so the object doesn't just drop to the ground. + // Called just before the simulation step. + private void Mover2(float timeStep) + { + // Don't do hovering while the object is selected. + if (!isActive) + return; + + OMV.Vector3 origPosition = m_controllingPrim.RawPosition; // DEBUG DEBUG (for printout below) + OMV.Vector3 addedForce = OMV.Vector3.Zero; + + // CorrectionVector is the movement vector required this step + OMV.Vector3 correctionVector = m_targetMotor.Step(timeStep, m_controllingPrim.RawPosition); + + // If we are very close to our target, turn off the movement motor. + if (m_targetMotor.ErrorIsZero()) + { + m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,zeroMovement,pos={1},mass={2}", + m_controllingPrim.LocalID, m_controllingPrim.RawPosition, m_controllingPrim.Mass); + m_controllingPrim.ForcePosition = m_targetMotor.TargetValue; + m_controllingPrim.ForceVelocity = OMV.Vector3.Zero; + // Setting the position does not cause the physics engine to generate a property update. Force it. + m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); + } + else + { + // First force to move us there -- the motor return a timestep scaled value. + addedForce = correctionVector / timeStep; + // Remove the existing velocity (only the moveToTarget force counts) + addedForce -= m_controllingPrim.RawVelocity; + // Overcome gravity. + addedForce -= m_controllingPrim.Gravity; + + // Add enough force to overcome the mass of the object + addedForce *= m_controllingPrim.Mass; + + m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */); + } + m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}", + m_controllingPrim.LocalID, origPosition, addedForce); } } } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index 48f842e91e..5ef69925f8 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -626,7 +626,7 @@ public sealed class BSCharacter : BSPhysObject OMV.Vector3 addForce = force / PhysScene.LastTimeStep; AddForce(addForce, pushforce, false); } - private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { + public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { if (force.IsFinite()) { OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude); diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs index ef662b5919..121470364b 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs @@ -144,7 +144,6 @@ public class BSVMotor : BSMotor Vector3 correction = Vector3.Zero; Vector3 error = TargetValue - CurrentValue; - LastError = error; if (!ErrorIsZero(error)) { correction = StepError(timeStep, error); @@ -179,6 +178,7 @@ public class BSVMotor : BSMotor MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}", BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue); } + LastError = error; return correction; } @@ -293,7 +293,6 @@ public class BSFMotor : BSMotor float correction = 0f; float error = TargetValue - CurrentValue; - LastError = error; if (!ErrorIsZero(error)) { correction = StepError(timeStep, error); @@ -328,6 +327,7 @@ public class BSFMotor : BSMotor MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); } + LastError = error; return CurrentValue; } @@ -363,7 +363,7 @@ public class BSFMotor : BSMotor // ============================================================================ // ============================================================================ -// Proportional, Integral, Derivitive Motor +// Proportional, Integral, Derivitive ("PID") Motor // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. public class BSPIDVMotor : BSVMotor { @@ -434,15 +434,14 @@ public class BSPIDVMotor : BSVMotor // A simple derivitive is the rate of change from the last error. Vector3 derivitive = (error - LastError) * timeStep; - LastError = error; // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) - Vector3 ret = error * timeStep * proportionFactor * FactorMix.X - + RunningIntegration * integralFactor * FactorMix.Y - + derivitive * derivFactor * FactorMix.Z + Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X + + RunningIntegration / TimeScale * integralFactor * FactorMix.Y + + derivitive / TimeScale * derivFactor * FactorMix.Z ; - MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", + MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); return ret; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs index a4c5e08693..a0d5c42172 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs @@ -210,6 +210,7 @@ public abstract class BSPhysObject : PhysicsActor AddAngularForce(force, pushforce, false); } public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); + public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); public abstract OMV.Vector3 ForceRotationalVelocity { get; set; } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 95bdc7b32f..90f74df450 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -450,6 +450,9 @@ public class BSPrim : BSPhysObject Gravity = ComputeGravity(Buoyancy); PhysScene.PE.SetGravity(PhysBody, Gravity); + OMV.Vector3 currentScale = PhysScene.PE.GetLocalScaling(PhysShape.physShapeInfo); // DEBUG DEBUG + DetailLog("{0},BSPrim.UpdateMassProperties,currentScale{1},shape={2}", LocalID, currentScale, PhysShape.physShapeInfo); // DEBUG DEBUG + Inertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass); PhysScene.PE.SetMassProps(PhysBody, physMass, Inertia); PhysScene.PE.UpdateInertiaTensor(PhysBody); @@ -1040,6 +1043,20 @@ public class BSPrim : BSPhysObject } } + public override OMV.Vector3 PIDTarget + { + set + { + base.PIDTarget = value; + BSActor actor; + if (PhysicalActors.TryGetActor(MoveToTargetActorName, out actor)) + { + // if the actor exists, tell it to refresh its values. + actor.Refresh(); + } + + } + } // Used for llSetHoverHeight and maybe vehicle height // Hover Height will override MoveTo target's Z public override bool PIDHoverActive { @@ -1063,7 +1080,7 @@ public class BSPrim : BSPhysObject // Applying a force just adds this to the total force on the object. // This added force will only last the next simulation tick. - public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { + public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { // for an object, doesn't matter if force is a pushforce or not if (IsPhysicallyActive) {