aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs792
1 files changed, 792 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..55d6945
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -0,0 +1,792 @@
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 * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
28 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
29 * ODEPrim.cs contains methods dealing with Prim editing, Prim
30 * characteristics and Kinetic motion.
31 * ODEDynamics.cs contains methods dealing with Prim Physical motion
32 * (dynamics) and the associated settings. Old Linear and angular
33 * motors for dynamic motion have been replace with MoveLinear()
34 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
35 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
36 * switch between 'VEHICLE' parameter use and general dynamics
37 * settings use.
38 *
39 */
40
41/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
42 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
43 * ODEPrim.cs contains methods dealing with Prim editing, Prim
44 * characteristics and Kinetic motion.
45 * ODEDynamics.cs contains methods dealing with Prim Physical motion
46 * (dynamics) and the associated settings. Old Linear and angular
47 * motors for dynamic motion have been replace with MoveLinear()
48 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
49 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
50 * switch between 'VEHICLE' parameter use and general dynamics
51 * settings use.
52 */
53
54using System;
55using System.Collections.Generic;
56using System.Reflection;
57using System.Runtime.InteropServices;
58using log4net;
59using OpenMetaverse;
60using Ode.NET;
61using OpenSim.Framework;
62using OpenSim.Region.Physics.Manager;
63
64namespace OpenSim.Region.Physics.OdePlugin
65{
66 public class ODEDynamics
67 {
68 public Vehicle Type
69 {
70 get { return m_type; }
71 }
72
73 public IntPtr Body
74 {
75 get { return m_body; }
76 }
77
78 private int frcount = 0; // Used to limit dynamics debug output to
79 // every 100th frame
80
81 // private OdeScene m_parentScene = null;
82 private IntPtr m_body = IntPtr.Zero;
83// private IntPtr m_jointGroup = IntPtr.Zero;
84// private IntPtr m_aMotor = IntPtr.Zero;
85
86 // Vehicle properties
87 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
88 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
89 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
90 // HOVER_TERRAIN_ONLY
91 // HOVER_GLOBAL_HEIGHT
92 // NO_DEFLECTION_UP
93 // HOVER_WATER_ONLY
94 // HOVER_UP_ONLY
95 // LIMIT_MOTOR_UP
96 // LIMIT_ROLL_ONLY
97
98 // Linear properties
99 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
100 //requested by LSL
101 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
102 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
103 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
104
105 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
106 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
107 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
108
109 //Angular properties
110 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
111
112 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
113 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
114 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
115
116 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
117// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
118 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
119
120 //Deflection properties
121 // private float m_angularDeflectionEfficiency = 0;
122 // private float m_angularDeflectionTimescale = 0;
123 // private float m_linearDeflectionEfficiency = 0;
124 // private float m_linearDeflectionTimescale = 0;
125
126 //Banking properties
127 // private float m_bankingEfficiency = 0;
128 // private float m_bankingMix = 0;
129 // private float m_bankingTimescale = 0;
130
131 //Hover and Buoyancy properties
132 private float m_VhoverHeight = 0f;
133// private float m_VhoverEfficiency = 0f;
134 private float m_VhoverTimescale = 0f;
135 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
136 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
137 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
138 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
139 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
140
141 //Attractor properties
142 private float m_verticalAttractionEfficiency = 1.0f; // damped
143 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
144
145
146
147
148
149 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
150 {
151 switch (pParam)
152 {
153 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
154 if (pValue < 0.01f) pValue = 0.01f;
155 // m_angularDeflectionEfficiency = pValue;
156 break;
157 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
158 if (pValue < 0.01f) pValue = 0.01f;
159 // m_angularDeflectionTimescale = pValue;
160 break;
161 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
162 if (pValue < 0.01f) pValue = 0.01f;
163 m_angularMotorDecayTimescale = pValue;
164 break;
165 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
166 if (pValue < 0.01f) pValue = 0.01f;
167 m_angularMotorTimescale = pValue;
168 break;
169 case Vehicle.BANKING_EFFICIENCY:
170 if (pValue < 0.01f) pValue = 0.01f;
171 // m_bankingEfficiency = pValue;
172 break;
173 case Vehicle.BANKING_MIX:
174 if (pValue < 0.01f) pValue = 0.01f;
175 // m_bankingMix = pValue;
176 break;
177 case Vehicle.BANKING_TIMESCALE:
178 if (pValue < 0.01f) pValue = 0.01f;
179 // m_bankingTimescale = pValue;
180 break;
181 case Vehicle.BUOYANCY:
182 if (pValue < -1f) pValue = -1f;
183 if (pValue > 1f) pValue = 1f;
184 m_VehicleBuoyancy = pValue;
185 break;
186// case Vehicle.HOVER_EFFICIENCY:
187// if (pValue < 0f) pValue = 0f;
188// if (pValue > 1f) pValue = 1f;
189// m_VhoverEfficiency = pValue;
190// break;
191 case Vehicle.HOVER_HEIGHT:
192 m_VhoverHeight = pValue;
193 break;
194 case Vehicle.HOVER_TIMESCALE:
195 if (pValue < 0.01f) pValue = 0.01f;
196 m_VhoverTimescale = pValue;
197 break;
198 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
199 if (pValue < 0.01f) pValue = 0.01f;
200 // m_linearDeflectionEfficiency = pValue;
201 break;
202 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
203 if (pValue < 0.01f) pValue = 0.01f;
204 // m_linearDeflectionTimescale = pValue;
205 break;
206 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
207 if (pValue < 0.01f) pValue = 0.01f;
208 m_linearMotorDecayTimescale = pValue;
209 break;
210 case Vehicle.LINEAR_MOTOR_TIMESCALE:
211 if (pValue < 0.01f) pValue = 0.01f;
212 m_linearMotorTimescale = pValue;
213 break;
214 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
215 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
216 if (pValue > 1.0f) pValue = 1.0f;
217 m_verticalAttractionEfficiency = pValue;
218 break;
219 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
220 if (pValue < 0.01f) pValue = 0.01f;
221 m_verticalAttractionTimescale = pValue;
222 break;
223
224 // These are vector properties but the engine lets you use a single float value to
225 // set all of the components to the same value
226 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
227 if (pValue > 30f) pValue = 30f;
228 if (pValue < 0.1f) pValue = 0.1f;
229 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
230 break;
231 case Vehicle.ANGULAR_MOTOR_DIRECTION:
232 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
233 UpdateAngDecay();
234 break;
235 case Vehicle.LINEAR_FRICTION_TIMESCALE:
236 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
237 break;
238 case Vehicle.LINEAR_MOTOR_DIRECTION:
239 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
240 UpdateLinDecay();
241 break;
242 case Vehicle.LINEAR_MOTOR_OFFSET:
243 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
244 break;
245
246 }
247
248 }//end ProcessFloatVehicleParam
249
250 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
251 {
252 switch (pParam)
253 {
254 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
255 if (pValue.X > 30f) pValue.X = 30f;
256 if (pValue.X < 0.1f) pValue.X = 0.1f;
257 if (pValue.Y > 30f) pValue.Y = 30f;
258 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
259 if (pValue.Z > 30f) pValue.Z = 30f;
260 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
261 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
262 break;
263 case Vehicle.ANGULAR_MOTOR_DIRECTION:
264 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
265 // Limit requested angular speed to 2 rps= 4 pi rads/sec
266 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
267 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
268 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
269 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
270 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
271 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
272 UpdateAngDecay();
273 break;
274 case Vehicle.LINEAR_FRICTION_TIMESCALE:
275 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
276 break;
277 case Vehicle.LINEAR_MOTOR_DIRECTION:
278 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
279 UpdateLinDecay();
280 break;
281 case Vehicle.LINEAR_MOTOR_OFFSET:
282 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
283 break;
284 }
285
286 }//end ProcessVectorVehicleParam
287
288 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
289 {
290 switch (pParam)
291 {
292 case Vehicle.REFERENCE_FRAME:
293 // m_referenceFrame = pValue;
294 break;
295 }
296
297 }//end ProcessRotationVehicleParam
298
299 internal void ProcessFlagsVehicleSet(int flags)
300 {
301 m_flags |= (VehicleFlag)flags;
302 }
303
304 internal void ProcessFlagsVehicleRemove(int flags)
305 {
306 m_flags &= ~((VehicleFlag)flags);
307 }
308
309 internal void ProcessTypeChange(Vehicle pType)
310 {
311 // Set Defaults For Type
312 m_type = pType;
313 switch (pType)
314 {
315 case Vehicle.TYPE_SLED:
316 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
317 m_angularFrictionTimescale = new Vector3(30, 30, 30);
318// m_lLinMotorVel = Vector3.Zero;
319 m_linearMotorTimescale = 1000;
320 m_linearMotorDecayTimescale = 120;
321 m_angularMotorDirection = Vector3.Zero;
322 m_angularMotorDVel = Vector3.Zero;
323 m_angularMotorTimescale = 1000;
324 m_angularMotorDecayTimescale = 120;
325 m_VhoverHeight = 0;
326// m_VhoverEfficiency = 1;
327 m_VhoverTimescale = 10;
328 m_VehicleBuoyancy = 0;
329 // m_linearDeflectionEfficiency = 1;
330 // m_linearDeflectionTimescale = 1;
331 // m_angularDeflectionEfficiency = 1;
332 // m_angularDeflectionTimescale = 1000;
333 // m_bankingEfficiency = 0;
334 // m_bankingMix = 1;
335 // m_bankingTimescale = 10;
336 // m_referenceFrame = Quaternion.Identity;
337 m_flags &=
338 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
339 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
340 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
341 break;
342 case Vehicle.TYPE_CAR:
343 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
344 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
345// m_lLinMotorVel = Vector3.Zero;
346 m_linearMotorTimescale = 1;
347 m_linearMotorDecayTimescale = 60;
348 m_angularMotorDirection = Vector3.Zero;
349 m_angularMotorDVel = Vector3.Zero;
350 m_angularMotorTimescale = 1;
351 m_angularMotorDecayTimescale = 0.8f;
352 m_VhoverHeight = 0;
353// m_VhoverEfficiency = 0;
354 m_VhoverTimescale = 1000;
355 m_VehicleBuoyancy = 0;
356 // // m_linearDeflectionEfficiency = 1;
357 // // m_linearDeflectionTimescale = 2;
358 // // m_angularDeflectionEfficiency = 0;
359 // m_angularDeflectionTimescale = 10;
360 m_verticalAttractionEfficiency = 1f;
361 m_verticalAttractionTimescale = 10f;
362 // m_bankingEfficiency = -0.2f;
363 // m_bankingMix = 1;
364 // m_bankingTimescale = 1;
365 // m_referenceFrame = Quaternion.Identity;
366 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
367 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
368 VehicleFlag.LIMIT_MOTOR_UP);
369 break;
370 case Vehicle.TYPE_BOAT:
371 m_linearFrictionTimescale = new Vector3(10, 3, 2);
372 m_angularFrictionTimescale = new Vector3(10,10,10);
373// m_lLinMotorVel = Vector3.Zero;
374 m_linearMotorTimescale = 5;
375 m_linearMotorDecayTimescale = 60;
376 m_angularMotorDirection = Vector3.Zero;
377 m_angularMotorDVel = Vector3.Zero;
378 m_angularMotorTimescale = 4;
379 m_angularMotorDecayTimescale = 4;
380 m_VhoverHeight = 0;
381// m_VhoverEfficiency = 0.5f;
382 m_VhoverTimescale = 2;
383 m_VehicleBuoyancy = 1;
384 // m_linearDeflectionEfficiency = 0.5f;
385 // m_linearDeflectionTimescale = 3;
386 // m_angularDeflectionEfficiency = 0.5f;
387 // m_angularDeflectionTimescale = 5;
388 m_verticalAttractionEfficiency = 0.5f;
389 m_verticalAttractionTimescale = 5f;
390 // m_bankingEfficiency = -0.3f;
391 // m_bankingMix = 0.8f;
392 // m_bankingTimescale = 1;
393 // m_referenceFrame = Quaternion.Identity;
394 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
395 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
396 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
397 VehicleFlag.LIMIT_MOTOR_UP);
398 break;
399 case Vehicle.TYPE_AIRPLANE:
400 m_linearFrictionTimescale = new Vector3(200, 10, 5);
401 m_angularFrictionTimescale = new Vector3(20, 20, 20);
402// m_lLinMotorVel = Vector3.Zero;
403 m_linearMotorTimescale = 2;
404 m_linearMotorDecayTimescale = 60;
405 m_angularMotorDirection = Vector3.Zero;
406 m_angularMotorDVel = Vector3.Zero;
407 m_angularMotorTimescale = 4;
408 m_angularMotorDecayTimescale = 4;
409 m_VhoverHeight = 0;
410// m_VhoverEfficiency = 0.5f;
411 m_VhoverTimescale = 1000;
412 m_VehicleBuoyancy = 0;
413 // m_linearDeflectionEfficiency = 0.5f;
414 // m_linearDeflectionTimescale = 3;
415 // m_angularDeflectionEfficiency = 1;
416 // m_angularDeflectionTimescale = 2;
417 m_verticalAttractionEfficiency = 0.9f;
418 m_verticalAttractionTimescale = 2f;
419 // m_bankingEfficiency = 1;
420 // m_bankingMix = 0.7f;
421 // m_bankingTimescale = 2;
422 // m_referenceFrame = Quaternion.Identity;
423 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
424 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
425 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
426 break;
427 case Vehicle.TYPE_BALLOON:
428 m_linearFrictionTimescale = new Vector3(5, 5, 5);
429 m_angularFrictionTimescale = new Vector3(10, 10, 10);
430 m_linearMotorTimescale = 5;
431 m_linearMotorDecayTimescale = 60;
432 m_angularMotorDirection = Vector3.Zero;
433 m_angularMotorDVel = Vector3.Zero;
434 m_angularMotorTimescale = 6;
435 m_angularMotorDecayTimescale = 10;
436 m_VhoverHeight = 5;
437// m_VhoverEfficiency = 0.8f;
438 m_VhoverTimescale = 10;
439 m_VehicleBuoyancy = 1;
440 // m_linearDeflectionEfficiency = 0;
441 // m_linearDeflectionTimescale = 5;
442 // m_angularDeflectionEfficiency = 0;
443 // m_angularDeflectionTimescale = 5;
444 m_verticalAttractionEfficiency = 1f;
445 m_verticalAttractionTimescale = 100f;
446 // m_bankingEfficiency = 0;
447 // m_bankingMix = 0.7f;
448 // m_bankingTimescale = 5;
449 // m_referenceFrame = Quaternion.Identity;
450 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
451 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
452 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
453 break;
454
455 }
456 }//end SetDefaultsForType
457
458 internal void Enable(IntPtr pBody, OdeScene pParentScene)
459 {
460 if (m_type == Vehicle.TYPE_NONE)
461 return;
462
463 m_body = pBody;
464 }
465
466 internal void Step(float pTimestep, OdeScene pParentScene)
467 {
468 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
469 return;
470 frcount++; // used to limit debug comment output
471 if (frcount > 24)
472 frcount = 0;
473
474 MoveLinear(pTimestep, pParentScene);
475 MoveAngular(pTimestep);
476 }// end Step
477
478 internal void Halt()
479 { // Kill all motions, when non-physical
480 m_linearMotorDirection = Vector3.Zero;
481 m_lLinMotorDVel = Vector3.Zero;
482 m_lLinObjectVel = Vector3.Zero;
483 m_wLinObjectVel = Vector3.Zero;
484 m_angularMotorDirection = Vector3.Zero;
485 m_lastAngularVelocity = Vector3.Zero;
486 m_angularMotorDVel = Vector3.Zero;
487 }
488
489 private void UpdateLinDecay()
490 {
491 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
492 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
493 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
494 } // else let the motor decay on its own
495
496 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
497 {
498 Vector3 acceleration = new Vector3(0f, 0f, 0f);
499
500 d.Quaternion rot = d.BodyGetQuaternion(Body);
501 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
502 Quaternion irotq = Quaternion.Inverse(rotq);
503 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
504 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
505 acceleration = vel_now - m_wLinObjectVel;
506 m_lLinObjectVel = vel_now * irotq;
507
508 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
509 {
510 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
511 {
512 float decayfactor = m_linearMotorDecayTimescale/pTimestep;
513 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
514 m_lLinMotorDVel -= decayAmount;
515 }
516 else
517 {
518 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
519 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep;
520 m_lLinMotorDVel -= decel;
521 }
522 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
523 {
524 m_lLinMotorDVel = Vector3.Zero;
525 }
526 else
527 {
528 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
529 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
530 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
531 }
532 }
533
534 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
535 {
536 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
537 if (m_linearMotorTimescale < 300.0f)
538 {
539 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
540 float linfactor = m_linearMotorTimescale/pTimestep;
541 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
542 m_lLinObjectVel += attackAmount;
543 }
544 if (m_linearFrictionTimescale.X < 300.0f)
545 {
546 float fricfactor = m_linearFrictionTimescale.X / pTimestep;
547 float fricX = m_lLinObjectVel.X / fricfactor;
548 m_lLinObjectVel.X -= fricX;
549 }
550 if (m_linearFrictionTimescale.Y < 300.0f)
551 {
552 float fricfactor = m_linearFrictionTimescale.Y / pTimestep;
553 float fricY = m_lLinObjectVel.Y / fricfactor;
554 m_lLinObjectVel.Y -= fricY;
555 }
556 if (m_linearFrictionTimescale.Z < 300.0f)
557 {
558 float fricfactor = m_linearFrictionTimescale.Z / pTimestep;
559//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
560 float fricZ = m_lLinObjectVel.Z / fricfactor;
561 m_lLinObjectVel.Z -= fricZ;
562 }
563 }
564 m_wLinObjectVel = m_lLinObjectVel * rotq;
565 // Add Gravity and Buoyancy
566 Vector3 grav = Vector3.Zero;
567 if(m_VehicleBuoyancy < 1.0f)
568 {
569 // There is some gravity, make a gravity force vector
570 // that is applied after object velocity.
571 d.Mass objMass;
572 d.BodyGetMass(Body, out objMass);
573 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
574 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
575 } // else its 1.0, no gravity.
576
577 // Check if hovering
578 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
579 {
580 // We should hover, get the target height
581 d.Vector3 pos = d.BodyGetPosition(Body);
582 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
583 {
584 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
585 }
586 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
587 {
588 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
589 }
590 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
591 {
592 m_VhoverTargetHeight = m_VhoverHeight;
593 }
594
595 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
596 {
597 // If body is aready heigher, use its height as target height
598 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
599 }
600
601// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
602// m_VhoverTimescale = 0f; // time to acheive height
603// pTimestep is time since last frame,in secs
604 float herr0 = pos.Z - m_VhoverTargetHeight;
605 // Replace Vertical speed with correction figure if significant
606 if(Math.Abs(herr0) > 0.01f )
607 {
608 d.Mass objMass;
609 d.BodyGetMass(Body, out objMass);
610 m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
611 //KF: m_VhoverEfficiency is not yet implemented
612 }
613 else
614 {
615 m_wLinObjectVel.Z = 0f;
616 }
617 }
618 else
619 { // not hovering, Gravity rules
620 m_wLinObjectVel.Z = vel_now.Z;
621//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z);
622 }
623 // Apply velocity
624 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
625 // apply gravity force
626 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
627//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
628 } // end MoveLinear()
629
630 private void UpdateAngDecay()
631 {
632 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
633 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
634 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
635 } // else let the motor decay on its own
636
637 private void MoveAngular(float pTimestep)
638 {
639 /*
640 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
641
642 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
643 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
644 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
645
646 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
647 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
648 */
649//if(frcount == 0) Console.WriteLine("MoveAngular ");
650
651 // Get what the body is doing, this includes 'external' influences
652 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
653 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
654//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
655// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
656// Vector3 initavel = angObjectVel;
657 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
658 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep);
659 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
660 // Decay Angular Motor 2.
661 if (m_angularMotorDecayTimescale < 300.0f)
662 {
663//####
664 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
665 {
666 float decayfactor = (m_angularMotorDecayTimescale)/pTimestep;
667 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
668 m_angularMotorDVel -= decayAmount;
669 }
670 else
671 {
672 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale;
673 m_angularMotorDVel -= decel;
674 }
675
676 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
677 {
678 m_angularMotorDVel = Vector3.Zero;
679 }
680 else
681 {
682 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
683 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
684 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
685 }
686 } // end decay angular motor
687//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
688
689//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
690 // Vertical attractor section
691 Vector3 vertattr = Vector3.Zero;
692
693 if(m_verticalAttractionTimescale < 300)
694 {
695 float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
696 // get present body rotation
697 d.Quaternion rot = d.BodyGetQuaternion(Body);
698 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
699 // make a vector pointing up
700 Vector3 verterr = Vector3.Zero;
701 verterr.Z = 1.0f;
702 // rotate it to Body Angle
703 verterr = verterr * rotq;
704 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
705 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
706 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
707
708 if (verterr.Z < 0.0f)
709 { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to
710 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
711//Console.WriteLine("InvertFlip");
712 verterr.X = 2.0f - verterr.X;
713 verterr.Y = 2.0f - verterr.Y;
714 }
715 verterr *= 0.5f;
716 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
717
718 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
719 {
720//if(frcount == 0)
721 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
722 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
723 vertattr.X = verterr.Y;
724 vertattr.Y = - verterr.X;
725 vertattr.Z = 0f;
726//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
727
728 // scaling appears better usingsquare-law
729 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
730 float bounce = 1.0f - damped;
731 // 0 = crit damp, 1 = bouncy
732 float oavz = angObjectVel.Z; // retain z velocity
733 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
734 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90.
735 angObjectVel.Z = oavz;
736//if(frcount == 0) Console.WriteLine("VA+");
737//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
738 }
739 else
740 {
741 // else error is very small
742 angObjectVel.X = 0f;
743 angObjectVel.Y = 0f;
744//if(frcount == 0) Console.WriteLine("VA0");
745 }
746 } // else vertical attractor is off
747//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
748
749 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
750 { // if motor or object have motion
751 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
752
753 if (m_angularMotorTimescale < 300.0f)
754 {
755 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
756 float angfactor = m_angularMotorTimescale/pTimestep;
757 Vector3 attackAmount = (attack_error/angfactor);
758 angObjectVel += attackAmount;
759//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
760//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
761 }
762
763 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep);
764 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep);
765 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep);
766 } // else no signif. motion
767
768//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
769 // Bank section tba
770 // Deflection section tba
771//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
772
773 m_lastAngularVelocity = angObjectVel;
774/*
775 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
776 {
777 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
778 }
779 else
780 {
781 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
782 }
783 */
784 // Apply to the body
785// Vector3 aInc = m_lastAngularVelocity - initavel;
786//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
787 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
788//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
789
790 } //end MoveAngular
791 }
792}
generated by cgit v1.1 at 2024-12-22 07:22:59 +0000