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