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