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