diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | 157 |
1 files changed, 147 insertions, 10 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs index 826261c..1904ddd 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | |||
@@ -66,9 +66,6 @@ public sealed class BSPrim : BSPhysObject | |||
66 | private float _restitution; | 66 | private float _restitution; |
67 | private bool _setAlwaysRun; | 67 | private bool _setAlwaysRun; |
68 | private bool _throttleUpdates; | 68 | private bool _throttleUpdates; |
69 | private bool _isColliding; | ||
70 | private bool _collidingGround; | ||
71 | private bool _collidingObj; | ||
72 | private bool _floatOnWater; | 69 | private bool _floatOnWater; |
73 | private OMV.Vector3 _rotationalVelocity; | 70 | private OMV.Vector3 _rotationalVelocity; |
74 | private bool _kinematic; | 71 | private bool _kinematic; |
@@ -76,13 +73,14 @@ public sealed class BSPrim : BSPhysObject | |||
76 | 73 | ||
77 | private BSDynamics _vehicle; | 74 | private BSDynamics _vehicle; |
78 | 75 | ||
76 | private BSVMotor _targetMotor; | ||
79 | private OMV.Vector3 _PIDTarget; | 77 | private OMV.Vector3 _PIDTarget; |
80 | private bool _usePID; | ||
81 | private float _PIDTau; | 78 | private float _PIDTau; |
82 | private bool _useHoverPID; | 79 | |
80 | private BSFMotor _hoverMotor; | ||
83 | private float _PIDHoverHeight; | 81 | private float _PIDHoverHeight; |
84 | private PIDHoverType _PIDHoverType; | 82 | private PIDHoverType _PIDHoverType; |
85 | private float _PIDHoverTao; | 83 | private float _PIDHoverTau; |
86 | 84 | ||
87 | public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, | 85 | public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, |
88 | OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) | 86 | OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) |
@@ -564,6 +562,11 @@ public sealed class BSPrim : BSPhysObject | |||
564 | } | 562 | } |
565 | return; | 563 | return; |
566 | } | 564 | } |
565 | public OMV.Vector3 RawVelocity | ||
566 | { | ||
567 | get { return _velocity; } | ||
568 | set { _velocity = value; } | ||
569 | } | ||
567 | public override OMV.Vector3 Velocity { | 570 | public override OMV.Vector3 Velocity { |
568 | get { return _velocity; } | 571 | get { return _velocity; } |
569 | set { | 572 | set { |
@@ -1004,13 +1007,120 @@ public sealed class BSPrim : BSPhysObject | |||
1004 | set { _PIDTau = value; } | 1007 | set { _PIDTau = value; } |
1005 | } | 1008 | } |
1006 | public override bool PIDActive { | 1009 | public override bool PIDActive { |
1007 | set { _usePID = value; } | 1010 | set { |
1011 | if (value) | ||
1012 | { | ||
1013 | // Turning the target on | ||
1014 | /* | ||
1015 | _targetMotor = new BSVMotor("BSPrim.PIDTarget", | ||
1016 | _PIDTau, // timeScale | ||
1017 | BSMotor.Infinite, // decay time scale | ||
1018 | BSMotor.InfiniteVector, // friction timescale | ||
1019 | 1f // efficiency | ||
1020 | ); | ||
1021 | _targetMotor.SetTarget(_PIDTarget); | ||
1022 | _targetMotor.SetCurrent(RawPosition); | ||
1023 | */ | ||
1024 | _targetMotor = new BSPIDVMotor("BSPrim.PIDTarget"); | ||
1025 | _targetMotor.SetTarget(_PIDTarget); | ||
1026 | _targetMotor.SetCurrent(RawPosition); | ||
1027 | _targetMotor.Efficiency = 1f; | ||
1028 | |||
1029 | _targetMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages. | ||
1030 | |||
1031 | RegisterPreStepAction("BSPrim.PIDTarget", LocalID, delegate(float timeStep) | ||
1032 | { | ||
1033 | // How far are we away from the target | ||
1034 | OMV.Vector3 distance = _PIDTarget - RawPosition; | ||
1035 | |||
1036 | // The amount of that distance we should cover per second | ||
1037 | OMV.Vector3 movementPerSecond = distance / _PIDTau; | ||
1038 | |||
1039 | OMV.Vector3 adjustedVelocity = movementPerSecond - RawVelocity; | ||
1040 | |||
1041 | // Apply force to overcome current velocity | ||
1042 | AddForce(-RawVelocity * Mass, false, true); | ||
1043 | // Get it moving to the point | ||
1044 | AddForce(movementPerSecond * Mass, false, true); | ||
1045 | |||
1046 | // Apply enough force to get to the speed needed to get to the point | ||
1047 | // The physics engine will do only a timestep's worth. | ||
1048 | // AddForce(adjustedVelocity * Mass, false, true); | ||
1049 | // PhysicsScene.PE.ApplyCentralImpulse(PhysBody, adjustedVelocity); | ||
1050 | |||
1051 | DetailLog("{0},BSPrim.PIDTarget,move,tgt={1},pos={2},vel={3},dist={4},tau={5},mvmt={6},newVel={7}", | ||
1052 | LocalID, _PIDTarget, RawPosition, RawVelocity, distance, _PIDTau, movementPerSecond, adjustedVelocity); | ||
1053 | |||
1054 | /* | ||
1055 | OMV.Vector3 movePosition = _targetMotor.Step(timeStep); | ||
1056 | |||
1057 | // 'movePosition' is where we'd like the prim to be at this moment. | ||
1058 | // Compute the amount of force to push us there. | ||
1059 | OMV.Vector3 moveForce = (movePosition - RawPosition) * Mass; | ||
1060 | |||
1061 | // If we are very close to our target, turn off the movement motor. | ||
1062 | if (_targetMotor.ErrorIsZero()) | ||
1063 | { | ||
1064 | DetailLog("{0},BSPrim.PIDTarget,zeroMovement,movePos={1},pos={2},mass={3},moveForce={4}", | ||
1065 | LocalID, movePosition, RawPosition, Mass, moveForce); | ||
1066 | moveForce = OMV.Vector3.Zero; | ||
1067 | ForcePosition = _targetMotor.TargetValue; | ||
1068 | _targetMotor.Enabled = false; | ||
1069 | } | ||
1070 | else | ||
1071 | { | ||
1072 | AddForce(moveForce, false, true); | ||
1073 | } | ||
1074 | DetailLog("{0},BSPrim.PIDTarget,move,movePos={1},moveForce={2},mass={3}", LocalID, movePosition, moveForce, Mass); | ||
1075 | */ | ||
1076 | }); | ||
1077 | } | ||
1078 | else | ||
1079 | { | ||
1080 | // Stop any targetting | ||
1081 | UnRegisterPreStepAction("BSPrim.PIDTarget", LocalID); | ||
1082 | } | ||
1083 | } | ||
1008 | } | 1084 | } |
1009 | 1085 | ||
1010 | // Used for llSetHoverHeight and maybe vehicle height | 1086 | // Used for llSetHoverHeight and maybe vehicle height |
1011 | // Hover Height will override MoveTo target's Z | 1087 | // Hover Height will override MoveTo target's Z |
1012 | public override bool PIDHoverActive { | 1088 | public override bool PIDHoverActive { |
1013 | set { _useHoverPID = value; } | 1089 | set { |
1090 | if (value) | ||
1091 | { | ||
1092 | // Turning the target on | ||
1093 | _hoverMotor = new BSFMotor("BSPrim.Hover", | ||
1094 | _PIDHoverTau, // timeScale | ||
1095 | BSMotor.Infinite, // decay time scale | ||
1096 | BSMotor.Infinite, // friction timescale | ||
1097 | 1f // efficiency | ||
1098 | ); | ||
1099 | _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight()); | ||
1100 | _hoverMotor.SetCurrent(RawPosition.Z); | ||
1101 | _hoverMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages. | ||
1102 | |||
1103 | RegisterPreStepAction("BSPrim.Hover", LocalID, delegate(float timeStep) | ||
1104 | { | ||
1105 | // TODO: Decide if the step parameters should be changed depending on the avatar's | ||
1106 | // state (flying, colliding, ...). There is code in ODE to do this. | ||
1107 | |||
1108 | _hoverMotor.SetTarget(ComputeCurrentPIDHoverHeight()); | ||
1109 | float targetHeight = _hoverMotor.Step(timeStep); | ||
1110 | |||
1111 | // 'targetHeight' is where we'd like the Z of the prim to be at this moment. | ||
1112 | // Compute the amount of force to push us there. | ||
1113 | float moveForce = (targetHeight - RawPosition.Z) * Mass / PhysicsScene.LastTimeStep; | ||
1114 | |||
1115 | AddForce(new OMV.Vector3(0f, 0f, moveForce), false, true); | ||
1116 | DetailLog("{0},BSPrim.Hover,move,targHt={1},moveForce={2},mass={3}", LocalID, targetHeight, moveForce, Mass); | ||
1117 | }); | ||
1118 | } | ||
1119 | else | ||
1120 | { | ||
1121 | UnRegisterPreStepAction("BSPrim.Hover", LocalID); | ||
1122 | } | ||
1123 | } | ||
1014 | } | 1124 | } |
1015 | public override float PIDHoverHeight { | 1125 | public override float PIDHoverHeight { |
1016 | set { _PIDHoverHeight = value; } | 1126 | set { _PIDHoverHeight = value; } |
@@ -1019,8 +1129,35 @@ public sealed class BSPrim : BSPhysObject | |||
1019 | set { _PIDHoverType = value; } | 1129 | set { _PIDHoverType = value; } |
1020 | } | 1130 | } |
1021 | public override float PIDHoverTau { | 1131 | public override float PIDHoverTau { |
1022 | set { _PIDHoverTao = value; } | 1132 | set { _PIDHoverTau = value; } |
1023 | } | 1133 | } |
1134 | // Based on current position, determine what we should be hovering at now. | ||
1135 | // Must recompute often. What if we walked offa cliff> | ||
1136 | private float ComputeCurrentPIDHoverHeight() | ||
1137 | { | ||
1138 | float ret = _PIDHoverHeight; | ||
1139 | float groundHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition); | ||
1140 | |||
1141 | switch (_PIDHoverType) | ||
1142 | { | ||
1143 | case PIDHoverType.Ground: | ||
1144 | ret = groundHeight + _PIDHoverHeight; | ||
1145 | break; | ||
1146 | case PIDHoverType.GroundAndWater: | ||
1147 | float waterHeight = PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition); | ||
1148 | if (groundHeight > waterHeight) | ||
1149 | { | ||
1150 | ret = groundHeight + _PIDHoverHeight; | ||
1151 | } | ||
1152 | else | ||
1153 | { | ||
1154 | ret = waterHeight + _PIDHoverHeight; | ||
1155 | } | ||
1156 | break; | ||
1157 | } | ||
1158 | return ret; | ||
1159 | } | ||
1160 | |||
1024 | 1161 | ||
1025 | // For RotLookAt | 1162 | // For RotLookAt |
1026 | public override OMV.Quaternion APIDTarget { set { return; } } | 1163 | public override OMV.Quaternion APIDTarget { set { return; } } |
@@ -1047,7 +1184,7 @@ public sealed class BSPrim : BSPhysObject | |||
1047 | } | 1184 | } |
1048 | 1185 | ||
1049 | OMV.Vector3 addForce = force; | 1186 | OMV.Vector3 addForce = force; |
1050 | DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce); | 1187 | // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce); |
1051 | 1188 | ||
1052 | PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate() | 1189 | PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate() |
1053 | { | 1190 | { |