aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs157
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 {