diff options
author | Robert Adams | 2013-06-30 13:39:58 -0700 |
---|---|---|
committer | Robert Adams | 2013-06-30 17:07:49 -0700 |
commit | 23516717e48095011c1c06d64785ef7d91754ff2 (patch) | |
tree | 706e3cb958e94d0989d2546ef07a463dc7858ff9 /OpenSim/Region | |
parent | BulletSim: add inTaintTime parameter to collision cache clear function. (diff) | |
download | opensim-SC-23516717e48095011c1c06d64785ef7d91754ff2.zip opensim-SC-23516717e48095011c1c06d64785ef7d91754ff2.tar.gz opensim-SC-23516717e48095011c1c06d64785ef7d91754ff2.tar.bz2 opensim-SC-23516717e48095011c1c06d64785ef7d91754ff2.tar.xz |
BulletSim: a better version of llMoveToTarget that doesn't go crazy.
There is still some overshoot but mostly fixes Mantis 6693.
Fix bug where moveToTarget was active for non-physical objects
and while selected.
Fix bug where move target was not getting changed if the script
changed the target during a move.
Diffstat (limited to 'OpenSim/Region')
5 files changed, 98 insertions, 19 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs index 75ff24e..bdf4bc0 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs | |||
@@ -50,7 +50,8 @@ public class BSActorMoveToTarget : BSActor | |||
50 | // BSActor.isActive | 50 | // BSActor.isActive |
51 | public override bool isActive | 51 | public override bool isActive |
52 | { | 52 | { |
53 | get { return Enabled; } | 53 | // MoveToTarget only works on physical prims |
54 | get { return Enabled && m_controllingPrim.IsPhysicallyActive; } | ||
54 | } | 55 | } |
55 | 56 | ||
56 | // Release any connections and resources used by the actor. | 57 | // Release any connections and resources used by the actor. |
@@ -102,16 +103,28 @@ public class BSActorMoveToTarget : BSActor | |||
102 | // We're taking over after this. | 103 | // We're taking over after this. |
103 | m_controllingPrim.ZeroMotion(true); | 104 | m_controllingPrim.ZeroMotion(true); |
104 | 105 | ||
105 | m_targetMotor = new BSVMotor("BSActorMoveToTargget.Activate", | 106 | /* Someday use the PID controller |
106 | m_controllingPrim.MoveToTargetTau, // timeScale | 107 | m_targetMotor = new BSPIDVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString()); |
107 | BSMotor.Infinite, // decay time scale | 108 | m_targetMotor.TimeScale = m_controllingPrim.MoveToTargetTau; |
108 | 1f // efficiency | 109 | m_targetMotor.Efficiency = 1f; |
110 | */ | ||
111 | m_targetMotor = new BSVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString(), | ||
112 | m_controllingPrim.MoveToTargetTau, // timeScale | ||
113 | BSMotor.Infinite, // decay time scale | ||
114 | 1f // efficiency | ||
109 | ); | 115 | ); |
110 | m_targetMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages. | 116 | m_targetMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages. |
111 | m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget); | 117 | m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget); |
112 | m_targetMotor.SetCurrent(m_controllingPrim.RawPosition); | 118 | m_targetMotor.SetCurrent(m_controllingPrim.RawPosition); |
113 | 119 | ||
114 | m_physicsScene.BeforeStep += Mover; | 120 | // m_physicsScene.BeforeStep += Mover; |
121 | m_physicsScene.BeforeStep += Mover2; | ||
122 | } | ||
123 | else | ||
124 | { | ||
125 | // If already allocated, make sure the target and other paramters are current | ||
126 | m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget); | ||
127 | m_targetMotor.SetCurrent(m_controllingPrim.RawPosition); | ||
115 | } | 128 | } |
116 | } | 129 | } |
117 | 130 | ||
@@ -119,12 +132,16 @@ public class BSActorMoveToTarget : BSActor | |||
119 | { | 132 | { |
120 | if (m_targetMotor != null) | 133 | if (m_targetMotor != null) |
121 | { | 134 | { |
122 | m_physicsScene.BeforeStep -= Mover; | 135 | // m_physicsScene.BeforeStep -= Mover; |
136 | m_physicsScene.BeforeStep -= Mover2; | ||
123 | m_targetMotor = null; | 137 | m_targetMotor = null; |
124 | } | 138 | } |
125 | } | 139 | } |
126 | 140 | ||
127 | // Called just before the simulation step. Update the vertical position for hoverness. | 141 | // Origional mover that set the objects position to move to the target. |
142 | // The problem was that gravity would keep trying to push the object down so | ||
143 | // the overall downward velocity would increase to infinity. | ||
144 | // Called just before the simulation step. | ||
128 | private void Mover(float timeStep) | 145 | private void Mover(float timeStep) |
129 | { | 146 | { |
130 | // Don't do hovering while the object is selected. | 147 | // Don't do hovering while the object is selected. |
@@ -142,6 +159,7 @@ public class BSActorMoveToTarget : BSActor | |||
142 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,zeroMovement,movePos={1},pos={2},mass={3}", | 159 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,zeroMovement,movePos={1},pos={2},mass={3}", |
143 | m_controllingPrim.LocalID, movePosition, m_controllingPrim.RawPosition, m_controllingPrim.Mass); | 160 | m_controllingPrim.LocalID, movePosition, m_controllingPrim.RawPosition, m_controllingPrim.Mass); |
144 | m_controllingPrim.ForcePosition = m_targetMotor.TargetValue; | 161 | m_controllingPrim.ForcePosition = m_targetMotor.TargetValue; |
162 | m_controllingPrim.ForceVelocity = OMV.Vector3.Zero; | ||
145 | // Setting the position does not cause the physics engine to generate a property update. Force it. | 163 | // Setting the position does not cause the physics engine to generate a property update. Force it. |
146 | m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); | 164 | m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); |
147 | } | 165 | } |
@@ -151,7 +169,51 @@ public class BSActorMoveToTarget : BSActor | |||
151 | // Setting the position does not cause the physics engine to generate a property update. Force it. | 169 | // Setting the position does not cause the physics engine to generate a property update. Force it. |
152 | m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); | 170 | m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); |
153 | } | 171 | } |
154 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", m_controllingPrim.LocalID, origPosition, movePosition); | 172 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", |
173 | m_controllingPrim.LocalID, origPosition, movePosition); | ||
174 | } | ||
175 | |||
176 | // Version of mover that applies forces to move the physical object to the target. | ||
177 | // Also overcomes gravity so the object doesn't just drop to the ground. | ||
178 | // Called just before the simulation step. | ||
179 | private void Mover2(float timeStep) | ||
180 | { | ||
181 | // Don't do hovering while the object is selected. | ||
182 | if (!isActive) | ||
183 | return; | ||
184 | |||
185 | OMV.Vector3 origPosition = m_controllingPrim.RawPosition; // DEBUG DEBUG (for printout below) | ||
186 | OMV.Vector3 addedForce = OMV.Vector3.Zero; | ||
187 | |||
188 | // CorrectionVector is the movement vector required this step | ||
189 | OMV.Vector3 correctionVector = m_targetMotor.Step(timeStep, m_controllingPrim.RawPosition); | ||
190 | |||
191 | // If we are very close to our target, turn off the movement motor. | ||
192 | if (m_targetMotor.ErrorIsZero()) | ||
193 | { | ||
194 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,zeroMovement,pos={1},mass={2}", | ||
195 | m_controllingPrim.LocalID, m_controllingPrim.RawPosition, m_controllingPrim.Mass); | ||
196 | m_controllingPrim.ForcePosition = m_targetMotor.TargetValue; | ||
197 | m_controllingPrim.ForceVelocity = OMV.Vector3.Zero; | ||
198 | // Setting the position does not cause the physics engine to generate a property update. Force it. | ||
199 | m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody); | ||
200 | } | ||
201 | else | ||
202 | { | ||
203 | // First force to move us there -- the motor return a timestep scaled value. | ||
204 | addedForce = correctionVector / timeStep; | ||
205 | // Remove the existing velocity (only the moveToTarget force counts) | ||
206 | addedForce -= m_controllingPrim.RawVelocity; | ||
207 | // Overcome gravity. | ||
208 | addedForce -= m_controllingPrim.Gravity; | ||
209 | |||
210 | // Add enough force to overcome the mass of the object | ||
211 | addedForce *= m_controllingPrim.Mass; | ||
212 | |||
213 | m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */); | ||
214 | } | ||
215 | m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}", | ||
216 | m_controllingPrim.LocalID, origPosition, addedForce); | ||
155 | } | 217 | } |
156 | } | 218 | } |
157 | } | 219 | } |
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs index 48f842e..5ef6992 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs | |||
@@ -626,7 +626,7 @@ public sealed class BSCharacter : BSPhysObject | |||
626 | OMV.Vector3 addForce = force / PhysScene.LastTimeStep; | 626 | OMV.Vector3 addForce = force / PhysScene.LastTimeStep; |
627 | AddForce(addForce, pushforce, false); | 627 | AddForce(addForce, pushforce, false); |
628 | } | 628 | } |
629 | private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { | 629 | public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { |
630 | if (force.IsFinite()) | 630 | if (force.IsFinite()) |
631 | { | 631 | { |
632 | OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude); | 632 | 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 ef662b5..1214703 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | |||
@@ -144,7 +144,6 @@ public class BSVMotor : BSMotor | |||
144 | 144 | ||
145 | Vector3 correction = Vector3.Zero; | 145 | Vector3 correction = Vector3.Zero; |
146 | Vector3 error = TargetValue - CurrentValue; | 146 | Vector3 error = TargetValue - CurrentValue; |
147 | LastError = error; | ||
148 | if (!ErrorIsZero(error)) | 147 | if (!ErrorIsZero(error)) |
149 | { | 148 | { |
150 | correction = StepError(timeStep, error); | 149 | correction = StepError(timeStep, error); |
@@ -179,6 +178,7 @@ public class BSVMotor : BSMotor | |||
179 | MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}", | 178 | MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}", |
180 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue); | 179 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue); |
181 | } | 180 | } |
181 | LastError = error; | ||
182 | 182 | ||
183 | return correction; | 183 | return correction; |
184 | } | 184 | } |
@@ -293,7 +293,6 @@ public class BSFMotor : BSMotor | |||
293 | 293 | ||
294 | float correction = 0f; | 294 | float correction = 0f; |
295 | float error = TargetValue - CurrentValue; | 295 | float error = TargetValue - CurrentValue; |
296 | LastError = error; | ||
297 | if (!ErrorIsZero(error)) | 296 | if (!ErrorIsZero(error)) |
298 | { | 297 | { |
299 | correction = StepError(timeStep, error); | 298 | correction = StepError(timeStep, error); |
@@ -328,6 +327,7 @@ public class BSFMotor : BSMotor | |||
328 | MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", | 327 | MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", |
329 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); | 328 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); |
330 | } | 329 | } |
330 | LastError = error; | ||
331 | 331 | ||
332 | return CurrentValue; | 332 | return CurrentValue; |
333 | } | 333 | } |
@@ -363,7 +363,7 @@ public class BSFMotor : BSMotor | |||
363 | 363 | ||
364 | // ============================================================================ | 364 | // ============================================================================ |
365 | // ============================================================================ | 365 | // ============================================================================ |
366 | // Proportional, Integral, Derivitive Motor | 366 | // Proportional, Integral, Derivitive ("PID") Motor |
367 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. | 367 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. |
368 | public class BSPIDVMotor : BSVMotor | 368 | public class BSPIDVMotor : BSVMotor |
369 | { | 369 | { |
@@ -434,15 +434,14 @@ public class BSPIDVMotor : BSVMotor | |||
434 | 434 | ||
435 | // A simple derivitive is the rate of change from the last error. | 435 | // A simple derivitive is the rate of change from the last error. |
436 | Vector3 derivitive = (error - LastError) * timeStep; | 436 | Vector3 derivitive = (error - LastError) * timeStep; |
437 | LastError = error; | ||
438 | 437 | ||
439 | // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) | 438 | // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) |
440 | Vector3 ret = error * timeStep * proportionFactor * FactorMix.X | 439 | Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X |
441 | + RunningIntegration * integralFactor * FactorMix.Y | 440 | + RunningIntegration / TimeScale * integralFactor * FactorMix.Y |
442 | + derivitive * derivFactor * FactorMix.Z | 441 | + derivitive / TimeScale * derivFactor * FactorMix.Z |
443 | ; | 442 | ; |
444 | 443 | ||
445 | MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", | 444 | MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", |
446 | BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); | 445 | BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); |
447 | 446 | ||
448 | return ret; | 447 | return ret; |
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs index a4c5e08..a0d5c42 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs | |||
@@ -210,6 +210,7 @@ public abstract class BSPhysObject : PhysicsActor | |||
210 | AddAngularForce(force, pushforce, false); | 210 | AddAngularForce(force, pushforce, false); |
211 | } | 211 | } |
212 | public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); | 212 | public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); |
213 | public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); | ||
213 | 214 | ||
214 | public abstract OMV.Vector3 ForceRotationalVelocity { get; set; } | 215 | public abstract OMV.Vector3 ForceRotationalVelocity { get; set; } |
215 | 216 | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 95bdc7b..90f74df 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | |||
@@ -450,6 +450,9 @@ public class BSPrim : BSPhysObject | |||
450 | Gravity = ComputeGravity(Buoyancy); | 450 | Gravity = ComputeGravity(Buoyancy); |
451 | PhysScene.PE.SetGravity(PhysBody, Gravity); | 451 | PhysScene.PE.SetGravity(PhysBody, Gravity); |
452 | 452 | ||
453 | OMV.Vector3 currentScale = PhysScene.PE.GetLocalScaling(PhysShape.physShapeInfo); // DEBUG DEBUG | ||
454 | DetailLog("{0},BSPrim.UpdateMassProperties,currentScale{1},shape={2}", LocalID, currentScale, PhysShape.physShapeInfo); // DEBUG DEBUG | ||
455 | |||
453 | Inertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass); | 456 | Inertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass); |
454 | PhysScene.PE.SetMassProps(PhysBody, physMass, Inertia); | 457 | PhysScene.PE.SetMassProps(PhysBody, physMass, Inertia); |
455 | PhysScene.PE.UpdateInertiaTensor(PhysBody); | 458 | PhysScene.PE.UpdateInertiaTensor(PhysBody); |
@@ -1040,6 +1043,20 @@ public class BSPrim : BSPhysObject | |||
1040 | } | 1043 | } |
1041 | } | 1044 | } |
1042 | 1045 | ||
1046 | public override OMV.Vector3 PIDTarget | ||
1047 | { | ||
1048 | set | ||
1049 | { | ||
1050 | base.PIDTarget = value; | ||
1051 | BSActor actor; | ||
1052 | if (PhysicalActors.TryGetActor(MoveToTargetActorName, out actor)) | ||
1053 | { | ||
1054 | // if the actor exists, tell it to refresh its values. | ||
1055 | actor.Refresh(); | ||
1056 | } | ||
1057 | |||
1058 | } | ||
1059 | } | ||
1043 | // Used for llSetHoverHeight and maybe vehicle height | 1060 | // Used for llSetHoverHeight and maybe vehicle height |
1044 | // Hover Height will override MoveTo target's Z | 1061 | // Hover Height will override MoveTo target's Z |
1045 | public override bool PIDHoverActive { | 1062 | public override bool PIDHoverActive { |
@@ -1063,7 +1080,7 @@ public class BSPrim : BSPhysObject | |||
1063 | 1080 | ||
1064 | // Applying a force just adds this to the total force on the object. | 1081 | // Applying a force just adds this to the total force on the object. |
1065 | // This added force will only last the next simulation tick. | 1082 | // This added force will only last the next simulation tick. |
1066 | public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { | 1083 | public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { |
1067 | // for an object, doesn't matter if force is a pushforce or not | 1084 | // for an object, doesn't matter if force is a pushforce or not |
1068 | if (IsPhysicallyActive) | 1085 | if (IsPhysicallyActive) |
1069 | { | 1086 | { |