aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs440
1 files changed, 216 insertions, 224 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
index 39cdc0f..008070b 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
@@ -32,11 +32,11 @@
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion 32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular 33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear() 34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to 36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics 37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use. 38 * settings use.
39 */ 39 */
40 40
41using System; 41using System;
42using System.Collections.Generic; 42using System.Collections.Generic;
@@ -53,7 +53,7 @@ namespace OpenSim.Region.Physics.OdePlugin
53 public class ODEDynamics 53 public class ODEDynamics
54 { 54 {
55 public Vehicle Type 55 public Vehicle Type
56 { 56 {
57 get { return m_type; } 57 get { return m_type; }
58 } 58 }
59 59
@@ -62,14 +62,14 @@ namespace OpenSim.Region.Physics.OdePlugin
62 get { return m_body; } 62 get { return m_body; }
63 } 63 }
64 64
65 private int frcount = 0; // Used to limit dynamics debug output to 65 private int frcount = 0; // Used to limit dynamics debug output to
66 // every 100th frame 66 // every 100th frame
67 67
68 // private OdeScene m_parentScene = null; 68 // private OdeScene m_parentScene = null;
69 private IntPtr m_body = IntPtr.Zero; 69 private IntPtr m_body = IntPtr.Zero;
70// private IntPtr m_jointGroup = IntPtr.Zero; 70// private IntPtr m_jointGroup = IntPtr.Zero;
71// private IntPtr m_aMotor = IntPtr.Zero; 71// private IntPtr m_aMotor = IntPtr.Zero;
72 72
73 73
74 // Vehicle properties 74 // Vehicle properties
75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind 75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
@@ -82,7 +82,7 @@ namespace OpenSim.Region.Physics.OdePlugin
82 // HOVER_UP_ONLY 82 // HOVER_UP_ONLY
83 // LIMIT_MOTOR_UP 83 // LIMIT_MOTOR_UP
84 // LIMIT_ROLL_ONLY 84 // LIMIT_ROLL_ONLY
85 85
86 // Linear properties 86 // Linear properties
87 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 87 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
88 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL 88 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
@@ -91,47 +91,43 @@ namespace OpenSim.Region.Physics.OdePlugin
91 private float m_linearMotorDecayTimescale = 0; 91 private float m_linearMotorDecayTimescale = 0;
92 private float m_linearMotorTimescale = 0; 92 private float m_linearMotorTimescale = 0;
93 private Vector3 m_lastLinearVelocityVector = Vector3.Zero; 93 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
94 // private bool m_LinearMotorSetLastFrame = false; 94 // private bool m_LinearMotorSetLastFrame = false;
95 // private Vector3 m_linearMotorOffset = Vector3.Zero; 95 // private Vector3 m_linearMotorOffset = Vector3.Zero;
96 96
97 //Angular properties 97 //Angular properties
98 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 98 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
99 private int m_angularMotorApply = 0; // application frame counter 99 private int m_angularMotorApply = 0; // application frame counter
100 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity 100 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
101 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 101 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
102 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 102 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
103 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 103 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
104 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 104 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
105 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body 105 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
106 106
107 //Deflection properties 107 //Deflection properties
108 // private float m_angularDeflectionEfficiency = 0; 108 // private float m_angularDeflectionEfficiency = 0;
109 // private float m_angularDeflectionTimescale = 0; 109 // private float m_angularDeflectionTimescale = 0;
110 // private float m_linearDeflectionEfficiency = 0; 110 // private float m_linearDeflectionEfficiency = 0;
111 // private float m_linearDeflectionTimescale = 0; 111 // private float m_linearDeflectionTimescale = 0;
112 112
113 //Banking properties 113 //Banking properties
114 // private float m_bankingEfficiency = 0; 114 // private float m_bankingEfficiency = 0;
115 // private float m_bankingMix = 0; 115 // private float m_bankingMix = 0;
116 // private float m_bankingTimescale = 0; 116 // private float m_bankingTimescale = 0;
117 117
118 //Hover and Buoyancy properties 118 //Hover and Buoyancy properties
119 private float m_VhoverHeight = 0f; 119 private float m_VhoverHeight = 0f;
120// private float m_VhoverEfficiency = 0f; 120// private float m_VhoverEfficiency = 0f;
121 private float m_VhoverTimescale = 0f; 121 private float m_VhoverTimescale = 0f;
122 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height 122 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
123 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. 123 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
124 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 124 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
125 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. 125 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
126 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 126 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
127 127
128 //Attractor properties 128 //Attractor properties
129 private float m_verticalAttractionEfficiency = 1.0f; // damped 129 private float m_verticalAttractionEfficiency = 1.0f; // damped
130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
131
132
133
134
135 131
136 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) 132 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
137 { 133 {
@@ -166,13 +162,13 @@ namespace OpenSim.Region.Physics.OdePlugin
166 // m_bankingTimescale = pValue; 162 // m_bankingTimescale = pValue;
167 break; 163 break;
168 case Vehicle.BUOYANCY: 164 case Vehicle.BUOYANCY:
169 if (pValue < -1f) pValue = -1f; 165 if (pValue < -1f) pValue = -1f;
170 if (pValue > 1f) pValue = 1f; 166 if (pValue > 1f) pValue = 1f;
171 m_VehicleBuoyancy = pValue; 167 m_VehicleBuoyancy = pValue;
172 break; 168 break;
173// case Vehicle.HOVER_EFFICIENCY: 169// case Vehicle.HOVER_EFFICIENCY:
174// if (pValue < 0f) pValue = 0f; 170// if (pValue < 0f) pValue = 0f;
175// if (pValue > 1f) pValue = 1f; 171// if (pValue > 1f) pValue = 1f;
176// m_VhoverEfficiency = pValue; 172// m_VhoverEfficiency = pValue;
177// break; 173// break;
178 case Vehicle.HOVER_HEIGHT: 174 case Vehicle.HOVER_HEIGHT:
@@ -199,7 +195,7 @@ namespace OpenSim.Region.Physics.OdePlugin
199 m_linearMotorTimescale = pValue; 195 m_linearMotorTimescale = pValue;
200 break; 196 break;
201 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 197 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
202 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable 198 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
203 if (pValue > 1.0f) pValue = 1.0f; 199 if (pValue > 1.0f) pValue = 1.0f;
204 m_verticalAttractionEfficiency = pValue; 200 m_verticalAttractionEfficiency = pValue;
205 break; 201 break;
@@ -207,8 +203,8 @@ namespace OpenSim.Region.Physics.OdePlugin
207 if (pValue < 0.01f) pValue = 0.01f; 203 if (pValue < 0.01f) pValue = 0.01f;
208 m_verticalAttractionTimescale = pValue; 204 m_verticalAttractionTimescale = pValue;
209 break; 205 break;
210 206
211 // These are vector properties but the engine lets you use a single float value to 207 // These are vector properties but the engine lets you use a single float value to
212 // set all of the components to the same value 208 // set all of the components to the same value
213 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 209 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
214 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 210 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -229,7 +225,6 @@ namespace OpenSim.Region.Physics.OdePlugin
229 break; 225 break;
230 226
231 } 227 }
232
233 }//end ProcessFloatVehicleParam 228 }//end ProcessFloatVehicleParam
234 229
235 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) 230 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -242,12 +237,12 @@ namespace OpenSim.Region.Physics.OdePlugin
242 case Vehicle.ANGULAR_MOTOR_DIRECTION: 237 case Vehicle.ANGULAR_MOTOR_DIRECTION:
243 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 238 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 // Limit requested angular speed to 2 rps= 4 pi rads/sec 239 // Limit requested angular speed to 2 rps= 4 pi rads/sec
245 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 240 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
246 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 241 if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
247 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 242 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
248 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 243 if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
249 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 244 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
250 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 245 if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
251 m_angularMotorApply = 10; 246 m_angularMotorApply = 10;
252 break; 247 break;
253 case Vehicle.LINEAR_FRICTION_TIMESCALE: 248 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -261,7 +256,6 @@ namespace OpenSim.Region.Physics.OdePlugin
261 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 256 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
262 break; 257 break;
263 } 258 }
264
265 }//end ProcessVectorVehicleParam 259 }//end ProcessVectorVehicleParam
266 260
267 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 261 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -272,12 +266,11 @@ namespace OpenSim.Region.Physics.OdePlugin
272 // m_referenceFrame = pValue; 266 // m_referenceFrame = pValue;
273 break; 267 break;
274 } 268 }
275
276 }//end ProcessRotationVehicleParam 269 }//end ProcessRotationVehicleParam
277 270
278 internal void ProcessTypeChange(Vehicle pType) 271 internal void ProcessTypeChange(Vehicle pType)
279 { 272 {
280 // Set Defaults For Type 273 // Set Defaults For Type
281 m_type = pType; 274 m_type = pType;
282 switch (pType) 275 switch (pType)
283 { 276 {
@@ -357,8 +350,8 @@ namespace OpenSim.Region.Physics.OdePlugin
357 // m_bankingMix = 0.8f; 350 // m_bankingMix = 0.8f;
358 // m_bankingTimescale = 1; 351 // m_bankingTimescale = 1;
359 // m_referenceFrame = Quaternion.Identity; 352 // m_referenceFrame = Quaternion.Identity;
360 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 353 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
361 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 354 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
362 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | 355 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
363 VehicleFlag.LIMIT_MOTOR_UP); 356 VehicleFlag.LIMIT_MOTOR_UP);
364 break; 357 break;
@@ -432,24 +425,25 @@ namespace OpenSim.Region.Physics.OdePlugin
432 { 425 {
433 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) 426 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
434 return; 427 return;
435 frcount++; // used to limit debug comment output 428 frcount++; // used to limit debug comment output
436 if (frcount > 100) 429 if (frcount > 100)
437 frcount = 0; 430 frcount = 0;
438 431
439 MoveLinear(pTimestep, pParentScene); 432 MoveLinear(pTimestep, pParentScene);
440 MoveAngular(pTimestep); 433 MoveAngular(pTimestep);
441 }// end Step 434 }// end Step
442 435
443 private void MoveLinear(float pTimestep, OdeScene _pParentScene) 436 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
444 { 437 {
445 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant 438 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
446 { 439 {
447 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 440 if (!d.BodyIsEnabled(Body))
441 d.BodyEnable(Body);
448 442
449 // add drive to body 443 // add drive to body
450 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); 444 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
451 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? 445 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
452 446
453 // This will work temporarily, but we really need to compare speed on an axis 447 // This will work temporarily, but we really need to compare speed on an axis
454 // KF: Limit body velocity to applied velocity? 448 // KF: Limit body velocity to applied velocity?
455 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) 449 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
@@ -458,7 +452,7 @@ namespace OpenSim.Region.Physics.OdePlugin
458 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; 452 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
459 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) 453 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
460 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; 454 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
461 455
462 // decay applied velocity 456 // decay applied velocity
463 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); 457 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
464 //Console.WriteLine("decay: " + decayfraction); 458 //Console.WriteLine("decay: " + decayfraction);
@@ -466,194 +460,192 @@ namespace OpenSim.Region.Physics.OdePlugin
466 //Console.WriteLine("actual: " + m_linearMotorDirection); 460 //Console.WriteLine("actual: " + m_linearMotorDirection);
467 } 461 }
468 else 462 else
469 { // requested is not significant 463 { // requested is not significant
470 // if what remains of applied is small, zero it. 464 // if what remains of applied is small, zero it.
471 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) 465 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
472 m_lastLinearVelocityVector = Vector3.Zero; 466 m_lastLinearVelocityVector = Vector3.Zero;
473 } 467 }
474 468
475 469 // convert requested object velocity to world-referenced vector
476 // convert requested object velocity to world-referenced vector
477 m_dir = m_lastLinearVelocityVector; 470 m_dir = m_lastLinearVelocityVector;
478 d.Quaternion rot = d.BodyGetQuaternion(Body); 471 d.Quaternion rot = d.BodyGetQuaternion(Body);
479 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 472 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
480 m_dir *= rotq; // apply obj rotation to velocity vector 473 m_dir *= rotq; // apply obj rotation to velocity vector
481 474
482 // add Gravity andBuoyancy 475 // add Gravity andBuoyancy
483 // KF: So far I have found no good method to combine a script-requested 476 // KF: So far I have found no good method to combine a script-requested
484 // .Z velocity and gravity. Therefore only 0g will used script-requested 477 // .Z velocity and gravity. Therefore only 0g will used script-requested
485 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. 478 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
486 Vector3 grav = Vector3.Zero; 479 Vector3 grav = Vector3.Zero;
487 if(m_VehicleBuoyancy < 1.0f) 480 if (m_VehicleBuoyancy < 1.0f)
488 { 481 {
489 // There is some gravity, make a gravity force vector 482 // There is some gravity, make a gravity force vector
490 // that is applied after object velocity. 483 // that is applied after object velocity.
491 d.Mass objMass; 484 d.Mass objMass;
492 d.BodyGetMass(Body, out objMass); 485 d.BodyGetMass(Body, out objMass);
493 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 486 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
494 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); 487 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
495 // Preserve the current Z velocity 488 // Preserve the current Z velocity
496 d.Vector3 vel_now = d.BodyGetLinearVel(Body); 489 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
497 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity 490 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
498 } // else its 1.0, no gravity. 491 } // else its 1.0, no gravity.
499 492
500 // Check if hovering 493 // Check if hovering
501 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 494 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
502 { 495 {
503 // We should hover, get the target height 496 // We should hover, get the target height
504 d.Vector3 pos = d.BodyGetPosition(Body); 497 d.Vector3 pos = d.BodyGetPosition(Body);
505 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) 498 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
506 { 499 {
507 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; 500 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
508 } 501 }
509 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) 502 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
510 { 503 {
511 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; 504 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
512 } 505 }
513 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) 506 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
514 { 507 {
515 m_VhoverTargetHeight = m_VhoverHeight; 508 m_VhoverTargetHeight = m_VhoverHeight;
516 } 509 }
517 510
518 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) 511 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
519 { 512 {
520 // If body is aready heigher, use its height as target height 513 // If body is aready heigher, use its height as target height
521 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; 514 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
522 } 515 }
523 516
524// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped 517// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
525// m_VhoverTimescale = 0f; // time to acheive height 518// m_VhoverTimescale = 0f; // time to acheive height
526// pTimestep is time since last frame,in secs 519// pTimestep is time since last frame,in secs
527 float herr0 = pos.Z - m_VhoverTargetHeight; 520 float herr0 = pos.Z - m_VhoverTargetHeight;
528 // Replace Vertical speed with correction figure if significant 521 // Replace Vertical speed with correction figure if significant
529 if(Math.Abs(herr0) > 0.01f ) 522 if (Math.Abs(herr0) > 0.01f)
530 { 523 {
531 d.Mass objMass; 524 d.Mass objMass;
532 d.BodyGetMass(Body, out objMass); 525 d.BodyGetMass(Body, out objMass);
533 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); 526 m_dir.Z = - ((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
534 //KF: m_VhoverEfficiency is not yet implemented 527 //KF: m_VhoverEfficiency is not yet implemented
535 } 528 }
536 else 529 else
537 { 530 {
538 m_dir.Z = 0f; 531 m_dir.Z = 0f;
539 } 532 }
540 } 533 }
541 534
542 // Apply velocity 535 // Apply velocity
543 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); 536 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
544 // apply gravity force 537 // apply gravity force
545 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 538 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
546 539
547 540
548 // apply friction 541 // apply friction
549 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); 542 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
550 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; 543 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
551 } // end MoveLinear() 544 } // end MoveLinear()
552 545
553 private void MoveAngular(float pTimestep) 546 private void MoveAngular(float pTimestep)
554 { 547 {
555 /* 548 /*
556 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 549 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
557 private int m_angularMotorApply = 0; // application frame counter 550 private int m_angularMotorApply = 0; // application frame counter
558 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) 551 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
559 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 552 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
560 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 553 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
561 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 554 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
562 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 555 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
563 */ 556 */
564 557
565 // Get what the body is doing, this includes 'external' influences 558 // Get what the body is doing, this includes 'external' influences
566 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); 559 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
567 // Vector3 angularVelocity = Vector3.Zero; 560 // Vector3 angularVelocity = Vector3.Zero;
568 561
569 if (m_angularMotorApply > 0) 562 if (m_angularMotorApply > 0)
570 { 563 {
571 // ramp up to new value 564 // ramp up to new value
572 // current velocity += error / ( time to get there / step interval ) 565 // current velocity += error / (time to get there / step interval)
573 // requested speed - last motor speed 566 // requested speed - last motor speed
574 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); 567 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
575 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); 568 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
576 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); 569 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
577 570
578 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected 571 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
579 // velocity may still be acheived. 572 // velocity may still be acheived.
580 } 573 }
581 else 574 else
582 { 575 {
583 // no motor recently applied, keep the body velocity 576 // no motor recently applied, keep the body velocity
584 /* m_angularMotorVelocity.X = angularVelocity.X; 577 /* m_angularMotorVelocity.X = angularVelocity.X;
585 m_angularMotorVelocity.Y = angularVelocity.Y; 578 m_angularMotorVelocity.Y = angularVelocity.Y;
586 m_angularMotorVelocity.Z = angularVelocity.Z; */ 579 m_angularMotorVelocity.Z = angularVelocity.Z; */
587 580
588 // and decay the velocity 581 // and decay the velocity
589 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); 582 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
590 } // end motor section 583 } // end motor section
591
592 584
593 // Vertical attractor section 585 // Vertical attractor section
594 Vector3 vertattr = Vector3.Zero; 586 Vector3 vertattr = Vector3.Zero;
595 587
596 if(m_verticalAttractionTimescale < 300) 588 if (m_verticalAttractionTimescale < 300)
597 {
598 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
599 // get present body rotation
600 d.Quaternion rot = d.BodyGetQuaternion(Body);
601 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
602 // make a vector pointing up
603 Vector3 verterr = Vector3.Zero;
604 verterr.Z = 1.0f;
605 // rotate it to Body Angle
606 verterr = verterr * rotq;
607 // 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.
608 // 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
609 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
610 if (verterr.Z < 0.0f)
611 {
612 verterr.X = 2.0f - verterr.X;
613 verterr.Y = 2.0f - verterr.Y;
614 }
615 // Error is 0 (no error) to +/- 2 (max error)
616 // scale it by VAservo
617 verterr = verterr * VAservo;
618//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
619
620 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
621 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
622 vertattr.X = verterr.Y;
623 vertattr.Y = - verterr.X;
624 vertattr.Z = 0f;
625
626 // scaling appears better usingsquare-law
627 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
628 vertattr.X += bounce * angularVelocity.X;
629 vertattr.Y += bounce * angularVelocity.Y;
630
631 } // else vertical attractor is off
632
633 // m_lastVertAttractor = vertattr;
634
635 // Bank section tba
636 // Deflection section tba
637
638 // Sum velocities
639 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
640
641 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
642 { 589 {
643 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 590 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
644 } 591 // get present body rotation
645 else 592 d.Quaternion rot = d.BodyGetQuaternion(Body);
646 { 593 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
647 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. 594 // make a vector pointing up
648 } 595 Vector3 verterr = Vector3.Zero;
649 596 verterr.Z = 1.0f;
650 // apply friction 597 // rotate it to Body Angle
598 verterr = verterr * rotq;
599 // 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.
600 // 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
601 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
602 if (verterr.Z < 0.0f)
603 {
604 verterr.X = 2.0f - verterr.X;
605 verterr.Y = 2.0f - verterr.Y;
606 }
607 // Error is 0 (no error) to +/- 2 (max error)
608 // scale it by VAservo
609 verterr = verterr * VAservo;
610//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
611
612 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
613 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
614 vertattr.X = verterr.Y;
615 vertattr.Y = - verterr.X;
616 vertattr.Z = 0f;
617
618 // scaling appears better usingsquare-law
619 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
620 vertattr.X += bounce * angularVelocity.X;
621 vertattr.Y += bounce * angularVelocity.Y;
622
623 } // else vertical attractor is off
624
625 // m_lastVertAttractor = vertattr;
626
627 // Bank section tba
628 // Deflection section tba
629
630 // Sum velocities
631 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
632
633 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
634 {
635 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
636 }
637 else
638 {
639 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
640 }
641
642 // apply friction
651 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); 643 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
652 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; 644 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
653 645
654 // Apply to the body 646 // Apply to the body
655 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 647 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
656 648
657 } //end MoveAngular 649 } //end MoveAngular
658 } 650 }
659} 651}
generated by cgit v1.1 at 2024-11-11 23:20:13 +0000