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.cs1377
1 files changed, 1377 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..415ad4f
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
@@ -0,0 +1,1377 @@
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 Vector3 grav = PhysicsScene.DefaultGravity * (1f - Prim.Buoyancy);
577 BulletSimAPI.SetGravity2(Prim.PhysBody.ptr, grav);
578
579 VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4}",
580 Prim.LocalID, m_vehicleMass, friction, localInertia, angularDamping);
581 }
582 else
583 {
584 BulletSimAPI.RemoveFromCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
585 }
586 }
587
588 public bool RemoveBodyDependencies(BSPhysObject prim)
589 {
590 // If active, we need to add our properties back when the body is rebuilt.
591 return IsActive;
592 }
593
594 public void RestoreBodyDependencies(BSPhysObject prim)
595 {
596 if (Prim.LocalID != prim.LocalID)
597 {
598 // The call should be on us by our prim. Error if not.
599 PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
600 LogHeader, prim.LocalID, Prim.LocalID);
601 return;
602 }
603 Refresh();
604 }
605
606 #region Known vehicle value functions
607 // Vehicle physical parameters that we buffer from constant getting and setting.
608 // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
609 // Changing is remembered and the parameter is stored back into the physics engine only if updated.
610 // This does two things: 1) saves continuious calls into unmanaged code, and
611 // 2) signals when a physics property update must happen back to the simulator
612 // to update values modified for the vehicle.
613 private int m_knownChanged;
614 private int m_knownHas;
615 private float m_knownTerrainHeight;
616 private float m_knownWaterLevel;
617 private Vector3 m_knownPosition;
618 private Vector3 m_knownVelocity;
619 private Vector3 m_knownForce;
620 private Quaternion m_knownOrientation;
621 private Vector3 m_knownRotationalVelocity;
622 private Vector3 m_knownRotationalForce;
623 private Vector3 m_knownForwardVelocity; // vehicle relative forward speed
624
625 private const int m_knownChangedPosition = 1 << 0;
626 private const int m_knownChangedVelocity = 1 << 1;
627 private const int m_knownChangedForce = 1 << 2;
628 private const int m_knownChangedOrientation = 1 << 3;
629 private const int m_knownChangedRotationalVelocity = 1 << 4;
630 private const int m_knownChangedRotationalForce = 1 << 5;
631 private const int m_knownChangedTerrainHeight = 1 << 6;
632 private const int m_knownChangedWaterLevel = 1 << 7;
633 private const int m_knownChangedForwardVelocity = 1 << 8;
634
635 private void ForgetKnownVehicleProperties()
636 {
637 m_knownHas = 0;
638 m_knownChanged = 0;
639 }
640 // Push all the changed values back into the physics engine
641 private void PushKnownChanged()
642 {
643 if (m_knownChanged != 0)
644 {
645 if ((m_knownChanged & m_knownChangedPosition) != 0)
646 Prim.ForcePosition = m_knownPosition;
647
648 if ((m_knownChanged & m_knownChangedOrientation) != 0)
649 Prim.ForceOrientation = m_knownOrientation;
650
651 if ((m_knownChanged & m_knownChangedVelocity) != 0)
652 {
653 Prim.ForceVelocity = m_knownVelocity;
654 BulletSimAPI.SetInterpolationLinearVelocity2(Prim.PhysBody.ptr, VehicleVelocity);
655 }
656
657 if ((m_knownChanged & m_knownChangedForce) != 0)
658 Prim.AddForce((Vector3)m_knownForce, false, true);
659
660 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
661 {
662 Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
663 // Fake out Bullet by making it think the velocity is the same as last time.
664 BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, m_knownRotationalVelocity);
665 }
666
667 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
668 Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
669
670 // If we set one of the values (ie, the physics engine didn't do it) we must force
671 // an UpdateProperties event to send the changes up to the simulator.
672 BulletSimAPI.PushUpdate2(Prim.PhysBody.ptr);
673 }
674 m_knownChanged = 0;
675 }
676
677 // Since the computation of terrain height can be a little involved, this routine
678 // is used to fetch the height only once for each vehicle simulation step.
679 private float GetTerrainHeight(Vector3 pos)
680 {
681 if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
682 {
683 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
684 m_knownHas |= m_knownChangedTerrainHeight;
685 }
686 return m_knownTerrainHeight;
687 }
688
689 // Since the computation of water level can be a little involved, this routine
690 // is used ot fetch the level only once for each vehicle simulation step.
691 private float GetWaterLevel(Vector3 pos)
692 {
693 if ((m_knownHas & m_knownChangedWaterLevel) == 0)
694 {
695 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
696 m_knownHas |= m_knownChangedWaterLevel;
697 }
698 return (float)m_knownWaterLevel;
699 }
700
701 private Vector3 VehiclePosition
702 {
703 get
704 {
705 if ((m_knownHas & m_knownChangedPosition) == 0)
706 {
707 m_knownPosition = Prim.ForcePosition;
708 m_knownHas |= m_knownChangedPosition;
709 }
710 return m_knownPosition;
711 }
712 set
713 {
714 m_knownPosition = value;
715 m_knownChanged |= m_knownChangedPosition;
716 m_knownHas |= m_knownChangedPosition;
717 }
718 }
719
720 private Quaternion VehicleOrientation
721 {
722 get
723 {
724 if ((m_knownHas & m_knownChangedOrientation) == 0)
725 {
726 m_knownOrientation = Prim.ForceOrientation;
727 m_knownHas |= m_knownChangedOrientation;
728 }
729 return m_knownOrientation;
730 }
731 set
732 {
733 m_knownOrientation = value;
734 m_knownChanged |= m_knownChangedOrientation;
735 m_knownHas |= m_knownChangedOrientation;
736 }
737 }
738
739 private Vector3 VehicleVelocity
740 {
741 get
742 {
743 if ((m_knownHas & m_knownChangedVelocity) == 0)
744 {
745 m_knownVelocity = Prim.ForceVelocity;
746 m_knownHas |= m_knownChangedVelocity;
747 }
748 return (Vector3)m_knownVelocity;
749 }
750 set
751 {
752 m_knownVelocity = value;
753 m_knownChanged |= m_knownChangedVelocity;
754 m_knownHas |= m_knownChangedVelocity;
755 }
756 }
757
758 private void VehicleAddForce(Vector3 aForce)
759 {
760 if ((m_knownHas & m_knownChangedForce) == 0)
761 {
762 m_knownForce = Vector3.Zero;
763 }
764 m_knownForce += aForce;
765 m_knownChanged |= m_knownChangedForce;
766 m_knownHas |= m_knownChangedForce;
767 }
768
769 private Vector3 VehicleRotationalVelocity
770 {
771 get
772 {
773 if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
774 {
775 m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
776 m_knownHas |= m_knownChangedRotationalVelocity;
777 }
778 return (Vector3)m_knownRotationalVelocity;
779 }
780 set
781 {
782 m_knownRotationalVelocity = value;
783 m_knownChanged |= m_knownChangedRotationalVelocity;
784 m_knownHas |= m_knownChangedRotationalVelocity;
785 }
786 }
787 private void VehicleAddAngularForce(Vector3 aForce)
788 {
789 if ((m_knownHas & m_knownChangedRotationalForce) == 0)
790 {
791 m_knownRotationalForce = Vector3.Zero;
792 }
793 m_knownRotationalForce += aForce;
794 m_knownChanged |= m_knownChangedRotationalForce;
795 m_knownHas |= m_knownChangedRotationalForce;
796 }
797 // Vehicle relative forward velocity
798 private Vector3 VehicleForwardVelocity
799 {
800 get
801 {
802 if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
803 {
804 m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
805 m_knownHas |= m_knownChangedForwardVelocity;
806 }
807 return m_knownForwardVelocity;
808 }
809 }
810 private float VehicleForwardSpeed
811 {
812 get
813 {
814 return VehicleForwardVelocity.X;
815 }
816 }
817
818 #endregion // Known vehicle value functions
819
820 // One step of the vehicle properties for the next 'pTimestep' seconds.
821 internal void Step(float pTimestep)
822 {
823 if (!IsActive) return;
824
825 ForgetKnownVehicleProperties();
826
827 MoveLinear(pTimestep);
828 MoveAngular(pTimestep);
829
830 LimitRotation(pTimestep);
831
832 // remember the position so next step we can limit absolute movement effects
833 m_lastPositionVector = VehiclePosition;
834
835 // If we forced the changing of some vehicle parameters, update the values and
836 // for the physics engine to note the changes so an UpdateProperties event will happen.
837 PushKnownChanged();
838
839 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
840 Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
841 }
842
843 // Apply the effect of the linear motor and other linear motions (like hover and float).
844 private void MoveLinear(float pTimestep)
845 {
846 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
847
848 // The movement computed in the linear motor is relative to the vehicle
849 // coordinates. Rotate the movement to world coordinates.
850 linearMotorContribution *= VehicleOrientation;
851
852 // ==================================================================
853 // Buoyancy: force to overcome gravity.
854 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
855 // So, if zero, don't change anything (let gravity happen). If one, negate the effect of gravity.
856 Vector3 buoyancyContribution = Prim.PhysicsScene.DefaultGravity * m_VehicleBuoyancy;
857
858 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
859
860 Vector3 hoverContribution = ComputeLinearHover(pTimestep);
861
862 ComputeLinearBlockingEndPoint(pTimestep);
863
864 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
865
866 // ==================================================================
867 Vector3 newVelocity = linearMotorContribution
868 + terrainHeightContribution
869 + hoverContribution
870 + limitMotorUpContribution;
871
872 Vector3 newForce = buoyancyContribution;
873
874 // If not changing some axis, reduce out velocity
875 if ((m_flags & (VehicleFlag.NO_X)) != 0)
876 newVelocity.X = 0;
877 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
878 newVelocity.Y = 0;
879 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
880 newVelocity.Z = 0;
881
882 // ==================================================================
883 // Clamp high or low velocities
884 float newVelocityLengthSq = newVelocity.LengthSquared();
885 if (newVelocityLengthSq > 1000f)
886 {
887 newVelocity /= newVelocity.Length();
888 newVelocity *= 1000f;
889 }
890 else if (newVelocityLengthSq < 0.001f)
891 newVelocity = Vector3.Zero;
892
893 // ==================================================================
894 // Stuff new linear velocity into the vehicle.
895 // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
896 VehicleVelocity = newVelocity;
897
898 // Other linear forces are applied as forces.
899 Vector3 totalDownForce = newForce * m_vehicleMass;
900 if (!totalDownForce.ApproxEquals(Vector3.Zero, 0.01f))
901 {
902 VehicleAddForce(totalDownForce);
903 }
904
905 VDetailLog("{0}, MoveLinear,done,newVel={1},totDown={2},IsColliding={3}",
906 Prim.LocalID, newVelocity, totalDownForce, Prim.IsColliding);
907 VDetailLog("{0}, MoveLinear,done,linContrib={1},terrContrib={2},hoverContrib={3},limitContrib={4},buoyContrib={5}",
908 Prim.LocalID,
909 linearMotorContribution, terrainHeightContribution, hoverContribution,
910 limitMotorUpContribution, buoyancyContribution
911 );
912
913 } // end MoveLinear()
914
915 public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
916 {
917 Vector3 ret = Vector3.Zero;
918 // If below the terrain, move us above the ground a little.
919 // TODO: Consider taking the rotated size of the object or possibly casting a ray.
920 if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
921 {
922 // TODO: correct position by applying force rather than forcing position.
923 Vector3 newPosition = VehiclePosition;
924 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
925 VehiclePosition = newPosition;
926 VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
927 Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
928 }
929 return ret;
930 }
931
932 public Vector3 ComputeLinearHover(float pTimestep)
933 {
934 Vector3 ret = Vector3.Zero;
935
936 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
937 // m_VhoverTimescale: time to achieve height
938 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
939 {
940 // We should hover, get the target height
941 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
942 {
943 m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
944 }
945 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
946 {
947 m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
948 }
949 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
950 {
951 m_VhoverTargetHeight = m_VhoverHeight;
952 }
953
954 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
955 {
956 // If body is already heigher, use its height as target height
957 if (VehiclePosition.Z > m_VhoverTargetHeight)
958 m_VhoverTargetHeight = VehiclePosition.Z;
959 }
960
961 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
962 {
963 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
964 {
965 Vector3 pos = VehiclePosition;
966 pos.Z = m_VhoverTargetHeight;
967 VehiclePosition = pos;
968 }
969 }
970 else
971 {
972 // Error is positive if below the target and negative if above.
973 float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
974 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
975
976 // TODO: implement m_VhoverEfficiency correctly
977 if (Math.Abs(verticalError) > m_VhoverEfficiency)
978 {
979 ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
980 }
981 }
982
983 VDetailLog("{0}, MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
984 Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
985 }
986
987 return ret;
988 }
989
990 public bool ComputeLinearBlockingEndPoint(float pTimestep)
991 {
992 bool changed = false;
993
994 Vector3 pos = VehiclePosition;
995 Vector3 posChange = pos - m_lastPositionVector;
996 if (m_BlockingEndPoint != Vector3.Zero)
997 {
998 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
999 {
1000 pos.X -= posChange.X + 1;
1001 changed = true;
1002 }
1003 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
1004 {
1005 pos.Y -= posChange.Y + 1;
1006 changed = true;
1007 }
1008 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
1009 {
1010 pos.Z -= posChange.Z + 1;
1011 changed = true;
1012 }
1013 if (pos.X <= 0)
1014 {
1015 pos.X += posChange.X + 1;
1016 changed = true;
1017 }
1018 if (pos.Y <= 0)
1019 {
1020 pos.Y += posChange.Y + 1;
1021 changed = true;
1022 }
1023 if (changed)
1024 {
1025 VehiclePosition = pos;
1026 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
1027 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
1028 }
1029 }
1030 return changed;
1031 }
1032
1033 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1034 // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
1035 // used with conjunction with banking: the strength of the banking will decay when the
1036 // vehicle no longer experiences collisions. The decay timescale is the same as
1037 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
1038 // when they are in mid jump.
1039 // TODO: this code is wrong. Also, what should it do for boats (height from water)?
1040 // This is just using the ground and a general collision check. Should really be using
1041 // a downward raycast to find what is below.
1042 public Vector3 ComputeLinearMotorUp(float pTimestep)
1043 {
1044 Vector3 ret = Vector3.Zero;
1045 float distanceAboveGround = 0f;
1046
1047 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
1048 {
1049 float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
1050 distanceAboveGround = VehiclePosition.Z - targetHeight;
1051 // Not colliding if the vehicle is off the ground
1052 if (!Prim.IsColliding)
1053 {
1054 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
1055 ret = new Vector3(0, 0, -distanceAboveGround);
1056 }
1057 // TODO: this calculation is wrong. From the description at
1058 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
1059 // has a decay factor. This says this force should
1060 // be computed with a motor.
1061 // TODO: add interaction with banking.
1062 }
1063 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
1064 Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret);
1065 return ret;
1066 }
1067
1068 // =======================================================================
1069 // =======================================================================
1070 // Apply the effect of the angular motor.
1071 // The 'contribution' is how much angular correction velocity each function wants.
1072 // All the contributions are added together and the resulting velocity is
1073 // set directly on the vehicle.
1074 private void MoveAngular(float pTimestep)
1075 {
1076 // The user wants this many radians per second angular change?
1077 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
1078
1079 // ==================================================================
1080 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
1081 // This flag prevents linear deflection parallel to world z-axis. This is useful
1082 // for preventing ground vehicles with large linear deflection, like bumper cars,
1083 // from climbing their linear deflection into the sky.
1084 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1085 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1086 {
1087 angularMotorContribution.X = 0f;
1088 angularMotorContribution.Y = 0f;
1089 VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
1090 }
1091
1092 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
1093
1094 Vector3 deflectionContribution = ComputeAngularDeflection();
1095
1096 Vector3 bankingContribution = ComputeAngularBanking();
1097
1098 // ==================================================================
1099 m_lastVertAttractor = verticalAttractionContribution;
1100
1101 m_lastAngularVelocity = angularMotorContribution
1102 + verticalAttractionContribution
1103 + deflectionContribution
1104 + bankingContribution;
1105
1106 // ==================================================================
1107 // Apply the correction velocity.
1108 // TODO: Should this be applied as an angular force (torque)?
1109 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1110 {
1111 VehicleRotationalVelocity = m_lastAngularVelocity;
1112
1113 VDetailLog("{0}, MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
1114 Prim.LocalID,
1115 angularMotorContribution, verticalAttractionContribution,
1116 bankingContribution, deflectionContribution,
1117 m_lastAngularVelocity
1118 );
1119 }
1120 else
1121 {
1122 // The vehicle is not adding anything angular wise.
1123 VehicleRotationalVelocity = Vector3.Zero;
1124 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
1125 }
1126
1127 // ==================================================================
1128 //Offset section
1129 if (m_linearMotorOffset != Vector3.Zero)
1130 {
1131 //Offset of linear velocity doesn't change the linear velocity,
1132 // but causes a torque to be applied, for example...
1133 //
1134 // IIIII >>> IIIII
1135 // IIIII >>> IIIII
1136 // IIIII >>> IIIII
1137 // ^
1138 // | Applying a force at the arrow will cause the object to move forward, but also rotate
1139 //
1140 //
1141 // The torque created is the linear velocity crossed with the offset
1142
1143 // TODO: this computation should be in the linear section
1144 // because that is where we know the impulse being applied.
1145 Vector3 torqueFromOffset = Vector3.Zero;
1146 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
1147 if (float.IsNaN(torqueFromOffset.X))
1148 torqueFromOffset.X = 0;
1149 if (float.IsNaN(torqueFromOffset.Y))
1150 torqueFromOffset.Y = 0;
1151 if (float.IsNaN(torqueFromOffset.Z))
1152 torqueFromOffset.Z = 0;
1153
1154 VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
1155 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
1156 }
1157
1158 }
1159 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1160 // Some vehicles, like boats, should always keep their up-side up. This can be done by
1161 // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
1162 // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
1163 // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
1164 // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
1165 // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
1166 // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
1167 public Vector3 ComputeAngularVerticalAttraction()
1168 {
1169 Vector3 ret = Vector3.Zero;
1170
1171 // If vertical attaction timescale is reasonable
1172 if (m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1173 {
1174 // Take a vector pointing up and convert it from world to vehicle relative coords.
1175 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
1176
1177 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
1178 // is now:
1179 // leaning to one side: rotated around the X axis with the Y value going
1180 // from zero (nearly straight up) to one (completely to the side)) or
1181 // leaning front-to-back: rotated around the Y axis with the value of X being between
1182 // zero and one.
1183 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
1184
1185 // Y error means needed rotation around X axis and visa versa.
1186 // Since the error goes from zero to one, the asin is the corresponding angle.
1187 ret.X = (float)Math.Asin(verticalError.Y);
1188 // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
1189 ret.Y = -(float)Math.Asin(verticalError.X);
1190
1191 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
1192 if (verticalError.Z < 0f)
1193 {
1194 ret.X += PIOverFour;
1195 ret.Y += PIOverFour;
1196 }
1197
1198 // 'ret' is now the necessary velocity to correct tilt in one second.
1199 // Correction happens over a number of seconds.
1200 Vector3 unscaledContrib = ret;
1201 ret /= m_verticalAttractionTimescale;
1202
1203 VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
1204 Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
1205 }
1206 return ret;
1207 }
1208
1209 // Return the angular correction to correct the direction the vehicle is pointing to be
1210 // the direction is should want to be pointing.
1211 // The vehicle is moving in some direction and correct its orientation to it is pointing
1212 // in that direction.
1213 // TODO: implement reference frame.
1214 public Vector3 ComputeAngularDeflection()
1215 {
1216 Vector3 ret = Vector3.Zero;
1217 return ret; // DEBUG DEBUG DEBUG
1218 // Disable angular deflection for the moment.
1219 // Since angularMotorUp and angularDeflection are computed independently, they will calculate
1220 // approximately the same X or Y correction. When added together (when contributions are combined)
1221 // this creates an over-correction and then wabbling as the target is overshot.
1222 // TODO: rethink how the different correction computations inter-relate.
1223
1224 if (m_angularDeflectionEfficiency != 0)
1225 {
1226 // The direction the vehicle is moving
1227 Vector3 movingDirection = VehicleVelocity;
1228 movingDirection.Normalize();
1229
1230 // The direction the vehicle is pointing
1231 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
1232 pointingDirection.Normalize();
1233
1234 // The difference between what is and what should be.
1235 Vector3 deflectionError = movingDirection - pointingDirection;
1236
1237 // Don't try to correct very large errors (not our job)
1238 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
1239 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
1240 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
1241
1242 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
1243
1244 // Scale the correction by recovery timescale and efficiency
1245 ret = (-deflectionError) * m_angularDeflectionEfficiency;
1246 ret /= m_angularDeflectionTimescale;
1247
1248 VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
1249 Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
1250 VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
1251 Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
1252 }
1253 return ret;
1254 }
1255
1256 // Return an angular change to rotate the vehicle around the Z axis when the vehicle
1257 // is tipped around the X axis.
1258 // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
1259 // The vertical attractor feature must be enabled in order for the banking behavior to
1260 // function. The way banking works is this: a rotation around the vehicle's roll-axis will
1261 // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
1262 // of the yaw effect will be proportional to the
1263 // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
1264 // velocity along its preferred axis of motion.
1265 // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
1266 // positive rotation (by the right-hand rule) about the roll-axis will effect a
1267 // (negative) torque around the yaw-axis, making it turn to the right--that is the
1268 // vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
1269 // Negating the banking coefficient will make it so that the vehicle leans to the
1270 // outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
1271 // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
1272 // banking vehicles do what you want rather than what the laws of physics allow.
1273 // For example, consider a real motorcycle...it must be moving forward in order for
1274 // it to turn while banking, however video-game motorcycles are often configured
1275 // to turn in place when at a dead stop--because they are often easier to control
1276 // that way using the limited interface of the keyboard or game controller. The
1277 // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
1278 // banking by functioning as a slider between a banking that is correspondingly
1279 // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
1280 // banking effect depends only on the vehicle's rotation about its roll-axis compared
1281 // to "dynamic" where the banking is also proportional to its velocity along its
1282 // roll-axis. Finding the best value of the "mixture" will probably require trial and error.
1283 // The time it takes for the banking behavior to defeat a preexisting angular velocity about the
1284 // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
1285 // bank quickly then give it a banking timescale of about a second or less, otherwise you can
1286 // make a sluggish vehicle by giving it a timescale of several seconds.
1287 public Vector3 ComputeAngularBanking()
1288 {
1289 Vector3 ret = Vector3.Zero;
1290
1291 if (m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
1292 {
1293 // This works by rotating a unit vector to the orientation of the vehicle. The
1294 // roll (tilt) will be Y component of a tilting Z vector (zero for no tilt
1295 // up to one for full over).
1296 Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
1297
1298 // Figure out the yaw value for this much roll.
1299 float turnComponent = rollComponents.Y * rollComponents.Y * m_bankingEfficiency;
1300 // Keep the sign
1301 if (rollComponents.Y < 0f)
1302 turnComponent = -turnComponent;
1303
1304 // TODO: there must be a better computation of the banking force.
1305 float bankingTurnForce = turnComponent;
1306
1307 // actual error = static turn error + dynamic turn error
1308 float mixedBankingError = bankingTurnForce * (1f - m_bankingMix) + bankingTurnForce * m_bankingMix * VehicleForwardSpeed;
1309 // TODO: the banking effect should not go to infinity but what to limit it to?
1310 mixedBankingError = ClampInRange(-20f, mixedBankingError, 20f);
1311
1312 // Build the force vector to change rotation from what it is to what it should be
1313 ret.Z = -mixedBankingError;
1314
1315 // Don't do it all at once.
1316 ret /= m_bankingTimescale;
1317
1318 VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},turnComp={3},bankErr={4},mixedBankErr={5},ret={6}",
1319 Prim.LocalID, rollComponents, VehicleForwardSpeed, turnComponent, bankingTurnForce, mixedBankingError, ret);
1320 }
1321 return ret;
1322 }
1323
1324 // This is from previous instantiations of XXXDynamics.cs.
1325 // Applies roll reference frame.
1326 // TODO: is this the right way to separate the code to do this operation?
1327 // Should this be in MoveAngular()?
1328 internal void LimitRotation(float timestep)
1329 {
1330 Quaternion rotq = VehicleOrientation;
1331 Quaternion m_rot = rotq;
1332 if (m_RollreferenceFrame != Quaternion.Identity)
1333 {
1334 if (rotq.X >= m_RollreferenceFrame.X)
1335 {
1336 m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
1337 }
1338 if (rotq.Y >= m_RollreferenceFrame.Y)
1339 {
1340 m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
1341 }
1342 if (rotq.X <= -m_RollreferenceFrame.X)
1343 {
1344 m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
1345 }
1346 if (rotq.Y <= -m_RollreferenceFrame.Y)
1347 {
1348 m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
1349 }
1350 }
1351 if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
1352 {
1353 m_rot.X = 0;
1354 m_rot.Y = 0;
1355 }
1356 if (rotq != m_rot)
1357 {
1358 VehicleOrientation = m_rot;
1359 VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
1360 }
1361
1362 }
1363
1364 private float ClampInRange(float low, float val, float high)
1365 {
1366 return Math.Max(low, Math.Min(val, high));
1367 // return Utils.Clamp(val, low, high);
1368 }
1369
1370 // Invoke the detailed logger and output something if it's enabled.
1371 private void VDetailLog(string msg, params Object[] args)
1372 {
1373 if (Prim.PhysicsScene.VehicleLoggingEnabled)
1374 Prim.PhysicsScene.DetailLog(msg, args);
1375 }
1376 }
1377}
generated by cgit v1.1 at 2024-12-23 15:14:56 +0000