aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs579
1 files changed, 318 insertions, 261 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..74eb9ab 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -80,10 +80,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
80 private Quaternion m_referenceFrame = Quaternion.Identity; 80 private Quaternion m_referenceFrame = Quaternion.Identity;
81 81
82 // Linear properties 82 // Linear properties
83 private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
83 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 84 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
84 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center 85 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
85 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL 86 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
86 private Vector3 m_newVelocity = Vector3.Zero; // velocity computed to be applied to body
87 private Vector3 m_linearFrictionTimescale = Vector3.Zero; 87 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
88 private float m_linearMotorDecayTimescale = 0; 88 private float m_linearMotorDecayTimescale = 0;
89 private float m_linearMotorTimescale = 0; 89 private float m_linearMotorTimescale = 0;
@@ -93,6 +93,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
93 // private Vector3 m_linearMotorOffset = Vector3.Zero; 93 // private Vector3 m_linearMotorOffset = Vector3.Zero;
94 94
95 //Angular properties 95 //Angular properties
96 private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
96 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 97 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
97 // private int m_angularMotorApply = 0; // application frame counter 98 // private int m_angularMotorApply = 0; // application frame counter
98 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity 99 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
@@ -124,6 +125,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
124 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 125 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
125 126
126 //Attractor properties 127 //Attractor properties
128 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
127 private float m_verticalAttractionEfficiency = 1.0f; // damped 129 private float m_verticalAttractionEfficiency = 1.0f; // damped
128 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
129 131
@@ -152,10 +154,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
152 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); 154 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
153 break; 155 break;
154 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: 156 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
155 m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f); 157 m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
158 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
156 break; 159 break;
157 case Vehicle.ANGULAR_MOTOR_TIMESCALE: 160 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
158 m_angularMotorTimescale = Math.Max(pValue, 0.01f); 161 m_angularMotorTimescale = Math.Max(pValue, 0.01f);
162 m_angularMotor.TimeScale = m_angularMotorTimescale;
159 break; 163 break;
160 case Vehicle.BANKING_EFFICIENCY: 164 case Vehicle.BANKING_EFFICIENCY:
161 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); 165 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f));
@@ -185,33 +189,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
185 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); 189 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
186 break; 190 break;
187 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: 191 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
188 m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f); 192 m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
193 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
189 break; 194 break;
190 case Vehicle.LINEAR_MOTOR_TIMESCALE: 195 case Vehicle.LINEAR_MOTOR_TIMESCALE:
191 m_linearMotorTimescale = Math.Max(pValue, 0.01f); 196 m_linearMotorTimescale = Math.Max(pValue, 0.01f);
197 m_linearMotor.TimeScale = m_linearMotorTimescale;
192 break; 198 break;
193 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 199 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
194 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); 200 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
201 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
195 break; 202 break;
196 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: 203 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
197 m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); 204 m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
205 m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
198 break; 206 break;
199 207
200 // These are vector properties but the engine lets you use a single float value to 208 // These are vector properties but the engine lets you use a single float value to
201 // set all of the components to the same value 209 // set all of the components to the same value
202 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 210 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
203 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 211 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
212 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
204 break; 213 break;
205 case Vehicle.ANGULAR_MOTOR_DIRECTION: 214 case Vehicle.ANGULAR_MOTOR_DIRECTION:
206 m_angularMotorDirection = new Vector3(pValue, pValue, pValue); 215 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
207 // m_angularMotorApply = 100; 216 m_angularMotor.SetTarget(m_angularMotorDirection);
208 break; 217 break;
209 case Vehicle.LINEAR_FRICTION_TIMESCALE: 218 case Vehicle.LINEAR_FRICTION_TIMESCALE:
210 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 219 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
220 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
211 break; 221 break;
212 case Vehicle.LINEAR_MOTOR_DIRECTION: 222 case Vehicle.LINEAR_MOTOR_DIRECTION:
213 m_linearMotorDirection = new Vector3(pValue, pValue, pValue); 223 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
214 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); 224 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
225 m_linearMotor.SetTarget(m_linearMotorDirection);
215 break; 226 break;
216 case Vehicle.LINEAR_MOTOR_OFFSET: 227 case Vehicle.LINEAR_MOTOR_OFFSET:
217 m_linearMotorOffset = new Vector3(pValue, pValue, pValue); 228 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -227,6 +238,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
227 { 238 {
228 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 239 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
229 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 240 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
241 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
230 break; 242 break;
231 case Vehicle.ANGULAR_MOTOR_DIRECTION: 243 case Vehicle.ANGULAR_MOTOR_DIRECTION:
232 // Limit requested angular speed to 2 rps= 4 pi rads/sec 244 // Limit requested angular speed to 2 rps= 4 pi rads/sec
@@ -234,14 +246,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
234 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); 246 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
235 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); 247 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
236 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 248 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
237 // m_angularMotorApply = 100; 249 m_angularMotor.SetTarget(m_angularMotorDirection);
238 break; 250 break;
239 case Vehicle.LINEAR_FRICTION_TIMESCALE: 251 case Vehicle.LINEAR_FRICTION_TIMESCALE:
240 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 252 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
253 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
241 break; 254 break;
242 case Vehicle.LINEAR_MOTOR_DIRECTION: 255 case Vehicle.LINEAR_MOTOR_DIRECTION:
243 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 256 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); 257 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
258 m_linearMotor.SetTarget(m_linearMotorDirection);
245 break; 259 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET: 260 case Vehicle.LINEAR_MOTOR_OFFSET:
247 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 261 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -303,7 +317,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
303 m_VhoverEfficiency = 0; 317 m_VhoverEfficiency = 0;
304 m_VhoverTimescale = 0; 318 m_VhoverTimescale = 0;
305 m_VehicleBuoyancy = 0; 319 m_VehicleBuoyancy = 0;
306 320
307 m_linearDeflectionEfficiency = 1; 321 m_linearDeflectionEfficiency = 1;
308 m_linearDeflectionTimescale = 1; 322 m_linearDeflectionTimescale = 1;
309 323
@@ -319,6 +333,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
319 333
320 m_referenceFrame = Quaternion.Identity; 334 m_referenceFrame = Quaternion.Identity;
321 m_flags = (VehicleFlag)0; 335 m_flags = (VehicleFlag)0;
336
322 break; 337 break;
323 338
324 case Vehicle.TYPE_SLED: 339 case Vehicle.TYPE_SLED:
@@ -351,10 +366,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
351 m_bankingMix = 1; 366 m_bankingMix = 1;
352 367
353 m_referenceFrame = Quaternion.Identity; 368 m_referenceFrame = Quaternion.Identity;
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); 369 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
355 m_flags &= 370 | VehicleFlag.HOVER_TERRAIN_ONLY
356 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | 371 | VehicleFlag.HOVER_GLOBAL_HEIGHT
357 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 372 | VehicleFlag.HOVER_UP_ONLY);
373 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
374 | VehicleFlag.LIMIT_ROLL_ONLY
375 | VehicleFlag.LIMIT_MOTOR_UP);
358 break; 376 break;
359 case Vehicle.TYPE_CAR: 377 case Vehicle.TYPE_CAR:
360 m_linearMotorDirection = Vector3.Zero; 378 m_linearMotorDirection = Vector3.Zero;
@@ -510,6 +528,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
510 | VehicleFlag.HOVER_GLOBAL_HEIGHT); 528 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
511 break; 529 break;
512 } 530 }
531
532 // Update any physical parameters based on this type.
533 Refresh();
534
535 m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
536 m_linearMotorDecayTimescale, m_linearFrictionTimescale,
537 1f);
538 m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
539
540 m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
541 m_angularMotorDecayTimescale, m_angularFrictionTimescale,
542 1f);
543 m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
544
545 m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
546 BSMotor.Infinite, BSMotor.InfiniteVector,
547 m_verticalAttractionEfficiency);
548 // Z goes away and we keep X and Y
549 m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
550 m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
551
552 // m_bankingMotor = new BSVMotor("BankingMotor", ...);
513 } 553 }
514 554
515 // Some of the properties of this prim may have changed. 555 // Some of the properties of this prim may have changed.
@@ -518,13 +558,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
518 { 558 {
519 if (IsActive) 559 if (IsActive)
520 { 560 {
521 // Friction effects are handled by this vehicle code 561 m_vehicleMass = Prim.Linkset.LinksetMass;
522 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
523 BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
524
525 // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
526 562
527 VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID); 563 // Friction effects are handled by this vehicle code
564 float friction = 0f;
565 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
566
567 // Moderate angular movement introduced by Bullet.
568 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
569 // Maybe compute linear and angular factor and damping from params.
570 float angularDamping = PhysicsScene.Params.vehicleAngularDamping;
571 BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
572
573 // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet
574 // Vector3 localInertia = new Vector3(1f, 1f, 1f);
575 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
576 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
577
578 VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
579 Prim.LocalID, friction, localInertia, angularDamping);
528 } 580 }
529 } 581 }
530 582
@@ -551,111 +603,109 @@ namespace OpenSim.Region.Physics.BulletSPlugin
551 { 603 {
552 if (!IsActive) return; 604 if (!IsActive) return;
553 605
554 // DEBUG
555 // Because Bullet does apply forces to the vehicle, our last computed
556 // linear and angular velocities are not what is happening now.
557 // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
558 // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
559 // m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time
560 // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG:
561 // END DEBUG
562
563 m_vehicleMass = Prim.Linkset.LinksetMass;
564
565 MoveLinear(pTimestep); 606 MoveLinear(pTimestep);
566 // Commented out for debug
567 MoveAngular(pTimestep); 607 MoveAngular(pTimestep);
568 // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false); // DEBUG DEBUG
569 // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity; // DEBUG DEBUG
570 608
571 LimitRotation(pTimestep); 609 LimitRotation(pTimestep);
572 610
573 // remember the position so next step we can limit absolute movement effects 611 // remember the position so next step we can limit absolute movement effects
574 m_lastPositionVector = Prim.ForcePosition; 612 m_lastPositionVector = Prim.ForcePosition;
575 613
576 VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}", // DEBUG DEBUG
577 Prim.LocalID,
578 BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
579 BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
580 Prim.Inertia,
581 m_vehicleMass
582 );
583 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 614 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
584 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); 615 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
585 }// end Step 616 }
586 617
587 // Apply the effect of the linear motor. 618 // Apply the effect of the linear motor.
588 // Also does hover and float. 619 // Also does hover and float.
589 private void MoveLinear(float pTimestep) 620 private void MoveLinear(float pTimestep)
590 { 621 {
591 // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates 622 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
592 // m_lastLinearVelocityVector is the current speed we are moving in that direction
593 if (m_linearMotorDirection.LengthSquared() > 0.001f)
594 {
595 Vector3 origDir = m_linearMotorDirection; // DEBUG
596 Vector3 origVel = m_lastLinearVelocityVector; // DEBUG
597 // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
598 Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG
599 623
600 // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete 624 // Rotate new object velocity from vehicle relative to world coordinates
601 Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep; 625 linearMotorContribution *= Prim.ForceOrientation;
602 m_lastLinearVelocityVector += addAmount;
603 626
604 float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep; 627 // ==================================================================
605 m_linearMotorDirection *= (1f - decayFactor); 628 // Gravity and Buoyancy
629 // There is some gravity, make a gravity force vector that is applied after object velocity.
630 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
631 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
606 632
607 // Rotate new object velocity from vehicle relative to world coordinates 633 Vector3 pos = Prim.ForcePosition;
608 m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation; 634 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
609 635
610 // Apply friction for next time 636 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
611 Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
612 m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
613 637
614 VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}", 638 Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
615 Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 639
616 m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity); 640 ComputeLinearBlockingEndPoint(pTimestep, ref pos);
617 } 641
618 else 642 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
619 { 643
620 // if what remains of direction is very small, zero it. 644 // ==================================================================
621 m_linearMotorDirection = Vector3.Zero; 645 Vector3 newVelocity = linearMotorContribution
622 m_lastLinearVelocityVector = Vector3.Zero; 646 + terrainHeightContribution
623 m_newVelocity = Vector3.Zero; 647 + hoverContribution
648 + limitMotorUpContribution;
649
650 // If not changing some axis, reduce out velocity
651 if ((m_flags & (VehicleFlag.NO_X)) != 0)
652 newVelocity.X = 0;
653 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
654 newVelocity.Y = 0;
655 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
656 newVelocity.Z = 0;
624 657
625 VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); 658 // ==================================================================
659 // Clamp REALLY high or low velocities
660 float newVelocityLengthSq = newVelocity.LengthSquared();
661 if (newVelocityLengthSq > 1e6f)
662 {
663 newVelocity /= newVelocity.Length();
664 newVelocity *= 1000f;
626 } 665 }
666 else if (newVelocityLengthSq < 1e-6f)
667 newVelocity = Vector3.Zero;
627 668
628 // m_newVelocity is velocity computed from linear motor in world coordinates 669 // ==================================================================
670 // Stuff new linear velocity into the vehicle
671 Prim.ForceVelocity = newVelocity;
672 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG
629 673
630 // Gravity and Buoyancy 674 // Other linear forces are applied as forces.
631 // There is some gravity, make a gravity force vector that is applied after object velocity. 675 Vector3 totalDownForce = grav * m_vehicleMass;
632 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 676 if (totalDownForce != Vector3.Zero)
633 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); 677 {
678 Prim.AddForce(totalDownForce, false);
679 }
634 680
635 /* 681 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
636 * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ... 682 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
637 // Preserve the current Z velocity 683 newVelocity, Prim.Velocity, totalDownForce);
638 Vector3 vel_now = m_prim.Velocity;
639 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
640 */
641 684
642 Vector3 pos = Prim.ForcePosition; 685 } // end MoveLinear()
643// 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);
644 686
687 public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight)
688 {
689 Vector3 ret = Vector3.Zero;
645 // If below the terrain, move us above the ground a little. 690 // If below the terrain, move us above the ground a little.
646 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
647 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. 691 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
648 // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. 692 // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
649 // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; 693 // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
650 // if (rotatedSize.Z < terrainHeight) 694 // if (rotatedSize.Z < terrainHeight)
651 if (pos.Z < terrainHeight) 695 if (pos.Z < terrainHeight)
652 { 696 {
697 // TODO: correct position by applying force rather than forcing position.
653 pos.Z = terrainHeight + 2; 698 pos.Z = terrainHeight + 2;
654 Prim.ForcePosition = pos; 699 Prim.ForcePosition = pos;
655 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); 700 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
656 } 701 }
702 return ret;
703 }
704
705 public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight)
706 {
707 Vector3 ret = Vector3.Zero;
657 708
658 // Check if hovering
659 // m_VhoverEfficiency: 0=bouncy, 1=totally damped 709 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
660 // m_VhoverTimescale: time to achieve height 710 // m_VhoverTimescale: time to achieve height
661 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 711 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
@@ -694,28 +744,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin
694 // RA: where does the 50 come from? 744 // RA: where does the 50 come from?
695 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); 745 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
696 // Replace Vertical speed with correction figure if significant 746 // Replace Vertical speed with correction figure if significant
697 if (Math.Abs(verticalError) > 0.01f) 747 if (verticalError > 0.01f)
698 { 748 {
699 m_newVelocity.Z += verticalCorrectionVelocity; 749 ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
700 //KF: m_VhoverEfficiency is not yet implemented 750 //KF: m_VhoverEfficiency is not yet implemented
701 } 751 }
702 else if (verticalError < -0.01) 752 else if (verticalError < -0.01)
703 { 753 {
704 m_newVelocity.Z -= verticalCorrectionVelocity; 754 ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
705 }
706 else
707 {
708 m_newVelocity.Z = 0f;
709 } 755 }
710 } 756 }
711 757
712 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight); 758 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}",
759 Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight);
713 } 760 }
714 761
762 return ret;
763 }
764
765 public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos)
766 {
767 bool changed = false;
768
715 Vector3 posChange = pos - m_lastPositionVector; 769 Vector3 posChange = pos - m_lastPositionVector;
716 if (m_BlockingEndPoint != Vector3.Zero) 770 if (m_BlockingEndPoint != Vector3.Zero)
717 { 771 {
718 bool changed = false;
719 if (pos.X >= (m_BlockingEndPoint.X - (float)1)) 772 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
720 { 773 {
721 pos.X -= posChange.X + 1; 774 pos.X -= posChange.X + 1;
@@ -748,75 +801,45 @@ namespace OpenSim.Region.Physics.BulletSPlugin
748 Prim.LocalID, m_BlockingEndPoint, posChange, pos); 801 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
749 } 802 }
750 } 803 }
804 return changed;
805 }
751 806
752 #region downForce 807 public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
753 Vector3 downForce = Vector3.Zero; 808 {
754 809 Vector3 ret = Vector3.Zero;
755 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 810 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
756 { 811 {
757 // If the vehicle is motoring into the sky, get it going back down. 812 // If the vehicle is motoring into the sky, get it going back down.
758 // Is this an angular force or both linear and angular??
759 float distanceAboveGround = pos.Z - terrainHeight; 813 float distanceAboveGround = pos.Z - terrainHeight;
760 if (distanceAboveGround > 2f) 814 if (distanceAboveGround > 1f)
761 { 815 {
762 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); 816 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
763 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); 817 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
764 downForce = new Vector3(0, 0, -distanceAboveGround); 818 ret = new Vector3(0, 0, -distanceAboveGround);
765 } 819 }
766 // TODO: this calculation is all wrong. From the description at 820 // TODO: this calculation is all wrong. From the description at
767 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce 821 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
768 // has a decay factor. This says this force should 822 // has a decay factor. This says this force should
769 // be computed with a motor. 823 // be computed with a motor.
770 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 824 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
771 Prim.LocalID, distanceAboveGround, downForce); 825 Prim.LocalID, distanceAboveGround, ret);
772 } 826 }
773 #endregion // downForce 827 return ret;
774 828 }
775 // If not changing some axis, reduce out velocity
776 if ((m_flags & (VehicleFlag.NO_X)) != 0)
777 m_newVelocity.X = 0;
778 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
779 m_newVelocity.Y = 0;
780 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
781 m_newVelocity.Z = 0;
782
783 // Clamp REALLY high or low velocities
784 if (m_newVelocity.LengthSquared() > 1e6f)
785 {
786 m_newVelocity /= m_newVelocity.Length();
787 m_newVelocity *= 1000f;
788 }
789 else if (m_newVelocity.LengthSquared() < 1e-6f)
790 m_newVelocity = Vector3.Zero;
791
792 // Stuff new linear velocity into the vehicle
793 Prim.ForceVelocity = m_newVelocity;
794 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG
795
796 Vector3 totalDownForce = downForce + grav;
797 if (totalDownForce != Vector3.Zero)
798 {
799 Prim.AddForce(totalDownForce * m_vehicleMass, false);
800 // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
801 }
802
803 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
804 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
805
806 } // end MoveLinear()
807 829
808 // ======================================================================= 830 // =======================================================================
831 // =======================================================================
809 // Apply the effect of the angular motor. 832 // Apply the effect of the angular motor.
810 private void MoveAngular(float pTimestep) 833 private void MoveAngular(float pTimestep)
811 { 834 {
812 // m_angularMotorDirection // angular velocity requested by LSL motor 835 // m_angularMotorDirection // angular velocity requested by LSL motor
813 // m_angularMotorApply // application frame counter
814 // m_angularMotorVelocity // current angular motor velocity (ramps up and down) 836 // m_angularMotorVelocity // current angular motor velocity (ramps up and down)
815 // m_angularMotorTimescale // motor angular velocity ramp up rate 837 // m_angularMotorTimescale // motor angular velocity ramp up time
816 // m_angularMotorDecayTimescale // motor angular velocity decay rate 838 // m_angularMotorDecayTimescale // motor angular velocity decay rate
817 // m_angularFrictionTimescale // body angular velocity decay rate 839 // m_angularFrictionTimescale // body angular velocity decay rate
818 // m_lastAngularVelocity // what was last applied to body 840 // m_lastAngularVelocity // what was last applied to body
819 841
842 /*
820 if (m_angularMotorDirection.LengthSquared() > 0.0001) 843 if (m_angularMotorDirection.LengthSquared() > 0.0001)
821 { 844 {
822 Vector3 origVel = m_angularMotorVelocity; 845 Vector3 origVel = m_angularMotorVelocity;
@@ -835,59 +858,152 @@ namespace OpenSim.Region.Physics.BulletSPlugin
835 { 858 {
836 m_angularMotorVelocity = Vector3.Zero; 859 m_angularMotorVelocity = Vector3.Zero;
837 } 860 }
861 */
862
863 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
838 864
839 #region Vertical attactor 865 // ==================================================================
866 // NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
867 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
868 {
869 angularMotorContribution.X = 0f;
870 angularMotorContribution.Y = 0f;
871 VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
872 }
840 873
841 Vector3 vertattr = Vector3.Zero; 874 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
842 Vector3 deflection = Vector3.Zero;
843 Vector3 banking = Vector3.Zero;
844 875
845 // If vertical attaction timescale is reasonable and we applied an angular force last time... 876 Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
846 if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) 877
878 Vector3 bankingContribution = ComputeAngularBanking(pTimestep);
879
880 // ==================================================================
881 m_lastVertAttractor = verticalAttractionContribution;
882
883 // Sum velocities
884 m_lastAngularVelocity = angularMotorContribution
885 + verticalAttractionContribution
886 + bankingContribution
887 + deflectionContribution;
888
889 // ==================================================================
890 //Offset section
891 if (m_linearMotorOffset != Vector3.Zero)
892 {
893 //Offset of linear velocity doesn't change the linear velocity,
894 // but causes a torque to be applied, for example...
895 //
896 // IIIII >>> IIIII
897 // IIIII >>> IIIII
898 // IIIII >>> IIIII
899 // ^
900 // | Applying a force at the arrow will cause the object to move forward, but also rotate
901 //
902 //
903 // The torque created is the linear velocity crossed with the offset
904
905 // TODO: this computation should be in the linear section
906 // because that is where we know the impulse being applied.
907 Vector3 torqueFromOffset = Vector3.Zero;
908 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
909 if (float.IsNaN(torqueFromOffset.X))
910 torqueFromOffset.X = 0;
911 if (float.IsNaN(torqueFromOffset.Y))
912 torqueFromOffset.Y = 0;
913 if (float.IsNaN(torqueFromOffset.Z))
914 torqueFromOffset.Z = 0;
915 torqueFromOffset *= m_vehicleMass;
916 Prim.ApplyTorqueImpulse(torqueFromOffset, true);
917 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
918 }
919
920 // ==================================================================
921 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
922 {
923 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
924 Prim.ZeroAngularMotion(true);
925 VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
926 }
927 else
847 { 928 {
848 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; 929 // Apply to the body.
849 if (Prim.IsColliding) 930 // The above calculates the absolute angular velocity needed. Angular velocity is massless.
850 VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); 931 // Since we are stuffing the angular velocity directly into the object, the computed
932 // velocity needs to be scaled by the timestep.
933 // Also remove any motion that is on the object so added motion is only from vehicle.
934 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
935 - Prim.ForceRotationalVelocity);
936 // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
937 Prim.ForceRotationalVelocity = applyAngularForce;
938
939 VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
940 Prim.LocalID,
941 angularMotorContribution, verticalAttractionContribution,
942 bankingContribution, deflectionContribution,
943 applyAngularForce, m_lastAngularVelocity
944 );
945 }
946 }
851 947
852 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 948 public Vector3 ComputeAngularVerticalAttraction(float pTimestep)
949 {
950 Vector3 ret = Vector3.Zero;
853 951
854 // Create a vector of the vehicle "up" in world coordinates 952 // If vertical attaction timescale is reasonable and we applied an angular force last time...
953 if (m_verticalAttractionTimescale < 500)
954 {
955 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
956 verticalError.Normalize();
957 m_verticalAttractionMotor.SetCurrent(verticalError);
958 m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
959 ret = m_verticalAttractionMotor.Step(pTimestep);
960 /*
961 // Take a vector pointing up and convert it from world to vehicle relative coords.
855 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; 962 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
856 // verticalError.X and .Y are the World error amounts. They are 0 when there is no 963 verticalError.Normalize();
857 // error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its 964
858 // side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall 965 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
859 // and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be 966 // is now leaning to one side (rotated around the X axis) and the Y value will
860 // modulated to prevent a stable inverted body. 967 // go from zero (nearly straight up) to one (completely to the side) or leaning
861 968 // front-to-back (rotated around the Y axis) and the value of X will be between
862 // Error is 0 (no error) to +/- 2 (max error) 969 // zero and one.
863 if (verticalError.Z < 0.0f) 970 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
971
972 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
973 if (verticalError.Z < 0f)
864 { 974 {
865 verticalError.X = 2.0f - verticalError.X; 975 verticalError.X = 2f - verticalError.X;
866 verticalError.Y = 2.0f - verticalError.Y; 976 verticalError.Y = 2f - verticalError.Y;
867 } 977 }
868 // scale it by VAservo (timestep and timescale)
869 verticalError = verticalError * VAservo;
870 978
871 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y 979 // Y error means needed rotation around X axis and visa versa.
872 // then .X increases, so change Body angular velocity X based on Y, and Y based on X. 980 verticalAttractionContribution.X = verticalError.Y;
873 // Z is not changed. 981 verticalAttractionContribution.Y = - verticalError.X;
874 vertattr.X = verticalError.Y; 982 verticalAttractionContribution.Z = 0f;
875 vertattr.Y = - verticalError.X; 983
876 vertattr.Z = 0f; 984 // scale by the time scale and timestep
985 Vector3 unscaledContrib = verticalAttractionContribution;
986 verticalAttractionContribution /= m_verticalAttractionTimescale;
987 verticalAttractionContribution *= pTimestep;
877 988
878 // scaling appears better usingsquare-law 989 // apply efficiency
879 Vector3 angularVelocity = Prim.ForceRotationalVelocity; 990 Vector3 preEfficiencyContrib = verticalAttractionContribution;
880 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 991 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
881 vertattr.X += bounce * angularVelocity.X; 992 verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
882 vertattr.Y += bounce * angularVelocity.Y;
883 993
884 VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}", 994 VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
885 Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr); 995 Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
996 m_verticalAttractionEfficiency, efficencySquared,
997 verticalAttractionContribution);
998 */
886 999
887 } 1000 }
888 #endregion // Vertical attactor 1001 return ret;
1002 }
889 1003
890 #region Deflection 1004 public Vector3 ComputeAngularDeflection(float pTimestep)
1005 {
1006 Vector3 ret = Vector3.Zero;
891 1007
892 if (m_angularDeflectionEfficiency != 0) 1008 if (m_angularDeflectionEfficiency != 0)
893 { 1009 {
@@ -899,32 +1015,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
899 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame); 1015 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
900 1016
901 // Scale by efficiency and timescale 1017 // Scale by efficiency and timescale
902 deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; 1018 ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
1019
1020 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret);
903 1021
904 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}",
905 Prim.LocalID, preferredAxisOfMotion, deflection);
906 // This deflection computation is not correct. 1022 // This deflection computation is not correct.
907 deflection = Vector3.Zero; 1023 ret = Vector3.Zero;
908 } 1024 }
1025 return ret;
1026 }
909 1027
910 #endregion 1028 public Vector3 ComputeAngularBanking(float pTimestep)
911 1029 {
912 #region Banking 1030 Vector3 ret = Vector3.Zero;
913 1031
914 if (m_bankingEfficiency != 0) 1032 if (m_bankingEfficiency != 0)
915 { 1033 {
916 Vector3 dir = Vector3.One * Prim.ForceOrientation; 1034 Vector3 dir = Vector3.One * Prim.ForceOrientation;
917 float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1); 1035 float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
918 //Changes which way it banks in and out of turns 1036 //Changes which way it banks in and out of turns
919 1037
920 //Use the square of the efficiency, as it looks much more how SL banking works 1038 //Use the square of the efficiency, as it looks much more how SL banking works
921 float effSquared = (m_bankingEfficiency*m_bankingEfficiency); 1039 float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
922 if (m_bankingEfficiency < 0) 1040 if (m_bankingEfficiency < 0)
923 effSquared *= -1; //Keep the negative! 1041 effSquared *= -1; //Keep the negative!
924 1042
925 float mix = Math.Abs(m_bankingMix); 1043 float mix = Math.Abs(m_bankingMix);
926 if (m_angularMotorVelocity.X == 0) 1044 if (m_angularMotorVelocity.X == 0)
927 { 1045 {
1046 // The vehicle is stopped
928 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) 1047 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
929 { 1048 {
930 Vector3 axisAngle; 1049 Vector3 axisAngle;
@@ -938,101 +1057,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
938 }*/ 1057 }*/
939 } 1058 }
940 else 1059 else
941 banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4; 1060 {
1061 ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
1062 }
1063
1064 //If they are colliding, we probably shouldn't shove the prim around... probably
942 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) 1065 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
943 //If they are colliding, we probably shouldn't shove the prim around... probably
944 { 1066 {
945 float angVelZ = m_angularMotorVelocity.X*-1; 1067 float angVelZ = m_angularMotorVelocity.X * -1;
946 /*if(angVelZ > mix) 1068 /*if(angVelZ > mix)
947 angVelZ = mix; 1069 angVelZ = mix;
948 else if(angVelZ < -mix) 1070 else if(angVelZ < -mix)
949 angVelZ = -mix;*/ 1071 angVelZ = -mix;*/
950 //This controls how fast and how far the banking occurs 1072 //This controls how fast and how far the banking occurs
951 Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0); 1073 Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
952 if (bankingRot.X > 3) 1074 if (bankingRot.X > 3)
953 bankingRot.X = 3; 1075 bankingRot.X = 3;
954 else if (bankingRot.X < -3) 1076 else if (bankingRot.X < -3)
955 bankingRot.X = -3; 1077 bankingRot.X = -3;
956 bankingRot *= Prim.ForceOrientation; 1078 bankingRot *= Prim.ForceOrientation;
957 banking += bankingRot; 1079 ret += bankingRot;
958 } 1080 }
959 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; 1081 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
960 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 1082 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}",
961 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking); 1083 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, ret);
962 } 1084 }
1085 return ret;
1086 }
963 1087
964 #endregion
965
966 m_lastVertAttractor = vertattr;
967
968 // Sum velocities
969 m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
970
971 #region Linear Motor Offset
972
973 //Offset section
974 if (m_linearMotorOffset != Vector3.Zero)
975 {
976 //Offset of linear velocity doesn't change the linear velocity,
977 // but causes a torque to be applied, for example...
978 //
979 // IIIII >>> IIIII
980 // IIIII >>> IIIII
981 // IIIII >>> IIIII
982 // ^
983 // | Applying a force at the arrow will cause the object to move forward, but also rotate
984 //
985 //
986 // The torque created is the linear velocity crossed with the offset
987
988 // NOTE: this computation does should be in the linear section
989 // because there we know the impulse being applied.
990 Vector3 torqueFromOffset = Vector3.Zero;
991 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
992 if (float.IsNaN(torqueFromOffset.X))
993 torqueFromOffset.X = 0;
994 if (float.IsNaN(torqueFromOffset.Y))
995 torqueFromOffset.Y = 0;
996 if (float.IsNaN(torqueFromOffset.Z))
997 torqueFromOffset.Z = 0;
998 torqueFromOffset *= m_vehicleMass;
999 Prim.ApplyTorqueImpulse(torqueFromOffset, true);
1000 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
1001 }
1002
1003 #endregion
1004
1005 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1006 {
1007 m_lastAngularVelocity.X = 0;
1008 m_lastAngularVelocity.Y = 0;
1009 VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
1010 }
1011
1012 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1013 {
1014 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
1015 Prim.ZeroAngularMotion(true);
1016 VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
1017 }
1018 else
1019 {
1020 // Apply to the body.
1021 // The above calculates the absolute angular velocity needed. Angular velocity is massless.
1022 // Since we are stuffing the angular velocity directly into the object, the computed
1023 // velocity needs to be scaled by the timestep.
1024 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
1025 Prim.ForceRotationalVelocity = applyAngularForce;
1026
1027 // Decay the angular movement for next time
1028 Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
1029 m_lastAngularVelocity *= Vector3.One - decayamount;
1030
1031 VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}",
1032 Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
1033 }
1034 } //end MoveAngular
1035 1088
1089 // This is from previous instantiations of XXXDynamics.cs.
1090 // Applies roll reference frame.
1091 // TODO: is this the right way to separate the code to do this operation?
1092 // Should this be in MoveAngular()?
1036 internal void LimitRotation(float timestep) 1093 internal void LimitRotation(float timestep)
1037 { 1094 {
1038 Quaternion rotq = Prim.ForceOrientation; 1095 Quaternion rotq = Prim.ForceOrientation;