aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
diff options
context:
space:
mode:
authorubit2013-04-28 20:40:11 +0200
committerubit2013-04-28 20:40:11 +0200
commit61ea7ee5a94e5e3d33fc77c1c316318850309c42 (patch)
tree1e589fc3b448b580d1cc25b52215ef5ce2d7ae78 /OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
parentMerge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork (diff)
parentController module for dynamic floaters (WIP) (diff)
downloadopensim-SC_OLD-61ea7ee5a94e5e3d33fc77c1c316318850309c42.zip
opensim-SC_OLD-61ea7ee5a94e5e3d33fc77c1c316318850309c42.tar.gz
opensim-SC_OLD-61ea7ee5a94e5e3d33fc77c1c316318850309c42.tar.bz2
opensim-SC_OLD-61ea7ee5a94e5e3d33fc77c1c316318850309c42.tar.xz
Merge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork
Conflicts: bin/Regions/Regions.ini.example
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs1264
1 files changed, 873 insertions, 391 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..65df741 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -24,28 +24,16 @@
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 * 26 *
27 27 * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
28/* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to 28 * are Copyright (c) 2009 Linden Research, Inc and are used under their license
29 * call the BulletSim system. 29 * of Creative Commons Attribution-Share Alike 3.0
30 */ 30 * (http://creativecommons.org/licenses/by-sa/3.0/).
31/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
32 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
33 * ODEPrim.cs contains methods dealing with Prim editing, Prim
34 * characteristics and Kinetic motion.
35 * ODEDynamics.cs contains methods dealing with Prim Physical motion
36 * (dynamics) and the associated settings. Old Linear and angular
37 * motors for dynamic motion have been replace with MoveLinear()
38 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
39 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
40 * switch between 'VEHICLE' parameter use and general dynamics
41 * settings use.
42 */ 31 */
43 32
44using System; 33using System;
45using System.Collections.Generic; 34using System.Collections.Generic;
46using System.Reflection; 35using System.Reflection;
47using System.Runtime.InteropServices; 36using System.Runtime.InteropServices;
48using log4net;
49using OpenMetaverse; 37using OpenMetaverse;
50using OpenSim.Framework; 38using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager; 39using OpenSim.Region.Physics.Manager;
@@ -80,10 +68,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
80 private Quaternion m_referenceFrame = Quaternion.Identity; 68 private Quaternion m_referenceFrame = Quaternion.Identity;
81 69
82 // Linear properties 70 // Linear properties
71 private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
83 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 72 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
84 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center 73 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
85 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL 74 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
86 private Vector3 m_newVelocity = Vector3.Zero; // velocity computed to be applied to body
87 private Vector3 m_linearFrictionTimescale = Vector3.Zero; 75 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
88 private float m_linearMotorDecayTimescale = 0; 76 private float m_linearMotorDecayTimescale = 0;
89 private float m_linearMotorTimescale = 0; 77 private float m_linearMotorTimescale = 0;
@@ -93,16 +81,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
93 // private Vector3 m_linearMotorOffset = Vector3.Zero; 81 // private Vector3 m_linearMotorOffset = Vector3.Zero;
94 82
95 //Angular properties 83 //Angular properties
84 private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
96 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 85 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
97 // private int m_angularMotorApply = 0; // application frame counter 86 // private int m_angularMotorApply = 0; // application frame counter
98 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity 87 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
99 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 88 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
100 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 89 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
101 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 90 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
102 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 91 private Vector3 m_lastAngularVelocity = Vector3.Zero;
103 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body 92 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
104 93
105 //Deflection properties 94 //Deflection properties
95 private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
106 private float m_angularDeflectionEfficiency = 0; 96 private float m_angularDeflectionEfficiency = 0;
107 private float m_angularDeflectionTimescale = 0; 97 private float m_angularDeflectionTimescale = 0;
108 private float m_linearDeflectionEfficiency = 0; 98 private float m_linearDeflectionEfficiency = 0;
@@ -114,33 +104,68 @@ namespace OpenSim.Region.Physics.BulletSPlugin
114 private float m_bankingTimescale = 0; 104 private float m_bankingTimescale = 0;
115 105
116 //Hover and Buoyancy properties 106 //Hover and Buoyancy properties
107 private BSVMotor m_hoverMotor = new BSVMotor("Hover");
117 private float m_VhoverHeight = 0f; 108 private float m_VhoverHeight = 0f;
118 private float m_VhoverEfficiency = 0f; 109 private float m_VhoverEfficiency = 0f;
119 private float m_VhoverTimescale = 0f; 110 private float m_VhoverTimescale = 0f;
120 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height 111 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
121 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. 112 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
122 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 113 private float m_VehicleBuoyancy = 0f;
123 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. 114 private Vector3 m_VehicleGravity = Vector3.Zero; // Gravity computed when buoyancy set
124 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
125 115
126 //Attractor properties 116 //Attractor properties
127 private float m_verticalAttractionEfficiency = 1.0f; // damped 117 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
128 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionCutoff = 500f; // per the documentation
120 // Timescale > cutoff means no vert attractor.
121 private float m_verticalAttractionTimescale = 510f;
122
123 // Just some recomputed constants:
124 static readonly float PIOverFour = ((float)Math.PI) / 4f;
125 static readonly float PIOverTwo = ((float)Math.PI) / 2f;
126
127 // For debugging, flags to turn on and off individual corrections.
128 public bool enableAngularVerticalAttraction;
129 public bool enableAngularDeflection;
130 public bool enableAngularBanking;
129 131
130 public BSDynamics(BSScene myScene, BSPrim myPrim) 132 public BSDynamics(BSScene myScene, BSPrim myPrim)
131 { 133 {
132 PhysicsScene = myScene; 134 PhysicsScene = myScene;
133 Prim = myPrim; 135 Prim = myPrim;
134 Type = Vehicle.TYPE_NONE; 136 Type = Vehicle.TYPE_NONE;
137 SetupVehicleDebugging();
138 }
139
140 // Stopgap debugging enablement. Allows source level debugging but still checking
141 // in changes by making enablement of debugging flags from INI file.
142 public void SetupVehicleDebugging()
143 {
144 enableAngularVerticalAttraction = true;
145 enableAngularDeflection = false;
146 enableAngularBanking = true;
147 if (BSParam.VehicleDebuggingEnabled)
148 {
149 enableAngularVerticalAttraction = true;
150 enableAngularDeflection = false;
151 enableAngularBanking = false;
152 }
135 } 153 }
136 154
137 // Return 'true' if this vehicle is doing vehicle things 155 // Return 'true' if this vehicle is doing vehicle things
138 public bool IsActive 156 public bool IsActive
139 { 157 {
140 get { return Type != Vehicle.TYPE_NONE; } 158 get { return (Type != Vehicle.TYPE_NONE && Prim.IsPhysicallyActive); }
141 } 159 }
142 160
143 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) 161 // Return 'true' if this a vehicle that should be sitting on the ground
162 public bool IsGroundVehicle
163 {
164 get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); }
165 }
166
167 #region Vehicle parameter setting
168 public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
144 { 169 {
145 VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue); 170 VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
146 switch (pParam) 171 switch (pParam)
@@ -152,13 +177,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
152 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); 177 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
153 break; 178 break;
154 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: 179 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
155 m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f); 180 m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
181 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
156 break; 182 break;
157 case Vehicle.ANGULAR_MOTOR_TIMESCALE: 183 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
158 m_angularMotorTimescale = Math.Max(pValue, 0.01f); 184 m_angularMotorTimescale = Math.Max(pValue, 0.01f);
185 m_angularMotor.TimeScale = m_angularMotorTimescale;
159 break; 186 break;
160 case Vehicle.BANKING_EFFICIENCY: 187 case Vehicle.BANKING_EFFICIENCY:
161 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); 188 m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
162 break; 189 break;
163 case Vehicle.BANKING_MIX: 190 case Vehicle.BANKING_MIX:
164 m_bankingMix = Math.Max(pValue, 0.01f); 191 m_bankingMix = Math.Max(pValue, 0.01f);
@@ -167,10 +194,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
167 m_bankingTimescale = Math.Max(pValue, 0.01f); 194 m_bankingTimescale = Math.Max(pValue, 0.01f);
168 break; 195 break;
169 case Vehicle.BUOYANCY: 196 case Vehicle.BUOYANCY:
170 m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f)); 197 m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
198 m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
171 break; 199 break;
172 case Vehicle.HOVER_EFFICIENCY: 200 case Vehicle.HOVER_EFFICIENCY:
173 m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f)); 201 m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
174 break; 202 break;
175 case Vehicle.HOVER_HEIGHT: 203 case Vehicle.HOVER_HEIGHT:
176 m_VhoverHeight = pValue; 204 m_VhoverHeight = pValue;
@@ -185,33 +213,41 @@ namespace OpenSim.Region.Physics.BulletSPlugin
185 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); 213 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
186 break; 214 break;
187 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: 215 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
188 m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f); 216 m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
217 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
189 break; 218 break;
190 case Vehicle.LINEAR_MOTOR_TIMESCALE: 219 case Vehicle.LINEAR_MOTOR_TIMESCALE:
191 m_linearMotorTimescale = Math.Max(pValue, 0.01f); 220 m_linearMotorTimescale = Math.Max(pValue, 0.01f);
221 m_linearMotor.TimeScale = m_linearMotorTimescale;
192 break; 222 break;
193 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 223 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
194 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); 224 m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
225 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
195 break; 226 break;
196 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: 227 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
197 m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); 228 m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
229 m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
198 break; 230 break;
199 231
200 // These are vector properties but the engine lets you use a single float value to 232 // These are vector properties but the engine lets you use a single float value to
201 // set all of the components to the same value 233 // set all of the components to the same value
202 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 234 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
203 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 235 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
236 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
204 break; 237 break;
205 case Vehicle.ANGULAR_MOTOR_DIRECTION: 238 case Vehicle.ANGULAR_MOTOR_DIRECTION:
206 m_angularMotorDirection = new Vector3(pValue, pValue, pValue); 239 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
207 // m_angularMotorApply = 100; 240 m_angularMotor.Zero();
241 m_angularMotor.SetTarget(m_angularMotorDirection);
208 break; 242 break;
209 case Vehicle.LINEAR_FRICTION_TIMESCALE: 243 case Vehicle.LINEAR_FRICTION_TIMESCALE:
210 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 244 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
245 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
211 break; 246 break;
212 case Vehicle.LINEAR_MOTOR_DIRECTION: 247 case Vehicle.LINEAR_MOTOR_DIRECTION:
213 m_linearMotorDirection = new Vector3(pValue, pValue, pValue); 248 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
214 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); 249 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
250 m_linearMotor.SetTarget(m_linearMotorDirection);
215 break; 251 break;
216 case Vehicle.LINEAR_MOTOR_OFFSET: 252 case Vehicle.LINEAR_MOTOR_OFFSET:
217 m_linearMotorOffset = new Vector3(pValue, pValue, pValue); 253 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -227,21 +263,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
227 { 263 {
228 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 264 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
229 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 265 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
266 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
230 break; 267 break;
231 case Vehicle.ANGULAR_MOTOR_DIRECTION: 268 case Vehicle.ANGULAR_MOTOR_DIRECTION:
232 // Limit requested angular speed to 2 rps= 4 pi rads/sec 269 // Limit requested angular speed to 2 rps= 4 pi rads/sec
233 pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f)); 270 pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
234 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); 271 pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
235 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); 272 pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
236 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 273 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
237 // m_angularMotorApply = 100; 274 m_angularMotor.Zero();
275 m_angularMotor.SetTarget(m_angularMotorDirection);
238 break; 276 break;
239 case Vehicle.LINEAR_FRICTION_TIMESCALE: 277 case Vehicle.LINEAR_FRICTION_TIMESCALE:
240 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 278 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
279 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
241 break; 280 break;
242 case Vehicle.LINEAR_MOTOR_DIRECTION: 281 case Vehicle.LINEAR_MOTOR_DIRECTION:
243 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 282 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); 283 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
284 m_linearMotor.SetTarget(m_linearMotorDirection);
245 break; 285 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET: 286 case Vehicle.LINEAR_MOTOR_OFFSET:
247 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 287 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -281,7 +321,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
281 } 321 }
282 } 322 }
283 323
284 internal void ProcessTypeChange(Vehicle pType) 324 public void ProcessTypeChange(Vehicle pType)
285 { 325 {
286 VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType); 326 VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
287 // Set Defaults For Type 327 // Set Defaults For Type
@@ -303,7 +343,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
303 m_VhoverEfficiency = 0; 343 m_VhoverEfficiency = 0;
304 m_VhoverTimescale = 0; 344 m_VhoverTimescale = 0;
305 m_VehicleBuoyancy = 0; 345 m_VehicleBuoyancy = 0;
306 346
307 m_linearDeflectionEfficiency = 1; 347 m_linearDeflectionEfficiency = 1;
308 m_linearDeflectionTimescale = 1; 348 m_linearDeflectionTimescale = 1;
309 349
@@ -319,6 +359,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
319 359
320 m_referenceFrame = Quaternion.Identity; 360 m_referenceFrame = Quaternion.Identity;
321 m_flags = (VehicleFlag)0; 361 m_flags = (VehicleFlag)0;
362
322 break; 363 break;
323 364
324 case Vehicle.TYPE_SLED: 365 case Vehicle.TYPE_SLED:
@@ -351,10 +392,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
351 m_bankingMix = 1; 392 m_bankingMix = 1;
352 393
353 m_referenceFrame = Quaternion.Identity; 394 m_referenceFrame = Quaternion.Identity;
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); 395 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
355 m_flags &= 396 | VehicleFlag.HOVER_TERRAIN_ONLY
356 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | 397 | VehicleFlag.HOVER_GLOBAL_HEIGHT
357 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 398 | VehicleFlag.HOVER_UP_ONLY);
399 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
400 | VehicleFlag.LIMIT_ROLL_ONLY
401 | VehicleFlag.LIMIT_MOTOR_UP);
402
358 break; 403 break;
359 case Vehicle.TYPE_CAR: 404 case Vehicle.TYPE_CAR:
360 m_linearMotorDirection = Vector3.Zero; 405 m_linearMotorDirection = Vector3.Zero;
@@ -498,6 +543,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
498 m_bankingEfficiency = 0; 543 m_bankingEfficiency = 0;
499 m_bankingMix = 0.7f; 544 m_bankingMix = 0.7f;
500 m_bankingTimescale = 5; 545 m_bankingTimescale = 5;
546
501 m_referenceFrame = Quaternion.Identity; 547 m_referenceFrame = Quaternion.Identity;
502 548
503 m_referenceFrame = Quaternion.Identity; 549 m_referenceFrame = Quaternion.Identity;
@@ -510,152 +556,467 @@ namespace OpenSim.Region.Physics.BulletSPlugin
510 | VehicleFlag.HOVER_GLOBAL_HEIGHT); 556 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
511 break; 557 break;
512 } 558 }
559
560 // Update any physical parameters based on this type.
561 Refresh();
562
563 m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
564 m_linearMotorDecayTimescale, m_linearFrictionTimescale,
565 1f);
566 m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
567
568 m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
569 m_angularMotorDecayTimescale, m_angularFrictionTimescale,
570 1f);
571 m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
572
573 /* Not implemented
574 m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
575 BSMotor.Infinite, BSMotor.InfiniteVector,
576 m_verticalAttractionEfficiency);
577 // Z goes away and we keep X and Y
578 m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
579 m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
580 */
581 }
582 #endregion // Vehicle parameter setting
583
584 public void Refresh()
585 {
586 // If asking for a refresh, reset the physical parameters before the next simulation step.
587 PhysicsScene.PostTaintObject("BSDynamics.Refresh", Prim.LocalID, delegate()
588 {
589 SetPhysicalParameters();
590 });
513 } 591 }
514 592
515 // Some of the properties of this prim may have changed. 593 // Some of the properties of this prim may have changed.
516 // Do any updating needed for a vehicle 594 // Do any updating needed for a vehicle
517 public void Refresh() 595 private void SetPhysicalParameters()
518 { 596 {
519 if (IsActive) 597 if (IsActive)
520 { 598 {
521 // Friction effects are handled by this vehicle code 599 // Remember the mass so we don't have to fetch it every step
522 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f); 600 m_vehicleMass = Prim.TotalMass;
523 BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f); 601
524 602 // Friction affects are handled by this vehicle code
525 // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f); 603 PhysicsScene.PE.SetFriction(Prim.PhysBody, BSParam.VehicleFriction);
526 604 PhysicsScene.PE.SetRestitution(Prim.PhysBody, BSParam.VehicleRestitution);
527 VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID); 605
606 // Moderate angular movement introduced by Bullet.
607 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
608 // Maybe compute linear and angular factor and damping from params.
609 PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, BSParam.VehicleAngularDamping);
610 PhysicsScene.PE.SetLinearFactor(Prim.PhysBody, BSParam.VehicleLinearFactor);
611 PhysicsScene.PE.SetAngularFactorV(Prim.PhysBody, BSParam.VehicleAngularFactor);
612
613 // Vehicles report collision events so we know when it's on the ground
614 PhysicsScene.PE.AddToCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
615
616 Prim.Inertia = PhysicsScene.PE.CalculateLocalInertia(Prim.PhysShape, m_vehicleMass);
617 PhysicsScene.PE.SetMassProps(Prim.PhysBody, m_vehicleMass, Prim.Inertia);
618 PhysicsScene.PE.UpdateInertiaTensor(Prim.PhysBody);
619
620 // Set the gravity for the vehicle depending on the buoyancy
621 // TODO: what should be done if prim and vehicle buoyancy differ?
622 m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
623 // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
624 PhysicsScene.PE.SetGravity(Prim.PhysBody, Vector3.Zero);
625
626 VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
627 Prim.LocalID, m_vehicleMass, Prim.Inertia, m_VehicleGravity,
628 BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
629 BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
630 );
631 }
632 else
633 {
634 if (Prim.PhysBody.HasPhysicalBody)
635 PhysicsScene.PE.RemoveFromCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
528 } 636 }
529 } 637 }
530 638
531 public bool RemoveBodyDependencies(BSPhysObject prim) 639 public bool RemoveBodyDependencies(BSPhysObject prim)
532 { 640 {
533 // If active, we need to add our properties back when the body is rebuilt. 641 Refresh();
534 return IsActive; 642 return IsActive;
535 } 643 }
536 644
537 public void RestoreBodyDependencies(BSPhysObject prim) 645 #region Known vehicle value functions
646 // Vehicle physical parameters that we buffer from constant getting and setting.
647 // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
648 // Changing is remembered and the parameter is stored back into the physics engine only if updated.
649 // This does two things: 1) saves continuious calls into unmanaged code, and
650 // 2) signals when a physics property update must happen back to the simulator
651 // to update values modified for the vehicle.
652 private int m_knownChanged;
653 private int m_knownHas;
654 private float m_knownTerrainHeight;
655 private float m_knownWaterLevel;
656 private Vector3 m_knownPosition;
657 private Vector3 m_knownVelocity;
658 private Vector3 m_knownForce;
659 private Vector3 m_knownForceImpulse;
660 private Quaternion m_knownOrientation;
661 private Vector3 m_knownRotationalVelocity;
662 private Vector3 m_knownRotationalForce;
663 private Vector3 m_knownRotationalImpulse;
664 private Vector3 m_knownForwardVelocity; // vehicle relative forward speed
665
666 private const int m_knownChangedPosition = 1 << 0;
667 private const int m_knownChangedVelocity = 1 << 1;
668 private const int m_knownChangedForce = 1 << 2;
669 private const int m_knownChangedForceImpulse = 1 << 3;
670 private const int m_knownChangedOrientation = 1 << 4;
671 private const int m_knownChangedRotationalVelocity = 1 << 5;
672 private const int m_knownChangedRotationalForce = 1 << 6;
673 private const int m_knownChangedRotationalImpulse = 1 << 7;
674 private const int m_knownChangedTerrainHeight = 1 << 8;
675 private const int m_knownChangedWaterLevel = 1 << 9;
676 private const int m_knownChangedForwardVelocity = 1 <<10;
677
678 public void ForgetKnownVehicleProperties()
679 {
680 m_knownHas = 0;
681 m_knownChanged = 0;
682 }
683 // Push all the changed values back into the physics engine
684 public void PushKnownChanged()
685 {
686 if (m_knownChanged != 0)
687 {
688 if ((m_knownChanged & m_knownChangedPosition) != 0)
689 Prim.ForcePosition = m_knownPosition;
690
691 if ((m_knownChanged & m_knownChangedOrientation) != 0)
692 Prim.ForceOrientation = m_knownOrientation;
693
694 if ((m_knownChanged & m_knownChangedVelocity) != 0)
695 {
696 Prim.ForceVelocity = m_knownVelocity;
697 // Fake out Bullet by making it think the velocity is the same as last time.
698 // Bullet does a bunch of smoothing for changing parameters.
699 // Since the vehicle is demanding this setting, we override Bullet's smoothing
700 // by telling Bullet the value was the same last time.
701 // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
702 }
703
704 if ((m_knownChanged & m_knownChangedForce) != 0)
705 Prim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
706
707 if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
708 Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
709
710 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
711 {
712 Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
713 // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
714 }
715
716 if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
717 Prim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
718
719 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
720 {
721 Prim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
722 }
723
724 // If we set one of the values (ie, the physics engine didn't do it) we must force
725 // an UpdateProperties event to send the changes up to the simulator.
726 PhysicsScene.PE.PushUpdate(Prim.PhysBody);
727 }
728 m_knownChanged = 0;
729 }
730
731 // Since the computation of terrain height can be a little involved, this routine
732 // is used to fetch the height only once for each vehicle simulation step.
733 Vector3 lastRememberedHeightPos;
734 private float GetTerrainHeight(Vector3 pos)
538 { 735 {
539 if (Prim.LocalID != prim.LocalID) 736 if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
540 { 737 {
541 // The call should be on us by our prim. Error if not. 738 lastRememberedHeightPos = pos;
542 PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}", 739 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
543 LogHeader, prim.LocalID, Prim.LocalID); 740 m_knownHas |= m_knownChangedTerrainHeight;
544 return; 741 }
742 return m_knownTerrainHeight;
743 }
744
745 // Since the computation of water level can be a little involved, this routine
746 // is used ot fetch the level only once for each vehicle simulation step.
747 private float GetWaterLevel(Vector3 pos)
748 {
749 if ((m_knownHas & m_knownChangedWaterLevel) == 0)
750 {
751 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
752 m_knownHas |= m_knownChangedWaterLevel;
753 }
754 return (float)m_knownWaterLevel;
755 }
756
757 private Vector3 VehiclePosition
758 {
759 get
760 {
761 if ((m_knownHas & m_knownChangedPosition) == 0)
762 {
763 m_knownPosition = Prim.ForcePosition;
764 m_knownHas |= m_knownChangedPosition;
765 }
766 return m_knownPosition;
767 }
768 set
769 {
770 m_knownPosition = value;
771 m_knownChanged |= m_knownChangedPosition;
772 m_knownHas |= m_knownChangedPosition;
545 } 773 }
546 Refresh();
547 } 774 }
548 775
776 private Quaternion VehicleOrientation
777 {
778 get
779 {
780 if ((m_knownHas & m_knownChangedOrientation) == 0)
781 {
782 m_knownOrientation = Prim.ForceOrientation;
783 m_knownHas |= m_knownChangedOrientation;
784 }
785 return m_knownOrientation;
786 }
787 set
788 {
789 m_knownOrientation = value;
790 m_knownChanged |= m_knownChangedOrientation;
791 m_knownHas |= m_knownChangedOrientation;
792 }
793 }
794
795 private Vector3 VehicleVelocity
796 {
797 get
798 {
799 if ((m_knownHas & m_knownChangedVelocity) == 0)
800 {
801 m_knownVelocity = Prim.ForceVelocity;
802 m_knownHas |= m_knownChangedVelocity;
803 }
804 return m_knownVelocity;
805 }
806 set
807 {
808 m_knownVelocity = value;
809 m_knownChanged |= m_knownChangedVelocity;
810 m_knownHas |= m_knownChangedVelocity;
811 }
812 }
813
814 private void VehicleAddForce(Vector3 pForce)
815 {
816 if ((m_knownHas & m_knownChangedForce) == 0)
817 {
818 m_knownForce = Vector3.Zero;
819 m_knownHas |= m_knownChangedForce;
820 }
821 m_knownForce += pForce;
822 m_knownChanged |= m_knownChangedForce;
823 }
824
825 private void VehicleAddForceImpulse(Vector3 pImpulse)
826 {
827 if ((m_knownHas & m_knownChangedForceImpulse) == 0)
828 {
829 m_knownForceImpulse = Vector3.Zero;
830 m_knownHas |= m_knownChangedForceImpulse;
831 }
832 m_knownForceImpulse += pImpulse;
833 m_knownChanged |= m_knownChangedForceImpulse;
834 }
835
836 private Vector3 VehicleRotationalVelocity
837 {
838 get
839 {
840 if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
841 {
842 m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
843 m_knownHas |= m_knownChangedRotationalVelocity;
844 }
845 return (Vector3)m_knownRotationalVelocity;
846 }
847 set
848 {
849 m_knownRotationalVelocity = value;
850 m_knownChanged |= m_knownChangedRotationalVelocity;
851 m_knownHas |= m_knownChangedRotationalVelocity;
852 }
853 }
854 private void VehicleAddAngularForce(Vector3 aForce)
855 {
856 if ((m_knownHas & m_knownChangedRotationalForce) == 0)
857 {
858 m_knownRotationalForce = Vector3.Zero;
859 }
860 m_knownRotationalForce += aForce;
861 m_knownChanged |= m_knownChangedRotationalForce;
862 m_knownHas |= m_knownChangedRotationalForce;
863 }
864 private void VehicleAddRotationalImpulse(Vector3 pImpulse)
865 {
866 if ((m_knownHas & m_knownChangedRotationalImpulse) == 0)
867 {
868 m_knownRotationalImpulse = Vector3.Zero;
869 m_knownHas |= m_knownChangedRotationalImpulse;
870 }
871 m_knownRotationalImpulse += pImpulse;
872 m_knownChanged |= m_knownChangedRotationalImpulse;
873 }
874
875 // Vehicle relative forward velocity
876 private Vector3 VehicleForwardVelocity
877 {
878 get
879 {
880 if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
881 {
882 m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
883 m_knownHas |= m_knownChangedForwardVelocity;
884 }
885 return m_knownForwardVelocity;
886 }
887 }
888 private float VehicleForwardSpeed
889 {
890 get
891 {
892 return VehicleForwardVelocity.X;
893 }
894 }
895
896 #endregion // Known vehicle value functions
897
549 // One step of the vehicle properties for the next 'pTimestep' seconds. 898 // One step of the vehicle properties for the next 'pTimestep' seconds.
550 internal void Step(float pTimestep) 899 internal void Step(float pTimestep)
551 { 900 {
552 if (!IsActive) return; 901 if (!IsActive) return;
553 902
554 // DEBUG 903 ForgetKnownVehicleProperties();
555 // Because Bullet does apply forces to the vehicle, our last computed
556 // linear and angular velocities are not what is happening now.
557 // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
558 // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
559 // m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time
560 // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG:
561 // END DEBUG
562
563 m_vehicleMass = Prim.Linkset.LinksetMass;
564 904
565 MoveLinear(pTimestep); 905 MoveLinear(pTimestep);
566 // Commented out for debug
567 MoveAngular(pTimestep); 906 MoveAngular(pTimestep);
568 // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false); // DEBUG DEBUG
569 // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity; // DEBUG DEBUG
570 907
571 LimitRotation(pTimestep); 908 LimitRotation(pTimestep);
572 909
573 // remember the position so next step we can limit absolute movement effects 910 // remember the position so next step we can limit absolute movement effects
574 m_lastPositionVector = Prim.ForcePosition; 911 m_lastPositionVector = VehiclePosition;
575 912
576 VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}", // DEBUG DEBUG 913 // If we forced the changing of some vehicle parameters, update the values and
577 Prim.LocalID, 914 // for the physics engine to note the changes so an UpdateProperties event will happen.
578 BulletSimAPI.GetFriction2(Prim.PhysBody.ptr), 915 PushKnownChanged();
579 BulletSimAPI.GetGravity2(Prim.PhysBody.ptr), 916
580 Prim.Inertia, 917 if (PhysicsScene.VehiclePhysicalLoggingEnabled)
581 m_vehicleMass 918 PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
582 ); 919
583 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 920 VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
584 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); 921 Prim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
585 }// end Step 922 }
586 923
587 // Apply the effect of the linear motor. 924 // Called after the simulation step
588 // Also does hover and float. 925 internal void PostStep(float pTimestep)
926 {
927 if (!IsActive) return;
928
929 if (PhysicsScene.VehiclePhysicalLoggingEnabled)
930 PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
931 }
932
933 // Apply the effect of the linear motor and other linear motions (like hover and float).
589 private void MoveLinear(float pTimestep) 934 private void MoveLinear(float pTimestep)
590 { 935 {
591 // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates 936 ComputeLinearVelocity(pTimestep);
592 // m_lastLinearVelocityVector is the current speed we are moving in that direction
593 if (m_linearMotorDirection.LengthSquared() > 0.001f)
594 {
595 Vector3 origDir = m_linearMotorDirection; // DEBUG
596 Vector3 origVel = m_lastLinearVelocityVector; // DEBUG
597 // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
598 Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG
599 937
600 // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete 938 ComputeLinearTerrainHeightCorrection(pTimestep);
601 Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
602 m_lastLinearVelocityVector += addAmount;
603 939
604 float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep; 940 ComputeLinearHover(pTimestep);
605 m_linearMotorDirection *= (1f - decayFactor);
606 941
607 // Rotate new object velocity from vehicle relative to world coordinates 942 ComputeLinearBlockingEndPoint(pTimestep);
608 m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
609 943
610 // Apply friction for next time 944 ComputeLinearMotorUp(pTimestep);
611 Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
612 m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
613 945
614 VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}", 946 ApplyGravity(pTimestep);
615 Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 947
616 m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity); 948 // If not changing some axis, reduce out velocity
617 } 949 if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
618 else
619 { 950 {
620 // if what remains of direction is very small, zero it. 951 Vector3 vel = VehicleVelocity;
621 m_linearMotorDirection = Vector3.Zero; 952 if ((m_flags & (VehicleFlag.NO_X)) != 0)
622 m_lastLinearVelocityVector = Vector3.Zero; 953 vel.X = 0;
623 m_newVelocity = Vector3.Zero; 954 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
955 vel.Y = 0;
956 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
957 vel.Z = 0;
958 VehicleVelocity = vel;
959 }
624 960
625 VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); 961 // ==================================================================
962 // Clamp high or low velocities
963 float newVelocityLengthSq = VehicleVelocity.LengthSquared();
964 if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
965 {
966 Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
967 VehicleVelocity /= VehicleVelocity.Length();
968 VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
969 VDetailLog("{0}, MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}",
970 Prim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
626 } 971 }
972 else if (newVelocityLengthSq < 0.001f)
973 VehicleVelocity = Vector3.Zero;
627 974
628 // m_newVelocity is velocity computed from linear motor in world coordinates 975 VDetailLog("{0}, MoveLinear,done,isColl={1},newVel={2}", Prim.LocalID, Prim.IsColliding, VehicleVelocity );
629 976
630 // Gravity and Buoyancy 977 } // end MoveLinear()
631 // There is some gravity, make a gravity force vector that is applied after object velocity.
632 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
633 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
634 978
635 /* 979 public void ComputeLinearVelocity(float pTimestep)
636 * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ... 980 {
637 // Preserve the current Z velocity 981 // Step the motor from the current value. Get the correction needed this step.
638 Vector3 vel_now = m_prim.Velocity; 982 Vector3 origVelW = VehicleVelocity; // DEBUG
639 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity 983 Vector3 currentVelV = VehicleVelocity * Quaternion.Inverse(VehicleOrientation);
640 */ 984 Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
985
986 // Motor is vehicle coordinates. Rotate it to world coordinates
987 Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
988
989 // If we're a ground vehicle, don't add any upward Z movement
990 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
991 {
992 if (linearMotorVelocityW.Z > 0f)
993 linearMotorVelocityW.Z = 0f;
994 }
995
996 // Add this correction to the velocity to make it faster/slower.
997 VehicleVelocity += linearMotorVelocityW;
641 998
642 Vector3 pos = Prim.ForcePosition; 999 VDetailLog("{0}, MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5}",
643// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); 1000 Prim.LocalID, origVelW, currentVelV, linearMotorCorrectionV, linearMotorVelocityW, VehicleVelocity);
1001 }
644 1002
1003 public void ComputeLinearTerrainHeightCorrection(float pTimestep)
1004 {
645 // If below the terrain, move us above the ground a little. 1005 // If below the terrain, move us above the ground a little.
646 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); 1006 // TODO: Consider taking the rotated size of the object or possibly casting a ray.
647 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. 1007 if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
648 // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
649 // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
650 // if (rotatedSize.Z < terrainHeight)
651 if (pos.Z < terrainHeight)
652 { 1008 {
653 pos.Z = terrainHeight + 2; 1009 // Force position because applying force won't get the vehicle through the terrain
654 Prim.ForcePosition = pos; 1010 Vector3 newPosition = VehiclePosition;
655 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); 1011 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
1012 VehiclePosition = newPosition;
1013 VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
1014 Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
656 } 1015 }
1016 }
657 1017
658 // Check if hovering 1018 public void ComputeLinearHover(float pTimestep)
1019 {
659 // m_VhoverEfficiency: 0=bouncy, 1=totally damped 1020 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
660 // m_VhoverTimescale: time to achieve height 1021 // m_VhoverTimescale: time to achieve height
661 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 1022 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
@@ -663,11 +1024,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
663 // We should hover, get the target height 1024 // We should hover, get the target height
664 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) 1025 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
665 { 1026 {
666 m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight; 1027 m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
667 } 1028 }
668 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) 1029 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
669 { 1030 {
670 m_VhoverTargetHeight = terrainHeight + m_VhoverHeight; 1031 m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
671 } 1032 }
672 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) 1033 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
673 { 1034 {
@@ -677,45 +1038,63 @@ namespace OpenSim.Region.Physics.BulletSPlugin
677 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) 1038 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
678 { 1039 {
679 // If body is already heigher, use its height as target height 1040 // If body is already heigher, use its height as target height
680 if (pos.Z > m_VhoverTargetHeight) 1041 if (VehiclePosition.Z > m_VhoverTargetHeight)
681 m_VhoverTargetHeight = pos.Z; 1042 m_VhoverTargetHeight = VehiclePosition.Z;
682 } 1043 }
1044
683 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) 1045 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
684 { 1046 {
685 if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) 1047 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
686 { 1048 {
1049 Vector3 pos = VehiclePosition;
687 pos.Z = m_VhoverTargetHeight; 1050 pos.Z = m_VhoverTargetHeight;
688 Prim.ForcePosition = pos; 1051 VehiclePosition = pos;
1052
1053 VDetailLog("{0}, MoveLinear,hover,pos={1},lockHoverHeight", Prim.LocalID, pos);
689 } 1054 }
690 } 1055 }
691 else 1056 else
692 { 1057 {
693 float verticalError = pos.Z - m_VhoverTargetHeight; 1058 // Error is positive if below the target and negative if above.
694 // RA: where does the 50 come from? 1059 Vector3 hpos = VehiclePosition;
695 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); 1060 float verticalError = m_VhoverTargetHeight - hpos.Z;
696 // Replace Vertical speed with correction figure if significant 1061 float verticalCorrection = verticalError / m_VhoverTimescale;
697 if (Math.Abs(verticalError) > 0.01f) 1062 verticalCorrection *= m_VhoverEfficiency;
698 { 1063
699 m_newVelocity.Z += verticalCorrectionVelocity; 1064 hpos.Z += verticalCorrection;
700 //KF: m_VhoverEfficiency is not yet implemented 1065 VehiclePosition = hpos;
701 } 1066
702 else if (verticalError < -0.01) 1067 // Since we are hovering, we need to do the opposite of falling -- get rid of world Z
703 { 1068 Vector3 vel = VehicleVelocity;
704 m_newVelocity.Z -= verticalCorrectionVelocity; 1069 vel.Z = 0f;
705 } 1070 VehicleVelocity = vel;
706 else 1071
707 { 1072 /*
708 m_newVelocity.Z = 0f; 1073 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
709 } 1074 Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity);
1075 verticalCorrection *= m_vehicleMass;
1076
1077 // TODO: implement m_VhoverEfficiency correctly
1078 VehicleAddForceImpulse(verticalCorrection);
1079 */
1080
1081 VDetailLog("{0}, MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
1082 Prim.LocalID, VehiclePosition, m_VhoverEfficiency,
1083 m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
1084 verticalError, verticalCorrection);
710 } 1085 }
711 1086
712 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight);
713 } 1087 }
1088 }
714 1089
1090 public bool ComputeLinearBlockingEndPoint(float pTimestep)
1091 {
1092 bool changed = false;
1093
1094 Vector3 pos = VehiclePosition;
715 Vector3 posChange = pos - m_lastPositionVector; 1095 Vector3 posChange = pos - m_lastPositionVector;
716 if (m_BlockingEndPoint != Vector3.Zero) 1096 if (m_BlockingEndPoint != Vector3.Zero)
717 { 1097 {
718 bool changed = false;
719 if (pos.X >= (m_BlockingEndPoint.X - (float)1)) 1098 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
720 { 1099 {
721 pos.X -= posChange.X + 1; 1100 pos.X -= posChange.X + 1;
@@ -743,233 +1122,119 @@ namespace OpenSim.Region.Physics.BulletSPlugin
743 } 1122 }
744 if (changed) 1123 if (changed)
745 { 1124 {
746 Prim.ForcePosition = pos; 1125 VehiclePosition = pos;
747 VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", 1126 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
748 Prim.LocalID, m_BlockingEndPoint, posChange, pos); 1127 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
749 } 1128 }
750 } 1129 }
1130 return changed;
1131 }
751 1132
752 #region downForce 1133 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
753 Vector3 downForce = Vector3.Zero; 1134 // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
754 1135 // used with conjunction with banking: the strength of the banking will decay when the
1136 // vehicle no longer experiences collisions. The decay timescale is the same as
1137 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
1138 // when they are in mid jump.
1139 // TODO: this code is wrong. Also, what should it do for boats (height from water)?
1140 // This is just using the ground and a general collision check. Should really be using
1141 // a downward raycast to find what is below.
1142 public void ComputeLinearMotorUp(float pTimestep)
1143 {
755 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 1144 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
756 { 1145 {
757 // If the vehicle is motoring into the sky, get it going back down. 1146 // This code tries to decide if the object is not on the ground and then pushing down
758 // Is this an angular force or both linear and angular?? 1147 /*
759 float distanceAboveGround = pos.Z - terrainHeight; 1148 float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
760 if (distanceAboveGround > 2f) 1149 distanceAboveGround = VehiclePosition.Z - targetHeight;
1150 // Not colliding if the vehicle is off the ground
1151 if (!Prim.IsColliding)
761 { 1152 {
762 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
763 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); 1153 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
764 downForce = new Vector3(0, 0, -distanceAboveGround); 1154 VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
765 } 1155 }
766 // TODO: this calculation is all wrong. From the description at 1156 // TODO: this calculation is wrong. From the description at
767 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce 1157 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
768 // has a decay factor. This says this force should 1158 // has a decay factor. This says this force should
769 // be computed with a motor. 1159 // be computed with a motor.
770 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 1160 // TODO: add interaction with banking.
771 Prim.LocalID, distanceAboveGround, downForce); 1161 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
772 } 1162 Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret);
773 #endregion // downForce 1163 */
774 1164
775 // If not changing some axis, reduce out velocity 1165 // Another approach is to measure if we're going up. If going up and not colliding,
776 if ((m_flags & (VehicleFlag.NO_X)) != 0) 1166 // the vehicle is in the air. Fix that by pushing down.
777 m_newVelocity.X = 0; 1167 if (!Prim.IsColliding && VehicleVelocity.Z > 0.1)
778 if ((m_flags & (VehicleFlag.NO_Y)) != 0) 1168 {
779 m_newVelocity.Y = 0; 1169 // Get rid of any of the velocity vector that is pushing us up.
780 if ((m_flags & (VehicleFlag.NO_Z)) != 0) 1170 float upVelocity = VehicleVelocity.Z;
781 m_newVelocity.Z = 0; 1171 VehicleVelocity += new Vector3(0, 0, -upVelocity);
782 1172
783 // Clamp REALLY high or low velocities 1173 /*
784 if (m_newVelocity.LengthSquared() > 1e6f) 1174 // If we're pointed up into the air, we should nose down
785 { 1175 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
786 m_newVelocity /= m_newVelocity.Length(); 1176 // The rotation around the Y axis is pitch up or down
787 m_newVelocity *= 1000f; 1177 if (pointingDirection.Y > 0.01f)
1178 {
1179 float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y);
1180 Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f);
1181 // Rotate into world coordinates and apply to vehicle
1182 angularCorrectionVector *= VehicleOrientation;
1183 VehicleAddAngularForce(angularCorrectionVector);
1184 VDetailLog("{0}, MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}",
1185 Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector);
1186 }
1187 */
1188 VDetailLog("{0}, MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}",
1189 Prim.LocalID, Prim.IsColliding, upVelocity, VehicleVelocity);
1190 }
788 } 1191 }
789 else if (m_newVelocity.LengthSquared() < 1e-6f) 1192 }
790 m_newVelocity = Vector3.Zero;
791 1193
792 // Stuff new linear velocity into the vehicle 1194 private void ApplyGravity(float pTimeStep)
793 Prim.ForceVelocity = m_newVelocity; 1195 {
794 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG 1196 Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
795 1197
796 Vector3 totalDownForce = downForce + grav; 1198 // Hack to reduce downward force if the vehicle is probably sitting on the ground
797 if (totalDownForce != Vector3.Zero) 1199 if (Prim.IsColliding && IsGroundVehicle)
798 { 1200 appliedGravity *= BSParam.VehicleGroundGravityFudge;
799 Prim.AddForce(totalDownForce * m_vehicleMass, false);
800 // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
801 }
802 1201
803 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", 1202 VehicleAddForce(appliedGravity);
804 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
805 1203
806 } // end MoveLinear() 1204 VDetailLog("{0}, MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={3}",
1205 Prim.LocalID, m_VehicleGravity,
1206 Prim.IsColliding, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
1207 }
807 1208
808 // ======================================================================= 1209 // =======================================================================
1210 // =======================================================================
809 // Apply the effect of the angular motor. 1211 // Apply the effect of the angular motor.
1212 // The 'contribution' is how much angular correction velocity each function wants.
1213 // All the contributions are added together and the resulting velocity is
1214 // set directly on the vehicle.
810 private void MoveAngular(float pTimestep) 1215 private void MoveAngular(float pTimestep)
811 { 1216 {
812 // m_angularMotorDirection // angular velocity requested by LSL motor 1217 ComputeAngularTurning(pTimestep);
813 // m_angularMotorApply // application frame counter
814 // m_angularMotorVelocity // current angular motor velocity (ramps up and down)
815 // m_angularMotorTimescale // motor angular velocity ramp up rate
816 // m_angularMotorDecayTimescale // motor angular velocity decay rate
817 // m_angularFrictionTimescale // body angular velocity decay rate
818 // m_lastAngularVelocity // what was last applied to body
819
820 if (m_angularMotorDirection.LengthSquared() > 0.0001)
821 {
822 Vector3 origVel = m_angularMotorVelocity;
823 Vector3 origDir = m_angularMotorDirection;
824
825 // new velocity += error / ( time to get there / step interval)
826 // requested direction - current vehicle direction
827 m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep);
828 // decay requested direction
829 m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
830 1218
831 VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}", 1219 ComputeAngularVerticalAttraction();
832 Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
833 }
834 else
835 {
836 m_angularMotorVelocity = Vector3.Zero;
837 }
838 1220
839 #region Vertical attactor 1221 ComputeAngularDeflection();
840 1222
841 Vector3 vertattr = Vector3.Zero; 1223 ComputeAngularBanking();
842 Vector3 deflection = Vector3.Zero;
843 Vector3 banking = Vector3.Zero;
844 1224
845 // If vertical attaction timescale is reasonable and we applied an angular force last time... 1225 // ==================================================================
846 if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) 1226 if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
847 { 1227 {
848 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; 1228 // The vehicle is not adding anything angular wise.
849 if (Prim.IsColliding) 1229 VehicleRotationalVelocity = Vector3.Zero;
850 VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); 1230 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
851
852 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
853
854 // Create a vector of the vehicle "up" in world coordinates
855 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
856 // verticalError.X and .Y are the World error amounts. They are 0 when there is no
857 // error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its
858 // side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall
859 // and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be
860 // modulated to prevent a stable inverted body.
861
862 // Error is 0 (no error) to +/- 2 (max error)
863 if (verticalError.Z < 0.0f)
864 {
865 verticalError.X = 2.0f - verticalError.X;
866 verticalError.Y = 2.0f - verticalError.Y;
867 }
868 // scale it by VAservo (timestep and timescale)
869 verticalError = verticalError * VAservo;
870
871 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
872 // then .X increases, so change Body angular velocity X based on Y, and Y based on X.
873 // Z is not changed.
874 vertattr.X = verticalError.Y;
875 vertattr.Y = - verticalError.X;
876 vertattr.Z = 0f;
877
878 // scaling appears better usingsquare-law
879 Vector3 angularVelocity = Prim.ForceRotationalVelocity;
880 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
881 vertattr.X += bounce * angularVelocity.X;
882 vertattr.Y += bounce * angularVelocity.Y;
883
884 VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
885 Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
886
887 }
888 #endregion // Vertical attactor
889
890 #region Deflection
891
892 if (m_angularDeflectionEfficiency != 0)
893 {
894 // Compute a scaled vector that points in the preferred axis (X direction)
895 Vector3 scaledDefaultDirection =
896 new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
897 // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
898 // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
899 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
900
901 // Scale by efficiency and timescale
902 deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
903
904 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}",
905 Prim.LocalID, preferredAxisOfMotion, deflection);
906 // This deflection computation is not correct.
907 deflection = Vector3.Zero;
908 } 1231 }
909 1232 else
910 #endregion
911
912 #region Banking
913
914 if (m_bankingEfficiency != 0)
915 { 1233 {
916 Vector3 dir = Vector3.One * Prim.ForceOrientation; 1234 VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", Prim.LocalID, VehicleRotationalVelocity);
917 float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
918 //Changes which way it banks in and out of turns
919
920 //Use the square of the efficiency, as it looks much more how SL banking works
921 float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
922 if (m_bankingEfficiency < 0)
923 effSquared *= -1; //Keep the negative!
924
925 float mix = Math.Abs(m_bankingMix);
926 if (m_angularMotorVelocity.X == 0)
927 {
928 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
929 {
930 Vector3 axisAngle;
931 float angle;
932 parent.Orientation.GetAxisAngle(out axisAngle, out angle);
933 Vector3 rotatedVel = parent.Velocity * parent.Orientation;
934 if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
935 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
936 else
937 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
938 }*/
939 }
940 else
941 banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
942 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
943 //If they are colliding, we probably shouldn't shove the prim around... probably
944 {
945 float angVelZ = m_angularMotorVelocity.X*-1;
946 /*if(angVelZ > mix)
947 angVelZ = mix;
948 else if(angVelZ < -mix)
949 angVelZ = -mix;*/
950 //This controls how fast and how far the banking occurs
951 Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
952 if (bankingRot.X > 3)
953 bankingRot.X = 3;
954 else if (bankingRot.X < -3)
955 bankingRot.X = -3;
956 bankingRot *= Prim.ForceOrientation;
957 banking += bankingRot;
958 }
959 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
960 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}",
961 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
962 } 1235 }
963 1236
964 #endregion 1237 // ==================================================================
965
966 m_lastVertAttractor = vertattr;
967
968 // Sum velocities
969 m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
970
971 #region Linear Motor Offset
972
973 //Offset section 1238 //Offset section
974 if (m_linearMotorOffset != Vector3.Zero) 1239 if (m_linearMotorOffset != Vector3.Zero)
975 { 1240 {
@@ -985,8 +1250,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
985 // 1250 //
986 // The torque created is the linear velocity crossed with the offset 1251 // The torque created is the linear velocity crossed with the offset
987 1252
988 // NOTE: this computation does should be in the linear section 1253 // TODO: this computation should be in the linear section
989 // because there we know the impulse being applied. 1254 // because that is where we know the impulse being applied.
990 Vector3 torqueFromOffset = Vector3.Zero; 1255 Vector3 torqueFromOffset = Vector3.Zero;
991 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); 1256 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
992 if (float.IsNaN(torqueFromOffset.X)) 1257 if (float.IsNaN(torqueFromOffset.X))
@@ -995,47 +1260,258 @@ namespace OpenSim.Region.Physics.BulletSPlugin
995 torqueFromOffset.Y = 0; 1260 torqueFromOffset.Y = 0;
996 if (float.IsNaN(torqueFromOffset.Z)) 1261 if (float.IsNaN(torqueFromOffset.Z))
997 torqueFromOffset.Z = 0; 1262 torqueFromOffset.Z = 0;
998 torqueFromOffset *= m_vehicleMass; 1263
999 Prim.ApplyTorqueImpulse(torqueFromOffset, true); 1264 VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
1000 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); 1265 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
1001 } 1266 }
1002 1267
1003 #endregion 1268 }
1269
1270 private void ComputeAngularTurning(float pTimestep)
1271 {
1272 // The user wants this many radians per second angular change?
1273 Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
1274 Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
1275
1276 // ==================================================================
1277 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1278 // This flag prevents linear deflection parallel to world z-axis. This is useful
1279 // for preventing ground vehicles with large linear deflection, like bumper cars,
1280 // from climbing their linear deflection into the sky.
1281 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1282 // TODO: This is here because this is where ODE put it but documentation says it
1283 // is a linear effect. Where should this check go?
1284 //if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1285 // {
1286 // angularMotorContributionV.X = 0f;
1287 // angularMotorContributionV.Y = 0f;
1288 // }
1289
1290 VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation;
1291 VDetailLog("{0}, MoveAngular,angularTurning,angularMotorContrib={1}", Prim.LocalID, angularMotorContributionV);
1292 }
1293
1294 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1295 // Some vehicles, like boats, should always keep their up-side up. This can be done by
1296 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
1297 // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
1298 // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
1299 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
1300 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
1301 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
1302 public void ComputeAngularVerticalAttraction()
1303 {
1004 1304
1005 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) 1305 // If vertical attaction timescale is reasonable
1306 if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1006 { 1307 {
1007 m_lastAngularVelocity.X = 0; 1308 // Possible solution derived from a discussion at:
1008 m_lastAngularVelocity.Y = 0; 1309 // http://stackoverflow.com/questions/14939657/computing-vector-from-quaternion-works-computing-quaternion-from-vector-does-no
1009 VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); 1310
1311 // Create a rotation that is only the vehicle's rotation around Z
1312 Vector3 currentEuler = Vector3.Zero;
1313 VehicleOrientation.GetEulerAngles(out currentEuler.X, out currentEuler.Y, out currentEuler.Z);
1314 Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEuler.Z);
1315
1316 // Create the axis that is perpendicular to the up vector and the rotated up vector.
1317 Vector3 differenceAxis = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleOrientation);
1318 // Compute the angle between those to vectors.
1319 double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleOrientation)));
1320 // 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
1321
1322 // Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
1323 // TODO: add 'efficiency'.
1324 differenceAngle /= m_verticalAttractionTimescale;
1325
1326 // Create the quaterian representing the correction angle
1327 Quaternion correctionRotation = Quaternion.CreateFromAxisAngle(differenceAxis, (float)differenceAngle);
1328
1329 // Turn that quaternion into Euler values to make it into velocities to apply.
1330 Vector3 vertContributionV = Vector3.Zero;
1331 correctionRotation.GetEulerAngles(out vertContributionV.X, out vertContributionV.Y, out vertContributionV.Z);
1332 vertContributionV *= -1f;
1333
1334 VehicleRotationalVelocity += vertContributionV;
1335
1336 VDetailLog("{0}, MoveAngular,verticalAttraction,diffAxis={1},diffAng={2},corrRot={3},contrib={4}",
1337 Prim.LocalID,
1338 differenceAxis,
1339 differenceAngle,
1340 correctionRotation,
1341 vertContributionV);
1342
1343 // ===================================================================
1344 /*
1345 Vector3 vertContributionV = Vector3.Zero;
1346 Vector3 origRotVelW = VehicleRotationalVelocity; // DEBUG DEBUG
1347
1348 // Take a vector pointing up and convert it from world to vehicle relative coords.
1349 Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleOrientation);
1350
1351 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
1352 // is now:
1353 // leaning to one side: rotated around the X axis with the Y value going
1354 // from zero (nearly straight up) to one (completely to the side)) or
1355 // leaning front-to-back: rotated around the Y axis with the value of X being between
1356 // zero and one.
1357 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
1358
1359 // Y error means needed rotation around X axis and visa versa.
1360 // Since the error goes from zero to one, the asin is the corresponding angle.
1361 vertContributionV.X = (float)Math.Asin(verticalError.Y);
1362 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
1363 vertContributionV.Y = -(float)Math.Asin(verticalError.X);
1364
1365 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
1366 if (verticalError.Z < 0f)
1367 {
1368 vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
1369 // vertContribution.Y -= PIOverFour;
1370 }
1371
1372 // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
1373 // Correction happens over a number of seconds.
1374 Vector3 unscaledContribVerticalErrorV = vertContributionV; // DEBUG DEBUG
1375
1376 // The correction happens over the user's time period
1377 vertContributionV /= m_verticalAttractionTimescale;
1378
1379 // Rotate the vehicle rotation to the world coordinates.
1380 VehicleRotationalVelocity += (vertContributionV * VehicleOrientation);
1381
1382 VDetailLog("{0}, MoveAngular,verticalAttraction,,origRotVW={1},vertError={2},unscaledV={3},eff={4},ts={5},vertContribV={6}",
1383 Prim.LocalID, origRotVelW, verticalError, unscaledContribVerticalErrorV,
1384 m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV);
1385 */
1010 } 1386 }
1387 }
1388
1389 // Angular correction to correct the direction the vehicle is pointing to be
1390 // the direction is should want to be pointing.
1391 // The vehicle is moving in some direction and correct its orientation to it is pointing
1392 // in that direction.
1393 // TODO: implement reference frame.
1394 public void ComputeAngularDeflection()
1395 {
1396 // Since angularMotorUp and angularDeflection are computed independently, they will calculate
1397 // approximately the same X or Y correction. When added together (when contributions are combined)
1398 // this creates an over-correction and then wabbling as the target is overshot.
1399 // TODO: rethink how the different correction computations inter-relate.
1011 1400
1012 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) 1401 if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
1013 { 1402 {
1014 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. 1403 Vector3 deflectContributionV = Vector3.Zero;
1015 Prim.ZeroAngularMotion(true); 1404
1016 VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); 1405 // The direction the vehicle is moving
1406 Vector3 movingDirection = VehicleVelocity;
1407 movingDirection.Normalize();
1408
1409 // If the vehicle is going backward, it is still pointing forward
1410 movingDirection *= Math.Sign(VehicleForwardSpeed);
1411
1412 // The direction the vehicle is pointing
1413 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
1414 pointingDirection.Normalize();
1415
1416 // The difference between what is and what should be.
1417 Vector3 deflectionError = movingDirection - pointingDirection;
1418
1419 // Don't try to correct very large errors (not our job)
1420 // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
1421 // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
1422 // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
1423 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
1424 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
1425 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
1426
1427 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
1428
1429 // Scale the correction by recovery timescale and efficiency
1430 deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
1431 deflectContributionV /= m_angularDeflectionTimescale;
1432
1433 VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
1434
1435 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
1436 Prim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
1437 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
1438 Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
1017 } 1439 }
1018 else 1440 }
1441
1442 // Angular change to rotate the vehicle around the Z axis when the vehicle
1443 // is tipped around the X axis.
1444 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1445 // The vertical attractor feature must be enabled in order for the banking behavior to
1446 // function. The way banking works is this: a rotation around the vehicle's roll-axis will
1447 // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
1448 // of the yaw effect will be proportional to the
1449 // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
1450 // velocity along its preferred axis of motion.
1451 // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
1452 // positive rotation (by the right-hand rule) about the roll-axis will effect a
1453 // (negative) torque around the yaw-axis, making it turn to the right--that is the
1454 // vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
1455 // Negating the banking coefficient will make it so that the vehicle leans to the
1456 // outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
1457 // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
1458 // banking vehicles do what you want rather than what the laws of physics allow.
1459 // For example, consider a real motorcycle...it must be moving forward in order for
1460 // it to turn while banking, however video-game motorcycles are often configured
1461 // to turn in place when at a dead stop--because they are often easier to control
1462 // that way using the limited interface of the keyboard or game controller. The
1463 // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
1464 // banking by functioning as a slider between a banking that is correspondingly
1465 // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
1466 // banking effect depends only on the vehicle's rotation about its roll-axis compared
1467 // to "dynamic" where the banking is also proportional to its velocity along its
1468 // roll-axis. Finding the best value of the "mixture" will probably require trial and error.
1469 // The time it takes for the banking behavior to defeat a preexisting angular velocity about the
1470 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
1471 // bank quickly then give it a banking timescale of about a second or less, otherwise you can
1472 // make a sluggish vehicle by giving it a timescale of several seconds.
1473 public void ComputeAngularBanking()
1474 {
1475 if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1019 { 1476 {
1020 // Apply to the body. 1477 Vector3 bankingContributionV = Vector3.Zero;
1021 // The above calculates the absolute angular velocity needed. Angular velocity is massless. 1478
1022 // Since we are stuffing the angular velocity directly into the object, the computed 1479 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
1023 // velocity needs to be scaled by the timestep. 1480 // As the vehicle rolls to the right or left, the Y value will increase from
1024 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity); 1481 // zero (straight up) to 1 or -1 (full tilt right or left)
1025 Prim.ForceRotationalVelocity = applyAngularForce; 1482 Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
1026 1483
1027 // Decay the angular movement for next time 1484 // Figure out the yaw value for this much roll.
1028 Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep; 1485 float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
1029 m_lastAngularVelocity *= Vector3.One - decayamount; 1486 // actual error = static turn error + dynamic turn error
1030 1487 float mixedYawAngle =(yawAngle * (1f - m_bankingMix)) + ((yawAngle * m_bankingMix) * VehicleForwardSpeed);
1031 VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 1488
1032 Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity); 1489 // TODO: the banking effect should not go to infinity but what to limit it to?
1490 // And what should happen when this is being added to a user defined yaw that is already PI*4?
1491 mixedYawAngle = ClampInRange(-12, mixedYawAngle, 12);
1492
1493 // Build the force vector to change rotation from what it is to what it should be
1494 bankingContributionV.Z = -mixedYawAngle;
1495
1496 // Don't do it all at once. Fudge because 1 second is too fast with most user defined roll as PI*4.
1497 bankingContributionV /= m_bankingTimescale * BSParam.VehicleAngularBankingTimescaleFudge;
1498
1499 //VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
1500 VehicleRotationalVelocity += bankingContributionV;
1501
1502
1503 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
1504 Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
1033 } 1505 }
1034 } //end MoveAngular 1506 }
1035 1507
1508 // This is from previous instantiations of XXXDynamics.cs.
1509 // Applies roll reference frame.
1510 // TODO: is this the right way to separate the code to do this operation?
1511 // Should this be in MoveAngular()?
1036 internal void LimitRotation(float timestep) 1512 internal void LimitRotation(float timestep)
1037 { 1513 {
1038 Quaternion rotq = Prim.ForceOrientation; 1514 Quaternion rotq = VehicleOrientation;
1039 Quaternion m_rot = rotq; 1515 Quaternion m_rot = rotq;
1040 if (m_RollreferenceFrame != Quaternion.Identity) 1516 if (m_RollreferenceFrame != Quaternion.Identity)
1041 { 1517 {
@@ -1063,12 +1539,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1063 } 1539 }
1064 if (rotq != m_rot) 1540 if (rotq != m_rot)
1065 { 1541 {
1066 Prim.ForceOrientation = m_rot; 1542 VehicleOrientation = m_rot;
1067 VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot); 1543 VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
1068 } 1544 }
1069 1545
1070 } 1546 }
1071 1547
1548 private float ClampInRange(float low, float val, float high)
1549 {
1550 return Math.Max(low, Math.Min(val, high));
1551 // return Utils.Clamp(val, low, high);
1552 }
1553
1072 // Invoke the detailed logger and output something if it's enabled. 1554 // Invoke the detailed logger and output something if it's enabled.
1073 private void VDetailLog(string msg, params Object[] args) 1555 private void VDetailLog(string msg, params Object[] args)
1074 { 1556 {
generated by cgit v1.1 at 2024-10-20 19:41:16 +0000