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.cs1003
1 files changed, 1003 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..d106677
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1003 @@
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 = 1000;
474 m_bankingEfficiency = 0;
475 m_bankingMix = 1;
476 m_bankingTimescale = 10;
477 m_flags &=
478 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
479 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
480 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
481 break;
482 case Vehicle.TYPE_CAR:
483 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
484 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
485 m_linearMotorTimescale = 1;
486 m_linearMotorDecayTimescale = 60 * m_invtimestep;
487 m_angularMotorTimescale = 1;
488 m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
489 m_VhoverHeight = 0;
490 m_VhoverEfficiency = 0;
491 m_VhoverTimescale = 1000;
492 m_VehicleBuoyancy = 0;
493 m_linearDeflectionEfficiency = 1;
494 m_linearDeflectionTimescale = 2;
495 m_angularDeflectionEfficiency = 0;
496 m_angularDeflectionTimescale = 10;
497 m_verticalAttractionEfficiency = 1f;
498 m_verticalAttractionTimescale = 10f;
499 m_bankingEfficiency = -0.2f;
500 m_bankingMix = 1;
501 m_bankingTimescale = 1;
502 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
503 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
504 VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
505 break;
506 case Vehicle.TYPE_BOAT:
507 m_linearFrictionTimescale = new Vector3(10, 3, 2);
508 m_angularFrictionTimescale = new Vector3(10, 10, 10);
509 m_linearMotorTimescale = 5;
510 m_linearMotorDecayTimescale = 60 * m_invtimestep;
511 m_angularMotorTimescale = 4;
512 m_angularMotorDecayTimescale = 4 * m_invtimestep;
513 m_VhoverHeight = 0;
514 m_VhoverEfficiency = 0.5f;
515 m_VhoverTimescale = 2;
516 m_VehicleBuoyancy = 1;
517 m_linearDeflectionEfficiency = 0.5f;
518 m_linearDeflectionTimescale = 3;
519 m_angularDeflectionEfficiency = 0.5f;
520 m_angularDeflectionTimescale = 5;
521 m_verticalAttractionEfficiency = 0.5f;
522 m_verticalAttractionTimescale = 5f;
523 m_bankingEfficiency = -0.3f;
524 m_bankingMix = 0.8f;
525 m_bankingTimescale = 1;
526 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
527 VehicleFlag.HOVER_GLOBAL_HEIGHT |
528 VehicleFlag.HOVER_UP_ONLY |
529 VehicleFlag.LIMIT_ROLL_ONLY);
530 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
531 VehicleFlag.LIMIT_MOTOR_UP |
532 VehicleFlag.HOVER_WATER_ONLY);
533 break;
534 case Vehicle.TYPE_AIRPLANE:
535 m_linearFrictionTimescale = new Vector3(200, 10, 5);
536 m_angularFrictionTimescale = new Vector3(20, 20, 20);
537 m_linearMotorTimescale = 2;
538 m_linearMotorDecayTimescale = 60 * m_invtimestep;
539 m_angularMotorTimescale = 4;
540 m_angularMotorDecayTimescale = 8 * m_invtimestep;
541 m_VhoverHeight = 0;
542 m_VhoverEfficiency = 0.5f;
543 m_VhoverTimescale = 1000;
544 m_VehicleBuoyancy = 0;
545 m_linearDeflectionEfficiency = 0.5f;
546 m_linearDeflectionTimescale = 0.5f;
547 m_angularDeflectionEfficiency = 1;
548 m_angularDeflectionTimescale = 2;
549 m_verticalAttractionEfficiency = 0.9f;
550 m_verticalAttractionTimescale = 2f;
551 m_bankingEfficiency = 1;
552 m_bankingMix = 0.7f;
553 m_bankingTimescale = 2;
554 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
555 VehicleFlag.HOVER_TERRAIN_ONLY |
556 VehicleFlag.HOVER_GLOBAL_HEIGHT |
557 VehicleFlag.HOVER_UP_ONLY |
558 VehicleFlag.NO_DEFLECTION_UP |
559 VehicleFlag.LIMIT_MOTOR_UP);
560 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
561 break;
562 case Vehicle.TYPE_BALLOON:
563 m_linearFrictionTimescale = new Vector3(5, 5, 5);
564 m_angularFrictionTimescale = new Vector3(10, 10, 10);
565 m_linearMotorTimescale = 5;
566 m_linearMotorDecayTimescale = 60 * m_invtimestep;
567 m_angularMotorTimescale = 6;
568 m_angularMotorDecayTimescale = 10 * m_invtimestep;
569 m_VhoverHeight = 5;
570 m_VhoverEfficiency = 0.8f;
571 m_VhoverTimescale = 10;
572 m_VehicleBuoyancy = 1;
573 m_linearDeflectionEfficiency = 0;
574 m_linearDeflectionTimescale = 5 * m_invtimestep;
575 m_angularDeflectionEfficiency = 0;
576 m_angularDeflectionTimescale = 5;
577 m_verticalAttractionEfficiency = 0f;
578 m_verticalAttractionTimescale = 1000f;
579 m_bankingEfficiency = 0;
580 m_bankingMix = 0.7f;
581 m_bankingTimescale = 5;
582 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
583 VehicleFlag.HOVER_TERRAIN_ONLY |
584 VehicleFlag.HOVER_UP_ONLY |
585 VehicleFlag.NO_DEFLECTION_UP |
586 VehicleFlag.LIMIT_MOTOR_UP);
587 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
588 VehicleFlag.HOVER_GLOBAL_HEIGHT);
589 break;
590 }
591
592 }//end SetDefaultsForType
593
594 internal void Stop()
595 {
596 m_lmEfect = 0;
597 m_amEfect = 0;
598 m_ffactor = 1f;
599 }
600
601 public static Vector3 Xrot(Quaternion rot)
602 {
603 Vector3 vec;
604 rot.Normalize(); // just in case
605 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
606 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
607 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
608 return vec;
609 }
610
611 public static Vector3 Zrot(Quaternion rot)
612 {
613 Vector3 vec;
614 rot.Normalize(); // just in case
615 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
616 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
617 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
618
619 return vec;
620 }
621
622 private const float pi = (float)Math.PI;
623 private const float halfpi = 0.5f * (float)Math.PI;
624
625 public static Vector3 ubitRot2Euler(Quaternion rot)
626 {
627 // returns roll in X
628 // pitch in Y
629 // yaw in Z
630 Vector3 vec;
631
632 // assuming rot is normalised
633 // rot.Normalize();
634
635 float zX = rot.X * rot.Z + rot.Y * rot.W;
636
637 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 if (zX > 0.49999f)
644 {
645 vec.X = 0;
646 vec.Y = halfpi;
647 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
648 }
649 else
650 {
651 vec.Y = (float)Math.Asin(2 * zX);
652
653 float sqw = rot.W * rot.W;
654
655 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
656 float zZ = rot.Z * rot.Z + sqw - 0.5f;
657
658 vec.X = (float)Math.Atan2(minuszY, zZ);
659
660 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
661 float yY = rot.X * rot.X + sqw - 0.5f;
662 vec.Z = (float)Math.Atan2(yX, yY);
663 }
664 return vec;
665 }
666
667 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
668 {
669 // assuming rot is normalised
670 // rot.Normalize();
671
672 float zX = rot.X * rot.Z + rot.Y * rot.W;
673
674 if (zX < -0.49999f)
675 {
676 roll = 0;
677 pitch = -halfpi;
678 }
679 else if (zX > 0.49999f)
680 {
681 roll = 0;
682 pitch = halfpi;
683 }
684 else
685 {
686 pitch = (float)Math.Asin(2 * zX);
687
688 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
689 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
690
691 roll = (float)Math.Atan2(minuszY, zZ);
692 }
693 return ;
694 }
695
696 internal void Step()
697 {
698 IntPtr Body = rootPrim.Body;
699
700 d.Quaternion rot = d.BodyGetQuaternion(Body);
701 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
702 Quaternion rotq = objrotq; // rotq = rotation of object
703 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
704 Quaternion irotq = Quaternion.Inverse(rotq);
705
706 d.Vector3 dvtmp;
707 Vector3 tmpV;
708 Vector3 curVel; // velocity in world
709 Vector3 curAngVel; // angular velocity in world
710 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
711 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
712 d.Vector3 dtorque = new d.Vector3();
713
714 dvtmp = d.BodyGetLinearVel(Body);
715 curVel.X = dvtmp.X;
716 curVel.Y = dvtmp.Y;
717 curVel.Z = dvtmp.Z;
718 Vector3 curLocalVel = curVel * irotq; // current velocity in local
719
720 dvtmp = d.BodyGetAngularVel(Body);
721 curAngVel.X = dvtmp.X;
722 curAngVel.Y = dvtmp.Y;
723 curAngVel.Z = dvtmp.Z;
724 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
725
726 // linear motor
727 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
728 {
729 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
730 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
731 tmpV *= rotq; // to world
732
733 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
734 tmpV.Z = 0;
735
736 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
737 {
738 // have offset, do it now
739 tmpV *= rootPrim.Mass;
740 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
741 }
742 else
743 {
744 force.X += tmpV.X;
745 force.Y += tmpV.Y;
746 force.Z += tmpV.Z;
747 }
748 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
749
750 m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
751 }
752 else
753 {
754 m_lmEfect = 0;
755 m_ffactor = 1f;
756 }
757
758 // friction
759 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
760 {
761 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
762 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
763 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
764 tmpV *= rotq; // to world
765 force.X += tmpV.X;
766 force.Y += tmpV.Y;
767 force.Z += tmpV.Z;
768 }
769
770 // hover
771 if (m_VhoverTimescale < 300)
772 {
773 d.Vector3 pos = d.BodyGetPosition(Body);
774
775 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
776 float perr;
777
778 // default to global but don't go underground
779 if (t < m_VhoverHeight)
780 perr = m_VhoverHeight - pos.Z;
781 else
782 perr = t - pos.Z; ;
783
784 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
785 {
786 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
787 {
788 perr += _pParentScene.GetWaterLevel();
789 }
790 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
791 {
792 perr += t;
793 }
794 else
795 {
796 float w = _pParentScene.GetWaterLevel();
797 if (t > w)
798 perr += t;
799 else
800 perr += w;
801 }
802 }
803 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
804 {
805 force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / m_timestep;
806 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
807 }
808 else // no buoyancy
809 force.Z += _pParentScene.gravityz;
810 }
811 else
812 {
813 // default gravity and Buoyancy
814 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
815 }
816
817 // linear deflection
818 if (m_linearDeflectionEfficiency > 0)
819 {
820 float len = curVel.Length();
821 if (len > 0.01) // if moving
822 {
823
824 Vector3 atAxis;
825 atAxis = Xrot(rotq); // where are we pointing to
826 atAxis *= len; // make it same size as world velocity vector
827
828 tmpV = -atAxis; // oposite direction
829 atAxis -= curVel; // error to one direction
830 len = atAxis.LengthSquared();
831
832 tmpV -= curVel; // error to oposite
833 float lens = tmpV.LengthSquared();
834
835 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
836 {
837 if (len < lens)
838 tmpV = atAxis;
839
840 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
841 force.X += tmpV.X;
842 force.Y += tmpV.Y;
843 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
844 force.Z += tmpV.Z;
845 }
846 }
847 }
848
849 // angular motor
850 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
851 {
852 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
853 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
854 torque.X += tmpV.X;
855 torque.Y += tmpV.Y;
856 torque.Z += tmpV.Z;
857 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
858 }
859 else
860 m_amEfect = 0;
861
862 // angular friction
863 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
864 {
865 torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
866 torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
867 torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
868 }
869
870 // angular deflection
871 if (m_angularDeflectionEfficiency > 0)
872 {
873 Vector3 dirv;
874
875 if (curLocalVel.X > 0.01f)
876 dirv = curLocalVel;
877 else if (curLocalVel.X < -0.01f)
878 // use oposite
879 dirv = -curLocalVel;
880 else
881 {
882 // make it fall into small positive x case
883 dirv.X = 0.01f;
884 dirv.Y = curLocalVel.Y;
885 dirv.Z = curLocalVel.Z;
886 }
887
888 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
889
890 if (Math.Abs(dirv.Z) > 0.01)
891 {
892 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
893 }
894
895 if (Math.Abs(dirv.Y) > 0.01)
896 {
897 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
898 }
899 }
900
901 // vertical atractor
902 if (m_verticalAttractionTimescale < 300)
903 {
904 float roll;
905 float pitch;
906
907 GetRollPitch(irotq, out roll, out pitch);
908
909 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale * m_invtimestep;
910 float ftmp2;
911 if (m_bankingEfficiency == 0)
912 ftmp2 = m_verticalAttractionEfficiency * m_invtimestep;
913 else
914 ftmp2 = 0;
915
916 if (roll > halfpi)
917 roll = pi - roll;
918 else if (roll < -halfpi)
919 roll = -pi - roll;
920
921 float effroll = pitch / halfpi;
922 effroll *= effroll;
923 effroll = 1 - effroll;
924 effroll *= roll;
925
926 if (Math.Abs(effroll) > 0.01) // roll
927 {
928 torque.X -= -effroll * ftmp + curLocalAngVel.X * ftmp2;
929 }
930
931 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
932 {
933 float effpitch = roll / halfpi;
934 effpitch *= effpitch;
935 effpitch = 1 - effpitch;
936 effpitch *= pitch;
937
938 if (Math.Abs(effpitch) > 0.01) // pitch
939 {
940 torque.Y -= -effpitch * ftmp + curLocalAngVel.Y * ftmp2;
941 }
942 }
943
944 if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
945 {
946
947 float broll = effroll;
948/*
949 if (broll > halfpi)
950 broll = pi - broll;
951 else if (broll < -halfpi)
952 broll = -pi - broll;
953*/
954 broll *= m_bankingEfficiency;
955 if (m_bankingMix != 0)
956 {
957 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
958 if (vfact > 1.0f) vfact = 1.0f;
959
960 if (curLocalVel.X >= 0)
961 broll *= (1 + (vfact - 1) * m_bankingMix);
962 else
963 broll *= -(1 + (vfact - 1) * m_bankingMix);
964 }
965 // make z rot be in world Z not local as seems to be in sl
966
967 broll = broll / m_bankingTimescale;
968
969 ftmp = -Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
970
971 tmpV.X = ftmp * curAngVel.X;
972 tmpV.Y = ftmp * curAngVel.Y;
973 tmpV.Z = broll + ftmp * curAngVel.Z;
974 tmpV *= irotq;
975
976 torque.X += tmpV.X;
977 torque.Y += tmpV.Y;
978 torque.Z += tmpV.Z;
979 }
980 }
981
982 d.Mass dmass;
983 d.BodyGetMass(Body,out dmass);
984
985 if (force.X != 0 || force.Y != 0 || force.Z != 0)
986 {
987 force *= dmass.mass;
988 d.BodySetForce(Body, force.X, force.Y, force.Z);
989 }
990
991 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
992 {
993 torque *= m_referenceFrame; // to object frame
994 dtorque.X = torque.X ;
995 dtorque.Y = torque.Y;
996 dtorque.Z = torque.Z;
997
998 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
999 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1000 }
1001 }
1002 }
1003}
generated by cgit v1.1 at 2024-12-23 20:21:28 +0000