OpenSim
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Events Macros
ODEDynamics.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 
28 /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29  * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30  * ODEPrim.cs contains methods dealing with Prim editing, Prim
31  * characteristics and Kinetic motion.
32  * ODEDynamics.cs contains methods dealing with Prim Physical motion
33  * (dynamics) and the associated settings. Old Linear and angular
34  * motors for dynamic motion have been replace with MoveLinear()
35  * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36  * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37  * switch between 'VEHICLE' parameter use and general dynamics
38  * settings use.
39  */
40 
41 // Extensive change Ubit 2012
42 
43 using System;
44 using System.Collections.Generic;
45 using System.Reflection;
46 using System.Runtime.InteropServices;
47 using log4net;
48 using OpenMetaverse;
49 using OdeAPI;
50 using OpenSim.Framework;
51 using OpenSim.Region.PhysicsModules.SharedBase;
52 
53 namespace OpenSim.Region.PhysicsModule.ubOde
54 {
55  public class ODEDynamics
56  {
57  public Vehicle Type
58  {
59  get { return m_type; }
60  }
61 
62  private OdePrim rootPrim;
63  private ODEScene _pParentScene;
64 
65  // Vehicle properties
66  // WARNING this are working copies for internel use
67  // their values may not be the corresponding parameter
68 
69  private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70  private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71 
72  private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73 
74  private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75  // HOVER_TERRAIN_ONLY
76  // HOVER_GLOBAL_HEIGHT
77  // NO_DEFLECTION_UP
78  // HOVER_WATER_ONLY
79  // HOVER_UP_ONLY
80  // LIMIT_MOTOR_UP
81  // LIMIT_ROLL_ONLY
82  private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83 
84  // Linear properties
85  private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86  private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87  private float m_linearMotorDecayTimescale = 120;
88  private float m_linearMotorTimescale = 1000;
89  private Vector3 m_linearMotorOffset = Vector3.Zero;
90 
91  //Angular properties
92  private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93  private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94  private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95  private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96 
97  //Deflection properties
98  private float m_angularDeflectionEfficiency = 0;
99  private float m_angularDeflectionTimescale = 1000;
100  private float m_linearDeflectionEfficiency = 0;
101  private float m_linearDeflectionTimescale = 1000;
102 
103  //Banking properties
104  private float m_bankingEfficiency = 0;
105  private float m_bankingMix = 0;
106  private float m_bankingTimescale = 1000;
107 
108  //Hover and Buoyancy properties
109  private float m_VhoverHeight = 0f;
110  private float m_VhoverEfficiency = 0f;
111  private float m_VhoverTimescale = 1000f;
112  private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113  // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114  // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115  // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116 
117  //Attractor properties
118  private float m_verticalAttractionEfficiency = 1.0f; // damped
119  private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120 
121 
122  // auxiliar
123  private float m_lmEfect = 0f; // current linear motor eficiency
124  private float m_lmDecay = 0f; // current linear decay
125 
126  private float m_amEfect = 0; // current angular motor eficiency
127  private float m_amDecay = 0f; // current linear decay
128 
129  private float m_ffactor = 1.0f;
130 
131  private float m_timestep = 0.02f;
132  private float m_invtimestep = 50;
133 
134 
135  float m_ampwr;
136  float m_amdampX;
137  float m_amdampY;
138  float m_amdampZ;
139 
140  float m_gravmod;
141 
142  public float FrictionFactor
143  {
144  get
145  {
146  return m_ffactor;
147  }
148  }
149 
150  public float GravMod
151  {
152  set
153  {
154  m_gravmod = value;
155  }
156  }
157 
158 
159  public ODEDynamics(OdePrim rootp)
160  {
161  rootPrim = rootp;
162  _pParentScene = rootPrim._parent_scene;
163  m_timestep = _pParentScene.ODE_STEPSIZE;
164  m_invtimestep = 1.0f / m_timestep;
165  m_gravmod = rootPrim.GravModifier;
166  }
167 
168  public void DoSetVehicle(VehicleData vd)
169  {
170  m_type = vd.m_type;
171  m_flags = vd.m_flags;
172 
173 
174  // Linear properties
175  m_linearMotorDirection = vd.m_linearMotorDirection;
176 
177  m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
178  if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
179  if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
180  if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
181 
182  m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
183  if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
184  m_linearMotorDecayTimescale += 0.2f;
185  m_linearMotorDecayTimescale *= m_invtimestep;
186 
187  m_linearMotorTimescale = vd.m_linearMotorTimescale;
188  if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
189 
190  m_linearMotorOffset = vd.m_linearMotorOffset;
191 
192  //Angular properties
193  m_angularMotorDirection = vd.m_angularMotorDirection;
194  m_angularMotorTimescale = vd.m_angularMotorTimescale;
195  if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
196 
197  m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
198  if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
199  m_angularMotorDecayTimescale *= m_invtimestep;
200 
201  m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
202  if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
203  if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
204  if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
205 
206  //Deflection properties
207  m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
208  m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
209  if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
210 
211  m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
212  m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
213  if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
214 
215  //Banking properties
216  m_bankingEfficiency = vd.m_bankingEfficiency;
217  m_bankingMix = vd.m_bankingMix;
218  m_bankingTimescale = vd.m_bankingTimescale;
219  if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
220 
221  //Hover and Buoyancy properties
222  m_VhoverHeight = vd.m_VhoverHeight;
223  m_VhoverEfficiency = vd.m_VhoverEfficiency;
224  m_VhoverTimescale = vd.m_VhoverTimescale;
225  if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
226 
227  m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
228 
229  //Attractor properties
230  m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
231  m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
232  if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
233 
234  // Axis
235  m_referenceFrame = vd.m_referenceFrame;
236 
237  m_lmEfect = 0;
238  m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
239  m_amEfect = 0;
240  m_ffactor = 1.0f;
241  }
242 
243  internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
244  {
245  float len;
246 
247  switch (pParam)
248  {
249  case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
250  if (pValue < 0f) pValue = 0f;
251  if (pValue > 1f) pValue = 1f;
252  m_angularDeflectionEfficiency = pValue;
253  break;
254  case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
255  if (pValue < m_timestep) pValue = m_timestep;
256  m_angularDeflectionTimescale = pValue;
257  break;
258  case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
259  if (pValue < m_timestep) pValue = m_timestep;
260  else if (pValue > 120) pValue = 120;
261  m_angularMotorDecayTimescale = pValue * m_invtimestep;
262  m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
263  break;
264  case Vehicle.ANGULAR_MOTOR_TIMESCALE:
265  if (pValue < m_timestep) pValue = m_timestep;
266  m_angularMotorTimescale = pValue;
267  break;
268  case Vehicle.BANKING_EFFICIENCY:
269  if (pValue < -1f) pValue = -1f;
270  if (pValue > 1f) pValue = 1f;
271  m_bankingEfficiency = pValue;
272  break;
273  case Vehicle.BANKING_MIX:
274  if (pValue < 0f) pValue = 0f;
275  if (pValue > 1f) pValue = 1f;
276  m_bankingMix = pValue;
277  break;
278  case Vehicle.BANKING_TIMESCALE:
279  if (pValue < m_timestep) pValue = m_timestep;
280  m_bankingTimescale = pValue;
281  break;
282  case Vehicle.BUOYANCY:
283  if (pValue < -1f) pValue = -1f;
284  if (pValue > 1f) pValue = 1f;
285  m_VehicleBuoyancy = pValue;
286  break;
287  case Vehicle.HOVER_EFFICIENCY:
288  if (pValue < 0f) pValue = 0f;
289  if (pValue > 1f) pValue = 1f;
290  m_VhoverEfficiency = pValue;
291  break;
292  case Vehicle.HOVER_HEIGHT:
293  m_VhoverHeight = pValue;
294  break;
295  case Vehicle.HOVER_TIMESCALE:
296  if (pValue < m_timestep) pValue = m_timestep;
297  m_VhoverTimescale = pValue;
298  break;
299  case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
300  if (pValue < 0f) pValue = 0f;
301  if (pValue > 1f) pValue = 1f;
302  m_linearDeflectionEfficiency = pValue;
303  break;
304  case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
305  if (pValue < m_timestep) pValue = m_timestep;
306  m_linearDeflectionTimescale = pValue;
307  break;
308  case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
309  if (pValue < m_timestep) pValue = m_timestep;
310  else if (pValue > 120) pValue = 120;
311  m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
312  m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
313  break;
314  case Vehicle.LINEAR_MOTOR_TIMESCALE:
315  if (pValue < m_timestep) pValue = m_timestep;
316  m_linearMotorTimescale = pValue;
317  break;
318  case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
319  if (pValue < 0f) pValue = 0f;
320  if (pValue > 1f) pValue = 1f;
321  m_verticalAttractionEfficiency = pValue;
322  break;
323  case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
324  if (pValue < m_timestep) pValue = m_timestep;
325  m_verticalAttractionTimescale = pValue;
326  break;
327 
328  // These are vector properties but the engine lets you use a single float value to
329  // set all of the components to the same value
330  case Vehicle.ANGULAR_FRICTION_TIMESCALE:
331  if (pValue < m_timestep) pValue = m_timestep;
332  m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
333  break;
334  case Vehicle.ANGULAR_MOTOR_DIRECTION:
335  m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
336  len = m_angularMotorDirection.Length();
337  if (len > 12.566f)
338  m_angularMotorDirection *= (12.566f / len);
339 
340  m_amEfect = 1.0f ; // turn it on
341  m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
342 
343  if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
344  && !rootPrim.m_isSelected && !rootPrim.m_disabled)
345  d.BodyEnable(rootPrim.Body);
346  break;
347  case Vehicle.LINEAR_FRICTION_TIMESCALE:
348  if (pValue < m_timestep) pValue = m_timestep;
349  m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
350  break;
351  case Vehicle.LINEAR_MOTOR_DIRECTION:
352  m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
353  len = m_linearMotorDirection.Length();
354  if (len > 100.0f)
355  m_linearMotorDirection *= (100.0f / len);
356 
357  m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
358  m_lmEfect = 1.0f; // turn it on
359 
360  m_ffactor = 0.0f;
361  if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
362  && !rootPrim.m_isSelected && !rootPrim.m_disabled)
363  d.BodyEnable(rootPrim.Body);
364  break;
365  case Vehicle.LINEAR_MOTOR_OFFSET:
366  m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
367  len = m_linearMotorOffset.Length();
368  if (len > 100.0f)
369  m_linearMotorOffset *= (100.0f / len);
370  break;
371  }
372  }//end ProcessFloatVehicleParam
373 
374  internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
375  {
376  float len;
377 
378  switch (pParam)
379  {
380  case Vehicle.ANGULAR_FRICTION_TIMESCALE:
381  if (pValue.X < m_timestep) pValue.X = m_timestep;
382  if (pValue.Y < m_timestep) pValue.Y = m_timestep;
383  if (pValue.Z < m_timestep) pValue.Z = m_timestep;
384 
385  m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
386  break;
387  case Vehicle.ANGULAR_MOTOR_DIRECTION:
388  m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
389  // Limit requested angular speed to 2 rps= 4 pi rads/sec
390  len = m_angularMotorDirection.Length();
391  if (len > 12.566f)
392  m_angularMotorDirection *= (12.566f / len);
393 
394  m_amEfect = 1.0f; // turn it on
395  m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
396 
397  if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
398  && !rootPrim.m_isSelected && !rootPrim.m_disabled)
399  d.BodyEnable(rootPrim.Body);
400  break;
401  case Vehicle.LINEAR_FRICTION_TIMESCALE:
402  if (pValue.X < m_timestep) pValue.X = m_timestep;
403  if (pValue.Y < m_timestep) pValue.Y = m_timestep;
404  if (pValue.Z < m_timestep) pValue.Z = m_timestep;
405  m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
406  break;
407  case Vehicle.LINEAR_MOTOR_DIRECTION:
408  m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
409  len = m_linearMotorDirection.Length();
410  if (len > 100.0f)
411  m_linearMotorDirection *= (100.0f / len);
412 
413  m_lmEfect = 1.0f; // turn it on
414  m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
415 
416  m_ffactor = 0.0f;
417  if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
418  && !rootPrim.m_isSelected && !rootPrim.m_disabled)
419  d.BodyEnable(rootPrim.Body);
420  break;
421  case Vehicle.LINEAR_MOTOR_OFFSET:
422  m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
423  len = m_linearMotorOffset.Length();
424  if (len > 100.0f)
425  m_linearMotorOffset *= (100.0f / len);
426  break;
427  case Vehicle.BLOCK_EXIT:
428  m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
429  break;
430  }
431  }//end ProcessVectorVehicleParam
432 
433  internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
434  {
435  switch (pParam)
436  {
437  case Vehicle.REFERENCE_FRAME:
438  // m_referenceFrame = Quaternion.Inverse(pValue);
439  m_referenceFrame = pValue;
440  break;
441  case Vehicle.ROLL_FRAME:
442  m_RollreferenceFrame = pValue;
443  break;
444  }
445  }//end ProcessRotationVehicleParam
446 
447  internal void ProcessVehicleFlags(int pParam, bool remove)
448  {
449  if (remove)
450  {
451  m_flags &= ~((VehicleFlag)pParam);
452  }
453  else
454  {
455  m_flags |= (VehicleFlag)pParam;
456  }
457  }//end ProcessVehicleFlags
458 
459  internal void ProcessTypeChange(Vehicle pType)
460  {
461  m_lmEfect = 0;
462 
463  m_amEfect = 0;
464  m_ffactor = 1f;
465 
466  m_linearMotorDirection = Vector3.Zero;
467  m_angularMotorDirection = Vector3.Zero;
468 
469  m_BlockingEndPoint = Vector3.Zero;
470  m_RollreferenceFrame = Quaternion.Identity;
471  m_linearMotorOffset = Vector3.Zero;
472 
473  m_referenceFrame = Quaternion.Identity;
474 
475  // Set Defaults For Type
476  m_type = pType;
477  switch (pType)
478  {
479  case Vehicle.TYPE_NONE:
480  m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
481  m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
482  m_linearMotorTimescale = 1000;
483  m_linearMotorDecayTimescale = 120 * m_invtimestep;
484  m_angularMotorTimescale = 1000;
485  m_angularMotorDecayTimescale = 1000 * m_invtimestep;
486  m_VhoverHeight = 0;
487  m_VhoverEfficiency = 1;
488  m_VhoverTimescale = 1000;
489  m_VehicleBuoyancy = 0;
490  m_linearDeflectionEfficiency = 0;
491  m_linearDeflectionTimescale = 1000;
492  m_angularDeflectionEfficiency = 0;
493  m_angularDeflectionTimescale = 1000;
494  m_bankingEfficiency = 0;
495  m_bankingMix = 1;
496  m_bankingTimescale = 1000;
497  m_verticalAttractionEfficiency = 0;
498  m_verticalAttractionTimescale = 1000;
499 
500  m_flags = (VehicleFlag)0;
501  break;
502 
503  case Vehicle.TYPE_SLED:
504  m_linearFrictionTimescale = new Vector3(30, 1, 1000);
505  m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
506  m_linearMotorTimescale = 1000;
507  m_linearMotorDecayTimescale = 120 * m_invtimestep;
508  m_angularMotorTimescale = 1000;
509  m_angularMotorDecayTimescale = 120 * m_invtimestep;
510  m_VhoverHeight = 0;
511  m_VhoverEfficiency = 1;
512  m_VhoverTimescale = 10;
513  m_VehicleBuoyancy = 0;
514  m_linearDeflectionEfficiency = 1;
515  m_linearDeflectionTimescale = 1;
516  m_angularDeflectionEfficiency = 0;
517  m_angularDeflectionTimescale = 10;
518  m_verticalAttractionEfficiency = 1;
519  m_verticalAttractionTimescale = 1000;
520  m_bankingEfficiency = 0;
521  m_bankingMix = 1;
522  m_bankingTimescale = 10;
523  m_flags &=
524  ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
525  VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
526  m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
527  VehicleFlag.LIMIT_ROLL_ONLY |
528  VehicleFlag.LIMIT_MOTOR_UP);
529  break;
530 
531  case Vehicle.TYPE_CAR:
532  m_linearFrictionTimescale = new Vector3(100, 2, 1000);
533  m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
534  m_linearMotorTimescale = 1;
535  m_linearMotorDecayTimescale = 60 * m_invtimestep;
536  m_angularMotorTimescale = 1;
537  m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
538  m_VhoverHeight = 0;
539  m_VhoverEfficiency = 0;
540  m_VhoverTimescale = 1000;
541  m_VehicleBuoyancy = 0;
542  m_linearDeflectionEfficiency = 1;
543  m_linearDeflectionTimescale = 2;
544  m_angularDeflectionEfficiency = 0;
545  m_angularDeflectionTimescale = 10;
546  m_verticalAttractionEfficiency = 1f;
547  m_verticalAttractionTimescale = 10f;
548  m_bankingEfficiency = -0.2f;
549  m_bankingMix = 1;
550  m_bankingTimescale = 1;
551  m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
552  VehicleFlag.HOVER_TERRAIN_ONLY |
553  VehicleFlag.HOVER_GLOBAL_HEIGHT);
554  m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
555  VehicleFlag.LIMIT_ROLL_ONLY |
556  VehicleFlag.LIMIT_MOTOR_UP |
557  VehicleFlag.HOVER_UP_ONLY);
558  break;
559  case Vehicle.TYPE_BOAT:
560  m_linearFrictionTimescale = new Vector3(10, 3, 2);
561  m_angularFrictionTimescale = new Vector3(10, 10, 10);
562  m_linearMotorTimescale = 5;
563  m_linearMotorDecayTimescale = 60 * m_invtimestep;
564  m_angularMotorTimescale = 4;
565  m_angularMotorDecayTimescale = 4 * m_invtimestep;
566  m_VhoverHeight = 0;
567  m_VhoverEfficiency = 0.5f;
568  m_VhoverTimescale = 2;
569  m_VehicleBuoyancy = 1;
570  m_linearDeflectionEfficiency = 0.5f;
571  m_linearDeflectionTimescale = 3;
572  m_angularDeflectionEfficiency = 0.5f;
573  m_angularDeflectionTimescale = 5;
574  m_verticalAttractionEfficiency = 0.5f;
575  m_verticalAttractionTimescale = 5f;
576  m_bankingEfficiency = -0.3f;
577  m_bankingMix = 0.8f;
578  m_bankingTimescale = 1;
579  m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
580  VehicleFlag.HOVER_GLOBAL_HEIGHT |
581  VehicleFlag.HOVER_UP_ONLY); // |
582 // VehicleFlag.LIMIT_ROLL_ONLY);
583  m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
584  VehicleFlag.LIMIT_MOTOR_UP |
585  VehicleFlag.HOVER_UP_ONLY | // new sl
586  VehicleFlag.HOVER_WATER_ONLY);
587  break;
588 
589  case Vehicle.TYPE_AIRPLANE:
590  m_linearFrictionTimescale = new Vector3(200, 10, 5);
591  m_angularFrictionTimescale = new Vector3(20, 20, 20);
592  m_linearMotorTimescale = 2;
593  m_linearMotorDecayTimescale = 60 * m_invtimestep;
594  m_angularMotorTimescale = 4;
595  m_angularMotorDecayTimescale = 8 * m_invtimestep;
596  m_VhoverHeight = 0;
597  m_VhoverEfficiency = 0.5f;
598  m_VhoverTimescale = 1000;
599  m_VehicleBuoyancy = 0;
600  m_linearDeflectionEfficiency = 0.5f;
601  m_linearDeflectionTimescale = 0.5f;
602  m_angularDeflectionEfficiency = 1;
603  m_angularDeflectionTimescale = 2;
604  m_verticalAttractionEfficiency = 0.9f;
605  m_verticalAttractionTimescale = 2f;
606  m_bankingEfficiency = 1;
607  m_bankingMix = 0.7f;
608  m_bankingTimescale = 2;
609  m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
610  VehicleFlag.HOVER_TERRAIN_ONLY |
611  VehicleFlag.HOVER_GLOBAL_HEIGHT |
612  VehicleFlag.HOVER_UP_ONLY |
613  VehicleFlag.NO_DEFLECTION_UP |
614  VehicleFlag.LIMIT_MOTOR_UP);
615  m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
616  break;
617 
618  case Vehicle.TYPE_BALLOON:
619  m_linearFrictionTimescale = new Vector3(5, 5, 5);
620  m_angularFrictionTimescale = new Vector3(10, 10, 10);
621  m_linearMotorTimescale = 5;
622  m_linearMotorDecayTimescale = 60 * m_invtimestep;
623  m_angularMotorTimescale = 6;
624  m_angularMotorDecayTimescale = 10 * m_invtimestep;
625  m_VhoverHeight = 5;
626  m_VhoverEfficiency = 0.8f;
627  m_VhoverTimescale = 10;
628  m_VehicleBuoyancy = 1;
629  m_linearDeflectionEfficiency = 0;
630  m_linearDeflectionTimescale = 5 * m_invtimestep;
631  m_angularDeflectionEfficiency = 0;
632  m_angularDeflectionTimescale = 5;
633  m_verticalAttractionEfficiency = 1f;
634  m_verticalAttractionTimescale = 1000f;
635  m_bankingEfficiency = 0;
636  m_bankingMix = 0.7f;
637  m_bankingTimescale = 5;
638  m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
639  VehicleFlag.HOVER_TERRAIN_ONLY |
640  VehicleFlag.HOVER_UP_ONLY |
641  VehicleFlag.NO_DEFLECTION_UP |
642  VehicleFlag.LIMIT_MOTOR_UP | //);
643  VehicleFlag.LIMIT_ROLL_ONLY | // new sl
644  VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
645 
646 // m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
647 // VehicleFlag.HOVER_GLOBAL_HEIGHT);
648  break;
649 
650  }
651 
652  m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
653  m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
654 
655  }//end SetDefaultsForType
656 
657  internal void Stop()
658  {
659  m_lmEfect = 0;
660  m_lmDecay = 0f;
661  m_amEfect = 0;
662  m_amDecay = 0;
663  m_ffactor = 1f;
664  }
665 
666  public static Vector3 Xrot(Quaternion rot)
667  {
668  Vector3 vec;
669  rot.Normalize(); // just in case
670  vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
671  vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
672  vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
673  return vec;
674  }
675 
676  public static Vector3 Zrot(Quaternion rot)
677  {
678  Vector3 vec;
679  rot.Normalize(); // just in case
680  vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
681  vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
682  vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
683 
684  return vec;
685  }
686 
687  private const float pi = (float)Math.PI;
688  private const float halfpi = 0.5f * (float)Math.PI;
689  private const float twopi = 2.0f * pi;
690 
691  public static Vector3 ubRot2Euler(Quaternion rot)
692  {
693  // returns roll in X
694  // pitch in Y
695  // yaw in Z
696  Vector3 vec;
697 
698  // assuming rot is normalised
699  // rot.Normalize();
700 
701  float zX = rot.X * rot.Z + rot.Y * rot.W;
702 
703  if (zX < -0.49999f)
704  {
705  vec.X = 0;
706  vec.Y = -halfpi;
707  vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
708  }
709  else if (zX > 0.49999f)
710  {
711  vec.X = 0;
712  vec.Y = halfpi;
713  vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
714  }
715  else
716  {
717  vec.Y = (float)Math.Asin(2 * zX);
718 
719  float sqw = rot.W * rot.W;
720 
721  float minuszY = rot.X * rot.W - rot.Y * rot.Z;
722  float zZ = rot.Z * rot.Z + sqw - 0.5f;
723 
724  vec.X = (float)Math.Atan2(minuszY, zZ);
725 
726  float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
727  float yY = rot.X * rot.X + sqw - 0.5f;
728  vec.Z = (float)Math.Atan2(yX, yY);
729  }
730  return vec;
731  }
732 
733  public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
734  {
735  // assuming rot is normalised
736  // rot.Normalize();
737 
738  float zX = rot.X * rot.Z + rot.Y * rot.W;
739 
740  if (zX < -0.49999f)
741  {
742  roll = 0;
743  pitch = -halfpi;
744  }
745  else if (zX > 0.49999f)
746  {
747  roll = 0;
748  pitch = halfpi;
749  }
750  else
751  {
752  pitch = (float)Math.Asin(2 * zX);
753 
754  float minuszY = rot.X * rot.W - rot.Y * rot.Z;
755  float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
756 
757  roll = (float)Math.Atan2(minuszY, zZ);
758  }
759  return ;
760  }
761 
762  internal void Step()
763  {
764  IntPtr Body = rootPrim.Body;
765 
766  d.Mass dmass;
767  d.BodyGetMass(Body, out dmass);
768 
769  d.Quaternion rot = d.BodyGetQuaternion(Body);
770  Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
771  Quaternion rotq = objrotq; // rotq = rotation of object
772  rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
773  Quaternion irotq = Quaternion.Inverse(rotq);
774 
775  d.Vector3 dvtmp;
776  Vector3 tmpV;
777  Vector3 curVel; // velocity in world
778  Vector3 curAngVel; // angular velocity in world
779  Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
780  Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
781  d.Vector3 dtorque = new d.Vector3();
782 
783  dvtmp = d.BodyGetLinearVel(Body);
784  curVel.X = dvtmp.X;
785  curVel.Y = dvtmp.Y;
786  curVel.Z = dvtmp.Z;
787  Vector3 curLocalVel = curVel * irotq; // current velocity in local
788 
789  dvtmp = d.BodyGetAngularVel(Body);
790  curAngVel.X = dvtmp.X;
791  curAngVel.Y = dvtmp.Y;
792  curAngVel.Z = dvtmp.Z;
793  Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
794 
795  float ldampZ = 0;
796 
797  // linear motor
798  if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
799  {
800  tmpV = m_linearMotorDirection - curLocalVel; // velocity error
801  tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
802  tmpV *= rotq; // to world
803 
804  if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
805  tmpV.Z = 0;
806 
807  if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
808  {
809  // have offset, do it now
810  tmpV *= dmass.mass;
811  d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
812  }
813  else
814  {
815  force.X += tmpV.X;
816  force.Y += tmpV.Y;
817  force.Z += tmpV.Z;
818  }
819 
820  m_lmEfect *= m_lmDecay;
821 // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
822  m_ffactor = 0.0f;
823  }
824  else
825  {
826  m_lmEfect = 0;
827  m_ffactor = 1f;
828  }
829 
830  // hover
831  if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
832  {
833  // d.Vector3 pos = d.BodyGetPosition(Body);
834  d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
835  pos.Z -= 0.21f; // minor offset that seems to be always there in sl
836 
837  float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
838  float perr;
839 
840  // default to global but don't go underground
841  perr = m_VhoverHeight - pos.Z;
842 
843  if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
844  {
845  if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
846  {
847  perr += _pParentScene.GetWaterLevel();
848  }
849  else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
850  {
851  perr += t;
852  }
853  else
854  {
855  float w = _pParentScene.GetWaterLevel();
856  if (t > w)
857  perr += t;
858  else
859  perr += w;
860  }
861  }
862  else if (t > m_VhoverHeight)
863  perr = t - pos.Z; ;
864 
865  if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
866  {
867  ldampZ = m_VhoverEfficiency * m_invtimestep;
868 
869  perr *= (1.0f + ldampZ) / m_VhoverTimescale;
870 
871  // force.Z += perr - curVel.Z * tmp;
872  force.Z += perr;
873  ldampZ *= -curVel.Z;
874 
875  force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
876  }
877  else // no buoyancy
878  force.Z += _pParentScene.gravityz;
879  }
880  else
881  {
882  // default gravity and Buoyancy
883  force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
884  }
885 
886  // linear deflection
887  if (m_linearDeflectionEfficiency > 0)
888  {
889  float len = curVel.Length();
890  if (len > 0.01) // if moving
891  {
892  Vector3 atAxis;
893  atAxis = Xrot(rotq); // where are we pointing to
894  atAxis *= len; // make it same size as world velocity vector
895 
896  tmpV = -atAxis; // oposite direction
897  atAxis -= curVel; // error to one direction
898  len = atAxis.LengthSquared();
899 
900  tmpV -= curVel; // error to oposite
901  float lens = tmpV.LengthSquared();
902 
903  if (len > 0.01 || lens > 0.01) // do nothing if close enougth
904  {
905  if (len < lens)
906  tmpV = atAxis;
907 
908  tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
909  force.X += tmpV.X;
910  force.Y += tmpV.Y;
911  if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
912  force.Z += tmpV.Z;
913  }
914  }
915  }
916 
917  // linear friction/damping
918  if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
919  {
920  tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
921  tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
922  tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
923  tmpV *= rotq; // to world
924 
925  if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
926  tmpV.Z = ldampZ;
927  force.X += tmpV.X;
928  force.Y += tmpV.Y;
929  force.Z += tmpV.Z;
930  }
931 
932  // vertical atractor
933  if (m_verticalAttractionTimescale < 300)
934  {
935  float roll;
936  float pitch;
937 
938 
939 
940  float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;
941 
942  float ftmp2;
943  ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
944  m_amdampX = ftmp2;
945 
946  m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
947 
948  GetRollPitch(irotq, out roll, out pitch);
949 
950  if (roll > halfpi)
951  roll = pi - roll;
952  else if (roll < -halfpi)
953  roll = -pi - roll;
954 
955  float effroll = pitch / halfpi;
956  effroll *= effroll;
957  effroll = 1 - effroll;
958  effroll *= roll;
959 
960 
961  torque.X += effroll * ftmp;
962 
963  if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
964  {
965  float effpitch = roll / halfpi;
966  effpitch *= effpitch;
967  effpitch = 1 - effpitch;
968  effpitch *= pitch;
969 
970  torque.Y += effpitch * ftmp;
971  }
972 
973  if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
974  {
975 
976  float broll = effroll;
977  /*
978  if (broll > halfpi)
979  broll = pi - broll;
980  else if (broll < -halfpi)
981  broll = -pi - broll;
982  */
983  broll *= m_bankingEfficiency;
984  if (m_bankingMix != 0)
985  {
986  float vfact = Math.Abs(curLocalVel.X) / 10.0f;
987  if (vfact > 1.0f) vfact = 1.0f;
988 
989  if (curLocalVel.X >= 0)
990  broll *= (1 + (vfact - 1) * m_bankingMix);
991  else
992  broll *= -(1 + (vfact - 1) * m_bankingMix);
993  }
994  // make z rot be in world Z not local as seems to be in sl
995 
996  broll = broll / m_bankingTimescale;
997 
998 
999  tmpV = Zrot(irotq);
1000  tmpV *= broll;
1001 
1002  torque.X += tmpV.X;
1003  torque.Y += tmpV.Y;
1004  torque.Z += tmpV.Z;
1005 
1006  m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
1007  m_amdampY = m_amdampZ;
1008 
1009  }
1010  else
1011  {
1012  m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1013  m_amdampY = m_amdampX;
1014  }
1015  }
1016  else
1017  {
1018  m_ampwr = 1.0f;
1019  m_amdampX = 1 / m_angularFrictionTimescale.X;
1020  m_amdampY = 1 / m_angularFrictionTimescale.Y;
1021  m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1022  }
1023 
1024  // angular motor
1025  if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
1026  {
1027  tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
1028  tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
1029  torque.X += tmpV.X * m_ampwr;
1030  torque.Y += tmpV.Y * m_ampwr;
1031  torque.Z += tmpV.Z;
1032 
1033  m_amEfect *= m_amDecay;
1034  }
1035  else
1036  m_amEfect = 0;
1037 
1038  // angular deflection
1039  if (m_angularDeflectionEfficiency > 0)
1040  {
1041  Vector3 dirv;
1042 
1043  if (curLocalVel.X > 0.01f)
1044  dirv = curLocalVel;
1045  else if (curLocalVel.X < -0.01f)
1046  // use oposite
1047  dirv = -curLocalVel;
1048  else
1049  {
1050  // make it fall into small positive x case
1051  dirv.X = 0.01f;
1052  dirv.Y = curLocalVel.Y;
1053  dirv.Z = curLocalVel.Z;
1054  }
1055 
1056  float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1057 
1058  if (Math.Abs(dirv.Z) > 0.01)
1059  {
1060  torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
1061  }
1062 
1063  if (Math.Abs(dirv.Y) > 0.01)
1064  {
1065  torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
1066  }
1067  }
1068 
1069  // angular friction
1070  if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1071  {
1072  torque.X -= curLocalAngVel.X * m_amdampX;
1073  torque.Y -= curLocalAngVel.Y * m_amdampY;
1074  torque.Z -= curLocalAngVel.Z * m_amdampZ;
1075  }
1076 
1077 
1078  if (force.X != 0 || force.Y != 0 || force.Z != 0)
1079  {
1080  force *= dmass.mass;
1081  d.BodyAddForce(Body, force.X, force.Y, force.Z);
1082  }
1083 
1084  if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1085  {
1086  torque *= m_referenceFrame; // to object frame
1087  dtorque.X = torque.X ;
1088  dtorque.Y = torque.Y;
1089  dtorque.Z = torque.Z;
1090 
1091  d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
1092  d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1093  }
1094  }
1095  }
1096 }
OpenSim.Region.PhysicsModule.ubOde.ODEDynamics.Xrot
static Vector3 Xrot(Quaternion rot)
Definition: ODEDynamics.cs:666
OpenSim.Region.PhysicsModule.ubOde.ODEDynamics.Zrot
static Vector3 Zrot(Quaternion rot)
Definition: ODEDynamics.cs:676
OpenSim.Region.PhysicsModule.ubOde.ODEDynamics.ubRot2Euler
static Vector3 ubRot2Euler(Quaternion rot)
Definition: ODEDynamics.cs:691
OpenSim.Region.PhysicsModule.ubOde.ODEDynamics.GetRollPitch
static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
Definition: ODEDynamics.cs:733
Generated on Wed Apr 6 2016 04:34:50 for OpenSim by   doxygen 1.8.6