aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs1374
1 files changed, 1374 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
new file mode 100644
index 0000000..72afacc
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
@@ -0,0 +1,1374 @@
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.Region.Physics.Manager;
39
40namespace OpenSim.Region.Physics.BulletSNPlugin
41{
42 public sealed class BSDynamics
43 {
44 private static string LogHeader = "[BULLETSIM VEHICLE]";
45
46 private BSScene PhysicsScene { get; set; }
47 // the prim this dynamic controller belongs to
48 private BSPrim Prim { get; set; }
49
50 // mass of the vehicle fetched each time we're calles
51 private float m_vehicleMass;
52
53 // Vehicle properties
54 public Vehicle Type { get; set; }
55
56 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
57 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
58 // HOVER_TERRAIN_ONLY
59 // HOVER_GLOBAL_HEIGHT
60 // NO_DEFLECTION_UP
61 // HOVER_WATER_ONLY
62 // HOVER_UP_ONLY
63 // LIMIT_MOTOR_UP
64 // LIMIT_ROLL_ONLY
65 private Vector3 m_BlockingEndPoint = Vector3.Zero;
66 private Quaternion m_RollreferenceFrame = Quaternion.Identity;
67 private Quaternion m_referenceFrame = Quaternion.Identity;
68
69 // Linear properties
70 private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
71 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
72 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
73 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
74 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
75 private float m_linearMotorDecayTimescale = 0;
76 private float m_linearMotorTimescale = 0;
77 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
78 private Vector3 m_lastPositionVector = Vector3.Zero;
79 // private bool m_LinearMotorSetLastFrame = false;
80 // private Vector3 m_linearMotorOffset = Vector3.Zero;
81
82 //Angular properties
83 private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
84 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
85 // private int m_angularMotorApply = 0; // application frame counter
86 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
87 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
88 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
89 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
90 private Vector3 m_lastAngularVelocity = Vector3.Zero;
91 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
92
93 //Deflection properties
94 private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
95 private float m_angularDeflectionEfficiency = 0;
96 private float m_angularDeflectionTimescale = 0;
97 private float m_linearDeflectionEfficiency = 0;
98 private float m_linearDeflectionTimescale = 0;
99
100 //Banking properties
101 private float m_bankingEfficiency = 0;
102 private float m_bankingMix = 0;
103 private float m_bankingTimescale = 0;
104
105 //Hover and Buoyancy properties
106 private BSVMotor m_hoverMotor = new BSVMotor("Hover");
107 private float m_VhoverHeight = 0f;
108 private float m_VhoverEfficiency = 0f;
109 private float m_VhoverTimescale = 0f;
110 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
111 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)
113 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
114 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
115
116 //Attractor properties
117 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
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 public BSDynamics(BSScene myScene, BSPrim myPrim)
128 {
129 PhysicsScene = myScene;
130 Prim = myPrim;
131 Type = Vehicle.TYPE_NONE;
132 }
133
134 // Return 'true' if this vehicle is doing vehicle things
135 public bool IsActive
136 {
137 get { return Type != Vehicle.TYPE_NONE && Prim.IsPhysical; }
138 }
139
140 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
141 {
142 VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
143 switch (pParam)
144 {
145 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
146 m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
147 break;
148 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
149 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
150 break;
151 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
152 m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
153 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
154 break;
155 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
156 m_angularMotorTimescale = Math.Max(pValue, 0.01f);
157 m_angularMotor.TimeScale = m_angularMotorTimescale;
158 break;
159 case Vehicle.BANKING_EFFICIENCY:
160 m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
161 break;
162 case Vehicle.BANKING_MIX:
163 m_bankingMix = Math.Max(pValue, 0.01f);
164 break;
165 case Vehicle.BANKING_TIMESCALE:
166 m_bankingTimescale = Math.Max(pValue, 0.01f);
167 break;
168 case Vehicle.BUOYANCY:
169 m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
170 break;
171 case Vehicle.HOVER_EFFICIENCY:
172 m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
173 break;
174 case Vehicle.HOVER_HEIGHT:
175 m_VhoverHeight = pValue;
176 break;
177 case Vehicle.HOVER_TIMESCALE:
178 m_VhoverTimescale = Math.Max(pValue, 0.01f);
179 break;
180 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
181 m_linearDeflectionEfficiency = Math.Max(pValue, 0.01f);
182 break;
183 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
184 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
185 break;
186 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
187 m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
188 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
189 break;
190 case Vehicle.LINEAR_MOTOR_TIMESCALE:
191 m_linearMotorTimescale = Math.Max(pValue, 0.01f);
192 m_linearMotor.TimeScale = m_linearMotorTimescale;
193 break;
194 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
195 m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
196 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
197 break;
198 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
199 m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
200 m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
201 break;
202
203 // These are vector properties but the engine lets you use a single float value to
204 // set all of the components to the same value
205 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
206 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
207 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
208 break;
209 case Vehicle.ANGULAR_MOTOR_DIRECTION:
210 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
211 m_angularMotor.SetTarget(m_angularMotorDirection);
212 break;
213 case Vehicle.LINEAR_FRICTION_TIMESCALE:
214 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
215 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
216 break;
217 case Vehicle.LINEAR_MOTOR_DIRECTION:
218 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
219 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
220 m_linearMotor.SetTarget(m_linearMotorDirection);
221 break;
222 case Vehicle.LINEAR_MOTOR_OFFSET:
223 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
224 break;
225
226 }
227 }//end ProcessFloatVehicleParam
228
229 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
230 {
231 VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
232 switch (pParam)
233 {
234 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
235 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
236 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
237 break;
238 case Vehicle.ANGULAR_MOTOR_DIRECTION:
239 // Limit requested angular speed to 2 rps= 4 pi rads/sec
240 pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
241 pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
242 pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
243 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 m_angularMotor.SetTarget(m_angularMotorDirection);
245 break;
246 case Vehicle.LINEAR_FRICTION_TIMESCALE:
247 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
248 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
249 break;
250 case Vehicle.LINEAR_MOTOR_DIRECTION:
251 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
252 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
253 m_linearMotor.SetTarget(m_linearMotorDirection);
254 break;
255 case Vehicle.LINEAR_MOTOR_OFFSET:
256 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
257 break;
258 case Vehicle.BLOCK_EXIT:
259 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
260 break;
261 }
262 }//end ProcessVectorVehicleParam
263
264 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
265 {
266 VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
267 switch (pParam)
268 {
269 case Vehicle.REFERENCE_FRAME:
270 m_referenceFrame = pValue;
271 break;
272 case Vehicle.ROLL_FRAME:
273 m_RollreferenceFrame = pValue;
274 break;
275 }
276 }//end ProcessRotationVehicleParam
277
278 internal void ProcessVehicleFlags(int pParam, bool remove)
279 {
280 VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", Prim.LocalID, pParam, remove);
281 VehicleFlag parm = (VehicleFlag)pParam;
282 if (pParam == -1)
283 m_flags = (VehicleFlag)0;
284 else
285 {
286 if (remove)
287 m_flags &= ~parm;
288 else
289 m_flags |= parm;
290 }
291 }
292
293 internal void ProcessTypeChange(Vehicle pType)
294 {
295 VDetailLog("{0},ProcessTypeChange,type={1}", Prim.LocalID, pType);
296 // Set Defaults For Type
297 Type = pType;
298 switch (pType)
299 {
300 case Vehicle.TYPE_NONE:
301 m_linearMotorDirection = Vector3.Zero;
302 m_linearMotorTimescale = 0;
303 m_linearMotorDecayTimescale = 0;
304 m_linearFrictionTimescale = new Vector3(0, 0, 0);
305
306 m_angularMotorDirection = Vector3.Zero;
307 m_angularMotorDecayTimescale = 0;
308 m_angularMotorTimescale = 0;
309 m_angularFrictionTimescale = new Vector3(0, 0, 0);
310
311 m_VhoverHeight = 0;
312 m_VhoverEfficiency = 0;
313 m_VhoverTimescale = 0;
314 m_VehicleBuoyancy = 0;
315
316 m_linearDeflectionEfficiency = 1;
317 m_linearDeflectionTimescale = 1;
318
319 m_angularDeflectionEfficiency = 0;
320 m_angularDeflectionTimescale = 1000;
321
322 m_verticalAttractionEfficiency = 0;
323 m_verticalAttractionTimescale = 0;
324
325 m_bankingEfficiency = 0;
326 m_bankingTimescale = 1000;
327 m_bankingMix = 1;
328
329 m_referenceFrame = Quaternion.Identity;
330 m_flags = (VehicleFlag)0;
331
332 break;
333
334 case Vehicle.TYPE_SLED:
335 m_linearMotorDirection = Vector3.Zero;
336 m_linearMotorTimescale = 1000;
337 m_linearMotorDecayTimescale = 120;
338 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
339
340 m_angularMotorDirection = Vector3.Zero;
341 m_angularMotorTimescale = 1000;
342 m_angularMotorDecayTimescale = 120;
343 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
344
345 m_VhoverHeight = 0;
346 m_VhoverEfficiency = 10; // TODO: this looks wrong!!
347 m_VhoverTimescale = 10;
348 m_VehicleBuoyancy = 0;
349
350 m_linearDeflectionEfficiency = 1;
351 m_linearDeflectionTimescale = 1;
352
353 m_angularDeflectionEfficiency = 1;
354 m_angularDeflectionTimescale = 1000;
355
356 m_verticalAttractionEfficiency = 0;
357 m_verticalAttractionTimescale = 0;
358
359 m_bankingEfficiency = 0;
360 m_bankingTimescale = 10;
361 m_bankingMix = 1;
362
363 m_referenceFrame = Quaternion.Identity;
364 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
365 | VehicleFlag.HOVER_TERRAIN_ONLY
366 | VehicleFlag.HOVER_GLOBAL_HEIGHT
367 | VehicleFlag.HOVER_UP_ONLY);
368 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
369 | VehicleFlag.LIMIT_ROLL_ONLY
370 | VehicleFlag.LIMIT_MOTOR_UP);
371
372 break;
373 case Vehicle.TYPE_CAR:
374 m_linearMotorDirection = Vector3.Zero;
375 m_linearMotorTimescale = 1;
376 m_linearMotorDecayTimescale = 60;
377 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
378
379 m_angularMotorDirection = Vector3.Zero;
380 m_angularMotorTimescale = 1;
381 m_angularMotorDecayTimescale = 0.8f;
382 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
383
384 m_VhoverHeight = 0;
385 m_VhoverEfficiency = 0;
386 m_VhoverTimescale = 1000;
387 m_VehicleBuoyancy = 0;
388
389 m_linearDeflectionEfficiency = 1;
390 m_linearDeflectionTimescale = 2;
391
392 m_angularDeflectionEfficiency = 0;
393 m_angularDeflectionTimescale = 10;
394
395 m_verticalAttractionEfficiency = 1f;
396 m_verticalAttractionTimescale = 10f;
397
398 m_bankingEfficiency = -0.2f;
399 m_bankingMix = 1;
400 m_bankingTimescale = 1;
401
402 m_referenceFrame = Quaternion.Identity;
403 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
404 | VehicleFlag.HOVER_TERRAIN_ONLY
405 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
406 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
407 | VehicleFlag.LIMIT_ROLL_ONLY
408 | VehicleFlag.LIMIT_MOTOR_UP
409 | VehicleFlag.HOVER_UP_ONLY);
410 break;
411 case Vehicle.TYPE_BOAT:
412 m_linearMotorDirection = Vector3.Zero;
413 m_linearMotorTimescale = 5;
414 m_linearMotorDecayTimescale = 60;
415 m_linearFrictionTimescale = new Vector3(10, 3, 2);
416
417 m_angularMotorDirection = Vector3.Zero;
418 m_angularMotorTimescale = 4;
419 m_angularMotorDecayTimescale = 4;
420 m_angularFrictionTimescale = new Vector3(10,10,10);
421
422 m_VhoverHeight = 0;
423 m_VhoverEfficiency = 0.5f;
424 m_VhoverTimescale = 2;
425 m_VehicleBuoyancy = 1;
426
427 m_linearDeflectionEfficiency = 0.5f;
428 m_linearDeflectionTimescale = 3;
429
430 m_angularDeflectionEfficiency = 0.5f;
431 m_angularDeflectionTimescale = 5;
432
433 m_verticalAttractionEfficiency = 0.5f;
434 m_verticalAttractionTimescale = 5f;
435
436 m_bankingEfficiency = -0.3f;
437 m_bankingMix = 0.8f;
438 m_bankingTimescale = 1;
439
440 m_referenceFrame = Quaternion.Identity;
441 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
442 | VehicleFlag.HOVER_GLOBAL_HEIGHT
443 | VehicleFlag.LIMIT_ROLL_ONLY
444 | VehicleFlag.HOVER_UP_ONLY);
445 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
446 | VehicleFlag.LIMIT_MOTOR_UP
447 | VehicleFlag.HOVER_WATER_ONLY);
448 break;
449 case Vehicle.TYPE_AIRPLANE:
450 m_linearMotorDirection = Vector3.Zero;
451 m_linearMotorTimescale = 2;
452 m_linearMotorDecayTimescale = 60;
453 m_linearFrictionTimescale = new Vector3(200, 10, 5);
454
455 m_angularMotorDirection = Vector3.Zero;
456 m_angularMotorTimescale = 4;
457 m_angularMotorDecayTimescale = 4;
458 m_angularFrictionTimescale = new Vector3(20, 20, 20);
459
460 m_VhoverHeight = 0;
461 m_VhoverEfficiency = 0.5f;
462 m_VhoverTimescale = 1000;
463 m_VehicleBuoyancy = 0;
464
465 m_linearDeflectionEfficiency = 0.5f;
466 m_linearDeflectionTimescale = 3;
467
468 m_angularDeflectionEfficiency = 1;
469 m_angularDeflectionTimescale = 2;
470
471 m_verticalAttractionEfficiency = 0.9f;
472 m_verticalAttractionTimescale = 2f;
473
474 m_bankingEfficiency = 1;
475 m_bankingMix = 0.7f;
476 m_bankingTimescale = 2;
477
478 m_referenceFrame = Quaternion.Identity;
479 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
480 | VehicleFlag.HOVER_TERRAIN_ONLY
481 | VehicleFlag.HOVER_GLOBAL_HEIGHT
482 | VehicleFlag.HOVER_UP_ONLY
483 | VehicleFlag.NO_DEFLECTION_UP
484 | VehicleFlag.LIMIT_MOTOR_UP);
485 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
486 break;
487 case Vehicle.TYPE_BALLOON:
488 m_linearMotorDirection = Vector3.Zero;
489 m_linearMotorTimescale = 5;
490 m_linearFrictionTimescale = new Vector3(5, 5, 5);
491 m_linearMotorDecayTimescale = 60;
492
493 m_angularMotorDirection = Vector3.Zero;
494 m_angularMotorTimescale = 6;
495 m_angularFrictionTimescale = new Vector3(10, 10, 10);
496 m_angularMotorDecayTimescale = 10;
497
498 m_VhoverHeight = 5;
499 m_VhoverEfficiency = 0.8f;
500 m_VhoverTimescale = 10;
501 m_VehicleBuoyancy = 1;
502
503 m_linearDeflectionEfficiency = 0;
504 m_linearDeflectionTimescale = 5;
505
506 m_angularDeflectionEfficiency = 0;
507 m_angularDeflectionTimescale = 5;
508
509 m_verticalAttractionEfficiency = 1f;
510 m_verticalAttractionTimescale = 100f;
511
512 m_bankingEfficiency = 0;
513 m_bankingMix = 0.7f;
514 m_bankingTimescale = 5;
515
516 m_referenceFrame = Quaternion.Identity;
517
518 m_referenceFrame = Quaternion.Identity;
519 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
520 | VehicleFlag.HOVER_TERRAIN_ONLY
521 | VehicleFlag.HOVER_UP_ONLY
522 | VehicleFlag.NO_DEFLECTION_UP
523 | VehicleFlag.LIMIT_MOTOR_UP);
524 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
525 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
526 break;
527 }
528
529 // Update any physical parameters based on this type.
530 Refresh();
531
532 m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
533 m_linearMotorDecayTimescale, m_linearFrictionTimescale,
534 1f);
535 m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
536
537 m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
538 m_angularMotorDecayTimescale, m_angularFrictionTimescale,
539 1f);
540 m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
541
542 m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
543 BSMotor.Infinite, BSMotor.InfiniteVector,
544 m_verticalAttractionEfficiency);
545 // Z goes away and we keep X and Y
546 m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
547 m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
548 }
549
550 // Some of the properties of this prim may have changed.
551 // Do any updating needed for a vehicle
552 public void Refresh()
553 {
554 if (IsActive)
555 {
556 // Remember the mass so we don't have to fetch it every step
557 m_vehicleMass = Prim.Linkset.LinksetMass;
558
559 // Friction affects are handled by this vehicle code
560 float friction = 0f;
561 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
562
563 // Moderate angular movement introduced by Bullet.
564 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
565 // Maybe compute linear and angular factor and damping from params.
566 float angularDamping = BSParam.VehicleAngularDamping;
567 BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
568
569 // Vehicles report collision events so we know when it's on the ground
570 BulletSimAPI.AddToCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
571
572 Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(Prim.PhysShape.ptr, m_vehicleMass);
573 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
574 BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
575
576 VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4}",
577 Prim.LocalID, m_vehicleMass, friction, localInertia, angularDamping);
578 }
579 else
580 {
581 BulletSimAPI.RemoveFromCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
582 }
583 }
584
585 public bool RemoveBodyDependencies(BSPhysObject prim)
586 {
587 // If active, we need to add our properties back when the body is rebuilt.
588 return IsActive;
589 }
590
591 public void RestoreBodyDependencies(BSPhysObject prim)
592 {
593 if (Prim.LocalID != prim.LocalID)
594 {
595 // The call should be on us by our prim. Error if not.
596 PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
597 LogHeader, prim.LocalID, Prim.LocalID);
598 return;
599 }
600 Refresh();
601 }
602
603 #region Known vehicle value functions
604 // Vehicle physical parameters that we buffer from constant getting and setting.
605 // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
606 // Changing is remembered and the parameter is stored back into the physics engine only if updated.
607 // This does two things: 1) saves continuious calls into unmanaged code, and
608 // 2) signals when a physics property update must happen back to the simulator
609 // to update values modified for the vehicle.
610 private int m_knownChanged;
611 private int m_knownHas;
612 private float m_knownTerrainHeight;
613 private float m_knownWaterLevel;
614 private Vector3 m_knownPosition;
615 private Vector3 m_knownVelocity;
616 private Vector3 m_knownForce;
617 private Quaternion m_knownOrientation;
618 private Vector3 m_knownRotationalVelocity;
619 private Vector3 m_knownRotationalForce;
620 private Vector3 m_knownForwardVelocity; // vehicle relative forward speed
621
622 private const int m_knownChangedPosition = 1 << 0;
623 private const int m_knownChangedVelocity = 1 << 1;
624 private const int m_knownChangedForce = 1 << 2;
625 private const int m_knownChangedOrientation = 1 << 3;
626 private const int m_knownChangedRotationalVelocity = 1 << 4;
627 private const int m_knownChangedRotationalForce = 1 << 5;
628 private const int m_knownChangedTerrainHeight = 1 << 6;
629 private const int m_knownChangedWaterLevel = 1 << 7;
630 private const int m_knownChangedForwardVelocity = 1 << 8;
631
632 private void ForgetKnownVehicleProperties()
633 {
634 m_knownHas = 0;
635 m_knownChanged = 0;
636 }
637 // Push all the changed values back into the physics engine
638 private void PushKnownChanged()
639 {
640 if (m_knownChanged != 0)
641 {
642 if ((m_knownChanged & m_knownChangedPosition) != 0)
643 Prim.ForcePosition = m_knownPosition;
644
645 if ((m_knownChanged & m_knownChangedOrientation) != 0)
646 Prim.ForceOrientation = m_knownOrientation;
647
648 if ((m_knownChanged & m_knownChangedVelocity) != 0)
649 {
650 Prim.ForceVelocity = m_knownVelocity;
651 BulletSimAPI.SetInterpolationLinearVelocity2(Prim.PhysBody.ptr, VehicleVelocity);
652 }
653
654 if ((m_knownChanged & m_knownChangedForce) != 0)
655 Prim.AddForce((Vector3)m_knownForce, false, true);
656
657 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
658 {
659 Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
660 // Fake out Bullet by making it think the velocity is the same as last time.
661 BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, m_knownRotationalVelocity);
662 }
663
664 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
665 Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
666
667 // If we set one of the values (ie, the physics engine didn't do it) we must force
668 // an UpdateProperties event to send the changes up to the simulator.
669 BulletSimAPI.PushUpdate2(Prim.PhysBody.ptr);
670 }
671 m_knownChanged = 0;
672 }
673
674 // Since the computation of terrain height can be a little involved, this routine
675 // is used to fetch the height only once for each vehicle simulation step.
676 private float GetTerrainHeight(Vector3 pos)
677 {
678 if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
679 {
680 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
681 m_knownHas |= m_knownChangedTerrainHeight;
682 }
683 return m_knownTerrainHeight;
684 }
685
686 // Since the computation of water level can be a little involved, this routine
687 // is used ot fetch the level only once for each vehicle simulation step.
688 private float GetWaterLevel(Vector3 pos)
689 {
690 if ((m_knownHas & m_knownChangedWaterLevel) == 0)
691 {
692 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
693 m_knownHas |= m_knownChangedWaterLevel;
694 }
695 return (float)m_knownWaterLevel;
696 }
697
698 private Vector3 VehiclePosition
699 {
700 get
701 {
702 if ((m_knownHas & m_knownChangedPosition) == 0)
703 {
704 m_knownPosition = Prim.ForcePosition;
705 m_knownHas |= m_knownChangedPosition;
706 }
707 return m_knownPosition;
708 }
709 set
710 {
711 m_knownPosition = value;
712 m_knownChanged |= m_knownChangedPosition;
713 m_knownHas |= m_knownChangedPosition;
714 }
715 }
716
717 private Quaternion VehicleOrientation
718 {
719 get
720 {
721 if ((m_knownHas & m_knownChangedOrientation) == 0)
722 {
723 m_knownOrientation = Prim.ForceOrientation;
724 m_knownHas |= m_knownChangedOrientation;
725 }
726 return m_knownOrientation;
727 }
728 set
729 {
730 m_knownOrientation = value;
731 m_knownChanged |= m_knownChangedOrientation;
732 m_knownHas |= m_knownChangedOrientation;
733 }
734 }
735
736 private Vector3 VehicleVelocity
737 {
738 get
739 {
740 if ((m_knownHas & m_knownChangedVelocity) == 0)
741 {
742 m_knownVelocity = Prim.ForceVelocity;
743 m_knownHas |= m_knownChangedVelocity;
744 }
745 return (Vector3)m_knownVelocity;
746 }
747 set
748 {
749 m_knownVelocity = value;
750 m_knownChanged |= m_knownChangedVelocity;
751 m_knownHas |= m_knownChangedVelocity;
752 }
753 }
754
755 private void VehicleAddForce(Vector3 aForce)
756 {
757 if ((m_knownHas & m_knownChangedForce) == 0)
758 {
759 m_knownForce = Vector3.Zero;
760 }
761 m_knownForce += aForce;
762 m_knownChanged |= m_knownChangedForce;
763 m_knownHas |= m_knownChangedForce;
764 }
765
766 private Vector3 VehicleRotationalVelocity
767 {
768 get
769 {
770 if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
771 {
772 m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
773 m_knownHas |= m_knownChangedRotationalVelocity;
774 }
775 return (Vector3)m_knownRotationalVelocity;
776 }
777 set
778 {
779 m_knownRotationalVelocity = value;
780 m_knownChanged |= m_knownChangedRotationalVelocity;
781 m_knownHas |= m_knownChangedRotationalVelocity;
782 }
783 }
784 private void VehicleAddAngularForce(Vector3 aForce)
785 {
786 if ((m_knownHas & m_knownChangedRotationalForce) == 0)
787 {
788 m_knownRotationalForce = Vector3.Zero;
789 }
790 m_knownRotationalForce += aForce;
791 m_knownChanged |= m_knownChangedRotationalForce;
792 m_knownHas |= m_knownChangedRotationalForce;
793 }
794 // Vehicle relative forward velocity
795 private Vector3 VehicleForwardVelocity
796 {
797 get
798 {
799 if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
800 {
801 m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
802 m_knownHas |= m_knownChangedForwardVelocity;
803 }
804 return m_knownForwardVelocity;
805 }
806 }
807 private float VehicleForwardSpeed
808 {
809 get
810 {
811 return VehicleForwardVelocity.X;
812 }
813 }
814
815 #endregion // Known vehicle value functions
816
817 // One step of the vehicle properties for the next 'pTimestep' seconds.
818 internal void Step(float pTimestep)
819 {
820 if (!IsActive) return;
821
822 ForgetKnownVehicleProperties();
823
824 MoveLinear(pTimestep);
825 MoveAngular(pTimestep);
826
827 LimitRotation(pTimestep);
828
829 // remember the position so next step we can limit absolute movement effects
830 m_lastPositionVector = VehiclePosition;
831
832 // If we forced the changing of some vehicle parameters, update the values and
833 // for the physics engine to note the changes so an UpdateProperties event will happen.
834 PushKnownChanged();
835
836 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
837 Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
838 }
839
840 // Apply the effect of the linear motor and other linear motions (like hover and float).
841 private void MoveLinear(float pTimestep)
842 {
843 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
844
845 // The movement computed in the linear motor is relative to the vehicle
846 // coordinates. Rotate the movement to world coordinates.
847 linearMotorContribution *= VehicleOrientation;
848
849 // ==================================================================
850 // Buoyancy: force to overcome gravity.
851 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
852 // So, if zero, don't change anything (let gravity happen). If one, negate the effect of gravity.
853 Vector3 buoyancyContribution = Prim.PhysicsScene.DefaultGravity * m_VehicleBuoyancy;
854
855 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
856
857 Vector3 hoverContribution = ComputeLinearHover(pTimestep);
858
859 ComputeLinearBlockingEndPoint(pTimestep);
860
861 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
862
863 // ==================================================================
864 Vector3 newVelocity = linearMotorContribution
865 + terrainHeightContribution
866 + hoverContribution
867 + limitMotorUpContribution;
868
869 Vector3 newForce = buoyancyContribution;
870
871 // If not changing some axis, reduce out velocity
872 if ((m_flags & (VehicleFlag.NO_X)) != 0)
873 newVelocity.X = 0;
874 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
875 newVelocity.Y = 0;
876 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
877 newVelocity.Z = 0;
878
879 // ==================================================================
880 // Clamp high or low velocities
881 float newVelocityLengthSq = newVelocity.LengthSquared();
882 if (newVelocityLengthSq > 1000f)
883 {
884 newVelocity /= newVelocity.Length();
885 newVelocity *= 1000f;
886 }
887 else if (newVelocityLengthSq < 0.001f)
888 newVelocity = Vector3.Zero;
889
890 // ==================================================================
891 // Stuff new linear velocity into the vehicle.
892 // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
893 VehicleVelocity = newVelocity;
894
895 // Other linear forces are applied as forces.
896 Vector3 totalDownForce = newForce * m_vehicleMass;
897 if (!totalDownForce.ApproxEquals(Vector3.Zero, 0.01f))
898 {
899 VehicleAddForce(totalDownForce);
900 }
901
902 VDetailLog("{0}, MoveLinear,done,newVel={1},totDown={2},IsColliding={3}",
903 Prim.LocalID, newVelocity, totalDownForce, Prim.IsColliding);
904 VDetailLog("{0}, MoveLinear,done,linContrib={1},terrContrib={2},hoverContrib={3},limitContrib={4},buoyContrib={5}",
905 Prim.LocalID,
906 linearMotorContribution, terrainHeightContribution, hoverContribution,
907 limitMotorUpContribution, buoyancyContribution
908 );
909
910 } // end MoveLinear()
911
912 public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
913 {
914 Vector3 ret = Vector3.Zero;
915 // If below the terrain, move us above the ground a little.
916 // TODO: Consider taking the rotated size of the object or possibly casting a ray.
917 if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
918 {
919 // TODO: correct position by applying force rather than forcing position.
920 Vector3 newPosition = VehiclePosition;
921 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
922 VehiclePosition = newPosition;
923 VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
924 Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
925 }
926 return ret;
927 }
928
929 public Vector3 ComputeLinearHover(float pTimestep)
930 {
931 Vector3 ret = Vector3.Zero;
932
933 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
934 // m_VhoverTimescale: time to achieve height
935 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
936 {
937 // We should hover, get the target height
938 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
939 {
940 m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
941 }
942 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
943 {
944 m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
945 }
946 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
947 {
948 m_VhoverTargetHeight = m_VhoverHeight;
949 }
950
951 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
952 {
953 // If body is already heigher, use its height as target height
954 if (VehiclePosition.Z > m_VhoverTargetHeight)
955 m_VhoverTargetHeight = VehiclePosition.Z;
956 }
957
958 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
959 {
960 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
961 {
962 Vector3 pos = VehiclePosition;
963 pos.Z = m_VhoverTargetHeight;
964 VehiclePosition = pos;
965 }
966 }
967 else
968 {
969 // Error is positive if below the target and negative if above.
970 float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
971 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
972
973 // TODO: implement m_VhoverEfficiency correctly
974 if (Math.Abs(verticalError) > m_VhoverEfficiency)
975 {
976 ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
977 }
978 }
979
980 VDetailLog("{0}, MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
981 Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
982 }
983
984 return ret;
985 }
986
987 public bool ComputeLinearBlockingEndPoint(float pTimestep)
988 {
989 bool changed = false;
990
991 Vector3 pos = VehiclePosition;
992 Vector3 posChange = pos - m_lastPositionVector;
993 if (m_BlockingEndPoint != Vector3.Zero)
994 {
995 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
996 {
997 pos.X -= posChange.X + 1;
998 changed = true;
999 }
1000 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
1001 {
1002 pos.Y -= posChange.Y + 1;
1003 changed = true;
1004 }
1005 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
1006 {
1007 pos.Z -= posChange.Z + 1;
1008 changed = true;
1009 }
1010 if (pos.X <= 0)
1011 {
1012 pos.X += posChange.X + 1;
1013 changed = true;
1014 }
1015 if (pos.Y <= 0)
1016 {
1017 pos.Y += posChange.Y + 1;
1018 changed = true;
1019 }
1020 if (changed)
1021 {
1022 VehiclePosition = pos;
1023 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
1024 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
1025 }
1026 }
1027 return changed;
1028 }
1029
1030 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1031 // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
1032 // used with conjunction with banking: the strength of the banking will decay when the
1033 // vehicle no longer experiences collisions. The decay timescale is the same as
1034 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
1035 // when they are in mid jump.
1036 // TODO: this code is wrong. Also, what should it do for boats (height from water)?
1037 // This is just using the ground and a general collision check. Should really be using
1038 // a downward raycast to find what is below.
1039 public Vector3 ComputeLinearMotorUp(float pTimestep)
1040 {
1041 Vector3 ret = Vector3.Zero;
1042 float distanceAboveGround = 0f;
1043
1044 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
1045 {
1046 float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
1047 distanceAboveGround = VehiclePosition.Z - targetHeight;
1048 // Not colliding if the vehicle is off the ground
1049 if (!Prim.IsColliding)
1050 {
1051 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
1052 ret = new Vector3(0, 0, -distanceAboveGround);
1053 }
1054 // TODO: this calculation is wrong. From the description at
1055 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
1056 // has a decay factor. This says this force should
1057 // be computed with a motor.
1058 // TODO: add interaction with banking.
1059 }
1060 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
1061 Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret);
1062 return ret;
1063 }
1064
1065 // =======================================================================
1066 // =======================================================================
1067 // Apply the effect of the angular motor.
1068 // The 'contribution' is how much angular correction velocity each function wants.
1069 // All the contributions are added together and the resulting velocity is
1070 // set directly on the vehicle.
1071 private void MoveAngular(float pTimestep)
1072 {
1073 // The user wants this many radians per second angular change?
1074 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
1075
1076 // ==================================================================
1077 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1078 // This flag prevents linear deflection parallel to world z-axis. This is useful
1079 // for preventing ground vehicles with large linear deflection, like bumper cars,
1080 // from climbing their linear deflection into the sky.
1081 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1082 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1083 {
1084 angularMotorContribution.X = 0f;
1085 angularMotorContribution.Y = 0f;
1086 VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
1087 }
1088
1089 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
1090
1091 Vector3 deflectionContribution = ComputeAngularDeflection();
1092
1093 Vector3 bankingContribution = ComputeAngularBanking();
1094
1095 // ==================================================================
1096 m_lastVertAttractor = verticalAttractionContribution;
1097
1098 m_lastAngularVelocity = angularMotorContribution
1099 + verticalAttractionContribution
1100 + deflectionContribution
1101 + bankingContribution;
1102
1103 // ==================================================================
1104 // Apply the correction velocity.
1105 // TODO: Should this be applied as an angular force (torque)?
1106 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1107 {
1108 VehicleRotationalVelocity = m_lastAngularVelocity;
1109
1110 VDetailLog("{0}, MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
1111 Prim.LocalID,
1112 angularMotorContribution, verticalAttractionContribution,
1113 bankingContribution, deflectionContribution,
1114 m_lastAngularVelocity
1115 );
1116 }
1117 else
1118 {
1119 // The vehicle is not adding anything angular wise.
1120 VehicleRotationalVelocity = Vector3.Zero;
1121 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
1122 }
1123
1124 // ==================================================================
1125 //Offset section
1126 if (m_linearMotorOffset != Vector3.Zero)
1127 {
1128 //Offset of linear velocity doesn't change the linear velocity,
1129 // but causes a torque to be applied, for example...
1130 //
1131 // IIIII >>> IIIII
1132 // IIIII >>> IIIII
1133 // IIIII >>> IIIII
1134 // ^
1135 // | Applying a force at the arrow will cause the object to move forward, but also rotate
1136 //
1137 //
1138 // The torque created is the linear velocity crossed with the offset
1139
1140 // TODO: this computation should be in the linear section
1141 // because that is where we know the impulse being applied.
1142 Vector3 torqueFromOffset = Vector3.Zero;
1143 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
1144 if (float.IsNaN(torqueFromOffset.X))
1145 torqueFromOffset.X = 0;
1146 if (float.IsNaN(torqueFromOffset.Y))
1147 torqueFromOffset.Y = 0;
1148 if (float.IsNaN(torqueFromOffset.Z))
1149 torqueFromOffset.Z = 0;
1150
1151 VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
1152 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
1153 }
1154
1155 }
1156 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1157 // Some vehicles, like boats, should always keep their up-side up. This can be done by
1158 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
1159 // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
1160 // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
1161 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
1162 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
1163 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
1164 public Vector3 ComputeAngularVerticalAttraction()
1165 {
1166 Vector3 ret = Vector3.Zero;
1167
1168 // If vertical attaction timescale is reasonable
1169 if (m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1170 {
1171 // Take a vector pointing up and convert it from world to vehicle relative coords.
1172 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
1173
1174 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
1175 // is now:
1176 // leaning to one side: rotated around the X axis with the Y value going
1177 // from zero (nearly straight up) to one (completely to the side)) or
1178 // leaning front-to-back: rotated around the Y axis with the value of X being between
1179 // zero and one.
1180 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
1181
1182 // Y error means needed rotation around X axis and visa versa.
1183 // Since the error goes from zero to one, the asin is the corresponding angle.
1184 ret.X = (float)Math.Asin(verticalError.Y);
1185 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
1186 ret.Y = -(float)Math.Asin(verticalError.X);
1187
1188 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
1189 if (verticalError.Z < 0f)
1190 {
1191 ret.X += PIOverFour;
1192 ret.Y += PIOverFour;
1193 }
1194
1195 // 'ret' is now the necessary velocity to correct tilt in one second.
1196 // Correction happens over a number of seconds.
1197 Vector3 unscaledContrib = ret;
1198 ret /= m_verticalAttractionTimescale;
1199
1200 VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
1201 Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
1202 }
1203 return ret;
1204 }
1205
1206 // Return the angular correction to correct the direction the vehicle is pointing to be
1207 // the direction is should want to be pointing.
1208 // The vehicle is moving in some direction and correct its orientation to it is pointing
1209 // in that direction.
1210 // TODO: implement reference frame.
1211 public Vector3 ComputeAngularDeflection()
1212 {
1213 Vector3 ret = Vector3.Zero;
1214 return ret; // DEBUG DEBUG DEBUG
1215 // Disable angular deflection for the moment.
1216 // Since angularMotorUp and angularDeflection are computed independently, they will calculate
1217 // approximately the same X or Y correction. When added together (when contributions are combined)
1218 // this creates an over-correction and then wabbling as the target is overshot.
1219 // TODO: rethink how the different correction computations inter-relate.
1220
1221 if (m_angularDeflectionEfficiency != 0)
1222 {
1223 // The direction the vehicle is moving
1224 Vector3 movingDirection = VehicleVelocity;
1225 movingDirection.Normalize();
1226
1227 // The direction the vehicle is pointing
1228 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
1229 pointingDirection.Normalize();
1230
1231 // The difference between what is and what should be.
1232 Vector3 deflectionError = movingDirection - pointingDirection;
1233
1234 // Don't try to correct very large errors (not our job)
1235 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
1236 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
1237 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
1238
1239 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
1240
1241 // Scale the correction by recovery timescale and efficiency
1242 ret = (-deflectionError) * m_angularDeflectionEfficiency;
1243 ret /= m_angularDeflectionTimescale;
1244
1245 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
1246 Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
1247 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
1248 Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
1249 }
1250 return ret;
1251 }
1252
1253 // Return an angular change to rotate the vehicle around the Z axis when the vehicle
1254 // is tipped around the X axis.
1255 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1256 // The vertical attractor feature must be enabled in order for the banking behavior to
1257 // function. The way banking works is this: a rotation around the vehicle's roll-axis will
1258 // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
1259 // of the yaw effect will be proportional to the
1260 // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
1261 // velocity along its preferred axis of motion.
1262 // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
1263 // positive rotation (by the right-hand rule) about the roll-axis will effect a
1264 // (negative) torque around the yaw-axis, making it turn to the right--that is the
1265 // vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
1266 // Negating the banking coefficient will make it so that the vehicle leans to the
1267 // outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
1268 // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
1269 // banking vehicles do what you want rather than what the laws of physics allow.
1270 // For example, consider a real motorcycle...it must be moving forward in order for
1271 // it to turn while banking, however video-game motorcycles are often configured
1272 // to turn in place when at a dead stop--because they are often easier to control
1273 // that way using the limited interface of the keyboard or game controller. The
1274 // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
1275 // banking by functioning as a slider between a banking that is correspondingly
1276 // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
1277 // banking effect depends only on the vehicle's rotation about its roll-axis compared
1278 // to "dynamic" where the banking is also proportional to its velocity along its
1279 // roll-axis. Finding the best value of the "mixture" will probably require trial and error.
1280 // The time it takes for the banking behavior to defeat a preexisting angular velocity about the
1281 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
1282 // bank quickly then give it a banking timescale of about a second or less, otherwise you can
1283 // make a sluggish vehicle by giving it a timescale of several seconds.
1284 public Vector3 ComputeAngularBanking()
1285 {
1286 Vector3 ret = Vector3.Zero;
1287
1288 if (m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1289 {
1290 // This works by rotating a unit vector to the orientation of the vehicle. The
1291 // roll (tilt) will be Y component of a tilting Z vector (zero for no tilt
1292 // up to one for full over).
1293 Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
1294
1295 // Figure out the yaw value for this much roll.
1296 float turnComponent = rollComponents.Y * rollComponents.Y * m_bankingEfficiency;
1297 // Keep the sign
1298 if (rollComponents.Y < 0f)
1299 turnComponent = -turnComponent;
1300
1301 // TODO: there must be a better computation of the banking force.
1302 float bankingTurnForce = turnComponent;
1303
1304 // actual error = static turn error + dynamic turn error
1305 float mixedBankingError = bankingTurnForce * (1f - m_bankingMix) + bankingTurnForce * m_bankingMix * VehicleForwardSpeed;
1306 // TODO: the banking effect should not go to infinity but what to limit it to?
1307 mixedBankingError = ClampInRange(-20f, mixedBankingError, 20f);
1308
1309 // Build the force vector to change rotation from what it is to what it should be
1310 ret.Z = -mixedBankingError;
1311
1312 // Don't do it all at once.
1313 ret /= m_bankingTimescale;
1314
1315 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},turnComp={3},bankErr={4},mixedBankErr={5},ret={6}",
1316 Prim.LocalID, rollComponents, VehicleForwardSpeed, turnComponent, bankingTurnForce, mixedBankingError, ret);
1317 }
1318 return ret;
1319 }
1320
1321 // This is from previous instantiations of XXXDynamics.cs.
1322 // Applies roll reference frame.
1323 // TODO: is this the right way to separate the code to do this operation?
1324 // Should this be in MoveAngular()?
1325 internal void LimitRotation(float timestep)
1326 {
1327 Quaternion rotq = VehicleOrientation;
1328 Quaternion m_rot = rotq;
1329 if (m_RollreferenceFrame != Quaternion.Identity)
1330 {
1331 if (rotq.X >= m_RollreferenceFrame.X)
1332 {
1333 m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
1334 }
1335 if (rotq.Y >= m_RollreferenceFrame.Y)
1336 {
1337 m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
1338 }
1339 if (rotq.X <= -m_RollreferenceFrame.X)
1340 {
1341 m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
1342 }
1343 if (rotq.Y <= -m_RollreferenceFrame.Y)
1344 {
1345 m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
1346 }
1347 }
1348 if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
1349 {
1350 m_rot.X = 0;
1351 m_rot.Y = 0;
1352 }
1353 if (rotq != m_rot)
1354 {
1355 VehicleOrientation = m_rot;
1356 VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
1357 }
1358
1359 }
1360
1361 private float ClampInRange(float low, float val, float high)
1362 {
1363 return Math.Max(low, Math.Min(val, high));
1364 // return Utils.Clamp(val, low, high);
1365 }
1366
1367 // Invoke the detailed logger and output something if it's enabled.
1368 private void VDetailLog(string msg, params Object[] args)
1369 {
1370 if (Prim.PhysicsScene.VehicleLoggingEnabled)
1371 Prim.PhysicsScene.DetailLog(msg, args);
1372 }
1373 }
1374}
generated by cgit v1.1 at 2024-12-24 15:21:10 +0000