From 2c6b269b6e53cae70e6de0c4380a9a4183c4f32d Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Thu, 3 Jan 2013 19:38:30 -0800 Subject: [PATCH] BulletSim: add initial implementation of llMoveToTarget and hover height. Not all there yet. --- .../Region/Physics/BulletSPlugin/BSPrim.cs | 157 ++++++++++++++++-- 1 file changed, 147 insertions(+), 10 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 826261c223..1904ddd132 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -66,9 +66,6 @@ public sealed class BSPrim : BSPhysObject private float _restitution; private bool _setAlwaysRun; private bool _throttleUpdates; - private bool _isColliding; - private bool _collidingGround; - private bool _collidingObj; private bool _floatOnWater; private OMV.Vector3 _rotationalVelocity; private bool _kinematic; @@ -76,13 +73,14 @@ public sealed class BSPrim : BSPhysObject private BSDynamics _vehicle; + private BSVMotor _targetMotor; private OMV.Vector3 _PIDTarget; - private bool _usePID; private float _PIDTau; - private bool _useHoverPID; + + private BSFMotor _hoverMotor; private float _PIDHoverHeight; private PIDHoverType _PIDHoverType; - private float _PIDHoverTao; + private float _PIDHoverTau; public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) @@ -564,6 +562,11 @@ public sealed class BSPrim : BSPhysObject } return; } + public OMV.Vector3 RawVelocity + { + get { return _velocity; } + set { _velocity = value; } + } public override OMV.Vector3 Velocity { get { return _velocity; } set { @@ -1004,13 +1007,120 @@ public sealed class BSPrim : BSPhysObject set { _PIDTau = value; } } public override bool PIDActive { - set { _usePID = value; } + set { + if (value) + { + // Turning the target on + /* + _targetMotor = new BSVMotor("BSPrim.PIDTarget", + _PIDTau, // timeScale + BSMotor.Infinite, // decay time scale + BSMotor.InfiniteVector, // friction timescale + 1f // efficiency + ); + _targetMotor.SetTarget(_PIDTarget); + _targetMotor.SetCurrent(RawPosition); + */ + _targetMotor = new BSPIDVMotor("BSPrim.PIDTarget"); + _targetMotor.SetTarget(_PIDTarget); + _targetMotor.SetCurrent(RawPosition); + _targetMotor.Efficiency = 1f; + + _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages. + + RegisterPreStepAction("BSPrim.PIDTarget", LocalID, delegate(float timeStep) + { + // How far are we away from the target + OMV.Vector3 distance = _PIDTarget - RawPosition; + + // The amount of that distance we should cover per second + OMV.Vector3 movementPerSecond = distance / _PIDTau; + + OMV.Vector3 adjustedVelocity = movementPerSecond - RawVelocity; + + // Apply force to overcome current velocity + AddForce(-RawVelocity * Mass, false, true); + // Get it moving to the point + AddForce(movementPerSecond * Mass, false, true); + + // Apply enough force to get to the speed needed to get to the point + // The physics engine will do only a timestep's worth. + // AddForce(adjustedVelocity * Mass, false, true); + // PhysicsScene.PE.ApplyCentralImpulse(PhysBody, adjustedVelocity); + + DetailLog("{0},BSPrim.PIDTarget,move,tgt={1},pos={2},vel={3},dist={4},tau={5},mvmt={6},newVel={7}", + LocalID, _PIDTarget, RawPosition, RawVelocity, distance, _PIDTau, movementPerSecond, adjustedVelocity); + + /* + OMV.Vector3 movePosition = _targetMotor.Step(timeStep); + + // 'movePosition' is where we'd like the prim to be at this moment. + // Compute the amount of force to push us there. + OMV.Vector3 moveForce = (movePosition - RawPosition) * Mass; + + // If we are very close to our target, turn off the movement motor. + if (_targetMotor.ErrorIsZero()) + { + DetailLog("{0},BSPrim.PIDTarget,zeroMovement,movePos={1},pos={2},mass={3},moveForce={4}", + LocalID, movePosition, RawPosition, Mass, moveForce); + moveForce = OMV.Vector3.Zero; + ForcePosition = _targetMotor.TargetValue; + _targetMotor.Enabled = false; + } + else + { + AddForce(moveForce, false, true); + } + DetailLog("{0},BSPrim.PIDTarget,move,movePos={1},moveForce={2},mass={3}", LocalID, movePosition, moveForce, Mass); + */ + }); + } + else + { + // Stop any targetting + UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID); + } + } } // Used for llSetHoverHeight and maybe vehicle height // Hover Height will override MoveTo target's Z public override bool PIDHoverActive { - set { _useHoverPID = value; } + set { + if (value) + { + // Turning the target on + _hoverMotor = new BSFMotor("BSPrim.Hover", + _PIDHoverTau, // timeScale + BSMotor.Infinite, // decay time scale + BSMotor.Infinite, // friction timescale + 1f // efficiency + ); + _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight()); + _hoverMotor.SetCurrent(RawPosition.Z); + _hoverMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages. + + RegisterPreStepAction("BSPrim.Hover", LocalID, delegate(float timeStep) + { + // TODO: Decide if the step parameters should be changed depending on the avatar's + // state (flying, colliding, ...). There is code in ODE to do this. + + _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight()); + float targetHeight = _hoverMotor.Step(timeStep); + + // 'targetHeight' is where we'd like the Z of the prim to be at this moment. + // Compute the amount of force to push us there. + float moveForce = (targetHeight - RawPosition.Z) * Mass / PhysicsScene.LastTimeStep; + + AddForce(new OMV.Vector3(0f, 0f, moveForce), false, true); + DetailLog("{0},BSPrim.Hover,move,targHt={1},moveForce={2},mass={3}", LocalID, targetHeight, moveForce, Mass); + }); + } + else + { + UnRegisterPreStepAction("BSPrim.Hover", LocalID); + } + } } public override float PIDHoverHeight { set { _PIDHoverHeight = value; } @@ -1019,8 +1129,35 @@ public sealed class BSPrim : BSPhysObject set { _PIDHoverType = value; } } public override float PIDHoverTau { - set { _PIDHoverTao = value; } + set { _PIDHoverTau = value; } } + // Based on current position, determine what we should be hovering at now. + // Must recompute often. What if we walked offa cliff> + private float ComputeCurrentPIDHoverHeight() + { + float ret = _PIDHoverHeight; + float groundHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition); + + switch (_PIDHoverType) + { + case PIDHoverType.Ground: + ret = groundHeight + _PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition); + if (groundHeight > waterHeight) + { + ret = groundHeight + _PIDHoverHeight; + } + else + { + ret = waterHeight + _PIDHoverHeight; + } + break; + } + return ret; + } + // For RotLookAt public override OMV.Quaternion APIDTarget { set { return; } } @@ -1047,7 +1184,7 @@ public sealed class BSPrim : BSPhysObject } OMV.Vector3 addForce = force; - DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce); + // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce); PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate() {