aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
authorKitto Flora2009-12-22 00:20:04 -0500
committerKitto Flora2009-12-22 00:20:04 -0500
commit0a29842caf5dbe711490b9b323ae922c418c6c30 (patch)
tree17f1403561f8bf1238dae53ff29f126b645b6793 /OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
parentMerge branch 'master' into careminster (diff)
downloadopensim-SC-0a29842caf5dbe711490b9b323ae922c418c6c30.zip
opensim-SC-0a29842caf5dbe711490b9b323ae922c418c6c30.tar.gz
opensim-SC-0a29842caf5dbe711490b9b323ae922c418c6c30.tar.bz2
opensim-SC-0a29842caf5dbe711490b9b323ae922c418c6c30.tar.xz
Include ChOdePlugin
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs673
1 files changed, 673 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..78b15be
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -0,0 +1,673 @@
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
87 // Vehicle properties
88 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
89 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
90 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
91 // HOVER_TERRAIN_ONLY
92 // HOVER_GLOBAL_HEIGHT
93 // NO_DEFLECTION_UP
94 // HOVER_WATER_ONLY
95 // HOVER_UP_ONLY
96 // LIMIT_MOTOR_UP
97 // LIMIT_ROLL_ONLY
98
99 // Linear properties
100 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
101 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
102 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
103 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
104 private float m_linearMotorDecayTimescale = 0;
105 private float m_linearMotorTimescale = 0;
106 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
107 // private bool m_LinearMotorSetLastFrame = false;
108 // private Vector3 m_linearMotorOffset = Vector3.Zero;
109
110 //Angular properties
111 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
112 private int m_angularMotorApply = 0; // application frame counter
113 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
114 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
115 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
116 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
117 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
118 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA 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 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
228 break;
229 case Vehicle.ANGULAR_MOTOR_DIRECTION:
230 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
231 m_angularMotorApply = 10;
232 break;
233 case Vehicle.LINEAR_FRICTION_TIMESCALE:
234 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
235 break;
236 case Vehicle.LINEAR_MOTOR_DIRECTION:
237 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
238 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
239 break;
240 case Vehicle.LINEAR_MOTOR_OFFSET:
241 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
242 break;
243
244 }
245
246 }//end ProcessFloatVehicleParam
247
248 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
249 {
250 switch (pParam)
251 {
252 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
253 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
254 break;
255 case Vehicle.ANGULAR_MOTOR_DIRECTION:
256 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
257 // Limit requested angular speed to 2 rps= 4 pi rads/sec
258 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
259 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
260 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
261 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
262 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
263 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
264 m_angularMotorApply = 10;
265 break;
266 case Vehicle.LINEAR_FRICTION_TIMESCALE:
267 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
268 break;
269 case Vehicle.LINEAR_MOTOR_DIRECTION:
270 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
271 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
272 break;
273 case Vehicle.LINEAR_MOTOR_OFFSET:
274 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
275 break;
276 }
277
278 }//end ProcessVectorVehicleParam
279
280 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
281 {
282 switch (pParam)
283 {
284 case Vehicle.REFERENCE_FRAME:
285 // m_referenceFrame = pValue;
286 break;
287 }
288
289 }//end ProcessRotationVehicleParam
290
291 internal void ProcessTypeChange(Vehicle pType)
292 {
293 // Set Defaults For Type
294 m_type = pType;
295 switch (pType)
296 {
297 case Vehicle.TYPE_SLED:
298 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
299 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
300 m_linearMotorDirection = Vector3.Zero;
301 m_linearMotorTimescale = 1000;
302 m_linearMotorDecayTimescale = 120;
303 m_angularMotorDirection = Vector3.Zero;
304 m_angularMotorTimescale = 1000;
305 m_angularMotorDecayTimescale = 120;
306 m_VhoverHeight = 0;
307// m_VhoverEfficiency = 1;
308 m_VhoverTimescale = 10;
309 m_VehicleBuoyancy = 0;
310 // m_linearDeflectionEfficiency = 1;
311 // m_linearDeflectionTimescale = 1;
312 // m_angularDeflectionEfficiency = 1;
313 // m_angularDeflectionTimescale = 1000;
314 // m_bankingEfficiency = 0;
315 // m_bankingMix = 1;
316 // m_bankingTimescale = 10;
317 // m_referenceFrame = Quaternion.Identity;
318 m_flags &=
319 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
320 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
321 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
322 break;
323 case Vehicle.TYPE_CAR:
324 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
325 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
326 m_linearMotorDirection = Vector3.Zero;
327 m_linearMotorTimescale = 1;
328 m_linearMotorDecayTimescale = 60;
329 m_angularMotorDirection = Vector3.Zero;
330 m_angularMotorTimescale = 1;
331 m_angularMotorDecayTimescale = 0.8f;
332 m_VhoverHeight = 0;
333// m_VhoverEfficiency = 0;
334 m_VhoverTimescale = 1000;
335 m_VehicleBuoyancy = 0;
336 // // m_linearDeflectionEfficiency = 1;
337 // // m_linearDeflectionTimescale = 2;
338 // // m_angularDeflectionEfficiency = 0;
339 // m_angularDeflectionTimescale = 10;
340 m_verticalAttractionEfficiency = 1f;
341 m_verticalAttractionTimescale = 10f;
342 // m_bankingEfficiency = -0.2f;
343 // m_bankingMix = 1;
344 // m_bankingTimescale = 1;
345 // m_referenceFrame = Quaternion.Identity;
346 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
347 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
348 VehicleFlag.LIMIT_MOTOR_UP);
349 break;
350 case Vehicle.TYPE_BOAT:
351 m_linearFrictionTimescale = new Vector3(10, 3, 2);
352 m_angularFrictionTimescale = new Vector3(10,10,10);
353 m_linearMotorDirection = Vector3.Zero;
354 m_linearMotorTimescale = 5;
355 m_linearMotorDecayTimescale = 60;
356 m_angularMotorDirection = Vector3.Zero;
357 m_angularMotorTimescale = 4;
358 m_angularMotorDecayTimescale = 4;
359 m_VhoverHeight = 0;
360// m_VhoverEfficiency = 0.5f;
361 m_VhoverTimescale = 2;
362 m_VehicleBuoyancy = 1;
363 // m_linearDeflectionEfficiency = 0.5f;
364 // m_linearDeflectionTimescale = 3;
365 // m_angularDeflectionEfficiency = 0.5f;
366 // m_angularDeflectionTimescale = 5;
367 m_verticalAttractionEfficiency = 0.5f;
368 m_verticalAttractionTimescale = 5f;
369 // m_bankingEfficiency = -0.3f;
370 // m_bankingMix = 0.8f;
371 // m_bankingTimescale = 1;
372 // m_referenceFrame = Quaternion.Identity;
373 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
374 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
375 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
376 VehicleFlag.LIMIT_MOTOR_UP);
377 break;
378 case Vehicle.TYPE_AIRPLANE:
379 m_linearFrictionTimescale = new Vector3(200, 10, 5);
380 m_angularFrictionTimescale = new Vector3(20, 20, 20);
381 m_linearMotorDirection = Vector3.Zero;
382 m_linearMotorTimescale = 2;
383 m_linearMotorDecayTimescale = 60;
384 m_angularMotorDirection = Vector3.Zero;
385 m_angularMotorTimescale = 4;
386 m_angularMotorDecayTimescale = 4;
387 m_VhoverHeight = 0;
388// m_VhoverEfficiency = 0.5f;
389 m_VhoverTimescale = 1000;
390 m_VehicleBuoyancy = 0;
391 // m_linearDeflectionEfficiency = 0.5f;
392 // m_linearDeflectionTimescale = 3;
393 // m_angularDeflectionEfficiency = 1;
394 // m_angularDeflectionTimescale = 2;
395 m_verticalAttractionEfficiency = 0.9f;
396 m_verticalAttractionTimescale = 2f;
397 // m_bankingEfficiency = 1;
398 // m_bankingMix = 0.7f;
399 // m_bankingTimescale = 2;
400 // m_referenceFrame = Quaternion.Identity;
401 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
402 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
403 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
404 break;
405 case Vehicle.TYPE_BALLOON:
406 m_linearFrictionTimescale = new Vector3(5, 5, 5);
407 m_angularFrictionTimescale = new Vector3(10, 10, 10);
408 m_linearMotorDirection = Vector3.Zero;
409 m_linearMotorTimescale = 5;
410 m_linearMotorDecayTimescale = 60;
411 m_angularMotorDirection = Vector3.Zero;
412 m_angularMotorTimescale = 6;
413 m_angularMotorDecayTimescale = 10;
414 m_VhoverHeight = 5;
415// m_VhoverEfficiency = 0.8f;
416 m_VhoverTimescale = 10;
417 m_VehicleBuoyancy = 1;
418 // m_linearDeflectionEfficiency = 0;
419 // m_linearDeflectionTimescale = 5;
420 // m_angularDeflectionEfficiency = 0;
421 // m_angularDeflectionTimescale = 5;
422 m_verticalAttractionEfficiency = 1f;
423 m_verticalAttractionTimescale = 100f;
424 // m_bankingEfficiency = 0;
425 // m_bankingMix = 0.7f;
426 // m_bankingTimescale = 5;
427 // m_referenceFrame = Quaternion.Identity;
428 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
429 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
430 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
431 break;
432
433 }
434 }//end SetDefaultsForType
435
436 internal void Enable(IntPtr pBody, OdeScene pParentScene)
437 {
438 if (m_type == Vehicle.TYPE_NONE)
439 return;
440
441 m_body = pBody;
442 }
443
444 internal void Step(float pTimestep, OdeScene pParentScene)
445 {
446 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
447 return;
448 frcount++; // used to limit debug comment output
449 if (frcount > 100)
450 frcount = 0;
451
452 MoveLinear(pTimestep, pParentScene);
453 MoveAngular(pTimestep);
454 }// end Step
455
456 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
457 {
458 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
459 {
460 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
461
462 // add drive to body
463 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
464 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
465
466 // This will work temporarily, but we really need to compare speed on an axis
467 // KF: Limit body velocity to applied velocity?
468 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
469 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
470 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
471 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
472 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
473 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
474
475 // decay applied velocity
476 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
477 //Console.WriteLine("decay: " + decayfraction);
478 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
479 //Console.WriteLine("actual: " + m_linearMotorDirection);
480 }
481 else
482 { // requested is not significant
483 // if what remains of applied is small, zero it.
484 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
485 m_lastLinearVelocityVector = Vector3.Zero;
486 }
487
488
489 // convert requested object velocity to world-referenced vector
490 m_dir = m_lastLinearVelocityVector;
491 d.Quaternion rot = d.BodyGetQuaternion(Body);
492 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
493 m_dir *= rotq; // apply obj rotation to velocity vector
494
495 // add Gravity and Buoyancy
496 // KF: So far I have found no good method to combine a script-requested
497 // .Z velocity and gravity. Therefore only 0g will used script-requested
498 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
499 Vector3 grav = Vector3.Zero;
500 if(m_VehicleBuoyancy < 1.0f)
501 {
502 // There is some gravity, make a gravity force vector
503 // that is applied after object velocity.
504 d.Mass objMass;
505 d.BodyGetMass(Body, out objMass);
506 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
507 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
508 // Preserve the current Z velocity
509 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
510 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
511 } // else its 1.0, no gravity.
512
513 // Check if hovering
514 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
515 {
516 // We should hover, get the target height
517 d.Vector3 pos = d.BodyGetPosition(Body);
518 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
519 {
520 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
521 }
522 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
523 {
524 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
525 }
526 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
527 {
528 m_VhoverTargetHeight = m_VhoverHeight;
529 }
530
531 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
532 {
533 // If body is aready heigher, use its height as target height
534 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
535 }
536
537// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
538// m_VhoverTimescale = 0f; // time to acheive height
539// pTimestep is time since last frame,in secs
540 float herr0 = pos.Z - m_VhoverTargetHeight;
541 // Replace Vertical speed with correction figure if significant
542 if(Math.Abs(herr0) > 0.01f )
543 {
544 d.Mass objMass;
545 d.BodyGetMass(Body, out objMass);
546 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
547 //KF: m_VhoverEfficiency is not yet implemented
548 }
549 else
550 {
551 m_dir.Z = 0f;
552 }
553 }
554
555 // Apply velocity
556 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
557 // apply gravity force
558 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
559
560
561 // apply friction
562 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
563 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
564 } // end MoveLinear()
565
566 private void MoveAngular(float pTimestep)
567 {
568 /*
569 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
570 private int m_angularMotorApply = 0; // application frame counter
571 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
572 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
573 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
574 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
575 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
576 */
577//if(frcount == 0) Console.WriteLine("MoveAngular ");
578
579 // Get what the body is doing, this includes 'external' influences
580 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
581 // Vector3 angularVelocity = Vector3.Zero;
582
583 if (m_angularMotorApply > 0)
584 {
585 // ramp up to new value
586 // current velocity += error / ( time to get there / step interval )
587 // requested speed - last motor speed
588 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
589 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
590 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
591
592 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
593 // velocity may still be acheived.
594 }
595 else
596 {
597 // no motor recently applied, keep the body velocity
598 /* m_angularMotorVelocity.X = angularVelocity.X;
599 m_angularMotorVelocity.Y = angularVelocity.Y;
600 m_angularMotorVelocity.Z = angularVelocity.Z; */
601
602 // and decay the velocity
603 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
604 } // end motor section
605
606
607 // Vertical attractor section
608 Vector3 vertattr = Vector3.Zero;
609
610 if(m_verticalAttractionTimescale < 300)
611 {
612 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
613 // get present body rotation
614 d.Quaternion rot = d.BodyGetQuaternion(Body);
615 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
616 // make a vector pointing up
617 Vector3 verterr = Vector3.Zero;
618 verterr.Z = 1.0f;
619 // rotate it to Body Angle
620 verterr = verterr * rotq;
621 // 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.
622 // 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
623 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
624 if (verterr.Z < 0.0f)
625 {
626 verterr.X = 2.0f - verterr.X;
627 verterr.Y = 2.0f - verterr.Y;
628 }
629 // Error is 0 (no error) to +/- 2 (max error)
630 // scale it by VAservo
631 verterr = verterr * VAservo;
632//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
633
634 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
635 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
636 vertattr.X = verterr.Y;
637 vertattr.Y = - verterr.X;
638 vertattr.Z = 0f;
639
640 // scaling appears better usingsquare-law
641 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
642 vertattr.X += bounce * angularVelocity.X;
643 vertattr.Y += bounce * angularVelocity.Y;
644
645 } // else vertical attractor is off
646
647 // m_lastVertAttractor = vertattr;
648
649 // Bank section tba
650 // Deflection section tba
651
652 // Sum velocities
653 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection
654
655 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
656 {
657 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
658 }
659 else
660 {
661 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
662 }
663
664 // apply friction
665 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
666 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
667
668 // Apply to the body
669 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
670
671 } //end MoveAngular
672 }
673}
generated by cgit v1.1 at 2024-12-18 14:55:36 +0000