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