aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs849
1 files changed, 849 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..80218e7
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,849 @@
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// 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 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
67 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
68
69 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
70
71 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
72 // HOVER_TERRAIN_ONLY
73 // HOVER_GLOBAL_HEIGHT
74 // NO_DEFLECTION_UP
75 // HOVER_WATER_ONLY
76 // HOVER_UP_ONLY
77 // LIMIT_MOTOR_UP
78 // LIMIT_ROLL_ONLY
79 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
80
81 // Linear properties
82 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
83 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
84 private float m_linearMotorDecayTimescale = 120;
85 private float m_linearMotorTimescale = 1000;
86 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
87 private Vector3 m_linearMotorOffset = Vector3.Zero;
88
89 //Angular properties
90 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
91 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
92 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
93 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
94 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
95
96 //Deflection properties
97 private float m_angularDeflectionEfficiency = 0;
98 private float m_angularDeflectionTimescale = 1000;
99 private float m_linearDeflectionEfficiency = 0;
100 private float m_linearDeflectionTimescale = 1000;
101
102 //Banking properties
103 private float m_bankingEfficiency = 0;
104 private float m_bankingMix = 0;
105 private float m_bankingTimescale = 0;
106
107 //Hover and Buoyancy properties
108 private float m_VhoverHeight = 0f;
109 private float m_VhoverEfficiency = 0f;
110 private float m_VhoverTimescale = 1000f;
111 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
112 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
113 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
114 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
115
116 //Attractor properties
117 private float m_verticalAttractionEfficiency = 1.0f; // damped
118 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
119
120 // auxiliar
121 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
122
123 private float m_lmEfect = 0; // current linear motor eficiency
124 private float m_amEfect = 0; // current angular motor eficiency
125
126
127 public ODEDynamics(OdePrim rootp)
128 {
129 rootPrim = rootp;
130 _pParentScene = rootPrim._parent_scene;
131 }
132
133 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
134 {
135 float len;
136 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
137 float timestep = _pParentScene.ODE_STEPSIZE;
138
139 switch (pParam)
140 {
141 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
142 if (pValue < 0f) pValue = 0f;
143 if (pValue > 1f) pValue = 1f;
144 m_angularDeflectionEfficiency = pValue;
145 break;
146 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
147 if (pValue < timestep) pValue = timestep;
148 m_angularDeflectionTimescale = pValue;
149 break;
150 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
151 // if (pValue < timestep) pValue = timestep;
152 // try to make impulses to work a bit better
153 if (pValue < 0.5f) pValue = 0.5f;
154 else if (pValue > 120) pValue = 120;
155 m_angularMotorDecayTimescale = pValue * invtimestep;
156 break;
157 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
158 if (pValue < timestep) pValue = timestep;
159 m_angularMotorTimescale = pValue;
160 break;
161 case Vehicle.BANKING_EFFICIENCY:
162 if (pValue < -1f) pValue = -1f;
163 if (pValue > 1f) pValue = 1f;
164 m_bankingEfficiency = pValue;
165 break;
166 case Vehicle.BANKING_MIX:
167 if (pValue < 0f) pValue = 0f;
168 if (pValue > 1f) pValue = 1f;
169 m_bankingMix = pValue;
170 break;
171 case Vehicle.BANKING_TIMESCALE:
172 if (pValue < timestep) pValue = timestep;
173 m_bankingTimescale = pValue;
174 break;
175 case Vehicle.BUOYANCY:
176 if (pValue < -1f) pValue = -1f;
177 if (pValue > 1f) pValue = 1f;
178 m_VehicleBuoyancy = pValue;
179 break;
180 case Vehicle.HOVER_EFFICIENCY:
181 if (pValue < 0f) pValue = 0f;
182 if (pValue > 1f) pValue = 1f;
183 m_VhoverEfficiency = pValue;
184 break;
185 case Vehicle.HOVER_HEIGHT:
186 m_VhoverHeight = pValue;
187 break;
188 case Vehicle.HOVER_TIMESCALE:
189 if (pValue < timestep) pValue = timestep;
190 m_VhoverTimescale = pValue;
191 break;
192 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
193 if (pValue < 0f) pValue = 0f;
194 if (pValue > 1f) pValue = 1f;
195 m_linearDeflectionEfficiency = pValue;
196 break;
197 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
198 if (pValue < timestep) pValue = timestep;
199 m_linearDeflectionTimescale = pValue;
200 break;
201 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
202 // if (pValue < timestep) pValue = timestep;
203 // try to make impulses to work a bit better
204 if (pValue < 0.5f) pValue = 0.5f;
205 else if (pValue > 120) pValue = 120;
206 m_linearMotorDecayTimescale = pValue * invtimestep;
207 break;
208 case Vehicle.LINEAR_MOTOR_TIMESCALE:
209 if (pValue < timestep) pValue = timestep;
210 m_linearMotorTimescale = pValue;
211 break;
212 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
213 if (pValue < 0f) pValue = 0f;
214 if (pValue > 1f) pValue = 1f;
215 m_verticalAttractionEfficiency = pValue;
216 break;
217 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
218 if (pValue < timestep) pValue = timestep;
219 m_verticalAttractionTimescale = pValue;
220 break;
221
222 // These are vector properties but the engine lets you use a single float value to
223 // set all of the components to the same value
224 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
225 if (pValue < timestep) pValue = timestep;
226 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
227 break;
228 case Vehicle.ANGULAR_MOTOR_DIRECTION:
229 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
230 len = m_angularMotorDirection.Length();
231 if (len > 12.566f)
232 m_angularMotorDirection *= (12.566f / len);
233 m_amEfect = 1.0f; // turn it on
234 break;
235 case Vehicle.LINEAR_FRICTION_TIMESCALE:
236 if (pValue < timestep) pValue = timestep;
237 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
238 break;
239 case Vehicle.LINEAR_MOTOR_DIRECTION:
240 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
241 len = m_linearMotorDirection.Length();
242 if (len > 30.0f)
243 m_linearMotorDirection *= (30.0f / len);
244 m_lmEfect = 1.0f; // turn it on
245 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET:
247 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
248 len = m_linearMotorOffset.Length();
249 if (len > 100.0f)
250 m_linearMotorOffset *= (100.0f / len);
251 break;
252 }
253 }//end ProcessFloatVehicleParam
254
255 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
256 {
257 float len;
258 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
259 float timestep = _pParentScene.ODE_STEPSIZE;
260 switch (pParam)
261 {
262 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
263 if (pValue.X < timestep) pValue.X = timestep;
264 if (pValue.Y < timestep) pValue.Y = timestep;
265 if (pValue.Z < timestep) pValue.Z = timestep;
266
267 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
268 break;
269 case Vehicle.ANGULAR_MOTOR_DIRECTION:
270 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
271 // Limit requested angular speed to 2 rps= 4 pi rads/sec
272 len = m_angularMotorDirection.Length();
273 if (len > 12.566f)
274 m_angularMotorDirection *= (12.566f / len);
275 m_amEfect = 1.0f; // turn it on
276 break;
277 case Vehicle.LINEAR_FRICTION_TIMESCALE:
278 if (pValue.X < timestep) pValue.X = timestep;
279 if (pValue.Y < timestep) pValue.Y = timestep;
280 if (pValue.Z < timestep) pValue.Z = timestep;
281 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
282 break;
283 case Vehicle.LINEAR_MOTOR_DIRECTION:
284 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
285 len = m_linearMotorDirection.Length();
286 if (len > 30.0f)
287 m_linearMotorDirection *= (30.0f / len);
288 m_lmEfect = 1.0f; // turn it on
289 break;
290 case Vehicle.LINEAR_MOTOR_OFFSET:
291 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
292 len = m_linearMotorOffset.Length();
293 if (len > 100.0f)
294 m_linearMotorOffset *= (100.0f / len);
295 break;
296 case Vehicle.BLOCK_EXIT:
297 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
298 break;
299 }
300 }//end ProcessVectorVehicleParam
301
302 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
303 {
304 switch (pParam)
305 {
306 case Vehicle.REFERENCE_FRAME:
307 m_referenceFrame = Quaternion.Inverse(pValue);
308 break;
309 case Vehicle.ROLL_FRAME:
310 m_RollreferenceFrame = pValue;
311 break;
312 }
313 }//end ProcessRotationVehicleParam
314
315 internal void ProcessVehicleFlags(int pParam, bool remove)
316 {
317 if (remove)
318 {
319 m_flags &= ~((VehicleFlag)pParam);
320 }
321 else
322 {
323 m_flags |= (VehicleFlag)pParam;
324 }
325 }//end ProcessVehicleFlags
326
327 internal void ProcessTypeChange(Vehicle pType)
328 {
329 float invtimestep = _pParentScene.ODE_STEPSIZE;
330 m_lmEfect = 0;
331 m_amEfect = 0;
332
333 m_linearMotorDirection = Vector3.Zero;
334 m_angularMotorDirection = Vector3.Zero;
335
336 m_BlockingEndPoint = Vector3.Zero;
337 m_RollreferenceFrame = Quaternion.Identity;
338 m_linearMotorOffset = Vector3.Zero;
339
340 m_referenceFrame = Quaternion.Identity;
341
342 // Set Defaults For Type
343 m_type = pType;
344 switch (pType)
345 {
346 case Vehicle.TYPE_NONE:
347 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
348 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
349 m_linearMotorTimescale = 1000;
350 m_linearMotorDecayTimescale = 120 * invtimestep;
351 m_angularMotorTimescale = 1000;
352 m_angularMotorDecayTimescale = 1000 * invtimestep;
353 m_VhoverHeight = 0;
354 m_VhoverTimescale = 1000;
355 m_VehicleBuoyancy = 0;
356 m_flags = (VehicleFlag)0;
357 break;
358
359 case Vehicle.TYPE_SLED:
360 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
361 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
362 m_linearMotorTimescale = 1000;
363 m_linearMotorDecayTimescale = 120 * invtimestep;
364 m_angularMotorTimescale = 1000;
365 m_angularMotorDecayTimescale = 120 * invtimestep;
366 m_VhoverHeight = 0;
367 m_VhoverEfficiency = 1;
368 m_VhoverTimescale = 10;
369 m_VehicleBuoyancy = 0;
370 m_linearDeflectionEfficiency = 1;
371 m_linearDeflectionTimescale = 1;
372 m_angularDeflectionEfficiency = 0;
373 m_angularDeflectionTimescale = 1000;
374 m_bankingEfficiency = 0;
375 m_bankingMix = 1;
376 m_bankingTimescale = 10;
377 m_flags &=
378 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
379 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
380 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
381 break;
382 case Vehicle.TYPE_CAR:
383 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
384 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
385 m_linearMotorTimescale = 1;
386 m_linearMotorDecayTimescale = 60 * invtimestep;
387 m_angularMotorTimescale = 1;
388 m_angularMotorDecayTimescale = 0.8f * invtimestep;
389 m_VhoverHeight = 0;
390 m_VhoverEfficiency = 0;
391 m_VhoverTimescale = 1000;
392 m_VehicleBuoyancy = 0;
393 m_linearDeflectionEfficiency = 1;
394 m_linearDeflectionTimescale = 2;
395 m_angularDeflectionEfficiency = 0;
396 m_angularDeflectionTimescale = 10;
397 m_verticalAttractionEfficiency = 1f;
398 m_verticalAttractionTimescale = 10f;
399 m_bankingEfficiency = -0.2f;
400 m_bankingMix = 1;
401 m_bankingTimescale = 1;
402 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
403 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
404 VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
405 break;
406 case Vehicle.TYPE_BOAT:
407 m_linearFrictionTimescale = new Vector3(10, 3, 2);
408 m_angularFrictionTimescale = new Vector3(10, 10, 10);
409 m_linearMotorTimescale = 5;
410 m_linearMotorDecayTimescale = 60 * invtimestep;
411 m_angularMotorTimescale = 4;
412 m_angularMotorDecayTimescale = 4 * invtimestep;
413 m_VhoverHeight = 0;
414 m_VhoverEfficiency = 0.5f;
415 m_VhoverTimescale = 2;
416 m_VehicleBuoyancy = 1;
417 m_linearDeflectionEfficiency = 0.5f;
418 m_linearDeflectionTimescale = 3;
419 m_angularDeflectionEfficiency = 0.5f;
420 m_angularDeflectionTimescale = 5;
421 m_verticalAttractionEfficiency = 0.5f;
422 m_verticalAttractionTimescale = 5f;
423 m_bankingEfficiency = -0.3f;
424 m_bankingMix = 0.8f;
425 m_bankingTimescale = 1;
426 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
427 VehicleFlag.HOVER_GLOBAL_HEIGHT |
428 VehicleFlag.HOVER_UP_ONLY |
429 VehicleFlag.LIMIT_ROLL_ONLY);
430 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
431 VehicleFlag.LIMIT_MOTOR_UP |
432 VehicleFlag.HOVER_WATER_ONLY);
433 break;
434 case Vehicle.TYPE_AIRPLANE:
435 m_linearFrictionTimescale = new Vector3(200, 10, 5);
436 m_angularFrictionTimescale = new Vector3(20, 20, 20);
437 m_linearMotorTimescale = 2;
438 m_linearMotorDecayTimescale = 60 * invtimestep;
439 m_angularMotorTimescale = 4;
440 m_angularMotorDecayTimescale = 8 * invtimestep;
441 m_VhoverHeight = 0;
442 m_VhoverEfficiency = 0.5f;
443 m_VhoverTimescale = 1000;
444 m_VehicleBuoyancy = 0;
445 m_linearDeflectionEfficiency = 0.5f;
446 m_linearDeflectionTimescale = 0.5f;
447 m_angularDeflectionEfficiency = 1;
448 m_angularDeflectionTimescale = 2;
449 m_verticalAttractionEfficiency = 0.9f;
450 m_verticalAttractionTimescale = 2f;
451 m_bankingEfficiency = 1;
452 m_bankingMix = 0.7f;
453 m_bankingTimescale = 2;
454 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
455 VehicleFlag.HOVER_TERRAIN_ONLY |
456 VehicleFlag.HOVER_GLOBAL_HEIGHT |
457 VehicleFlag.HOVER_UP_ONLY |
458 VehicleFlag.NO_DEFLECTION_UP |
459 VehicleFlag.LIMIT_MOTOR_UP);
460 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
461 break;
462 case Vehicle.TYPE_BALLOON:
463 m_linearFrictionTimescale = new Vector3(5, 5, 5);
464 m_angularFrictionTimescale = new Vector3(10, 10, 10);
465 m_linearMotorTimescale = 5;
466 m_linearMotorDecayTimescale = 60 * invtimestep;
467 m_angularMotorTimescale = 6;
468 m_angularMotorDecayTimescale = 10 * invtimestep;
469 m_VhoverHeight = 5;
470 m_VhoverEfficiency = 0.8f;
471 m_VhoverTimescale = 10;
472 m_VehicleBuoyancy = 1;
473 m_linearDeflectionEfficiency = 0;
474 m_linearDeflectionTimescale = 5 * invtimestep;
475 m_angularDeflectionEfficiency = 0;
476 m_angularDeflectionTimescale = 5;
477 m_verticalAttractionEfficiency = 0f;
478 m_verticalAttractionTimescale = 1000f;
479 m_bankingEfficiency = 0;
480 m_bankingMix = 0.7f;
481 m_bankingTimescale = 5;
482 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
483 VehicleFlag.HOVER_TERRAIN_ONLY |
484 VehicleFlag.HOVER_UP_ONLY |
485 VehicleFlag.NO_DEFLECTION_UP |
486 VehicleFlag.LIMIT_MOTOR_UP);
487 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
488 VehicleFlag.HOVER_GLOBAL_HEIGHT);
489 break;
490 }
491
492 }//end SetDefaultsForType
493
494 internal void Stop()
495 {
496 m_lmEfect = 0;
497 m_amEfect = 0;
498 }
499
500 public static Vector3 Xrot(Quaternion rot)
501 {
502 Vector3 vec;
503 rot.Normalize(); // just in case
504 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
505 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
506 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
507 return vec;
508 }
509
510 public static Vector3 Zrot(Quaternion rot)
511 {
512 Vector3 vec;
513 rot.Normalize(); // just in case
514 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
515 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
516 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
517
518 return vec;
519 }
520
521 private const float halfpi = 0.5f * (float)Math.PI;
522
523 public static Vector3 ubitRot2Euler(Quaternion rot)
524 {
525 // returns roll in X
526 // pitch in Y
527 // yaw in Z
528 Vector3 vec;
529
530 // assuming rot is normalised
531 // rot.Normalize();
532
533 float zX = rot.X * rot.Z + rot.Y * rot.W;
534
535 if (zX < -0.49999f)
536 {
537 vec.X = 0;
538 vec.Y = -halfpi;
539 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
540 }
541 else if (zX > 0.49999f)
542 {
543 vec.X = 0;
544 vec.Y = halfpi;
545 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
546 }
547 else
548 {
549 vec.Y = (float)Math.Asin(2 * zX);
550
551 float sqw = rot.W * rot.W;
552
553 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
554 float zZ = rot.Z * rot.Z + sqw - 0.5f;
555
556 vec.X = (float)Math.Atan2(minuszY, zZ);
557
558 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
559 float yY = rot.X * rot.X + sqw - 0.5f;
560 vec.Z = (float)Math.Atan2(yX, yY);
561 }
562 return vec;
563 }
564
565 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
566 {
567 // assuming rot is normalised
568 // rot.Normalize();
569
570 float zX = rot.X * rot.Z + rot.Y * rot.W;
571
572 if (zX < -0.49999f)
573 {
574 roll = 0;
575 pitch = -halfpi;
576 }
577 else if (zX > 0.49999f)
578 {
579 roll = 0;
580 pitch = halfpi;
581 }
582 else
583 {
584 pitch = (float)Math.Asin(2 * zX);
585
586 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
587 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
588
589 roll = (float)Math.Atan2(minuszY, zZ);
590 }
591 return ;
592 }
593
594 internal void Step()//float pTimestep)
595 {
596 IntPtr Body = rootPrim.Body;
597
598 d.Quaternion rot = d.BodyGetQuaternion(Body);
599 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
600 Quaternion rotq = objrotq; // rotq = rotation of object
601 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
602 Quaternion irotq = Quaternion.Inverse(rotq);
603
604 d.Vector3 dvtmp;
605 Vector3 tmpV;
606 Vector3 curVel; // velocity in world
607 Vector3 curAngVel; // angular velocity in world
608 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
609 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
610 d.Vector3 dtorque = new d.Vector3();
611
612 dvtmp = d.BodyGetLinearVel(Body);
613 curVel.X = dvtmp.X;
614 curVel.Y = dvtmp.Y;
615 curVel.Z = dvtmp.Z;
616 Vector3 curLocalVel = curVel * irotq; // current velocity in local
617
618 dvtmp = d.BodyGetAngularVel(Body);
619 curAngVel.X = dvtmp.X;
620 curAngVel.Y = dvtmp.Y;
621 curAngVel.Z = dvtmp.Z;
622 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
623
624 // linear motor
625 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
626 {
627 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
628 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
629 tmpV *= rotq; // to world
630
631 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
632 tmpV.Z = 0;
633
634 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
635 {
636 // have offset, do it now
637 tmpV *= rootPrim.Mass;
638 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
639 }
640 else
641 {
642 force.X += tmpV.X;
643 force.Y += tmpV.Y;
644 force.Z += tmpV.Z;
645 }
646 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
647 }
648 else
649 m_lmEfect = 0;
650
651 // friction
652 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
653 {
654 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
655 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
656 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
657 tmpV *= rotq; // to world
658 force.X += tmpV.X;
659 force.Y += tmpV.Y;
660 force.Z += tmpV.Z;
661 }
662
663 // hover
664 if (m_VhoverTimescale < 300)
665 {
666 d.Vector3 pos = d.BodyGetPosition(Body);
667
668 // default to global
669 float perr = m_VhoverHeight - pos.Z;;
670
671 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
672 {
673 perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
674 }
675 else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
676 {
677 perr += _pParentScene.GetWaterLevel();
678 }
679 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
680 {
681 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
682 float w = _pParentScene.GetWaterLevel();
683 if (t > w)
684 perr += t;
685 else
686 perr += w;
687 }
688
689 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
690 {
691 force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
692 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
693 }
694 else // no buoyancy
695 force.Z += _pParentScene.gravityz;
696 }
697 else
698 {
699 // default gravity and buoancy
700 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
701 }
702
703 // linear deflection
704 if (m_linearDeflectionEfficiency > 0)
705 {
706 float len = curVel.Length();
707 Vector3 atAxis;
708 atAxis = Xrot(rotq); // where are we pointing to
709 atAxis *= len; // make it same size as world velocity vector
710 tmpV = -atAxis; // oposite direction
711 atAxis -= curVel; // error to one direction
712 len = atAxis.LengthSquared();
713 tmpV -= curVel; // error to oposite
714 float lens = tmpV.LengthSquared();
715 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
716 {
717 if (len < lens)
718 tmpV = atAxis;
719
720 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
721 force.X += tmpV.X;
722 force.Y += tmpV.Y;
723 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
724 force.Z += tmpV.Z;
725 }
726 }
727
728 // angular motor
729 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
730 {
731 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
732 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
733 torque.X += tmpV.X;
734 torque.Y += tmpV.Y;
735 torque.Z += tmpV.Z;
736 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
737 }
738 else
739 m_amEfect = 0;
740
741 // angular friction
742 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
743 {
744 torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
745 torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
746 torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
747 }
748
749 // angular deflection
750 if (m_angularDeflectionEfficiency > 0)
751 {
752 Vector3 dirv;
753
754 if (curLocalVel.X > 0.01f)
755 dirv = curLocalVel;
756 else if (curLocalVel.X < -0.01f)
757 // use oposite
758 dirv = -curLocalVel;
759 else
760 {
761 // make it fall into small positive x case
762 dirv.X = 0.01f;
763 dirv.Y = curLocalVel.Y;
764 dirv.Z = curLocalVel.Z;
765 }
766
767 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
768
769 if (Math.Abs(dirv.Z) > 0.01)
770 {
771 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
772 }
773
774 if (Math.Abs(dirv.Y) > 0.01)
775 {
776 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
777 }
778 }
779
780 // vertical atractor
781 if (m_verticalAttractionTimescale < 300)
782 {
783 float roll;
784 float pitch;
785
786 GetRollPitch(irotq, out roll, out pitch);
787
788 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
789 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
790
791 if (Math.Abs(roll) > 0.01) // roll
792 {
793 torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2;
794 }
795
796 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
797 {
798 torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2;
799 }
800
801 if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01)
802 {
803 float broll = roll * m_bankingEfficiency; ;
804 if (m_bankingMix != 0)
805 {
806 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
807 if (vfact > 1.0f) vfact = 1.0f;
808 if (curLocalVel.X >= 0)
809 broll *= ((1 - m_bankingMix) + vfact);
810 else
811 broll *= -((1 - m_bankingMix) + vfact);
812 }
813 broll = (broll - curLocalAngVel.Z) / m_bankingTimescale;
814 // torque.Z += broll;
815
816 // make z rot be in world Z not local as seems to be in sl
817 tmpV.X = 0;
818 tmpV.Y = 0;
819 tmpV.Z = broll;
820 tmpV *= irotq;
821
822 torque.X += tmpV.X;
823 torque.Y += tmpV.Y;
824 torque.Z += tmpV.Z;
825 }
826 }
827
828 d.Mass dmass;
829 d.BodyGetMass(Body,out dmass);
830
831 if (force.X != 0 || force.Y != 0 || force.Z != 0)
832 {
833 force *= dmass.mass;
834 d.BodySetForce(Body, force.X, force.Y, force.Z);
835 }
836
837 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
838 {
839 torque *= m_referenceFrame; // to object frame
840 dtorque.X = torque.X;
841 dtorque.Y = torque.Y;
842 dtorque.Z = torque.Z;
843
844 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
845 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
846 }
847 }
848 }
849}
generated by cgit v1.1 at 2024-11-09 01:38:13 +0000