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