aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs1205
1 files changed, 1205 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
new file mode 100644
index 0000000..ce10065
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs
@@ -0,0 +1,1205 @@
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
41// Extensive change Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.PhysicsModules.SharedBase;
52
53namespace OpenSim.Region.PhysicsModule.ubOde
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private ODEScene _pParentScene;
64
65 // Vehicle properties
66 // WARNING this are working copies for internel use
67 // their values may not be the corresponding parameter
68
69 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
70 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
71
72 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
73
74 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
75 // HOVER_TERRAIN_ONLY
76 // HOVER_GLOBAL_HEIGHT
77 // NO_DEFLECTION_UP
78 // HOVER_WATER_ONLY
79 // HOVER_UP_ONLY
80 // LIMIT_MOTOR_UP
81 // LIMIT_ROLL_ONLY
82 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
83
84 // Linear properties
85 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
86 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
87 private float m_linearMotorDecayTimescale = 120;
88 private float m_linearMotorTimescale = 1000;
89 private Vector3 m_linearMotorOffset = Vector3.Zero;
90
91 //Angular properties
92 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
93 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
94 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
95 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
96
97 //Deflection properties
98 private float m_angularDeflectionEfficiency = 0;
99 private float m_angularDeflectionTimescale = 1000;
100 private float m_linearDeflectionEfficiency = 0;
101 private float m_linearDeflectionTimescale = 1000;
102
103 //Banking properties
104 private float m_bankingEfficiency = 0;
105 private float m_bankingMix = 0;
106 private float m_bankingTimescale = 1000;
107
108 //Hover and Buoyancy properties
109 private float m_VhoverHeight = 0f;
110 private float m_VhoverEfficiency = 0f;
111 private float m_VhoverTimescale = 1000f;
112 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
113 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
114 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
115 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
116
117 //Attractor properties
118 private float m_verticalAttractionEfficiency = 1.0f; // damped
119 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
120
121
122 // auxiliar
123 private float m_lmEfect = 0f; // current linear motor eficiency
124 private float m_lmDecay = 0f; // current linear decay
125
126 private float m_amEfect = 0; // current angular motor eficiency
127 private float m_amDecay = 0f; // current linear decay
128
129 private float m_ffactor = 1.0f;
130
131 private float m_timestep = 0.02f;
132 private float m_invtimestep = 50;
133
134
135 float m_ampwr;
136 float m_amdampX;
137 float m_amdampY;
138 float m_amdampZ;
139
140 float m_gravmod;
141
142 public float FrictionFactor
143 {
144 get
145 {
146 return m_ffactor;
147 }
148 }
149
150 public float GravMod
151 {
152 set
153 {
154 m_gravmod = value;
155 }
156 }
157
158
159 public ODEDynamics(OdePrim rootp)
160 {
161 rootPrim = rootp;
162 _pParentScene = rootPrim._parent_scene;
163 m_timestep = _pParentScene.ODE_STEPSIZE;
164 m_invtimestep = 1.0f / m_timestep;
165 m_gravmod = rootPrim.GravModifier;
166 }
167
168 public void DoSetVehicle(VehicleData vd)
169 {
170 m_type = vd.m_type;
171 m_flags = vd.m_flags;
172
173
174 // Linear properties
175 m_linearMotorDirection = vd.m_linearMotorDirection;
176
177 m_linearFrictionTimescale = vd.m_linearFrictionTimescale;
178 if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep;
179 if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep;
180 if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep;
181
182 m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale;
183 if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep;
184 m_linearMotorDecayTimescale += 0.2f;
185 m_linearMotorDecayTimescale *= m_invtimestep;
186
187 m_linearMotorTimescale = vd.m_linearMotorTimescale;
188 if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep;
189
190 m_linearMotorOffset = vd.m_linearMotorOffset;
191
192 //Angular properties
193 m_angularMotorDirection = vd.m_angularMotorDirection;
194 m_angularMotorTimescale = vd.m_angularMotorTimescale;
195 if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep;
196
197 m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale;
198 if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep;
199 m_angularMotorDecayTimescale *= m_invtimestep;
200
201 m_angularFrictionTimescale = vd.m_angularFrictionTimescale;
202 if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep;
203 if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep;
204 if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep;
205
206 //Deflection properties
207 m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency;
208 m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale;
209 if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep;
210
211 m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency;
212 m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale;
213 if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep;
214
215 //Banking properties
216 m_bankingEfficiency = vd.m_bankingEfficiency;
217 m_bankingMix = vd.m_bankingMix;
218 m_bankingTimescale = vd.m_bankingTimescale;
219 if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep;
220
221 //Hover and Buoyancy properties
222 m_VhoverHeight = vd.m_VhoverHeight;
223 m_VhoverEfficiency = vd.m_VhoverEfficiency;
224 m_VhoverTimescale = vd.m_VhoverTimescale;
225 if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep;
226
227 m_VehicleBuoyancy = vd.m_VehicleBuoyancy;
228
229 //Attractor properties
230 m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency;
231 m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale;
232 if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep;
233
234 // Axis
235 m_referenceFrame = vd.m_referenceFrame;
236
237 m_lmEfect = 0;
238 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
239 m_amEfect = 0;
240 m_ffactor = 1.0f;
241 }
242
243 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
244 {
245 float len;
246 if(float.IsNaN(pValue) || float.IsInfinity(pValue))
247 return;
248
249 switch (pParam)
250 {
251 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
252 if (pValue < 0f) pValue = 0f;
253 if (pValue > 1f) pValue = 1f;
254 m_angularDeflectionEfficiency = pValue;
255 break;
256 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
257 if (pValue < m_timestep) pValue = m_timestep;
258 m_angularDeflectionTimescale = pValue;
259 break;
260 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
261 if (pValue < m_timestep) pValue = m_timestep;
262 else if (pValue > 120) pValue = 120;
263 m_angularMotorDecayTimescale = pValue * m_invtimestep;
264 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
265 break;
266 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
267 if (pValue < m_timestep) pValue = m_timestep;
268 m_angularMotorTimescale = pValue;
269 break;
270 case Vehicle.BANKING_EFFICIENCY:
271 if (pValue < -1f) pValue = -1f;
272 if (pValue > 1f) pValue = 1f;
273 m_bankingEfficiency = pValue;
274 break;
275 case Vehicle.BANKING_MIX:
276 if (pValue < 0f) pValue = 0f;
277 if (pValue > 1f) pValue = 1f;
278 m_bankingMix = pValue;
279 break;
280 case Vehicle.BANKING_TIMESCALE:
281 if (pValue < m_timestep) pValue = m_timestep;
282 m_bankingTimescale = pValue;
283 break;
284 case Vehicle.BUOYANCY:
285 if (pValue < -1f) pValue = -1f;
286 if (pValue > 1f) pValue = 1f;
287 m_VehicleBuoyancy = pValue;
288 break;
289 case Vehicle.HOVER_EFFICIENCY:
290 if (pValue < 0f) pValue = 0f;
291 if (pValue > 1f) pValue = 1f;
292 m_VhoverEfficiency = pValue;
293 break;
294 case Vehicle.HOVER_HEIGHT:
295 m_VhoverHeight = pValue;
296 break;
297 case Vehicle.HOVER_TIMESCALE:
298 if (pValue < m_timestep) pValue = m_timestep;
299 m_VhoverTimescale = pValue;
300 break;
301 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
302 if (pValue < 0f) pValue = 0f;
303 if (pValue > 1f) pValue = 1f;
304 m_linearDeflectionEfficiency = pValue;
305 break;
306 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
307 if (pValue < m_timestep) pValue = m_timestep;
308 m_linearDeflectionTimescale = pValue;
309 break;
310 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
311 if (pValue < m_timestep) pValue = m_timestep;
312 else if (pValue > 120) pValue = 120;
313 m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep;
314 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
315 break;
316 case Vehicle.LINEAR_MOTOR_TIMESCALE:
317 if (pValue < m_timestep) pValue = m_timestep;
318 m_linearMotorTimescale = pValue;
319 break;
320 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
321 if (pValue < 0f) pValue = 0f;
322 if (pValue > 1f) pValue = 1f;
323 m_verticalAttractionEfficiency = pValue;
324 break;
325 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
326 if (pValue < m_timestep) pValue = m_timestep;
327 m_verticalAttractionTimescale = pValue;
328 break;
329
330 // These are vector properties but the engine lets you use a single float value to
331 // set all of the components to the same value
332 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
333 if (pValue < m_timestep) pValue = m_timestep;
334 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
335 break;
336 case Vehicle.ANGULAR_MOTOR_DIRECTION:
337 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
338 len = m_angularMotorDirection.Length();
339 if (len > 12.566f)
340 m_angularMotorDirection *= (12.566f / len);
341
342 m_amEfect = 1.0f ; // turn it on
343 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
344
345 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
346 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
347 d.BodyEnable(rootPrim.Body);
348
349 break;
350 case Vehicle.LINEAR_FRICTION_TIMESCALE:
351 if (pValue < m_timestep) pValue = m_timestep;
352 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
353 break;
354 case Vehicle.LINEAR_MOTOR_DIRECTION:
355 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
356 len = m_linearMotorDirection.Length();
357 if (len > 100.0f)
358 m_linearMotorDirection *= (100.0f / len);
359
360 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
361 m_lmEfect = 1.0f; // turn it on
362
363 m_ffactor = 0.0f;
364 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
365 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
366 d.BodyEnable(rootPrim.Body);
367 break;
368 case Vehicle.LINEAR_MOTOR_OFFSET:
369 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
370 len = m_linearMotorOffset.Length();
371 if (len > 100.0f)
372 m_linearMotorOffset *= (100.0f / len);
373 break;
374 }
375 }//end ProcessFloatVehicleParam
376
377 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
378 {
379 float len;
380 if(!pValue.IsFinite())
381 return;
382
383 switch (pParam)
384 {
385 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
386 if (pValue.X < m_timestep) pValue.X = m_timestep;
387 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
388 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
389
390 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
391 break;
392 case Vehicle.ANGULAR_MOTOR_DIRECTION:
393 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
394 // Limit requested angular speed to 2 rps= 4 pi rads/sec
395 len = m_angularMotorDirection.Length();
396 if (len > 12.566f)
397 m_angularMotorDirection *= (12.566f / len);
398
399 m_amEfect = 1.0f; // turn it on
400 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
401
402 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
403 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
404 d.BodyEnable(rootPrim.Body);
405 break;
406 case Vehicle.LINEAR_FRICTION_TIMESCALE:
407 if (pValue.X < m_timestep) pValue.X = m_timestep;
408 if (pValue.Y < m_timestep) pValue.Y = m_timestep;
409 if (pValue.Z < m_timestep) pValue.Z = m_timestep;
410 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
411 break;
412 case Vehicle.LINEAR_MOTOR_DIRECTION:
413 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
414 len = m_linearMotorDirection.Length();
415 if (len > 100.0f)
416 m_linearMotorDirection *= (100.0f / len);
417
418 m_lmEfect = 1.0f; // turn it on
419 m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale;
420
421 m_ffactor = 0.0f;
422 if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body)
423 && !rootPrim.m_isSelected && !rootPrim.m_disabled)
424 d.BodyEnable(rootPrim.Body);
425 break;
426 case Vehicle.LINEAR_MOTOR_OFFSET:
427 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
428 len = m_linearMotorOffset.Length();
429 if (len > 100.0f)
430 m_linearMotorOffset *= (100.0f / len);
431 break;
432 case Vehicle.BLOCK_EXIT:
433 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
434 break;
435 }
436 }//end ProcessVectorVehicleParam
437
438 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
439 {
440 switch (pParam)
441 {
442 case Vehicle.REFERENCE_FRAME:
443 // m_referenceFrame = Quaternion.Inverse(pValue);
444 m_referenceFrame = pValue;
445 break;
446 case Vehicle.ROLL_FRAME:
447 m_RollreferenceFrame = pValue;
448 break;
449 }
450 }//end ProcessRotationVehicleParam
451
452 internal void ProcessVehicleFlags(int pParam, bool remove)
453 {
454 if (remove)
455 {
456 m_flags &= ~((VehicleFlag)pParam);
457 }
458 else
459 {
460 m_flags |= (VehicleFlag)pParam;
461 }
462 }//end ProcessVehicleFlags
463
464 internal void ProcessTypeChange(Vehicle pType)
465 {
466 m_lmEfect = 0;
467
468 m_amEfect = 0;
469 m_ffactor = 1f;
470
471 m_linearMotorDirection = Vector3.Zero;
472 m_angularMotorDirection = Vector3.Zero;
473
474 m_BlockingEndPoint = Vector3.Zero;
475 m_RollreferenceFrame = Quaternion.Identity;
476 m_linearMotorOffset = Vector3.Zero;
477
478 m_referenceFrame = Quaternion.Identity;
479
480 // Set Defaults For Type
481 m_type = pType;
482 switch (pType)
483 {
484 case Vehicle.TYPE_NONE:
485 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
486 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
487 m_linearMotorTimescale = 1000;
488 m_linearMotorDecayTimescale = 120 * m_invtimestep;
489 m_angularMotorTimescale = 1000;
490 m_angularMotorDecayTimescale = 1000 * m_invtimestep;
491 m_VhoverHeight = 0;
492 m_VhoverEfficiency = 1;
493 m_VhoverTimescale = 1000;
494 m_VehicleBuoyancy = 0;
495 m_linearDeflectionEfficiency = 0;
496 m_linearDeflectionTimescale = 1000;
497 m_angularDeflectionEfficiency = 0;
498 m_angularDeflectionTimescale = 1000;
499 m_bankingEfficiency = 0;
500 m_bankingMix = 1;
501 m_bankingTimescale = 1000;
502 m_verticalAttractionEfficiency = 0;
503 m_verticalAttractionTimescale = 1000;
504
505 m_flags = (VehicleFlag)0;
506 break;
507
508 case Vehicle.TYPE_SLED:
509 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
510 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
511 m_linearMotorTimescale = 1000;
512 m_linearMotorDecayTimescale = 120 * m_invtimestep;
513 m_angularMotorTimescale = 1000;
514 m_angularMotorDecayTimescale = 120 * m_invtimestep;
515 m_VhoverHeight = 0;
516 m_VhoverEfficiency = 1;
517 m_VhoverTimescale = 10;
518 m_VehicleBuoyancy = 0;
519 m_linearDeflectionEfficiency = 1;
520 m_linearDeflectionTimescale = 1;
521 m_angularDeflectionEfficiency = 0;
522 m_angularDeflectionTimescale = 10;
523 m_verticalAttractionEfficiency = 1;
524 m_verticalAttractionTimescale = 1000;
525 m_bankingEfficiency = 0;
526 m_bankingMix = 1;
527 m_bankingTimescale = 10;
528 m_flags &=
529 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
530 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
531 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
532 VehicleFlag.LIMIT_ROLL_ONLY |
533 VehicleFlag.LIMIT_MOTOR_UP);
534 break;
535
536 case Vehicle.TYPE_CAR:
537 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
538 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
539 m_linearMotorTimescale = 1;
540 m_linearMotorDecayTimescale = 60 * m_invtimestep;
541 m_angularMotorTimescale = 1;
542 m_angularMotorDecayTimescale = 0.8f * m_invtimestep;
543 m_VhoverHeight = 0;
544 m_VhoverEfficiency = 0;
545 m_VhoverTimescale = 1000;
546 m_VehicleBuoyancy = 0;
547 m_linearDeflectionEfficiency = 1;
548 m_linearDeflectionTimescale = 2;
549 m_angularDeflectionEfficiency = 0;
550 m_angularDeflectionTimescale = 10;
551 m_verticalAttractionEfficiency = 1f;
552 m_verticalAttractionTimescale = 10f;
553 m_bankingEfficiency = -0.2f;
554 m_bankingMix = 1;
555 m_bankingTimescale = 1;
556 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
557 VehicleFlag.HOVER_TERRAIN_ONLY |
558 VehicleFlag.HOVER_GLOBAL_HEIGHT);
559 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
560 VehicleFlag.LIMIT_ROLL_ONLY |
561 VehicleFlag.LIMIT_MOTOR_UP |
562 VehicleFlag.HOVER_UP_ONLY);
563 break;
564 case Vehicle.TYPE_BOAT:
565 m_linearFrictionTimescale = new Vector3(10, 3, 2);
566 m_angularFrictionTimescale = new Vector3(10, 10, 10);
567 m_linearMotorTimescale = 5;
568 m_linearMotorDecayTimescale = 60 * m_invtimestep;
569 m_angularMotorTimescale = 4;
570 m_angularMotorDecayTimescale = 4 * m_invtimestep;
571 m_VhoverHeight = 0;
572 m_VhoverEfficiency = 0.5f;
573 m_VhoverTimescale = 2;
574 m_VehicleBuoyancy = 1;
575 m_linearDeflectionEfficiency = 0.5f;
576 m_linearDeflectionTimescale = 3;
577 m_angularDeflectionEfficiency = 0.5f;
578 m_angularDeflectionTimescale = 5;
579 m_verticalAttractionEfficiency = 0.5f;
580 m_verticalAttractionTimescale = 5f;
581 m_bankingEfficiency = -0.3f;
582 m_bankingMix = 0.8f;
583 m_bankingTimescale = 1;
584 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
585 VehicleFlag.HOVER_GLOBAL_HEIGHT |
586 VehicleFlag.HOVER_UP_ONLY); // |
587// VehicleFlag.LIMIT_ROLL_ONLY);
588 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
589 VehicleFlag.LIMIT_MOTOR_UP |
590 VehicleFlag.HOVER_UP_ONLY | // new sl
591 VehicleFlag.HOVER_WATER_ONLY);
592 break;
593
594 case Vehicle.TYPE_AIRPLANE:
595 m_linearFrictionTimescale = new Vector3(200, 10, 5);
596 m_angularFrictionTimescale = new Vector3(20, 20, 20);
597 m_linearMotorTimescale = 2;
598 m_linearMotorDecayTimescale = 60 * m_invtimestep;
599 m_angularMotorTimescale = 4;
600 m_angularMotorDecayTimescale = 8 * m_invtimestep;
601 m_VhoverHeight = 0;
602 m_VhoverEfficiency = 0.5f;
603 m_VhoverTimescale = 1000;
604 m_VehicleBuoyancy = 0;
605 m_linearDeflectionEfficiency = 0.5f;
606 m_linearDeflectionTimescale = 0.5f;
607 m_angularDeflectionEfficiency = 1;
608 m_angularDeflectionTimescale = 2;
609 m_verticalAttractionEfficiency = 0.9f;
610 m_verticalAttractionTimescale = 2f;
611 m_bankingEfficiency = 1;
612 m_bankingMix = 0.7f;
613 m_bankingTimescale = 2;
614 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
615 VehicleFlag.HOVER_TERRAIN_ONLY |
616 VehicleFlag.HOVER_GLOBAL_HEIGHT |
617 VehicleFlag.HOVER_UP_ONLY |
618 VehicleFlag.NO_DEFLECTION_UP |
619 VehicleFlag.LIMIT_MOTOR_UP);
620 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
621 break;
622
623 case Vehicle.TYPE_BALLOON:
624 m_linearFrictionTimescale = new Vector3(5, 5, 5);
625 m_angularFrictionTimescale = new Vector3(10, 10, 10);
626 m_linearMotorTimescale = 5;
627 m_linearMotorDecayTimescale = 60 * m_invtimestep;
628 m_angularMotorTimescale = 6;
629 m_angularMotorDecayTimescale = 10 * m_invtimestep;
630 m_VhoverHeight = 5;
631 m_VhoverEfficiency = 0.8f;
632 m_VhoverTimescale = 10;
633 m_VehicleBuoyancy = 1;
634 m_linearDeflectionEfficiency = 0;
635 m_linearDeflectionTimescale = 5 * m_invtimestep;
636 m_angularDeflectionEfficiency = 0;
637 m_angularDeflectionTimescale = 5;
638 m_verticalAttractionEfficiency = 1f;
639 m_verticalAttractionTimescale = 1000f;
640 m_bankingEfficiency = 0;
641 m_bankingMix = 0.7f;
642 m_bankingTimescale = 5;
643 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
644 VehicleFlag.HOVER_TERRAIN_ONLY |
645 VehicleFlag.HOVER_UP_ONLY |
646 VehicleFlag.NO_DEFLECTION_UP |
647 VehicleFlag.LIMIT_MOTOR_UP | //);
648 VehicleFlag.LIMIT_ROLL_ONLY | // new sl
649 VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl
650
651// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
652// VehicleFlag.HOVER_GLOBAL_HEIGHT);
653 break;
654
655 }
656 // disable mouse steering
657 m_flags &= ~(VehicleFlag.MOUSELOOK_STEER |
658 VehicleFlag.MOUSELOOK_BANK |
659 VehicleFlag.CAMERA_DECOUPLED);
660
661 m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale);
662 m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale;
663
664 }//end SetDefaultsForType
665
666 internal void Stop()
667 {
668 m_lmEfect = 0;
669 m_lmDecay = 0f;
670 m_amEfect = 0;
671 m_amDecay = 0;
672 m_ffactor = 1f;
673 }
674
675 public static Vector3 Xrot(Quaternion rot)
676 {
677 Vector3 vec;
678 rot.Normalize(); // just in case
679 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
680 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
681 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
682 return vec;
683 }
684
685 public static Vector3 Zrot(Quaternion rot)
686 {
687 Vector3 vec;
688 rot.Normalize(); // just in case
689 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
690 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
691 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
692
693 return vec;
694 }
695
696 private const float pi = (float)Math.PI;
697 private const float halfpi = 0.5f * (float)Math.PI;
698 private const float twopi = 2.0f * pi;
699
700 public static Vector3 ubRot2Euler(Quaternion rot)
701 {
702 // returns roll in X
703 // pitch in Y
704 // yaw in Z
705 Vector3 vec;
706
707 // assuming rot is normalised
708 // rot.Normalize();
709
710 float zX = rot.X * rot.Z + rot.Y * rot.W;
711
712 if (zX < -0.49999f)
713 {
714 vec.X = 0;
715 vec.Y = -halfpi;
716 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
717 }
718 else if (zX > 0.49999f)
719 {
720 vec.X = 0;
721 vec.Y = halfpi;
722 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
723 }
724 else
725 {
726 vec.Y = (float)Math.Asin(2 * zX);
727
728 float sqw = rot.W * rot.W;
729
730 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
731 float zZ = rot.Z * rot.Z + sqw - 0.5f;
732
733 vec.X = (float)Math.Atan2(minuszY, zZ);
734
735 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
736 float yY = rot.X * rot.X + sqw - 0.5f;
737 vec.Z = (float)Math.Atan2(yX, yY);
738 }
739 return vec;
740 }
741
742 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
743 {
744 // assuming rot is normalised
745 // rot.Normalize();
746
747 float zX = rot.X * rot.Z + rot.Y * rot.W;
748
749 if (zX < -0.49999f)
750 {
751 roll = 0;
752 pitch = -halfpi;
753 }
754 else if (zX > 0.49999f)
755 {
756 roll = 0;
757 pitch = halfpi;
758 }
759 else
760 {
761 pitch = (float)Math.Asin(2 * zX);
762
763 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
764 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
765
766 roll = (float)Math.Atan2(minuszY, zZ);
767 }
768 return ;
769 }
770
771 internal void Step()
772 {
773 IntPtr Body = rootPrim.Body;
774
775 d.Mass dmass;
776 d.BodyGetMass(Body, out dmass);
777
778 d.Quaternion rot = d.BodyGetQuaternion(Body);
779 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
780 Quaternion rotq = objrotq; // rotq = rotation of object
781 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
782 Quaternion irotq = Quaternion.Inverse(rotq);
783
784 d.Vector3 dvtmp;
785 Vector3 tmpV;
786 Vector3 curVel; // velocity in world
787 Vector3 curAngVel; // angular velocity in world
788 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
789 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
790 d.Vector3 dtorque = new d.Vector3();
791
792 dvtmp = d.BodyGetLinearVel(Body);
793 curVel.X = dvtmp.X;
794 curVel.Y = dvtmp.Y;
795 curVel.Z = dvtmp.Z;
796 Vector3 curLocalVel = curVel * irotq; // current velocity in local
797
798 dvtmp = d.BodyGetAngularVel(Body);
799 curAngVel.X = dvtmp.X;
800 curAngVel.Y = dvtmp.Y;
801 curAngVel.Z = dvtmp.Z;
802 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
803
804 float ldampZ = 0;
805
806 bool mousemode = false;
807 bool mousemodebank = false;
808
809 float bankingEfficiency;
810 float verticalAttractionTimescale = m_verticalAttractionTimescale;
811
812 if((m_flags & (VehicleFlag.MOUSELOOK_STEER | VehicleFlag.MOUSELOOK_BANK)) != 0 )
813 {
814 mousemode = true;
815 mousemodebank = (m_flags & VehicleFlag.MOUSELOOK_BANK) != 0;
816 if(mousemodebank)
817 {
818 bankingEfficiency = m_bankingEfficiency;
819 if(verticalAttractionTimescale < 149.9)
820 verticalAttractionTimescale *= 2.0f; // reduce current instability
821 }
822 else
823 bankingEfficiency = 0;
824 }
825 else
826 bankingEfficiency = m_bankingEfficiency;
827
828 // linear motor
829 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
830 {
831 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
832 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
833 tmpV *= rotq; // to world
834
835 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
836 tmpV.Z = 0;
837
838 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
839 {
840 // have offset, do it now
841 tmpV *= dmass.mass;
842 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
843 }
844 else
845 {
846 force.X += tmpV.X;
847 force.Y += tmpV.Y;
848 force.Z += tmpV.Z;
849 }
850
851 m_lmEfect *= m_lmDecay;
852// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
853 m_ffactor = 0.0f;
854 }
855 else
856 {
857 m_lmEfect = 0;
858 m_ffactor = 1f;
859 }
860
861 // hover
862 if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
863 {
864 // d.Vector3 pos = d.BodyGetPosition(Body);
865 d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
866 pos.Z -= 0.21f; // minor offset that seems to be always there in sl
867
868 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
869 float perr;
870
871 // default to global but don't go underground
872 perr = m_VhoverHeight - pos.Z;
873
874 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
875 {
876 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
877 {
878 perr += _pParentScene.GetWaterLevel();
879 }
880 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
881 {
882 perr += t;
883 }
884 else
885 {
886 float w = _pParentScene.GetWaterLevel();
887 if (t > w)
888 perr += t;
889 else
890 perr += w;
891 }
892 }
893 else if (t > m_VhoverHeight)
894 perr = t - pos.Z; ;
895
896 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
897 {
898 ldampZ = m_VhoverEfficiency * m_invtimestep;
899
900 perr *= (1.0f + ldampZ) / m_VhoverTimescale;
901
902 // force.Z += perr - curVel.Z * tmp;
903 force.Z += perr;
904 ldampZ *= -curVel.Z;
905
906 force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
907 }
908 else // no buoyancy
909 force.Z += _pParentScene.gravityz;
910 }
911 else
912 {
913 // default gravity and Buoyancy
914 force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
915 }
916
917 // linear deflection
918 if (m_linearDeflectionEfficiency > 0)
919 {
920 float len = curVel.Length();
921 if (len > 0.01) // if moving
922 {
923 Vector3 atAxis;
924 atAxis = Xrot(rotq); // where are we pointing to
925 atAxis *= len; // make it same size as world velocity vector
926
927 tmpV = -atAxis; // oposite direction
928 atAxis -= curVel; // error to one direction
929 len = atAxis.LengthSquared();
930
931 tmpV -= curVel; // error to oposite
932 float lens = tmpV.LengthSquared();
933
934 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
935 {
936 if (len < lens)
937 tmpV = atAxis;
938
939 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
940 force.X += tmpV.X;
941 force.Y += tmpV.Y;
942 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
943 force.Z += tmpV.Z;
944 }
945 }
946 }
947
948 // linear friction/damping
949 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
950 {
951 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
952 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
953 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
954 tmpV *= rotq; // to world
955
956 if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
957 tmpV.Z = ldampZ;
958 force.X += tmpV.X;
959 force.Y += tmpV.Y;
960 force.Z += tmpV.Z;
961 }
962
963 // vertical atractor
964 if (verticalAttractionTimescale < 300)
965 {
966 float roll;
967 float pitch;
968
969 float ftmp = m_invtimestep / verticalAttractionTimescale / verticalAttractionTimescale;
970
971 float ftmp2;
972 ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
973 m_amdampX = ftmp2;
974
975 m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;
976
977 GetRollPitch(irotq, out roll, out pitch);
978
979 if (roll > halfpi)
980 roll = pi - roll;
981 else if (roll < -halfpi)
982 roll = -pi - roll;
983
984 float effroll = pitch / halfpi;
985 effroll *= effroll;
986 effroll = 1 - effroll;
987 effroll *= roll;
988
989 torque.X += effroll * ftmp;
990
991 if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
992 {
993 float effpitch = roll / halfpi;
994 effpitch *= effpitch;
995 effpitch = 1 - effpitch;
996 effpitch *= pitch;
997
998 torque.Y += effpitch * ftmp;
999 }
1000
1001 if (bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
1002 {
1003
1004 float broll = effroll;
1005 /*
1006 if (broll > halfpi)
1007 broll = pi - broll;
1008 else if (broll < -halfpi)
1009 broll = -pi - broll;
1010 */
1011 broll *= m_bankingEfficiency;
1012 if (m_bankingMix != 0)
1013 {
1014 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
1015 if (vfact > 1.0f) vfact = 1.0f;
1016
1017 if (curLocalVel.X >= 0)
1018 broll *= (1 + (vfact - 1) * m_bankingMix);
1019 else
1020 broll *= -(1 + (vfact - 1) * m_bankingMix);
1021 }
1022 // make z rot be in world Z not local as seems to be in sl
1023
1024 broll = broll / m_bankingTimescale;
1025
1026
1027 tmpV = Zrot(irotq);
1028 tmpV *= broll;
1029
1030 torque.X += tmpV.X;
1031 torque.Y += tmpV.Y;
1032 torque.Z += tmpV.Z;
1033
1034 m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
1035 m_amdampY = m_amdampZ;
1036
1037 }
1038 else
1039 {
1040 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1041 m_amdampY = m_amdampX;
1042 }
1043 }
1044 else
1045 {
1046 m_ampwr = 1.0f;
1047 m_amdampX = 1 / m_angularFrictionTimescale.X;
1048 m_amdampY = 1 / m_angularFrictionTimescale.Y;
1049 m_amdampZ = 1 / m_angularFrictionTimescale.Z;
1050 }
1051
1052 if(mousemode)
1053 {
1054 CameraData cam = rootPrim.TryGetCameraData();
1055 if(cam.Valid && cam.MouseLook)
1056 {
1057 Vector3 dirv = cam.CameraAtAxis * irotq;
1058
1059 float invamts = 1.0f/m_angularMotorTimescale;
1060 float tmp;
1061
1062 // get out of x == 0 plane
1063 if(Math.Abs(dirv.X) < 0.001f)
1064 dirv.X = 0.001f;
1065
1066 if (Math.Abs(dirv.Z) > 0.01)
1067 {
1068 tmp = -(float)Math.Atan2(dirv.Z, dirv.X) * m_angularMotorDirection.Y;
1069 if(tmp < -4f)
1070 tmp = -4f;
1071 else if(tmp > 4f)
1072 tmp = 4f;
1073 torque.Y += (tmp - curLocalAngVel.Y) * invamts;
1074 torque.Y -= curLocalAngVel.Y * m_amdampY;
1075 }
1076 else
1077 torque.Y -= curLocalAngVel.Y * m_invtimestep;
1078
1079 if (Math.Abs(dirv.Y) > 0.01)
1080 {
1081 if(mousemodebank)
1082 {
1083 tmp = -(float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.X;
1084 if(tmp < -4f)
1085 tmp = -4f;
1086 else if(tmp > 4f)
1087 tmp = 4f;
1088 torque.X += (tmp - curLocalAngVel.X) * invamts;
1089 }
1090 else
1091 {
1092 tmp = (float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.Z;
1093 tmp *= invamts;
1094 if(tmp < -4f)
1095 tmp = -4f;
1096 else if(tmp > 4f)
1097 tmp = 4f;
1098 torque.Z += (tmp - curLocalAngVel.Z) * invamts;
1099 }
1100 torque.X -= curLocalAngVel.X * m_amdampX;
1101 torque.Z -= curLocalAngVel.Z * m_amdampZ;
1102 }
1103 else
1104 {
1105 if(mousemodebank)
1106 torque.X -= curLocalAngVel.X * m_invtimestep;
1107 else
1108 torque.Z -= curLocalAngVel.Z * m_invtimestep;
1109 }
1110 }
1111 else
1112 {
1113 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1114 {
1115 torque.X -= curLocalAngVel.X * 10f;
1116 torque.Y -= curLocalAngVel.Y * 10f;
1117 torque.Z -= curLocalAngVel.Z * 10f;
1118 }
1119 }
1120 }
1121 else
1122 {
1123 // angular motor
1124 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
1125 {
1126 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
1127 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
1128 torque.X += tmpV.X * m_ampwr;
1129 torque.Y += tmpV.Y * m_ampwr;
1130 torque.Z += tmpV.Z;
1131
1132 m_amEfect *= m_amDecay;
1133 }
1134 else
1135 m_amEfect = 0;
1136
1137 // angular deflection
1138 if (m_angularDeflectionEfficiency > 0)
1139 {
1140 Vector3 dirv;
1141
1142 if (curLocalVel.X > 0.01f)
1143 dirv = curLocalVel;
1144 else if (curLocalVel.X < -0.01f)
1145 // use oposite
1146 dirv = -curLocalVel;
1147 else
1148 {
1149 // make it fall into small positive x case
1150 dirv.X = 0.01f;
1151 dirv.Y = curLocalVel.Y;
1152 dirv.Z = curLocalVel.Z;
1153 }
1154
1155 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1156
1157 if (Math.Abs(dirv.Z) > 0.01)
1158 {
1159 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
1160 }
1161
1162 if (Math.Abs(dirv.Y) > 0.01)
1163 {
1164 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
1165 }
1166 }
1167
1168 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
1169 {
1170 torque.X -= curLocalAngVel.X * m_amdampX;
1171 torque.Y -= curLocalAngVel.Y * m_amdampY;
1172 torque.Z -= curLocalAngVel.Z * m_amdampZ;
1173 }
1174 }
1175
1176 force *= dmass.mass;
1177
1178 force += rootPrim.m_force;
1179 force += rootPrim.m_forceacc;
1180 rootPrim.m_forceacc = Vector3.Zero;
1181
1182 if (force.X != 0 || force.Y != 0 || force.Z != 0)
1183 {
1184 d.BodyAddForce(Body, force.X, force.Y, force.Z);
1185 }
1186
1187 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1188 {
1189 torque *= m_referenceFrame; // to object frame
1190 dtorque.X = torque.X ;
1191 dtorque.Y = torque.Y;
1192 dtorque.Z = torque.Z;
1193
1194 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
1195 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
1196 }
1197
1198 torque = rootPrim.m_torque;
1199 torque += rootPrim.m_angularForceacc;
1200 rootPrim.m_angularForceacc = Vector3.Zero;
1201 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
1202 d.BodyAddTorque(Body,torque.X, torque.Y, torque.Z);
1203 }
1204 }
1205}
generated by cgit v1.1 at 2024-11-16 15:56:30 +0000