opensim – Blame information for rev 1

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