aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs1800
1 files changed, 1800 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
new file mode 100644
index 0000000..c6d6331
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
@@ -0,0 +1,1800 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
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.
26 *
27 * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
28 * are Copyright (c) 2009 Linden Research, Inc and are used under their license
29 * of Creative Commons Attribution-Share Alike 3.0
30 * (http://creativecommons.org/licenses/by-sa/3.0/).
31 */
32
33using System;
34using System.Collections.Generic;
35using System.Reflection;
36using System.Runtime.InteropServices;
37using OpenMetaverse;
38using OpenSim.Framework;
39using OpenSim.Region.Physics.Manager;
40
41namespace OpenSim.Region.Physics.BulletSPlugin
42{
43 public sealed class BSDynamics : BSActor
44 {
45#pragma warning disable 414
46 private static string LogHeader = "[BULLETSIM VEHICLE]";
47#pragma warning restore 414
48
49 // the prim this dynamic controller belongs to
50 private BSPrimLinkable ControllingPrim { get; set; }
51
52 private bool m_haveRegisteredForSceneEvents;
53
54 // mass of the vehicle fetched each time we're calles
55 private float m_vehicleMass;
56
57 // Vehicle properties
58 public Vehicle Type { get; set; }
59
60 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
61 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
62 // HOVER_TERRAIN_ONLY
63 // HOVER_GLOBAL_HEIGHT
64 // NO_DEFLECTION_UP
65 // HOVER_WATER_ONLY
66 // HOVER_UP_ONLY
67 // LIMIT_MOTOR_UP
68 // LIMIT_ROLL_ONLY
69 private Vector3 m_BlockingEndPoint = Vector3.Zero;
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity;
71 private Quaternion m_referenceFrame = Quaternion.Identity;
72
73 // Linear properties
74 private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
75 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
76 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
77 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
78 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
79 private float m_linearMotorDecayTimescale = 1;
80 private float m_linearMotorTimescale = 1;
81 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
82 private Vector3 m_lastPositionVector = Vector3.Zero;
83 // private bool m_LinearMotorSetLastFrame = false;
84 // private Vector3 m_linearMotorOffset = Vector3.Zero;
85
86 //Angular properties
87 private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
88 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
89 // private int m_angularMotorApply = 0; // application frame counter
90 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
91 private float m_angularMotorTimescale = 1; // motor angular velocity ramp up rate
92 private float m_angularMotorDecayTimescale = 1; // motor angular velocity decay rate
93 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
94 private Vector3 m_lastAngularVelocity = Vector3.Zero;
95 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
96
97 //Deflection properties
98 private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
99 private float m_angularDeflectionEfficiency = 0;
100 private float m_angularDeflectionTimescale = 0;
101 private float m_linearDeflectionEfficiency = 0;
102 private float m_linearDeflectionTimescale = 0;
103
104 //Banking properties
105 private float m_bankingEfficiency = 0;
106 private float m_bankingMix = 1;
107 private float m_bankingTimescale = 0;
108
109 //Hover and Buoyancy properties
110 private BSVMotor m_hoverMotor = new BSVMotor("Hover");
111 private float m_VhoverHeight = 0f;
112 private float m_VhoverEfficiency = 0f;
113 private float m_VhoverTimescale = 0f;
114 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
115 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
116 private float m_VehicleBuoyancy = 0f;
117 private Vector3 m_VehicleGravity = Vector3.Zero; // Gravity computed when buoyancy set
118
119 //Attractor properties
120 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
121 private float m_verticalAttractionEfficiency = 1.0f; // damped
122 private float m_verticalAttractionCutoff = 500f; // per the documentation
123 // Timescale > cutoff means no vert attractor.
124 private float m_verticalAttractionTimescale = 510f;
125
126 // Just some recomputed constants:
127#pragma warning disable 414
128 static readonly float TwoPI = ((float)Math.PI) * 2f;
129 static readonly float FourPI = ((float)Math.PI) * 4f;
130 static readonly float PIOverFour = ((float)Math.PI) / 4f;
131 static readonly float PIOverTwo = ((float)Math.PI) / 2f;
132#pragma warning restore 414
133
134 public BSDynamics(BSScene myScene, BSPrim myPrim, string actorName)
135 : base(myScene, myPrim, actorName)
136 {
137 Type = Vehicle.TYPE_NONE;
138 m_haveRegisteredForSceneEvents = false;
139
140 ControllingPrim = myPrim as BSPrimLinkable;
141 if (ControllingPrim == null)
142 {
143 // THIS CANNOT HAPPEN!!
144 }
145 VDetailLog("{0},Creation", ControllingPrim.LocalID);
146 }
147
148 // Return 'true' if this vehicle is doing vehicle things
149 public bool IsActive
150 {
151 get { return (Type != Vehicle.TYPE_NONE && ControllingPrim.IsPhysicallyActive); }
152 }
153
154 // Return 'true' if this a vehicle that should be sitting on the ground
155 public bool IsGroundVehicle
156 {
157 get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); }
158 }
159
160 #region Vehicle parameter setting
161 public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
162 {
163 VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
164 float clampTemp;
165
166 switch (pParam)
167 {
168 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
169 m_angularDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
170 break;
171 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
172 m_angularDeflectionTimescale = ClampInRange(0.25f, pValue, 120);
173 break;
174 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
175 m_angularMotorDecayTimescale = ClampInRange(0.25f, pValue, 120);
176 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
177 break;
178 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
179 m_angularMotorTimescale = ClampInRange(0.25f, pValue, 120);
180 m_angularMotor.TimeScale = m_angularMotorTimescale;
181 break;
182 case Vehicle.BANKING_EFFICIENCY:
183 m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
184 break;
185 case Vehicle.BANKING_MIX:
186 m_bankingMix = ClampInRange(0.01f, pValue, 1);
187 break;
188 case Vehicle.BANKING_TIMESCALE:
189 m_bankingTimescale = ClampInRange(0.25f, pValue, 120);
190 break;
191 case Vehicle.BUOYANCY:
192 m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
193 m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
194 break;
195 case Vehicle.HOVER_EFFICIENCY:
196 m_VhoverEfficiency = ClampInRange(0.01f, pValue, 1f);
197 break;
198 case Vehicle.HOVER_HEIGHT:
199 m_VhoverHeight = ClampInRange(0f, pValue, 1000000f);
200 break;
201 case Vehicle.HOVER_TIMESCALE:
202 m_VhoverTimescale = ClampInRange(0.01f, pValue, 120);
203 break;
204 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
205 m_linearDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
206 break;
207 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
208 m_linearDeflectionTimescale = ClampInRange(0.01f, pValue, 120);
209 break;
210 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
211 m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
212 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
213 break;
214 case Vehicle.LINEAR_MOTOR_TIMESCALE:
215 m_linearMotorTimescale = ClampInRange(0.01f, pValue, 120);
216 m_linearMotor.TimeScale = m_linearMotorTimescale;
217 break;
218 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
219 m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
220 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
221 break;
222 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
223 m_verticalAttractionTimescale = ClampInRange(0.01f, pValue, 120);
224 m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
225 break;
226
227 // These are vector properties but the engine lets you use a single float value to
228 // set all of the components to the same value
229 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
230 clampTemp = ClampInRange(0.01f, pValue, 120);
231 m_angularFrictionTimescale = new Vector3(clampTemp, clampTemp, clampTemp);
232 break;
233 case Vehicle.ANGULAR_MOTOR_DIRECTION:
234 clampTemp = ClampInRange(-TwoPI, pValue, TwoPI);
235 m_angularMotorDirection = new Vector3(clampTemp, clampTemp, clampTemp);
236 m_angularMotor.Zero();
237 m_angularMotor.SetTarget(m_angularMotorDirection);
238 break;
239 case Vehicle.LINEAR_FRICTION_TIMESCALE:
240 clampTemp = ClampInRange(0.01f, pValue, 120);
241 m_linearFrictionTimescale = new Vector3(clampTemp, clampTemp, clampTemp);
242 break;
243 case Vehicle.LINEAR_MOTOR_DIRECTION:
244 clampTemp = ClampInRange(-BSParam.MaxLinearVelocity, pValue, BSParam.MaxLinearVelocity);
245 m_linearMotorDirection = new Vector3(clampTemp, clampTemp, clampTemp);
246 m_linearMotorDirectionLASTSET = new Vector3(clampTemp, clampTemp, clampTemp);
247 m_linearMotor.SetTarget(m_linearMotorDirection);
248 break;
249 case Vehicle.LINEAR_MOTOR_OFFSET:
250 clampTemp = ClampInRange(-1000, pValue, 1000);
251 m_linearMotorOffset = new Vector3(clampTemp, clampTemp, clampTemp);
252 break;
253
254 }
255 }//end ProcessFloatVehicleParam
256
257 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
258 {
259 VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
260 switch (pParam)
261 {
262 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
263 pValue.X = ClampInRange(0.25f, pValue.X, 120);
264 pValue.Y = ClampInRange(0.25f, pValue.Y, 120);
265 pValue.Z = ClampInRange(0.25f, pValue.Z, 120);
266 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
267 break;
268 case Vehicle.ANGULAR_MOTOR_DIRECTION:
269 // Limit requested angular speed to 2 rps= 4 pi rads/sec
270 pValue.X = ClampInRange(-FourPI, pValue.X, FourPI);
271 pValue.Y = ClampInRange(-FourPI, pValue.Y, FourPI);
272 pValue.Z = ClampInRange(-FourPI, pValue.Z, FourPI);
273 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
274 m_angularMotor.Zero();
275 m_angularMotor.SetTarget(m_angularMotorDirection);
276 break;
277 case Vehicle.LINEAR_FRICTION_TIMESCALE:
278 pValue.X = ClampInRange(0.25f, pValue.X, 120);
279 pValue.Y = ClampInRange(0.25f, pValue.Y, 120);
280 pValue.Z = ClampInRange(0.25f, pValue.Z, 120);
281 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
282 break;
283 case Vehicle.LINEAR_MOTOR_DIRECTION:
284 pValue.X = ClampInRange(-BSParam.MaxLinearVelocity, pValue.X, BSParam.MaxLinearVelocity);
285 pValue.Y = ClampInRange(-BSParam.MaxLinearVelocity, pValue.Y, BSParam.MaxLinearVelocity);
286 pValue.Z = ClampInRange(-BSParam.MaxLinearVelocity, pValue.Z, BSParam.MaxLinearVelocity);
287 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
288 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
289 m_linearMotor.SetTarget(m_linearMotorDirection);
290 break;
291 case Vehicle.LINEAR_MOTOR_OFFSET:
292 // Not sure the correct range to limit this variable
293 pValue.X = ClampInRange(-1000, pValue.X, 1000);
294 pValue.Y = ClampInRange(-1000, pValue.Y, 1000);
295 pValue.Z = ClampInRange(-1000, pValue.Z, 1000);
296 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
297 break;
298 case Vehicle.BLOCK_EXIT:
299 // Not sure the correct range to limit this variable
300 pValue.X = ClampInRange(-10000, pValue.X, 10000);
301 pValue.Y = ClampInRange(-10000, pValue.Y, 10000);
302 pValue.Z = ClampInRange(-10000, pValue.Z, 10000);
303 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
304 break;
305 }
306 }//end ProcessVectorVehicleParam
307
308 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
309 {
310 VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
311 switch (pParam)
312 {
313 case Vehicle.REFERENCE_FRAME:
314 m_referenceFrame = pValue;
315 break;
316 case Vehicle.ROLL_FRAME:
317 m_RollreferenceFrame = pValue;
318 break;
319 }
320 }//end ProcessRotationVehicleParam
321
322 internal void ProcessVehicleFlags(int pParam, bool remove)
323 {
324 VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", ControllingPrim.LocalID, pParam, remove);
325 VehicleFlag parm = (VehicleFlag)pParam;
326 if (pParam == -1)
327 m_flags = (VehicleFlag)0;
328 else
329 {
330 if (remove)
331 m_flags &= ~parm;
332 else
333 m_flags |= parm;
334 }
335 }
336
337 public void ProcessTypeChange(Vehicle pType)
338 {
339 VDetailLog("{0},ProcessTypeChange,type={1}", ControllingPrim.LocalID, pType);
340 // Set Defaults For Type
341 Type = pType;
342 switch (pType)
343 {
344 case Vehicle.TYPE_NONE:
345 m_linearMotorDirection = Vector3.Zero;
346 m_linearMotorTimescale = 0;
347 m_linearMotorDecayTimescale = 0;
348 m_linearFrictionTimescale = new Vector3(0, 0, 0);
349
350 m_angularMotorDirection = Vector3.Zero;
351 m_angularMotorDecayTimescale = 0;
352 m_angularMotorTimescale = 0;
353 m_angularFrictionTimescale = new Vector3(0, 0, 0);
354
355 m_VhoverHeight = 0;
356 m_VhoverEfficiency = 0;
357 m_VhoverTimescale = 0;
358 m_VehicleBuoyancy = 0;
359
360 m_linearDeflectionEfficiency = 1;
361 m_linearDeflectionTimescale = 1;
362
363 m_angularDeflectionEfficiency = 0;
364 m_angularDeflectionTimescale = 1000;
365
366 m_verticalAttractionEfficiency = 0;
367 m_verticalAttractionTimescale = 0;
368
369 m_bankingEfficiency = 0;
370 m_bankingTimescale = 1000;
371 m_bankingMix = 1;
372
373 m_referenceFrame = Quaternion.Identity;
374 m_flags = (VehicleFlag)0;
375
376 break;
377
378 case Vehicle.TYPE_SLED:
379 m_linearMotorDirection = Vector3.Zero;
380 m_linearMotorTimescale = 1000;
381 m_linearMotorDecayTimescale = 120;
382 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
383
384 m_angularMotorDirection = Vector3.Zero;
385 m_angularMotorTimescale = 1000;
386 m_angularMotorDecayTimescale = 120;
387 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
388
389 m_VhoverHeight = 0;
390 m_VhoverEfficiency = 10; // TODO: this looks wrong!!
391 m_VhoverTimescale = 10;
392 m_VehicleBuoyancy = 0;
393
394 m_linearDeflectionEfficiency = 1;
395 m_linearDeflectionTimescale = 1;
396
397 m_angularDeflectionEfficiency = 1;
398 m_angularDeflectionTimescale = 1000;
399
400 m_verticalAttractionEfficiency = 0;
401 m_verticalAttractionTimescale = 0;
402
403 m_bankingEfficiency = 0;
404 m_bankingTimescale = 10;
405 m_bankingMix = 1;
406
407 m_referenceFrame = Quaternion.Identity;
408 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
409 | VehicleFlag.HOVER_TERRAIN_ONLY
410 | VehicleFlag.HOVER_GLOBAL_HEIGHT
411 | VehicleFlag.HOVER_UP_ONLY);
412 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
413 | VehicleFlag.LIMIT_ROLL_ONLY
414 | VehicleFlag.LIMIT_MOTOR_UP);
415
416 break;
417 case Vehicle.TYPE_CAR:
418 m_linearMotorDirection = Vector3.Zero;
419 m_linearMotorTimescale = 1;
420 m_linearMotorDecayTimescale = 60;
421 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
422
423 m_angularMotorDirection = Vector3.Zero;
424 m_angularMotorTimescale = 1;
425 m_angularMotorDecayTimescale = 0.8f;
426 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
427
428 m_VhoverHeight = 0;
429 m_VhoverEfficiency = 0;
430 m_VhoverTimescale = 1000;
431 m_VehicleBuoyancy = 0;
432
433 m_linearDeflectionEfficiency = 1;
434 m_linearDeflectionTimescale = 2;
435
436 m_angularDeflectionEfficiency = 0;
437 m_angularDeflectionTimescale = 10;
438
439 m_verticalAttractionEfficiency = 1f;
440 m_verticalAttractionTimescale = 10f;
441
442 m_bankingEfficiency = -0.2f;
443 m_bankingMix = 1;
444 m_bankingTimescale = 1;
445
446 m_referenceFrame = Quaternion.Identity;
447 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
448 | VehicleFlag.HOVER_TERRAIN_ONLY
449 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
450 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
451 | VehicleFlag.LIMIT_ROLL_ONLY
452 | VehicleFlag.LIMIT_MOTOR_UP
453 | VehicleFlag.HOVER_UP_ONLY);
454 break;
455 case Vehicle.TYPE_BOAT:
456 m_linearMotorDirection = Vector3.Zero;
457 m_linearMotorTimescale = 5;
458 m_linearMotorDecayTimescale = 60;
459 m_linearFrictionTimescale = new Vector3(10, 3, 2);
460
461 m_angularMotorDirection = Vector3.Zero;
462 m_angularMotorTimescale = 4;
463 m_angularMotorDecayTimescale = 4;
464 m_angularFrictionTimescale = new Vector3(10,10,10);
465
466 m_VhoverHeight = 0;
467 m_VhoverEfficiency = 0.5f;
468 m_VhoverTimescale = 2;
469 m_VehicleBuoyancy = 1;
470
471 m_linearDeflectionEfficiency = 0.5f;
472 m_linearDeflectionTimescale = 3;
473
474 m_angularDeflectionEfficiency = 0.5f;
475 m_angularDeflectionTimescale = 5;
476
477 m_verticalAttractionEfficiency = 0.5f;
478 m_verticalAttractionTimescale = 5f;
479
480 m_bankingEfficiency = -0.3f;
481 m_bankingMix = 0.8f;
482 m_bankingTimescale = 1;
483
484 m_referenceFrame = Quaternion.Identity;
485 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
486 | VehicleFlag.HOVER_GLOBAL_HEIGHT
487 | VehicleFlag.LIMIT_ROLL_ONLY
488 | VehicleFlag.HOVER_UP_ONLY);
489 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
490 | VehicleFlag.LIMIT_MOTOR_UP
491 | VehicleFlag.HOVER_WATER_ONLY);
492 break;
493 case Vehicle.TYPE_AIRPLANE:
494 m_linearMotorDirection = Vector3.Zero;
495 m_linearMotorTimescale = 2;
496 m_linearMotorDecayTimescale = 60;
497 m_linearFrictionTimescale = new Vector3(200, 10, 5);
498
499 m_angularMotorDirection = Vector3.Zero;
500 m_angularMotorTimescale = 4;
501 m_angularMotorDecayTimescale = 4;
502 m_angularFrictionTimescale = new Vector3(20, 20, 20);
503
504 m_VhoverHeight = 0;
505 m_VhoverEfficiency = 0.5f;
506 m_VhoverTimescale = 1000;
507 m_VehicleBuoyancy = 0;
508
509 m_linearDeflectionEfficiency = 0.5f;
510 m_linearDeflectionTimescale = 3;
511
512 m_angularDeflectionEfficiency = 1;
513 m_angularDeflectionTimescale = 2;
514
515 m_verticalAttractionEfficiency = 0.9f;
516 m_verticalAttractionTimescale = 2f;
517
518 m_bankingEfficiency = 1;
519 m_bankingMix = 0.7f;
520 m_bankingTimescale = 2;
521
522 m_referenceFrame = Quaternion.Identity;
523 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
524 | VehicleFlag.HOVER_TERRAIN_ONLY
525 | VehicleFlag.HOVER_GLOBAL_HEIGHT
526 | VehicleFlag.HOVER_UP_ONLY
527 | VehicleFlag.NO_DEFLECTION_UP
528 | VehicleFlag.LIMIT_MOTOR_UP);
529 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
530 break;
531 case Vehicle.TYPE_BALLOON:
532 m_linearMotorDirection = Vector3.Zero;
533 m_linearMotorTimescale = 5;
534 m_linearFrictionTimescale = new Vector3(5, 5, 5);
535 m_linearMotorDecayTimescale = 60;
536
537 m_angularMotorDirection = Vector3.Zero;
538 m_angularMotorTimescale = 6;
539 m_angularFrictionTimescale = new Vector3(10, 10, 10);
540 m_angularMotorDecayTimescale = 10;
541
542 m_VhoverHeight = 5;
543 m_VhoverEfficiency = 0.8f;
544 m_VhoverTimescale = 10;
545 m_VehicleBuoyancy = 1;
546
547 m_linearDeflectionEfficiency = 0;
548 m_linearDeflectionTimescale = 5;
549
550 m_angularDeflectionEfficiency = 0;
551 m_angularDeflectionTimescale = 5;
552
553 m_verticalAttractionEfficiency = 1f;
554 m_verticalAttractionTimescale = 100f;
555
556 m_bankingEfficiency = 0;
557 m_bankingMix = 0.7f;
558 m_bankingTimescale = 5;
559
560 m_referenceFrame = Quaternion.Identity;
561
562 m_referenceFrame = Quaternion.Identity;
563 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
564 | VehicleFlag.HOVER_TERRAIN_ONLY
565 | VehicleFlag.HOVER_UP_ONLY
566 | VehicleFlag.NO_DEFLECTION_UP
567 | VehicleFlag.LIMIT_MOTOR_UP);
568 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
569 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
570 break;
571 }
572
573 m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, m_linearMotorDecayTimescale, 1f);
574 // m_linearMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
575
576 m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, m_angularMotorDecayTimescale, 1f);
577 // m_angularMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
578
579 /* Not implemented
580 m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
581 BSMotor.Infinite, BSMotor.InfiniteVector,
582 m_verticalAttractionEfficiency);
583 // Z goes away and we keep X and Y
584 m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
585 */
586
587 if (this.Type == Vehicle.TYPE_NONE)
588 {
589 UnregisterForSceneEvents();
590 }
591 else
592 {
593 RegisterForSceneEvents();
594 }
595
596 // Update any physical parameters based on this type.
597 Refresh();
598 }
599 #endregion // Vehicle parameter setting
600
601 // BSActor.Refresh()
602 public override void Refresh()
603 {
604 // If asking for a refresh, reset the physical parameters before the next simulation step.
605 // Called whether active or not since the active state may be updated before the next step.
606 m_physicsScene.PostTaintObject("BSDynamics.Refresh", ControllingPrim.LocalID, delegate()
607 {
608 SetPhysicalParameters();
609 });
610 }
611
612 // Some of the properties of this prim may have changed.
613 // Do any updating needed for a vehicle
614 private void SetPhysicalParameters()
615 {
616 if (IsActive)
617 {
618 // Remember the mass so we don't have to fetch it every step
619 m_vehicleMass = ControllingPrim.TotalMass;
620
621 // Friction affects are handled by this vehicle code
622 // m_physicsScene.PE.SetFriction(ControllingPrim.PhysBody, BSParam.VehicleFriction);
623 // m_physicsScene.PE.SetRestitution(ControllingPrim.PhysBody, BSParam.VehicleRestitution);
624 ControllingPrim.Linkset.SetPhysicalFriction(BSParam.VehicleFriction);
625 ControllingPrim.Linkset.SetPhysicalRestitution(BSParam.VehicleRestitution);
626
627 // Moderate angular movement introduced by Bullet.
628 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
629 // Maybe compute linear and angular factor and damping from params.
630 m_physicsScene.PE.SetAngularDamping(ControllingPrim.PhysBody, BSParam.VehicleAngularDamping);
631 m_physicsScene.PE.SetLinearFactor(ControllingPrim.PhysBody, BSParam.VehicleLinearFactor);
632 m_physicsScene.PE.SetAngularFactorV(ControllingPrim.PhysBody, BSParam.VehicleAngularFactor);
633
634 // Vehicles report collision events so we know when it's on the ground
635 // m_physicsScene.PE.AddToCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
636 ControllingPrim.Linkset.AddToPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
637
638 // Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(ControllingPrim.PhysShape.physShapeInfo, m_vehicleMass);
639 // ControllingPrim.Inertia = inertia * BSParam.VehicleInertiaFactor;
640 // m_physicsScene.PE.SetMassProps(ControllingPrim.PhysBody, m_vehicleMass, ControllingPrim.Inertia);
641 // m_physicsScene.PE.UpdateInertiaTensor(ControllingPrim.PhysBody);
642 ControllingPrim.Linkset.ComputeAndSetLocalInertia(BSParam.VehicleInertiaFactor, m_vehicleMass);
643
644 // Set the gravity for the vehicle depending on the buoyancy
645 // TODO: what should be done if prim and vehicle buoyancy differ?
646 m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
647 // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
648 // m_physicsScene.PE.SetGravity(ControllingPrim.PhysBody, Vector3.Zero);
649 ControllingPrim.Linkset.SetPhysicalGravity(Vector3.Zero);
650
651 VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
652 ControllingPrim.LocalID, m_vehicleMass, ControllingPrim.Inertia, m_VehicleGravity,
653 BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
654 BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
655 );
656 }
657 else
658 {
659 if (ControllingPrim.PhysBody.HasPhysicalBody)
660 m_physicsScene.PE.RemoveFromCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
661 // ControllingPrim.Linkset.RemoveFromPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
662 }
663 }
664
665 // BSActor.RemoveBodyDependencies
666 public override void RemoveDependencies()
667 {
668 Refresh();
669 }
670
671 // BSActor.Release()
672 public override void Dispose()
673 {
674 VDetailLog("{0},Dispose", ControllingPrim.LocalID);
675 UnregisterForSceneEvents();
676 Type = Vehicle.TYPE_NONE;
677 Enabled = false;
678 return;
679 }
680
681 private void RegisterForSceneEvents()
682 {
683 if (!m_haveRegisteredForSceneEvents)
684 {
685 m_physicsScene.BeforeStep += this.Step;
686 m_physicsScene.AfterStep += this.PostStep;
687 ControllingPrim.OnPreUpdateProperty += this.PreUpdateProperty;
688 m_haveRegisteredForSceneEvents = true;
689 }
690 }
691
692 private void UnregisterForSceneEvents()
693 {
694 if (m_haveRegisteredForSceneEvents)
695 {
696 m_physicsScene.BeforeStep -= this.Step;
697 m_physicsScene.AfterStep -= this.PostStep;
698 ControllingPrim.OnPreUpdateProperty -= this.PreUpdateProperty;
699 m_haveRegisteredForSceneEvents = false;
700 }
701 }
702
703 private void PreUpdateProperty(ref EntityProperties entprop)
704 {
705 // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
706 // TODO: handle physics introduced by Bullet with computed vehicle physics.
707 if (IsActive)
708 {
709 entprop.RotationalVelocity = Vector3.Zero;
710 }
711 }
712
713 #region Known vehicle value functions
714 // Vehicle physical parameters that we buffer from constant getting and setting.
715 // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
716 // Changing is remembered and the parameter is stored back into the physics engine only if updated.
717 // This does two things: 1) saves continuious calls into unmanaged code, and
718 // 2) signals when a physics property update must happen back to the simulator
719 // to update values modified for the vehicle.
720 private int m_knownChanged;
721 private int m_knownHas;
722 private float m_knownTerrainHeight;
723 private float m_knownWaterLevel;
724 private Vector3 m_knownPosition;
725 private Vector3 m_knownVelocity;
726 private Vector3 m_knownForce;
727 private Vector3 m_knownForceImpulse;
728 private Quaternion m_knownOrientation;
729 private Vector3 m_knownRotationalVelocity;
730 private Vector3 m_knownRotationalForce;
731 private Vector3 m_knownRotationalImpulse;
732
733 private const int m_knownChangedPosition = 1 << 0;
734 private const int m_knownChangedVelocity = 1 << 1;
735 private const int m_knownChangedForce = 1 << 2;
736 private const int m_knownChangedForceImpulse = 1 << 3;
737 private const int m_knownChangedOrientation = 1 << 4;
738 private const int m_knownChangedRotationalVelocity = 1 << 5;
739 private const int m_knownChangedRotationalForce = 1 << 6;
740 private const int m_knownChangedRotationalImpulse = 1 << 7;
741 private const int m_knownChangedTerrainHeight = 1 << 8;
742 private const int m_knownChangedWaterLevel = 1 << 9;
743
744 public void ForgetKnownVehicleProperties()
745 {
746 m_knownHas = 0;
747 m_knownChanged = 0;
748 }
749 // Push all the changed values back into the physics engine
750 public void PushKnownChanged()
751 {
752 if (m_knownChanged != 0)
753 {
754 if ((m_knownChanged & m_knownChangedPosition) != 0)
755 ControllingPrim.ForcePosition = m_knownPosition;
756
757 if ((m_knownChanged & m_knownChangedOrientation) != 0)
758 ControllingPrim.ForceOrientation = m_knownOrientation;
759
760 if ((m_knownChanged & m_knownChangedVelocity) != 0)
761 {
762 ControllingPrim.ForceVelocity = m_knownVelocity;
763 // Fake out Bullet by making it think the velocity is the same as last time.
764 // Bullet does a bunch of smoothing for changing parameters.
765 // Since the vehicle is demanding this setting, we override Bullet's smoothing
766 // by telling Bullet the value was the same last time.
767 // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
768 }
769
770 if ((m_knownChanged & m_knownChangedForce) != 0)
771 ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
772
773 if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
774 ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
775
776 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
777 {
778 ControllingPrim.ForceRotationalVelocity = m_knownRotationalVelocity;
779 // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
780 }
781
782 if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
783 ControllingPrim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
784
785 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
786 {
787 ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
788 }
789
790 // If we set one of the values (ie, the physics engine didn't do it) we must force
791 // an UpdateProperties event to send the changes up to the simulator.
792 m_physicsScene.PE.PushUpdate(ControllingPrim.PhysBody);
793 }
794 m_knownChanged = 0;
795 }
796
797 // Since the computation of terrain height can be a little involved, this routine
798 // is used to fetch the height only once for each vehicle simulation step.
799 Vector3 lastRememberedHeightPos = new Vector3(-1, -1, -1);
800 private float GetTerrainHeight(Vector3 pos)
801 {
802 if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
803 {
804 lastRememberedHeightPos = pos;
805 m_knownTerrainHeight = ControllingPrim.PhysScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
806 m_knownHas |= m_knownChangedTerrainHeight;
807 }
808 return m_knownTerrainHeight;
809 }
810
811 // Since the computation of water level can be a little involved, this routine
812 // is used ot fetch the level only once for each vehicle simulation step.
813 Vector3 lastRememberedWaterHeightPos = new Vector3(-1, -1, -1);
814 private float GetWaterLevel(Vector3 pos)
815 {
816 if ((m_knownHas & m_knownChangedWaterLevel) == 0 || pos != lastRememberedWaterHeightPos)
817 {
818 lastRememberedWaterHeightPos = pos;
819 m_knownWaterLevel = ControllingPrim.PhysScene.TerrainManager.GetWaterLevelAtXYZ(pos);
820 m_knownHas |= m_knownChangedWaterLevel;
821 }
822 return m_knownWaterLevel;
823 }
824
825 private Vector3 VehiclePosition
826 {
827 get
828 {
829 if ((m_knownHas & m_knownChangedPosition) == 0)
830 {
831 m_knownPosition = ControllingPrim.ForcePosition;
832 m_knownHas |= m_knownChangedPosition;
833 }
834 return m_knownPosition;
835 }
836 set
837 {
838 m_knownPosition = value;
839 m_knownChanged |= m_knownChangedPosition;
840 m_knownHas |= m_knownChangedPosition;
841 }
842 }
843
844 private Quaternion VehicleOrientation
845 {
846 get
847 {
848 if ((m_knownHas & m_knownChangedOrientation) == 0)
849 {
850 m_knownOrientation = ControllingPrim.ForceOrientation;
851 m_knownHas |= m_knownChangedOrientation;
852 }
853 return m_knownOrientation;
854 }
855 set
856 {
857 m_knownOrientation = value;
858 m_knownChanged |= m_knownChangedOrientation;
859 m_knownHas |= m_knownChangedOrientation;
860 }
861 }
862
863 private Vector3 VehicleVelocity
864 {
865 get
866 {
867 if ((m_knownHas & m_knownChangedVelocity) == 0)
868 {
869 m_knownVelocity = ControllingPrim.ForceVelocity;
870 m_knownHas |= m_knownChangedVelocity;
871 }
872 return m_knownVelocity;
873 }
874 set
875 {
876 m_knownVelocity = value;
877 m_knownChanged |= m_knownChangedVelocity;
878 m_knownHas |= m_knownChangedVelocity;
879 }
880 }
881
882 private void VehicleAddForce(Vector3 pForce)
883 {
884 if ((m_knownHas & m_knownChangedForce) == 0)
885 {
886 m_knownForce = Vector3.Zero;
887 m_knownHas |= m_knownChangedForce;
888 }
889 m_knownForce += pForce;
890 m_knownChanged |= m_knownChangedForce;
891 }
892
893 private void VehicleAddForceImpulse(Vector3 pImpulse)
894 {
895 if ((m_knownHas & m_knownChangedForceImpulse) == 0)
896 {
897 m_knownForceImpulse = Vector3.Zero;
898 m_knownHas |= m_knownChangedForceImpulse;
899 }
900 m_knownForceImpulse += pImpulse;
901 m_knownChanged |= m_knownChangedForceImpulse;
902 }
903
904 private Vector3 VehicleRotationalVelocity
905 {
906 get
907 {
908 if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
909 {
910 m_knownRotationalVelocity = ControllingPrim.ForceRotationalVelocity;
911 m_knownHas |= m_knownChangedRotationalVelocity;
912 }
913 return (Vector3)m_knownRotationalVelocity;
914 }
915 set
916 {
917 m_knownRotationalVelocity = value;
918 m_knownChanged |= m_knownChangedRotationalVelocity;
919 m_knownHas |= m_knownChangedRotationalVelocity;
920 }
921 }
922 private void VehicleAddAngularForce(Vector3 aForce)
923 {
924 if ((m_knownHas & m_knownChangedRotationalForce) == 0)
925 {
926 m_knownRotationalForce = Vector3.Zero;
927 }
928 m_knownRotationalForce += aForce;
929 m_knownChanged |= m_knownChangedRotationalForce;
930 m_knownHas |= m_knownChangedRotationalForce;
931 }
932 private void VehicleAddRotationalImpulse(Vector3 pImpulse)
933 {
934 if ((m_knownHas & m_knownChangedRotationalImpulse) == 0)
935 {
936 m_knownRotationalImpulse = Vector3.Zero;
937 m_knownHas |= m_knownChangedRotationalImpulse;
938 }
939 m_knownRotationalImpulse += pImpulse;
940 m_knownChanged |= m_knownChangedRotationalImpulse;
941 }
942
943 // Vehicle relative forward velocity
944 private Vector3 VehicleForwardVelocity
945 {
946 get
947 {
948 return VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleFrameOrientation));
949 }
950 }
951
952 private float VehicleForwardSpeed
953 {
954 get
955 {
956 return VehicleForwardVelocity.X;
957 }
958 }
959 private Quaternion VehicleFrameOrientation
960 {
961 get
962 {
963 return VehicleOrientation * m_referenceFrame;
964 }
965 }
966
967 #endregion // Known vehicle value functions
968
969 // One step of the vehicle properties for the next 'pTimestep' seconds.
970 internal void Step(float pTimestep)
971 {
972 if (!IsActive) return;
973
974 ForgetKnownVehicleProperties();
975
976 MoveLinear(pTimestep);
977 MoveAngular(pTimestep);
978
979 LimitRotation(pTimestep);
980
981 // remember the position so next step we can limit absolute movement effects
982 m_lastPositionVector = VehiclePosition;
983
984 // If we forced the changing of some vehicle parameters, update the values and
985 // for the physics engine to note the changes so an UpdateProperties event will happen.
986 PushKnownChanged();
987
988 if (m_physicsScene.VehiclePhysicalLoggingEnabled)
989 m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
990
991 VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
992 ControllingPrim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
993 }
994
995 // Called after the simulation step
996 internal void PostStep(float pTimestep)
997 {
998 if (!IsActive) return;
999
1000 if (m_physicsScene.VehiclePhysicalLoggingEnabled)
1001 m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
1002 }
1003
1004 // Apply the effect of the linear motor and other linear motions (like hover and float).
1005 private void MoveLinear(float pTimestep)
1006 {
1007 ComputeLinearVelocity(pTimestep);
1008
1009 ComputeLinearDeflection(pTimestep);
1010
1011 ComputeLinearTerrainHeightCorrection(pTimestep);
1012
1013 ComputeLinearHover(pTimestep);
1014
1015 ComputeLinearBlockingEndPoint(pTimestep);
1016
1017 ComputeLinearMotorUp(pTimestep);
1018
1019 ApplyGravity(pTimestep);
1020
1021 // If not changing some axis, reduce out velocity
1022 if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
1023 {
1024 Vector3 vel = VehicleVelocity;
1025 if ((m_flags & (VehicleFlag.NO_X)) != 0)
1026 {
1027 vel.X = 0;
1028 }
1029 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
1030 {
1031 vel.Y = 0;
1032 }
1033 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
1034 {
1035 vel.Z = 0;
1036 }
1037 VehicleVelocity = vel;
1038 }
1039
1040 // ==================================================================
1041 // Clamp high or low velocities
1042 float newVelocityLengthSq = VehicleVelocity.LengthSquared();
1043 if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
1044 {
1045 Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
1046 VehicleVelocity /= VehicleVelocity.Length();
1047 VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
1048 VDetailLog("{0}, MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}",
1049 ControllingPrim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
1050 }
1051 else if (newVelocityLengthSq < BSParam.VehicleMinLinearVelocitySquared)
1052 {
1053 Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
1054 VDetailLog("{0}, MoveLinear,clampMin,origVelW={1},lenSq={2}",
1055 ControllingPrim.LocalID, origVelW, newVelocityLengthSq);
1056 VehicleVelocity = Vector3.Zero;
1057 }
1058
1059 VDetailLog("{0}, MoveLinear,done,isColl={1},newVel={2}", ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, VehicleVelocity );
1060
1061 } // end MoveLinear()
1062
1063 public void ComputeLinearVelocity(float pTimestep)
1064 {
1065 // Step the motor from the current value. Get the correction needed this step.
1066 Vector3 origVelW = VehicleVelocity; // DEBUG
1067 Vector3 currentVelV = VehicleForwardVelocity;
1068 Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
1069
1070 // Friction reduces vehicle motion based on absolute speed. Slow vehicle down by friction.
1071 Vector3 frictionFactorV = ComputeFrictionFactor(m_linearFrictionTimescale, pTimestep);
1072 linearMotorCorrectionV -= (currentVelV * frictionFactorV);
1073
1074 // Motor is vehicle coordinates. Rotate it to world coordinates
1075 Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleFrameOrientation;
1076
1077 // If we're a ground vehicle, don't add any upward Z movement
1078 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
1079 {
1080 if (linearMotorVelocityW.Z > 0f)
1081 linearMotorVelocityW.Z = 0f;
1082 }
1083
1084 // Add this correction to the velocity to make it faster/slower.
1085 VehicleVelocity += linearMotorVelocityW;
1086
1087 VDetailLog("{0}, MoveLinear,velocity,origVelW={1},velV={2},tgt={3},correctV={4},correctW={5},newVelW={6},fricFact={7}",
1088 ControllingPrim.LocalID, origVelW, currentVelV, m_linearMotor.TargetValue, linearMotorCorrectionV,
1089 linearMotorVelocityW, VehicleVelocity, frictionFactorV);
1090 }
1091
1092 //Given a Deflection Effiency and a Velocity, Returns a Velocity that is Partially Deflected onto the X Axis
1093 //Clamped so that a DeflectionTimescale of less then 1 does not increase force over original velocity
1094 private void ComputeLinearDeflection(float pTimestep)
1095 {
1096 Vector3 linearDeflectionV = Vector3.Zero;
1097 Vector3 velocityV = VehicleForwardVelocity;
1098
1099 if (BSParam.VehicleEnableLinearDeflection)
1100 {
1101 // Velocity in Y and Z dimensions is movement to the side or turning.
1102 // Compute deflection factor from the to the side and rotational velocity
1103 linearDeflectionV.Y = SortedClampInRange(0, (velocityV.Y * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Y);
1104 linearDeflectionV.Z = SortedClampInRange(0, (velocityV.Z * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Z);
1105
1106 // Velocity to the side and around is corrected and moved into the forward direction
1107 linearDeflectionV.X += Math.Abs(linearDeflectionV.Y);
1108 linearDeflectionV.X += Math.Abs(linearDeflectionV.Z);
1109
1110 // Scale the deflection to the fractional simulation time
1111 linearDeflectionV *= pTimestep;
1112
1113 // Subtract the sideways and rotational velocity deflection factors while adding the correction forward
1114 linearDeflectionV *= new Vector3(1, -1, -1);
1115
1116 // Correction is vehicle relative. Convert to world coordinates.
1117 Vector3 linearDeflectionW = linearDeflectionV * VehicleFrameOrientation;
1118
1119 // Optionally, if not colliding, don't effect world downward velocity. Let falling things fall.
1120 if (BSParam.VehicleLinearDeflectionNotCollidingNoZ && !m_controllingPrim.HasSomeCollision)
1121 {
1122 linearDeflectionW.Z = 0f;
1123 }
1124
1125 VehicleVelocity += linearDeflectionW;
1126
1127 VDetailLog("{0}, MoveLinear,LinearDeflection,linDefEff={1},linDefTS={2},linDeflectionV={3}",
1128 ControllingPrim.LocalID, m_linearDeflectionEfficiency, m_linearDeflectionTimescale, linearDeflectionV);
1129 }
1130 }
1131
1132 public void ComputeLinearTerrainHeightCorrection(float pTimestep)
1133 {
1134 // If below the terrain, move us above the ground a little.
1135 // TODO: Consider taking the rotated size of the object or possibly casting a ray.
1136 if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
1137 {
1138 // Force position because applying force won't get the vehicle through the terrain
1139 Vector3 newPosition = VehiclePosition;
1140 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
1141 VehiclePosition = newPosition;
1142 VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
1143 ControllingPrim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
1144 }
1145 }
1146
1147 public void ComputeLinearHover(float pTimestep)
1148 {
1149 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
1150 // m_VhoverTimescale: time to achieve height
1151 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0 && (m_VhoverHeight > 0) && (m_VhoverTimescale < 300))
1152 {
1153 // We should hover, get the target height
1154 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
1155 {
1156 m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
1157 }
1158 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
1159 {
1160 m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
1161 }
1162 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
1163 {
1164 m_VhoverTargetHeight = m_VhoverHeight;
1165 }
1166 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
1167 {
1168 // If body is already heigher, use its height as target height
1169 if (VehiclePosition.Z > m_VhoverTargetHeight)
1170 {
1171 m_VhoverTargetHeight = VehiclePosition.Z;
1172
1173 // A 'misfeature' of this flag is that if the vehicle is above it's hover height,
1174 // the vehicle's buoyancy goes away. This is an SL bug that got used by so many
1175 // scripts that it could not be changed.
1176 // So, if above the height, reapply gravity if buoyancy had it turned off.
1177 if (m_VehicleBuoyancy != 0)
1178 {
1179 Vector3 appliedGravity = ControllingPrim.ComputeGravity(ControllingPrim.Buoyancy) * m_vehicleMass;
1180 VehicleAddForce(appliedGravity);
1181 }
1182 }
1183 }
1184
1185 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
1186 {
1187 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
1188 {
1189 Vector3 pos = VehiclePosition;
1190 pos.Z = m_VhoverTargetHeight;
1191 VehiclePosition = pos;
1192
1193 VDetailLog("{0}, MoveLinear,hover,pos={1},lockHoverHeight", ControllingPrim.LocalID, pos);
1194 }
1195 }
1196 else
1197 {
1198 // Error is positive if below the target and negative if above.
1199 Vector3 hpos = VehiclePosition;
1200 float verticalError = m_VhoverTargetHeight - hpos.Z;
1201 float verticalCorrection = verticalError / m_VhoverTimescale;
1202 verticalCorrection *= m_VhoverEfficiency;
1203
1204 hpos.Z += verticalCorrection;
1205 VehiclePosition = hpos;
1206
1207 // Since we are hovering, we need to do the opposite of falling -- get rid of world Z
1208 Vector3 vel = VehicleVelocity;
1209 vel.Z = 0f;
1210 VehicleVelocity = vel;
1211
1212 /*
1213 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
1214 Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity);
1215 verticalCorrection *= m_vehicleMass;
1216
1217 // TODO: implement m_VhoverEfficiency correctly
1218 VehicleAddForceImpulse(verticalCorrection);
1219 */
1220
1221 VDetailLog("{0}, MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
1222 ControllingPrim.LocalID, VehiclePosition, m_VhoverEfficiency,
1223 m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
1224 verticalError, verticalCorrection);
1225 }
1226 }
1227 }
1228
1229 public bool ComputeLinearBlockingEndPoint(float pTimestep)
1230 {
1231 bool changed = false;
1232
1233 Vector3 pos = VehiclePosition;
1234 Vector3 posChange = pos - m_lastPositionVector;
1235 if (m_BlockingEndPoint != Vector3.Zero)
1236 {
1237 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
1238 {
1239 pos.X -= posChange.X + 1;
1240 changed = true;
1241 }
1242 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
1243 {
1244 pos.Y -= posChange.Y + 1;
1245 changed = true;
1246 }
1247 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
1248 {
1249 pos.Z -= posChange.Z + 1;
1250 changed = true;
1251 }
1252 if (pos.X <= 0)
1253 {
1254 pos.X += posChange.X + 1;
1255 changed = true;
1256 }
1257 if (pos.Y <= 0)
1258 {
1259 pos.Y += posChange.Y + 1;
1260 changed = true;
1261 }
1262 if (changed)
1263 {
1264 VehiclePosition = pos;
1265 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
1266 ControllingPrim.LocalID, m_BlockingEndPoint, posChange, pos);
1267 }
1268 }
1269 return changed;
1270 }
1271
1272 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1273 // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
1274 // used with conjunction with banking: the strength of the banking will decay when the
1275 // vehicle no longer experiences collisions. The decay timescale is the same as
1276 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
1277 // when they are in mid jump.
1278 // TODO: this code is wrong. Also, what should it do for boats (height from water)?
1279 // This is just using the ground and a general collision check. Should really be using
1280 // a downward raycast to find what is below.
1281 public void ComputeLinearMotorUp(float pTimestep)
1282 {
1283 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
1284 {
1285 // This code tries to decide if the object is not on the ground and then pushing down
1286 /*
1287 float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
1288 distanceAboveGround = VehiclePosition.Z - targetHeight;
1289 // Not colliding if the vehicle is off the ground
1290 if (!Prim.HasSomeCollision)
1291 {
1292 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
1293 VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
1294 }
1295 // TODO: this calculation is wrong. From the description at
1296 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
1297 // has a decay factor. This says this force should
1298 // be computed with a motor.
1299 // TODO: add interaction with banking.
1300 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
1301 Prim.LocalID, distanceAboveGround, Prim.HasSomeCollision, ret);
1302 */
1303
1304 // Another approach is to measure if we're going up. If going up and not colliding,
1305 // the vehicle is in the air. Fix that by pushing down.
1306 if (!ControllingPrim.HasSomeCollision && VehicleVelocity.Z > 0.1)
1307 {
1308 // Get rid of any of the velocity vector that is pushing us up.
1309 float upVelocity = VehicleVelocity.Z;
1310 VehicleVelocity += new Vector3(0, 0, -upVelocity);
1311
1312 /*
1313 // If we're pointed up into the air, we should nose down
1314 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
1315 // The rotation around the Y axis is pitch up or down
1316 if (pointingDirection.Y > 0.01f)
1317 {
1318 float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y);
1319 Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f);
1320 // Rotate into world coordinates and apply to vehicle
1321 angularCorrectionVector *= VehicleOrientation;
1322 VehicleAddAngularForce(angularCorrectionVector);
1323 VDetailLog("{0}, MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}",
1324 Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector);
1325 }
1326 */
1327 VDetailLog("{0}, MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}",
1328 ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, upVelocity, VehicleVelocity);
1329 }
1330 }
1331 }
1332
1333 private void ApplyGravity(float pTimeStep)
1334 {
1335 Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
1336
1337 // Hack to reduce downward force if the vehicle is probably sitting on the ground
1338 if (ControllingPrim.HasSomeCollision && IsGroundVehicle)
1339 appliedGravity *= BSParam.VehicleGroundGravityFudge;
1340
1341 VehicleAddForce(appliedGravity);
1342
1343 VDetailLog("{0}, MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={5}",
1344 ControllingPrim.LocalID, m_VehicleGravity,
1345 ControllingPrim.HasSomeCollision, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
1346 }
1347
1348 // =======================================================================
1349 // =======================================================================
1350 // Apply the effect of the angular motor.
1351 // The 'contribution' is how much angular correction velocity each function wants.
1352 // All the contributions are added together and the resulting velocity is
1353 // set directly on the vehicle.
1354 private void MoveAngular(float pTimestep)
1355 {
1356 ComputeAngularTurning(pTimestep);
1357
1358 ComputeAngularVerticalAttraction();
1359
1360 ComputeAngularDeflection();
1361
1362 ComputeAngularBanking();
1363
1364 // ==================================================================
1365 if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
1366 {
1367 // The vehicle is not adding anything angular wise.
1368 VehicleRotationalVelocity = Vector3.Zero;
1369 VDetailLog("{0}, MoveAngular,done,zero", ControllingPrim.LocalID);
1370 }
1371 else
1372 {
1373 VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", ControllingPrim.LocalID, VehicleRotationalVelocity);
1374 }
1375
1376 // ==================================================================
1377 //Offset section
1378 if (m_linearMotorOffset != Vector3.Zero)
1379 {
1380 //Offset of linear velocity doesn't change the linear velocity,
1381 // but causes a torque to be applied, for example...
1382 //
1383 // IIIII >>> IIIII
1384 // IIIII >>> IIIII
1385 // IIIII >>> IIIII
1386 // ^
1387 // | Applying a force at the arrow will cause the object to move forward, but also rotate
1388 //
1389 //
1390 // The torque created is the linear velocity crossed with the offset
1391
1392 // TODO: this computation should be in the linear section
1393 // because that is where we know the impulse being applied.
1394 Vector3 torqueFromOffset = Vector3.Zero;
1395 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
1396 if (float.IsNaN(torqueFromOffset.X))
1397 torqueFromOffset.X = 0;
1398 if (float.IsNaN(torqueFromOffset.Y))
1399 torqueFromOffset.Y = 0;
1400 if (float.IsNaN(torqueFromOffset.Z))
1401 torqueFromOffset.Z = 0;
1402
1403 VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
1404 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", ControllingPrim.LocalID, torqueFromOffset);
1405 }
1406
1407 }
1408
1409 private void ComputeAngularTurning(float pTimestep)
1410 {
1411 // The user wants this many radians per second angular change?
1412 Vector3 origVehicleRotationalVelocity = VehicleRotationalVelocity; // DEBUG DEBUG
1413 Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleFrameOrientation);
1414 Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
1415
1416 // ==================================================================
1417 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1418 // This flag prevents linear deflection parallel to world z-axis. This is useful
1419 // for preventing ground vehicles with large linear deflection, like bumper cars,
1420 // from climbing their linear deflection into the sky.
1421 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1422 // TODO: This is here because this is where ODE put it but documentation says it
1423 // is a linear effect. Where should this check go?
1424 //if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1425 // {
1426 // angularMotorContributionV.X = 0f;
1427 // angularMotorContributionV.Y = 0f;
1428 // }
1429
1430 // Reduce any velocity by friction.
1431 Vector3 frictionFactorW = ComputeFrictionFactor(m_angularFrictionTimescale, pTimestep);
1432 angularMotorContributionV -= (currentAngularV * frictionFactorW);
1433
1434 Vector3 angularMotorContributionW = angularMotorContributionV * VehicleFrameOrientation;
1435 VehicleRotationalVelocity += angularMotorContributionW;
1436
1437 VDetailLog("{0}, MoveAngular,angularTurning,curAngVelV={1},origVehRotVel={2},vehRotVel={3},frictFact={4}, angContribV={5},angContribW={6}",
1438 ControllingPrim.LocalID, currentAngularV, origVehicleRotationalVelocity, VehicleRotationalVelocity, frictionFactorW, angularMotorContributionV, angularMotorContributionW);
1439 }
1440
1441 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1442 // Some vehicles, like boats, should always keep their up-side up. This can be done by
1443 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
1444 // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
1445 // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
1446 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
1447 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
1448 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
1449 public void ComputeAngularVerticalAttraction()
1450 {
1451
1452 // If vertical attaction timescale is reasonable
1453 if (BSParam.VehicleEnableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1454 {
1455 Vector3 vehicleUpAxis = Vector3.UnitZ * VehicleFrameOrientation;
1456 switch (BSParam.VehicleAngularVerticalAttractionAlgorithm)
1457 {
1458 case 0:
1459 {
1460 //Another formula to try got from :
1461 //http://answers.unity3d.com/questions/10425/how-to-stabilize-angular-motion-alignment-of-hover.html
1462
1463 // Flipping what was originally a timescale into a speed variable and then multiplying it by 2
1464 // since only computing half the distance between the angles.
1465 float verticalAttractionSpeed = (1 / m_verticalAttractionTimescale) * 2.0f;
1466
1467 // Make a prediction of where the up axis will be when this is applied rather then where it is now as
1468 // this makes for a smoother adjustment and less fighting between the various forces.
1469 Vector3 predictedUp = vehicleUpAxis * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
1470
1471 // This is only half the distance to the target so it will take 2 seconds to complete the turn.
1472 Vector3 torqueVector = Vector3.Cross(predictedUp, Vector3.UnitZ);
1473
1474 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != 0)
1475 {
1476 Vector3 vehicleForwardAxis = Vector3.UnitX * VehicleFrameOrientation;
1477 torqueVector = ProjectVector(torqueVector, vehicleForwardAxis);
1478 }
1479
1480 // Scale vector by our timescale since it is an acceleration it is r/s^2 or radians a timescale squared
1481 Vector3 vertContributionV = torqueVector * verticalAttractionSpeed * verticalAttractionSpeed;
1482
1483 VehicleRotationalVelocity += vertContributionV;
1484
1485 VDetailLog("{0}, MoveAngular,verticalAttraction,vertAttrSpeed={1},upAxis={2},PredictedUp={3},torqueVector={4},contrib={5}",
1486 ControllingPrim.LocalID,
1487 verticalAttractionSpeed,
1488 vehicleUpAxis,
1489 predictedUp,
1490 torqueVector,
1491 vertContributionV);
1492 break;
1493 }
1494 case 1:
1495 {
1496 // Possible solution derived from a discussion at:
1497 // http://stackoverflow.com/questions/14939657/computing-vector-from-quaternion-works-computing-quaternion-from-vector-does-no
1498
1499 // Create a rotation that is only the vehicle's rotation around Z
1500 Vector3 currentEulerW = Vector3.Zero;
1501 VehicleFrameOrientation.GetEulerAngles(out currentEulerW.X, out currentEulerW.Y, out currentEulerW.Z);
1502 Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEulerW.Z);
1503
1504 // Create the axis that is perpendicular to the up vector and the rotated up vector.
1505 Vector3 differenceAxisW = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleFrameOrientation);
1506 // Compute the angle between those to vectors.
1507 double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleFrameOrientation)));
1508 // 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
1509
1510 // Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
1511 // TODO: add 'efficiency'.
1512 // differenceAngle /= m_verticalAttractionTimescale;
1513
1514 // Create the quaterian representing the correction angle
1515 Quaternion correctionRotationW = Quaternion.CreateFromAxisAngle(differenceAxisW, (float)differenceAngle);
1516
1517 // Turn that quaternion into Euler values to make it into velocities to apply.
1518 Vector3 vertContributionW = Vector3.Zero;
1519 correctionRotationW.GetEulerAngles(out vertContributionW.X, out vertContributionW.Y, out vertContributionW.Z);
1520 vertContributionW *= -1f;
1521 vertContributionW /= m_verticalAttractionTimescale;
1522
1523 VehicleRotationalVelocity += vertContributionW;
1524
1525 VDetailLog("{0}, MoveAngular,verticalAttraction,upAxis={1},diffAxis={2},diffAng={3},corrRot={4},contrib={5}",
1526 ControllingPrim.LocalID,
1527 vehicleUpAxis,
1528 differenceAxisW,
1529 differenceAngle,
1530 correctionRotationW,
1531 vertContributionW);
1532 break;
1533 }
1534 case 2:
1535 {
1536 Vector3 vertContributionV = Vector3.Zero;
1537 Vector3 origRotVelW = VehicleRotationalVelocity; // DEBUG DEBUG
1538
1539 // Take a vector pointing up and convert it from world to vehicle relative coords.
1540 Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleFrameOrientation);
1541
1542 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
1543 // is now:
1544 // leaning to one side: rotated around the X axis with the Y value going
1545 // from zero (nearly straight up) to one (completely to the side)) or
1546 // leaning front-to-back: rotated around the Y axis with the value of X being between
1547 // zero and one.
1548 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
1549
1550 // Y error means needed rotation around X axis and visa versa.
1551 // Since the error goes from zero to one, the asin is the corresponding angle.
1552 vertContributionV.X = (float)Math.Asin(verticalError.Y);
1553 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
1554 vertContributionV.Y = -(float)Math.Asin(verticalError.X);
1555
1556 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
1557 if (verticalError.Z < 0f)
1558 {
1559 vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
1560 // vertContribution.Y -= PIOverFour;
1561 }
1562
1563 // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
1564 // Correction happens over a number of seconds.
1565 Vector3 unscaledContribVerticalErrorV = vertContributionV; // DEBUG DEBUG
1566
1567 // The correction happens over the user's time period
1568 vertContributionV /= m_verticalAttractionTimescale;
1569
1570 // Rotate the vehicle rotation to the world coordinates.
1571 VehicleRotationalVelocity += (vertContributionV * VehicleFrameOrientation);
1572
1573 VDetailLog("{0}, MoveAngular,verticalAttraction,,upAxis={1},origRotVW={2},vertError={3},unscaledV={4},eff={5},ts={6},vertContribV={7}",
1574 ControllingPrim.LocalID,
1575 vehicleUpAxis,
1576 origRotVelW,
1577 verticalError,
1578 unscaledContribVerticalErrorV,
1579 m_verticalAttractionEfficiency,
1580 m_verticalAttractionTimescale,
1581 vertContributionV);
1582 break;
1583 }
1584 default:
1585 {
1586 break;
1587 }
1588 }
1589 }
1590 }
1591
1592 // Angular correction to correct the direction the vehicle is pointing to be
1593 // the direction is should want to be pointing.
1594 // The vehicle is moving in some direction and correct its orientation to it is pointing
1595 // in that direction.
1596 // TODO: implement reference frame.
1597 public void ComputeAngularDeflection()
1598 {
1599
1600 if (BSParam.VehicleEnableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
1601 {
1602 Vector3 deflectContributionV = Vector3.Zero;
1603
1604 // The direction the vehicle is moving
1605 Vector3 movingDirection = VehicleVelocity;
1606 movingDirection.Normalize();
1607
1608 // If the vehicle is going backward, it is still pointing forward
1609 movingDirection *= Math.Sign(VehicleForwardSpeed);
1610
1611 // The direction the vehicle is pointing
1612 Vector3 pointingDirection = Vector3.UnitX * VehicleFrameOrientation;
1613 //Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
1614 // from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
1615 Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
1616 predictedPointingDirection.Normalize();
1617
1618 // The difference between what is and what should be.
1619 // Vector3 deflectionError = movingDirection - predictedPointingDirection;
1620 Vector3 deflectionError = Vector3.Cross(movingDirection, predictedPointingDirection);
1621
1622 // Don't try to correct very large errors (not our job)
1623 // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
1624 // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
1625 // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
1626 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
1627 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
1628 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
1629
1630 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
1631
1632 // Scale the correction by recovery timescale and efficiency
1633 // Not modeling a spring so clamp the scale to no more then the arc
1634 deflectContributionV = (-deflectionError) * ClampInRange(0, m_angularDeflectionEfficiency/m_angularDeflectionTimescale,1f);
1635 //deflectContributionV /= m_angularDeflectionTimescale;
1636
1637 VehicleRotationalVelocity += deflectContributionV;
1638 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
1639 ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
1640 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3},PredictedPointingDir={4}",
1641 ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale, predictedPointingDirection);
1642 }
1643 }
1644
1645 // Angular change to rotate the vehicle around the Z axis when the vehicle
1646 // is tipped around the X axis.
1647 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1648 // The vertical attractor feature must be enabled in order for the banking behavior to
1649 // function. The way banking works is this: a rotation around the vehicle's roll-axis will
1650 // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
1651 // of the yaw effect will be proportional to the
1652 // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
1653 // velocity along its preferred axis of motion.
1654 // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
1655 // positive rotation (by the right-hand rule) about the roll-axis will effect a
1656 // (negative) torque around the yaw-axis, making it turn to the right--that is the
1657 // vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
1658 // Negating the banking coefficient will make it so that the vehicle leans to the
1659 // outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
1660 // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
1661 // banking vehicles do what you want rather than what the laws of physics allow.
1662 // For example, consider a real motorcycle...it must be moving forward in order for
1663 // it to turn while banking, however video-game motorcycles are often configured
1664 // to turn in place when at a dead stop--because they are often easier to control
1665 // that way using the limited interface of the keyboard or game controller. The
1666 // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
1667 // banking by functioning as a slider between a banking that is correspondingly
1668 // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
1669 // banking effect depends only on the vehicle's rotation about its roll-axis compared
1670 // to "dynamic" where the banking is also proportional to its velocity along its
1671 // roll-axis. Finding the best value of the "mixture" will probably require trial and error.
1672 // The time it takes for the banking behavior to defeat a preexisting angular velocity about the
1673 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
1674 // bank quickly then give it a banking timescale of about a second or less, otherwise you can
1675 // make a sluggish vehicle by giving it a timescale of several seconds.
1676 public void ComputeAngularBanking()
1677 {
1678 if (BSParam.VehicleEnableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1679 {
1680 Vector3 bankingContributionV = Vector3.Zero;
1681
1682 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
1683 // As the vehicle rolls to the right or left, the Y value will increase from
1684 // zero (straight up) to 1 or -1 (full tilt right or left)
1685 Vector3 rollComponents = Vector3.UnitZ * VehicleFrameOrientation;
1686
1687 // Figure out the yaw value for this much roll.
1688 float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
1689 // actual error = static turn error + dynamic turn error
1690 float mixedYawAngle =(yawAngle * (1f - m_bankingMix)) + ((yawAngle * m_bankingMix) * VehicleForwardSpeed);
1691
1692 // TODO: the banking effect should not go to infinity but what to limit it to?
1693 // And what should happen when this is being added to a user defined yaw that is already PI*4?
1694 mixedYawAngle = ClampInRange(-FourPI, mixedYawAngle, FourPI);
1695
1696 // Build the force vector to change rotation from what it is to what it should be
1697 bankingContributionV.Z = -mixedYawAngle;
1698
1699 // Don't do it all at once. Fudge because 1 second is too fast with most user defined roll as PI*4.
1700 bankingContributionV /= m_bankingTimescale * BSParam.VehicleAngularBankingTimescaleFudge;
1701
1702 VehicleRotationalVelocity += bankingContributionV;
1703
1704
1705 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
1706 ControllingPrim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
1707 }
1708 }
1709
1710 // This is from previous instantiations of XXXDynamics.cs.
1711 // Applies roll reference frame.
1712 // TODO: is this the right way to separate the code to do this operation?
1713 // Should this be in MoveAngular()?
1714 internal void LimitRotation(float timestep)
1715 {
1716 Quaternion rotq = VehicleOrientation;
1717 Quaternion m_rot = rotq;
1718 if (m_RollreferenceFrame != Quaternion.Identity)
1719 {
1720 if (rotq.X >= m_RollreferenceFrame.X)
1721 {
1722 m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
1723 }
1724 if (rotq.Y >= m_RollreferenceFrame.Y)
1725 {
1726 m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
1727 }
1728 if (rotq.X <= -m_RollreferenceFrame.X)
1729 {
1730 m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
1731 }
1732 if (rotq.Y <= -m_RollreferenceFrame.Y)
1733 {
1734 m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
1735 }
1736 }
1737 if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
1738 {
1739 m_rot.X = 0;
1740 m_rot.Y = 0;
1741 }
1742 if (rotq != m_rot)
1743 {
1744 VehicleOrientation = m_rot;
1745 VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", ControllingPrim.LocalID, rotq, m_rot);
1746 }
1747
1748 }
1749
1750 // Given a friction vector (reduction in seconds) and a timestep, return the factor to reduce
1751 // some value by to apply this friction.
1752 private Vector3 ComputeFrictionFactor(Vector3 friction, float pTimestep)
1753 {
1754 Vector3 frictionFactor = Vector3.Zero;
1755 if (friction != BSMotor.InfiniteVector)
1756 {
1757 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
1758 // Individual friction components can be 'infinite' so compute each separately.
1759 frictionFactor.X = (friction.X == BSMotor.Infinite) ? 0f : (1f / friction.X);
1760 frictionFactor.Y = (friction.Y == BSMotor.Infinite) ? 0f : (1f / friction.Y);
1761 frictionFactor.Z = (friction.Z == BSMotor.Infinite) ? 0f : (1f / friction.Z);
1762 frictionFactor *= pTimestep;
1763 }
1764 return frictionFactor;
1765 }
1766
1767 private float SortedClampInRange(float clampa, float val, float clampb)
1768 {
1769 if (clampa > clampb)
1770 {
1771 float temp = clampa;
1772 clampa = clampb;
1773 clampb = temp;
1774 }
1775 return ClampInRange(clampa, val, clampb);
1776
1777 }
1778
1779 //Given a Vector and a unit vector will return the amount of the vector is on the same axis as the unit.
1780 private Vector3 ProjectVector(Vector3 vector, Vector3 onNormal)
1781 {
1782 float vectorDot = Vector3.Dot(vector, onNormal);
1783 return onNormal * vectorDot;
1784
1785 }
1786
1787 private float ClampInRange(float low, float val, float high)
1788 {
1789 return Math.Max(low, Math.Min(val, high));
1790 // return Utils.Clamp(val, low, high);
1791 }
1792
1793 // Invoke the detailed logger and output something if it's enabled.
1794 private void VDetailLog(string msg, params Object[] args)
1795 {
1796 if (ControllingPrim.PhysScene.VehicleLoggingEnabled)
1797 ControllingPrim.PhysScene.DetailLog(msg, args);
1798 }
1799 }
1800}
generated by cgit v1.1 at 2024-12-30 14:18:42 +0000