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