aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
authorUbitUmarov2012-02-08 15:24:10 +0000
committerUbitUmarov2012-02-08 15:24:10 +0000
commit815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91 (patch)
tree84a2cd2a6c76c23923388d629b54eab4b49b4cf7 /OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
parentPush more NPC stuff into threads (diff)
downloadopensim-SC_OLD-815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91.zip
opensim-SC_OLD-815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91.tar.gz
opensim-SC_OLD-815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91.tar.bz2
opensim-SC_OLD-815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91.tar.xz
UbitODE plugin initial commit
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs1035
1 files changed, 1035 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..363cbef
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,1035 @@
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
28/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30 * ODEPrim.cs contains methods dealing with Prim editing, Prim
31 * characteristics and Kinetic motion.
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use.
39 */
40
41using System;
42using System.Collections.Generic;
43using System.Reflection;
44using System.Runtime.InteropServices;
45using log4net;
46using OpenMetaverse;
47using OdeAPI;
48using OpenSim.Framework;
49using OpenSim.Region.Physics.Manager;
50
51namespace OpenSim.Region.Physics.OdePlugin
52{
53 public class ODEDynamics
54 {
55 public Vehicle Type
56 {
57 get { return m_type; }
58 }
59
60// private OdeScene m_parentScene = null;
61// private IntPtr m_aMotor = IntPtr.Zero;
62
63
64 private OdePrim rootPrim;
65 private OdeScene _pParentScene;
66
67
68
69
70 private Vector3 refUpAxis = new Vector3(0, 0, 1);
71 private Vector3 refAtAxis = new Vector3(1, 0, 0);
72
73
74 // Vehicle properties
75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
76
77 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
78 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
79 // HOVER_TERRAIN_ONLY
80 // HOVER_GLOBAL_HEIGHT
81 // NO_DEFLECTION_UP
82 // HOVER_WATER_ONLY
83 // HOVER_UP_ONLY
84 // LIMIT_MOTOR_UP
85 // LIMIT_ROLL_ONLY
86 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
87 private Quaternion m_RollreferenceFrame = Quaternion.Identity;
88
89 // Linear properties
90 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
91 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
92 private float m_linearMotorDecayTimescale = 120;
93 private float m_linearMotorTimescale = 1000;
94 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
95 private Vector3 m_linearMotorOffset = Vector3.Zero;
96
97 //Angular properties
98 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
99 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
100 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
101 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
102 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
103
104 //Deflection properties
105 private float m_angularDeflectionEfficiency = 0;
106 private float m_angularDeflectionTimescale = 1000;
107 private float m_linearDeflectionEfficiency = 0;
108 private float m_linearDeflectionTimescale = 1000;
109
110 //Banking properties
111 private float m_bankingEfficiency = 0;
112 private float m_bankingMix = 0;
113 private float m_bankingTimescale = 0;
114
115 //Hover and Buoyancy properties
116 private float m_VhoverHeight = 0f;
117 private float m_VhoverEfficiency = 0f;
118 private float m_VhoverTimescale = 1000f;
119 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
120 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
121 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
122 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
123 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
124
125 //Attractor properties
126 private float m_verticalAttractionEfficiency = 1.0f; // damped
127 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
128
129 // special contact data for vehicles
130 public ContactData VehiculeContactData = new ContactData(0f, 0.1f);
131
132 // auxiliar
133 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
134
135 private float m_lmEfect = 0; // current linear motor eficiency
136 private float m_amEfect = 0; // current angular motor eficiency
137
138
139 public ODEDynamics(OdePrim rootp)
140 {
141 rootPrim = rootp;
142 _pParentScene = rootPrim._parent_scene;
143 }
144
145 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
146 {
147 float len;
148 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
149 float timestep = _pParentScene.ODE_STEPSIZE;
150
151 switch (pParam)
152 {
153 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
154 if (pValue < 0f) pValue = 0f;
155 if (pValue > 1f) pValue = 1f;
156 m_angularDeflectionEfficiency = pValue;
157 break;
158 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
159 if (pValue < timestep) pValue = timestep;
160 m_angularDeflectionTimescale = pValue;
161 break;
162 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
163 if (pValue < timestep) pValue = timestep;
164 else if (pValue > 120) pValue = 120;
165 m_angularMotorDecayTimescale = pValue * invtimestep;
166 break;
167 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
168 if (pValue < timestep) pValue = timestep;
169 m_angularMotorTimescale = pValue;
170 break;
171 case Vehicle.BANKING_EFFICIENCY:
172 if (pValue < -1f) pValue = -1f;
173 if (pValue > 1f) pValue = 1f;
174 m_bankingEfficiency = pValue;
175 break;
176 case Vehicle.BANKING_MIX:
177 if (pValue < 0f) pValue = 0f;
178 if (pValue > 1f) pValue = 1f;
179 m_bankingMix = pValue;
180 break;
181 case Vehicle.BANKING_TIMESCALE:
182 if (pValue < timestep) pValue = timestep;
183 m_bankingTimescale = pValue;
184 break;
185 case Vehicle.BUOYANCY:
186 if (pValue < -1f) pValue = -1f;
187 if (pValue > 1f) pValue = 1f;
188 m_VehicleBuoyancy = pValue;
189 break;
190 case Vehicle.HOVER_EFFICIENCY:
191 if (pValue < 0f) pValue = 0f;
192 if (pValue > 1f) pValue = 1f;
193 m_VhoverEfficiency = pValue;
194 break;
195 case Vehicle.HOVER_HEIGHT:
196 m_VhoverHeight = pValue;
197 break;
198 case Vehicle.HOVER_TIMESCALE:
199 if (pValue < timestep) pValue = timestep;
200 m_VhoverTimescale = pValue;
201 break;
202 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
203 if (pValue < 0f) pValue = 0f;
204 if (pValue > 1f) pValue = 1f;
205 m_linearDeflectionEfficiency = pValue;
206 break;
207 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
208 if (pValue < timestep) pValue = timestep;
209 m_linearDeflectionTimescale = pValue;
210 break;
211 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
212 if (pValue < timestep) pValue = timestep;
213 else if (pValue > 120) pValue = 120;
214 m_linearMotorDecayTimescale = pValue * invtimestep;
215 break;
216 case Vehicle.LINEAR_MOTOR_TIMESCALE:
217 if (pValue < timestep) pValue = timestep;
218 m_linearMotorTimescale = pValue;
219 break;
220 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
221 if (pValue < 0f) pValue = 0f;
222 if (pValue > 1f) pValue = 1f;
223 m_verticalAttractionEfficiency = pValue;
224 break;
225 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
226 if (pValue < timestep) pValue = timestep;
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 if (pValue < timestep) pValue = timestep;
234 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
235 break;
236 case Vehicle.ANGULAR_MOTOR_DIRECTION:
237 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
238 len = m_angularMotorDirection.Length();
239 if (len > 12.566f)
240 m_angularMotorDirection *= (12.566f / len);
241 m_amEfect = 1.0f; // turn it on
242 break;
243 case Vehicle.LINEAR_FRICTION_TIMESCALE:
244 if (pValue < timestep) pValue = timestep;
245 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
246 break;
247 case Vehicle.LINEAR_MOTOR_DIRECTION:
248 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
249 len = m_linearMotorDirection.Length();
250 if (len > 30.0f)
251 m_linearMotorDirection *= (30.0f / len);
252 m_lmEfect = 1.0f; // turn it on
253 break;
254 case Vehicle.LINEAR_MOTOR_OFFSET:
255 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
256 len = m_linearMotorOffset.Length();
257 if (len > 100.0f)
258 m_linearMotorOffset *= (100.0f / len);
259 break;
260 }
261 }//end ProcessFloatVehicleParam
262
263 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
264 {
265 float len;
266 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
267 float timestep = _pParentScene.ODE_STEPSIZE;
268 switch (pParam)
269 {
270
271 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
272 if (pValue.X < timestep) pValue.X = timestep;
273 if (pValue.Y < timestep) pValue.Y = timestep;
274 if (pValue.Z < timestep) pValue.Z = timestep;
275
276 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
277 break;
278 case Vehicle.ANGULAR_MOTOR_DIRECTION:
279 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
280 // Limit requested angular speed to 2 rps= 4 pi rads/sec
281 len = m_angularMotorDirection.Length();
282 if (len > 12.566f)
283 m_angularMotorDirection *= (12.566f / len);
284 m_amEfect = 1.0f; // turn it on
285 break;
286 case Vehicle.LINEAR_FRICTION_TIMESCALE:
287 if (pValue.X < timestep) pValue.X = timestep;
288 if (pValue.Y < timestep) pValue.Y = timestep;
289 if (pValue.Z < timestep) pValue.Z = timestep;
290 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
291 break;
292 case Vehicle.LINEAR_MOTOR_DIRECTION:
293 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
294 len = m_linearMotorDirection.Length();
295 if (len > 30.0f)
296 m_linearMotorDirection *= (30.0f / len);
297 m_lmEfect = 1.0f; // turn it on
298 break;
299 case Vehicle.LINEAR_MOTOR_OFFSET:
300 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
301 len = m_linearMotorOffset.Length();
302 if (len > 100.0f)
303 m_linearMotorOffset *= (100.0f / len);
304 break;
305 case Vehicle.BLOCK_EXIT:
306 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
307 break;
308 }
309 }//end ProcessVectorVehicleParam
310
311 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
312 {
313 switch (pParam)
314 {
315 case Vehicle.REFERENCE_FRAME:
316 m_referenceFrame = Quaternion.Inverse(pValue);
317 break;
318 case Vehicle.ROLL_FRAME:
319 m_RollreferenceFrame = pValue;
320 break;
321 }
322 }//end ProcessRotationVehicleParam
323
324 internal void ProcessVehicleFlags(int pParam, bool remove)
325 {
326 if (remove)
327 {
328 m_flags &= ~((VehicleFlag)pParam);
329 }
330 else
331 {
332 m_flags |= (VehicleFlag)pParam;
333 }
334 }//end ProcessVehicleFlags
335
336 internal void ProcessTypeChange(Vehicle pType)
337 {
338 float invtimestep = _pParentScene.ODE_STEPSIZE;
339 m_lmEfect = 0;
340 m_amEfect = 0;
341
342 m_linearMotorDirection = Vector3.Zero;
343 m_angularMotorDirection = Vector3.Zero;
344
345 m_BlockingEndPoint = Vector3.Zero;
346 m_RollreferenceFrame = Quaternion.Identity;
347 m_linearMotorOffset = Vector3.Zero;
348
349 m_referenceFrame = Quaternion.Identity;
350
351 // Set Defaults For Type
352 m_type = pType;
353 switch (pType)
354 {
355 case Vehicle.TYPE_NONE:
356 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
357 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
358 m_linearMotorTimescale = 1000;
359 m_linearMotorDecayTimescale = 120 * invtimestep;
360 m_angularMotorTimescale = 1000;
361 m_angularMotorDecayTimescale = 1000 * invtimestep;
362 m_VhoverHeight = 0;
363 m_VhoverTimescale = 1000;
364 m_VehicleBuoyancy = 0;
365 m_flags = (VehicleFlag)0;
366 break;
367
368 case Vehicle.TYPE_SLED:
369 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
370 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
371 m_linearMotorTimescale = 1000;
372 m_linearMotorDecayTimescale = 120 * invtimestep;
373 m_angularMotorTimescale = 1000;
374 m_angularMotorDecayTimescale = 120 * invtimestep;
375 m_VhoverHeight = 0;
376 m_VhoverEfficiency = 1;
377 m_VhoverTimescale = 10;
378 m_VehicleBuoyancy = 0;
379 m_linearDeflectionEfficiency = 1;
380 m_linearDeflectionTimescale = 1;
381 m_angularDeflectionEfficiency = 0;
382 m_angularDeflectionTimescale = 1000;
383 m_bankingEfficiency = 0;
384 m_bankingMix = 1;
385 m_bankingTimescale = 10;
386 m_flags &=
387 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
388 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
389 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
390 break;
391 case Vehicle.TYPE_CAR:
392 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
393 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
394 m_linearMotorTimescale = 1;
395 m_linearMotorDecayTimescale = 60 * invtimestep;
396 m_angularMotorTimescale = 1;
397 m_angularMotorDecayTimescale = 0.8f * invtimestep;
398 m_VhoverHeight = 0;
399 m_VhoverEfficiency = 0;
400 m_VhoverTimescale = 1000;
401 m_VehicleBuoyancy = 0;
402 m_linearDeflectionEfficiency = 1;
403 m_linearDeflectionTimescale = 2;
404 m_angularDeflectionEfficiency = 0;
405 m_angularDeflectionTimescale = 10;
406 m_verticalAttractionEfficiency = 1f;
407 m_verticalAttractionTimescale = 10f;
408 m_bankingEfficiency = -0.2f;
409 m_bankingMix = 1;
410 m_bankingTimescale = 1;
411 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
412 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
413 VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
414 break;
415 case Vehicle.TYPE_BOAT:
416 m_linearFrictionTimescale = new Vector3(10, 3, 2);
417 m_angularFrictionTimescale = new Vector3(10, 10, 10);
418 m_linearMotorTimescale = 5;
419 m_linearMotorDecayTimescale = 60 * invtimestep;
420 m_angularMotorTimescale = 4;
421 m_angularMotorDecayTimescale = 4 * invtimestep;
422 m_VhoverHeight = 0;
423 m_VhoverEfficiency = 0.5f;
424 m_VhoverTimescale = 2;
425 m_VehicleBuoyancy = 1;
426 m_linearDeflectionEfficiency = 0.5f;
427 m_linearDeflectionTimescale = 3;
428 m_angularDeflectionEfficiency = 0.5f;
429 m_angularDeflectionTimescale = 5;
430 m_verticalAttractionEfficiency = 0.5f;
431 m_verticalAttractionTimescale = 5f;
432 m_bankingEfficiency = -0.3f;
433 m_bankingMix = 0.8f;
434 m_bankingTimescale = 1;
435 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
436 VehicleFlag.HOVER_GLOBAL_HEIGHT |
437 VehicleFlag.HOVER_UP_ONLY |
438 VehicleFlag.LIMIT_ROLL_ONLY);
439 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
440 VehicleFlag.LIMIT_MOTOR_UP |
441 VehicleFlag.HOVER_WATER_ONLY);
442 break;
443 case Vehicle.TYPE_AIRPLANE:
444 m_linearFrictionTimescale = new Vector3(200, 10, 5);
445 m_angularFrictionTimescale = new Vector3(20, 20, 20);
446 m_linearMotorTimescale = 2;
447 m_linearMotorDecayTimescale = 60 * invtimestep;
448 m_angularMotorTimescale = 4;
449 m_angularMotorDecayTimescale = 8 * invtimestep;
450 m_VhoverHeight = 0;
451 m_VhoverEfficiency = 0.5f;
452 m_VhoverTimescale = 1000;
453 m_VehicleBuoyancy = 0;
454 m_linearDeflectionEfficiency = 0.5f;
455 m_linearDeflectionTimescale = 0.5f;
456 m_angularDeflectionEfficiency = 1;
457 m_angularDeflectionTimescale = 2;
458 m_verticalAttractionEfficiency = 0.9f;
459 m_verticalAttractionTimescale = 2f;
460 m_bankingEfficiency = 1;
461 m_bankingMix = 0.7f;
462 m_bankingTimescale = 2;
463 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
464 VehicleFlag.HOVER_TERRAIN_ONLY |
465 VehicleFlag.HOVER_GLOBAL_HEIGHT |
466 VehicleFlag.HOVER_UP_ONLY |
467 VehicleFlag.NO_DEFLECTION_UP |
468 VehicleFlag.LIMIT_MOTOR_UP);
469 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
470 break;
471 case Vehicle.TYPE_BALLOON:
472 m_linearFrictionTimescale = new Vector3(5, 5, 5);
473 m_angularFrictionTimescale = new Vector3(10, 10, 10);
474 m_linearMotorTimescale = 5;
475 m_linearMotorDecayTimescale = 60 * invtimestep;
476 m_angularMotorTimescale = 6;
477 m_angularMotorDecayTimescale = 10 * invtimestep;
478 m_VhoverHeight = 5;
479 m_VhoverEfficiency = 0.8f;
480 m_VhoverTimescale = 10;
481 m_VehicleBuoyancy = 1;
482 m_linearDeflectionEfficiency = 0;
483 m_linearDeflectionTimescale = 5 * invtimestep;
484 m_angularDeflectionEfficiency = 0;
485 m_angularDeflectionTimescale = 5;
486 m_verticalAttractionEfficiency = 0f;
487 m_verticalAttractionTimescale = 1000f;
488 m_bankingEfficiency = 0;
489 m_bankingMix = 0.7f;
490 m_bankingTimescale = 5;
491 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
492 VehicleFlag.HOVER_TERRAIN_ONLY |
493 VehicleFlag.HOVER_UP_ONLY |
494 VehicleFlag.NO_DEFLECTION_UP |
495 VehicleFlag.LIMIT_MOTOR_UP);
496 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
497 VehicleFlag.HOVER_GLOBAL_HEIGHT);
498 break;
499 }
500
501 }//end SetDefaultsForType
502
503 internal void Stop()
504 {
505 m_lmEfect = 0;
506 m_amEfect = 0;
507 }
508
509 public static Vector3 Xrot(Quaternion rot)
510 {
511 Vector3 vec;
512 rot.Normalize(); // just in case
513 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
514 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
515 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
516 return vec;
517 }
518
519 public static Vector3 Zrot(Quaternion rot)
520 {
521 Vector3 vec;
522 rot.Normalize(); // just in case
523 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
524 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
525 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
526
527 return vec;
528 }
529
530 private const float halfpi = 0.5f * (float)Math.PI;
531
532 public static Vector3 ubitRot2Euler(Quaternion rot)
533 {
534 // returns roll in X
535 // pitch in Y
536 // yaw in Z
537 Vector3 vec;
538
539 // assuming rot is normalised
540 // rot.Normalize();
541
542 float zX = rot.X * rot.Z + rot.Y * rot.W;
543
544 if (zX < -0.49999f)
545 {
546 vec.X = 0;
547 vec.Y = -halfpi;
548 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
549 }
550 else if (zX > 0.49999f)
551 {
552 vec.X = 0;
553 vec.Y = halfpi;
554 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
555 }
556 else
557 {
558 vec.Y = (float)Math.Asin(2 * zX);
559
560 float sqw = rot.W * rot.W;
561
562 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
563 float zZ = rot.Z * rot.Z + sqw - 0.5f;
564
565 vec.X = (float)Math.Atan2(minuszY, zZ);
566
567 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
568 float yY = rot.X * rot.X + sqw - 0.5f;
569 vec.Z = (float)Math.Atan2(yX, yY);
570 }
571 return vec;
572 }
573
574
575 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
576 {
577 // assuming rot is normalised
578 // rot.Normalize();
579
580 float zX = rot.X * rot.Z + rot.Y * rot.W;
581
582 if (zX < -0.49999f)
583 {
584 roll = 0;
585 pitch = -halfpi;
586 }
587 else if (zX > 0.49999f)
588 {
589 roll = 0;
590 pitch = halfpi;
591 }
592 else
593 {
594 pitch = (float)Math.Asin(2 * zX);
595
596 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
597 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
598
599 roll = (float)Math.Atan2(minuszY, zZ);
600 }
601 return ;
602 }
603
604 internal void Step()//float pTimestep)
605 {
606 IntPtr Body = rootPrim.Body;
607
608 d.Quaternion rot = d.BodyGetQuaternion(Body);
609 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
610 Quaternion rotq = objrotq; // rotq = rotation of object
611 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
612 Quaternion irotq = Quaternion.Inverse(rotq);
613
614 d.Vector3 dvtmp;
615 Vector3 tmpV;
616 dvtmp = d.BodyGetLinearVel(Body);
617 Vector3 curVel;
618 curVel.X = dvtmp.X;
619 curVel.Y = dvtmp.Y;
620 curVel.Z = dvtmp.Z;
621 Vector3 curLocalVel = curVel * irotq; // current velocity in local
622
623 dvtmp = d.BodyGetAngularVel(Body);
624 Vector3 curAngVel;
625 curAngVel.X = dvtmp.X;
626 curAngVel.Y = dvtmp.Y;
627 curAngVel.Z = dvtmp.Z;
628 Vector3 curLocalAngVel = curAngVel * irotq; // current velocity in local
629
630 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
631 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame
632 d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame
633
634 bool doathing = false;
635
636 // linear motor
637 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
638 {
639 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
640 if (tmpV.LengthSquared() > 1e-6f)
641 {
642 tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep
643 tmpV *= rotq; // to world
644
645 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
646 tmpV.Z = 0;
647
648 if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0)
649 {
650 // have offset, do it now
651 tmpV *= rootPrim.Mass;
652 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
653 }
654 else
655 {
656 force.X += tmpV.X;
657 force.Y += tmpV.Y;
658 force.Z += tmpV.Z;
659 }
660 }
661 m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale);
662 }
663 else
664 m_lmEfect = 0;
665
666 // friction
667 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
668 {
669 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
670 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
671 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
672 tmpV *= rotq; // to world
673 force.X += tmpV.X;
674 force.Y += tmpV.Y;
675 force.Z += tmpV.Z;
676 }
677
678 // hover
679 if (m_VhoverTimescale < 300)
680 {
681 d.Vector3 pos = d.BodyGetPosition(Body);
682
683 // default to global
684 float perr = m_VhoverHeight - pos.Z;;
685
686 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
687 {
688 perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
689 }
690 else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
691 {
692 perr += _pParentScene.GetWaterLevel();
693 }
694 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
695 {
696 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
697 float w = _pParentScene.GetWaterLevel();
698 if (t > w)
699 perr += t;
700 else
701 perr += w;
702 }
703
704 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
705 {
706 force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
707 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
708 }
709 else // no buoyancy
710 force.Z += _pParentScene.gravityz;
711 }
712 else
713 {
714 // default gravity and buoancy
715 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
716 }
717
718 // linear deflection
719 if (m_linearDeflectionEfficiency > 0)
720 {
721 float len = curVel.Length();
722 Vector3 atAxis = refAtAxis;
723 atAxis *= rotq; // at axis rotated to world
724 atAxis = Xrot(rotq);
725 tmpV = atAxis * len;
726 tmpV -= curVel; // velocity error
727 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
728 force.X += tmpV.X;
729 force.Y += tmpV.Y;
730 if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0)
731 force.Z += tmpV.Z;
732 }
733
734 // angular motor
735 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
736 {
737 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
738 if (tmpV.LengthSquared() > 1e-6f)
739 {
740 tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep
741 tmpV *= m_referenceFrame; // to object
742 dtorque.X += tmpV.X;
743 dtorque.Y += tmpV.Y;
744 dtorque.Z += tmpV.Z;
745 }
746 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
747 }
748 else
749 m_amEfect = 0;
750
751 // angular friction
752 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
753 {
754 tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X;
755 tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y;
756 tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z;
757 tmpV *= m_referenceFrame; // to object
758 dtorque.X += tmpV.X;
759 dtorque.Y += tmpV.Y;
760 dtorque.Z += tmpV.Z;
761 }
762
763 // angular deflection
764 if (m_angularDeflectionEfficiency > 0)
765 {
766 doathing = false;
767 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE;
768 tmpV.X = 0;
769 if (Math.Abs(curLocalVel.Z) > 0.01)
770 {
771 tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp;
772 doathing = true;
773 }
774 else
775 tmpV.Y = 0;
776 if (Math.Abs(curLocalVel.Y) > 0.01)
777 {
778 tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp;
779 doathing = true;
780 }
781 else
782 tmpV.Z = 0;
783
784 if (doathing)
785 {
786 tmpV *= m_referenceFrame; // to object
787 dtorque.X += tmpV.X;
788 dtorque.Y += tmpV.Y;
789 dtorque.Z += tmpV.Z;
790 }
791 }
792
793 // vertical atractor
794 if (m_verticalAttractionTimescale < 300)
795 {
796 doathing = false;
797 float roll;
798 float pitch;
799
800 GetRollPitch(rotq, out roll, out pitch);
801
802
803 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
804 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
805
806 if (Math.Abs(roll) > 0.01) // roll
807 {
808 tmpV.X = -roll * ftmp;
809 tmpV.X -= curLocalAngVel.X * ftmp2;
810 doathing = true;
811 }
812 else
813 {
814 tmpV.X = 0;
815 }
816
817 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
818 {
819 tmpV.Y = -pitch * ftmp;
820 tmpV.Y -= curLocalAngVel.Y * ftmp2;
821 doathing = true;
822 }
823 else
824 {
825 tmpV.Y = 0;
826 }
827
828 tmpV.Z = 0;
829
830 if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01)
831 tmpV.Z = 0;
832 else
833 {
834 float broll = -roll * m_bankingEfficiency; ;
835 if (m_bankingMix != 0)
836 {
837 float vfact = m_bankingMix * Math.Abs(curLocalVel.X) / 10.0f;
838 if (vfact < m_bankingMix)
839 broll *= ((1 - m_bankingMix) + vfact);
840 }
841
842 tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale;
843 doathing = true;
844 }
845
846 if (doathing)
847 {
848
849 tmpV *= m_referenceFrame; // to object
850 dtorque.X += tmpV.X;
851 dtorque.Y += tmpV.Y;
852 dtorque.Z += tmpV.Z;
853 }
854 }
855 /*
856 d.Vector3 pos = d.BodyGetPosition(Body);
857 // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
858 Vector3 posChange = new Vector3();
859 posChange.X = pos.X - m_lastPositionVector.X;
860 posChange.Y = pos.Y - m_lastPositionVector.Y;
861 posChange.Z = pos.Z - m_lastPositionVector.Z;
862 double Zchange = Math.Abs(posChange.Z);
863 if (m_BlockingEndPoint != Vector3.Zero)
864 {
865 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
866 {
867 pos.X -= posChange.X + 1;
868 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
869 }
870 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
871 {
872 pos.Y -= posChange.Y + 1;
873 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
874 }
875 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
876 {
877 pos.Z -= posChange.Z + 1;
878 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
879 }
880 if (pos.X <= 0)
881 {
882 pos.X += posChange.X + 1;
883 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
884 }
885 if (pos.Y <= 0)
886 {
887 pos.Y += posChange.Y + 1;
888 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
889 }
890 }
891 if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
892 {
893 pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
894 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
895 }
896
897 }
898 if ((m_flags & (VehicleFlag.NO_X)) != 0)
899 {
900 m_dir.X = 0;
901 }
902 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
903 {
904 m_dir.Y = 0;
905 }
906 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
907 {
908 m_dir.Z = 0;
909 }
910
911
912 */
913 // angular part
914 /*
915
916 // Get what the body is doing, this includes 'external' influences
917/*
918 Vector3 angularVelocity = Vector3.Zero;
919
920 // Vertical attractor section
921 Vector3 vertattr = Vector3.Zero;
922
923 if (m_verticalAttractionTimescale < 300)
924 {
925 float VAservo = 0.2f / m_verticalAttractionTimescale;
926 // get present body rotation
927 // make a vector pointing up
928 Vector3 verterr = Vector3.Zero;
929 verterr.Z = 1.0f;
930 // rotate it to Body Angle
931 verterr = verterr * rotq;
932 // 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.
933 // 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
934 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
935 if (verterr.Z < 0.0f)
936 {
937 verterr.X = 2.0f - verterr.X;
938 verterr.Y = 2.0f - verterr.Y;
939 }
940 // Error is 0 (no error) to +/- 2 (max error)
941 // scale it by VAservo
942 verterr = verterr * VAservo;
943//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
944
945 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
946 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
947 vertattr.X = verterr.Y;
948 vertattr.Y = - verterr.X;
949 vertattr.Z = 0f;
950
951 // scaling appears better usingsquare-law
952 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
953 vertattr.X += bounce * angularVelocity.X;
954 vertattr.Y += bounce * angularVelocity.Y;
955
956 } // else vertical attractor is off
957
958 // m_lastVertAttractor = vertattr;
959
960 // Bank section tba
961 // Deflection section tba
962
963 // Sum velocities
964 m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection
965
966 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
967 {
968 m_lastAngularVelocity.X = 0;
969 m_lastAngularVelocity.Y = 0;
970 }
971
972 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
973 {
974 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
975 }
976 else
977 {
978 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
979 }
980*/
981
982 d.Mass dmass;
983 d.BodyGetMass(Body,out dmass);
984
985 if (force.X != 0 || force.Y != 0 || force.Z != 0)
986 {
987 force *= dmass.mass;
988 d.BodySetForce(Body, force.X, force.Y, force.Z);
989 }
990
991 if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0)
992 {
993 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
994 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
995 }
996
997 //end MoveAngular
998
999 // limit rotations
1000/*
1001 bool changed = false;
1002
1003 if (m_RollreferenceFrame != Quaternion.Identity)
1004 {
1005 if (rotq.X >= m_RollreferenceFrame.X)
1006 {
1007 rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
1008 }
1009 if (rotq.Y >= m_RollreferenceFrame.Y)
1010 {
1011 rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
1012 }
1013 if (rotq.X <= -m_RollreferenceFrame.X)
1014 {
1015 rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
1016 }
1017 if (rotq.Y <= -m_RollreferenceFrame.Y)
1018 {
1019 rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
1020 }
1021 changed = true;
1022 }
1023
1024 if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
1025 {
1026 rot.X = 0;
1027 rot.Y = 0;
1028 changed = true;
1029 }
1030 if (changed)
1031 d.BodySetQuaternion(Body, ref rot);
1032*/
1033 }
1034 }
1035}
generated by cgit v1.1 at 2024-07-16 03:03:07 +0000