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