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