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