aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
diff options
context:
space:
mode:
authorteravus2012-12-23 15:21:25 -0500
committerteravus2012-12-23 15:21:25 -0500
commit92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf (patch)
treeeabcbb758a7512222e84cb51b51b6822cdbf561b /OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
parentRevert "Whitespace change to trigger bot" (diff)
downloadopensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.zip
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.gz
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.bz2
opensim-SC_OLD-92e4f9f412046f8f7926c99c9e56c3a8b6b2edbf.tar.xz
* Initial commit of BulletSimN (BulletSNPlugin). Purely C# implementation of BulletSim. This is designed to be /as close as possible/ to the BulletSim plugin while still being entirely in the managed space to make keeping it up to date easy as possible (no thinking work). This implementation is /slower/ then the c++ version just because it's fully managed, so it's not appropriate for huge sims, but it will run small ones OK. At the moment, it supports all known features of BulletSim. Think of it like.. POS but everything works. To use this plugin, set the physics plugin to BulletSimN.
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-07-07 20:36:06 +0000