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