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