aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
diff options
context:
space:
mode:
authorBlueWall2012-12-03 20:17:00 -0500
committerBlueWall2012-12-03 20:17:00 -0500
commit0c7d6adef158d5cb25d8eb60287c5cb0cceaa953 (patch)
treef766959f3442b78aa9202a4f61cf127508245345 /OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
parentXmlRpcGridRouter (diff)
parentBulletSim: Reduce idle region physics overhead where there are MANY (diff)
downloadopensim-SC_OLD-0c7d6adef158d5cb25d8eb60287c5cb0cceaa953.zip
opensim-SC_OLD-0c7d6adef158d5cb25d8eb60287c5cb0cceaa953.tar.gz
opensim-SC_OLD-0c7d6adef158d5cb25d8eb60287c5cb0cceaa953.tar.bz2
opensim-SC_OLD-0c7d6adef158d5cb25d8eb60287c5cb0cceaa953.tar.xz
Merge branch 'master' of ssh://opensimulator.org/var/git/opensim
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs504
1 files changed, 297 insertions, 207 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 3a73fba..be8a502 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -45,9 +45,7 @@ using System;
45using System.Collections.Generic; 45using System.Collections.Generic;
46using System.Reflection; 46using System.Reflection;
47using System.Runtime.InteropServices; 47using System.Runtime.InteropServices;
48using log4net;
49using OpenMetaverse; 48using OpenMetaverse;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager; 49using OpenSim.Region.Physics.Manager;
52 50
53namespace OpenSim.Region.Physics.BulletSPlugin 51namespace OpenSim.Region.Physics.BulletSPlugin
@@ -100,7 +98,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
100 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 98 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
101 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 99 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
102 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 100 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
103 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 101 private Vector3 m_lastAngularCorrection = Vector3.Zero;
104 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body 102 private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
105 103
106 //Deflection properties 104 //Deflection properties
@@ -113,6 +111,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
113 private float m_bankingEfficiency = 0; 111 private float m_bankingEfficiency = 0;
114 private float m_bankingMix = 0; 112 private float m_bankingMix = 0;
115 private float m_bankingTimescale = 0; 113 private float m_bankingTimescale = 0;
114 private Vector3 m_lastBanking = Vector3.Zero;
116 115
117 //Hover and Buoyancy properties 116 //Hover and Buoyancy properties
118 private float m_VhoverHeight = 0f; 117 private float m_VhoverHeight = 0f;
@@ -127,7 +126,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
127 //Attractor properties 126 //Attractor properties
128 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction"); 127 private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
129 private float m_verticalAttractionEfficiency = 1.0f; // damped 128 private float m_verticalAttractionEfficiency = 1.0f; // damped
130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 129 private float m_verticalAttractionTimescale = 600f; // Timescale > 500 means no vert attractor.
131 130
132 public BSDynamics(BSScene myScene, BSPrim myPrim) 131 public BSDynamics(BSScene myScene, BSPrim myPrim)
133 { 132 {
@@ -154,7 +153,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
154 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); 153 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
155 break; 154 break;
156 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: 155 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
157 m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120)); 156 m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
158 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale; 157 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
159 break; 158 break;
160 case Vehicle.ANGULAR_MOTOR_TIMESCALE: 159 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
@@ -162,7 +161,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
162 m_angularMotor.TimeScale = m_angularMotorTimescale; 161 m_angularMotor.TimeScale = m_angularMotorTimescale;
163 break; 162 break;
164 case Vehicle.BANKING_EFFICIENCY: 163 case Vehicle.BANKING_EFFICIENCY:
165 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); 164 m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
166 break; 165 break;
167 case Vehicle.BANKING_MIX: 166 case Vehicle.BANKING_MIX:
168 m_bankingMix = Math.Max(pValue, 0.01f); 167 m_bankingMix = Math.Max(pValue, 0.01f);
@@ -171,10 +170,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
171 m_bankingTimescale = Math.Max(pValue, 0.01f); 170 m_bankingTimescale = Math.Max(pValue, 0.01f);
172 break; 171 break;
173 case Vehicle.BUOYANCY: 172 case Vehicle.BUOYANCY:
174 m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f)); 173 m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
175 break; 174 break;
176 case Vehicle.HOVER_EFFICIENCY: 175 case Vehicle.HOVER_EFFICIENCY:
177 m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f)); 176 m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
178 break; 177 break;
179 case Vehicle.HOVER_HEIGHT: 178 case Vehicle.HOVER_HEIGHT:
180 m_VhoverHeight = pValue; 179 m_VhoverHeight = pValue;
@@ -189,7 +188,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
189 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); 188 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
190 break; 189 break;
191 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: 190 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
192 m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120)); 191 m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
193 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale; 192 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
194 break; 193 break;
195 case Vehicle.LINEAR_MOTOR_TIMESCALE: 194 case Vehicle.LINEAR_MOTOR_TIMESCALE:
@@ -197,7 +196,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
197 m_linearMotor.TimeScale = m_linearMotorTimescale; 196 m_linearMotor.TimeScale = m_linearMotorTimescale;
198 break; 197 break;
199 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 198 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
200 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); 199 m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
201 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency; 200 m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
202 break; 201 break;
203 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: 202 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
@@ -242,9 +241,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
242 break; 241 break;
243 case Vehicle.ANGULAR_MOTOR_DIRECTION: 242 case Vehicle.ANGULAR_MOTOR_DIRECTION:
244 // Limit requested angular speed to 2 rps= 4 pi rads/sec 243 // Limit requested angular speed to 2 rps= 4 pi rads/sec
245 pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f)); 244 pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
246 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); 245 pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
247 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); 246 pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
248 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 247 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
249 m_angularMotor.SetTarget(m_angularMotorDirection); 248 m_angularMotor.SetTarget(m_angularMotorDirection);
250 break; 249 break;
@@ -330,6 +329,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
330 m_bankingEfficiency = 0; 329 m_bankingEfficiency = 0;
331 m_bankingTimescale = 1000; 330 m_bankingTimescale = 1000;
332 m_bankingMix = 1; 331 m_bankingMix = 1;
332 m_lastBanking = Vector3.Zero;
333 333
334 m_referenceFrame = Quaternion.Identity; 334 m_referenceFrame = Quaternion.Identity;
335 m_flags = (VehicleFlag)0; 335 m_flags = (VehicleFlag)0;
@@ -364,6 +364,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
364 m_bankingEfficiency = 0; 364 m_bankingEfficiency = 0;
365 m_bankingTimescale = 10; 365 m_bankingTimescale = 10;
366 m_bankingMix = 1; 366 m_bankingMix = 1;
367 m_lastBanking = Vector3.Zero;
367 368
368 m_referenceFrame = Quaternion.Identity; 369 m_referenceFrame = Quaternion.Identity;
369 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY 370 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -402,6 +403,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
402 m_bankingEfficiency = -0.2f; 403 m_bankingEfficiency = -0.2f;
403 m_bankingMix = 1; 404 m_bankingMix = 1;
404 m_bankingTimescale = 1; 405 m_bankingTimescale = 1;
406 m_lastBanking = Vector3.Zero;
405 407
406 m_referenceFrame = Quaternion.Identity; 408 m_referenceFrame = Quaternion.Identity;
407 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY 409 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -440,14 +442,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
440 m_bankingEfficiency = -0.3f; 442 m_bankingEfficiency = -0.3f;
441 m_bankingMix = 0.8f; 443 m_bankingMix = 0.8f;
442 m_bankingTimescale = 1; 444 m_bankingTimescale = 1;
445 m_lastBanking = Vector3.Zero;
443 446
444 m_referenceFrame = Quaternion.Identity; 447 m_referenceFrame = Quaternion.Identity;
445 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY 448 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
446 | VehicleFlag.HOVER_GLOBAL_HEIGHT 449 | VehicleFlag.HOVER_GLOBAL_HEIGHT
447 | VehicleFlag.LIMIT_ROLL_ONLY 450 | VehicleFlag.LIMIT_ROLL_ONLY
448 | VehicleFlag.LIMIT_MOTOR_UP
449 | VehicleFlag.HOVER_UP_ONLY); 451 | VehicleFlag.HOVER_UP_ONLY);
450 m_flags |= (VehicleFlag.NO_DEFLECTION_UP 452 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
453 | VehicleFlag.LIMIT_MOTOR_UP
451 | VehicleFlag.HOVER_WATER_ONLY); 454 | VehicleFlag.HOVER_WATER_ONLY);
452 break; 455 break;
453 case Vehicle.TYPE_AIRPLANE: 456 case Vehicle.TYPE_AIRPLANE:
@@ -478,6 +481,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
478 m_bankingEfficiency = 1; 481 m_bankingEfficiency = 1;
479 m_bankingMix = 0.7f; 482 m_bankingMix = 0.7f;
480 m_bankingTimescale = 2; 483 m_bankingTimescale = 2;
484 m_lastBanking = Vector3.Zero;
481 485
482 m_referenceFrame = Quaternion.Identity; 486 m_referenceFrame = Quaternion.Identity;
483 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY 487 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
@@ -516,6 +520,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
516 m_bankingEfficiency = 0; 520 m_bankingEfficiency = 0;
517 m_bankingMix = 0.7f; 521 m_bankingMix = 0.7f;
518 m_bankingTimescale = 5; 522 m_bankingTimescale = 5;
523 m_lastBanking = Vector3.Zero;
524
519 m_referenceFrame = Quaternion.Identity; 525 m_referenceFrame = Quaternion.Identity;
520 526
521 m_referenceFrame = Quaternion.Identity; 527 m_referenceFrame = Quaternion.Identity;
@@ -558,9 +564,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
558 { 564 {
559 if (IsActive) 565 if (IsActive)
560 { 566 {
567 // Remember the mass so we don't have to fetch it every step
561 m_vehicleMass = Prim.Linkset.LinksetMass; 568 m_vehicleMass = Prim.Linkset.LinksetMass;
562 569
563 // Friction effects are handled by this vehicle code 570 // Friction affects are handled by this vehicle code
564 float friction = 0f; 571 float friction = 0f;
565 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction); 572 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
566 573
@@ -574,6 +581,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
574 // Vector3 localInertia = new Vector3(1f, 1f, 1f); 581 // Vector3 localInertia = new Vector3(1f, 1f, 1f);
575 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass); 582 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
576 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia); 583 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
584 BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
577 585
578 VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}", 586 VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
579 Prim.LocalID, friction, localInertia, angularDamping); 587 Prim.LocalID, friction, localInertia, angularDamping);
@@ -598,31 +606,167 @@ namespace OpenSim.Region.Physics.BulletSPlugin
598 Refresh(); 606 Refresh();
599 } 607 }
600 608
609 #region Known vehicle value functions
610 // Vehicle physical parameters that we buffer from constant getting and setting.
611 // The "m_known*" variables are initialized to 'null', fetched only if referenced
612 // and stored back into the physics engine only if updated.
613 // This does two things: 1) saves continuious calls into unmanaged code, and
614 // 2) signals when a physics property update must happen back to the simulator
615 // to update values modified for the vehicle.
616 private int m_knownChanged;
617 private float? m_knownTerrainHeight;
618 private float? m_knownWaterLevel;
619 private Vector3? m_knownPosition;
620 private Vector3? m_knownVelocity;
621 private Quaternion? m_knownOrientation;
622 private Vector3? m_knownRotationalVelocity;
623
624 private const int m_knownChangedPosition = 1 << 0;
625 private const int m_knownChangedVelocity = 1 << 1;
626 private const int m_knownChangedOrientation = 1 << 2;
627 private const int m_knownChangedRotationalVelocity = 1 << 3;
628
629 private void ForgetKnownVehicleProperties()
630 {
631 m_knownTerrainHeight = null;
632 m_knownWaterLevel = null;
633 m_knownPosition = null;
634 m_knownVelocity = null;
635 m_knownOrientation = null;
636 m_knownRotationalVelocity = null;
637 m_knownChanged = 0;
638 }
639 private void PushKnownChanged()
640 {
641 if (m_knownChanged != 0)
642 {
643 if ((m_knownChanged & m_knownChangedPosition) != 0)
644 Prim.ForcePosition = VehiclePosition;
645 if ((m_knownChanged & m_knownChangedOrientation) != 0)
646 Prim.ForceOrientation = VehicleOrientation;
647 if ((m_knownChanged & m_knownChangedVelocity) != 0)
648 Prim.ForceVelocity = VehicleVelocity;
649 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
650 {
651 Prim.ForceRotationalVelocity = VehicleRotationalVelocity;
652 BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, VehicleRotationalVelocity);
653 }
654 // If we set one of the values (ie, the physics engine didn't do it) we must force
655 // an UpdateProperties event to send the changes up to the simulator.
656 BulletSimAPI.PushUpdate2(Prim.PhysBody.ptr);
657 }
658 }
659
660 // Since the computation of terrain height can be a little involved, this routine
661 // is used ot fetch the height only once for each vehicle simulation step.
662 private float GetTerrainHeight(Vector3 pos)
663 {
664 if (m_knownTerrainHeight == null)
665 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
666 return (float)m_knownTerrainHeight;
667 }
668
669 // Since the computation of water level can be a little involved, this routine
670 // is used ot fetch the level only once for each vehicle simulation step.
671 private float GetWaterLevel(Vector3 pos)
672 {
673 if (m_knownWaterLevel == null)
674 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
675 return (float)m_knownWaterLevel;
676 }
677
678 private Vector3 VehiclePosition
679 {
680 get
681 {
682 if (m_knownPosition == null)
683 m_knownPosition = Prim.ForcePosition;
684 return (Vector3)m_knownPosition;
685 }
686 set
687 {
688 m_knownPosition = value;
689 m_knownChanged |= m_knownChangedPosition;
690 }
691 }
692
693 private Quaternion VehicleOrientation
694 {
695 get
696 {
697 if (m_knownOrientation == null)
698 m_knownOrientation = Prim.ForceOrientation;
699 return (Quaternion)m_knownOrientation;
700 }
701 set
702 {
703 m_knownOrientation = value;
704 m_knownChanged |= m_knownChangedOrientation;
705 }
706 }
707
708 private Vector3 VehicleVelocity
709 {
710 get
711 {
712 if (m_knownVelocity == null)
713 m_knownVelocity = Prim.ForceVelocity;
714 return (Vector3)m_knownVelocity;
715 }
716 set
717 {
718 m_knownVelocity = value;
719 m_knownChanged |= m_knownChangedVelocity;
720 }
721 }
722
723 private Vector3 VehicleRotationalVelocity
724 {
725 get
726 {
727 if (m_knownRotationalVelocity == null)
728 m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
729 return (Vector3)m_knownRotationalVelocity;
730 }
731 set
732 {
733 m_knownRotationalVelocity = value;
734 m_knownChanged |= m_knownChangedRotationalVelocity;
735 }
736 }
737 #endregion // Known vehicle value functions
738
601 // One step of the vehicle properties for the next 'pTimestep' seconds. 739 // One step of the vehicle properties for the next 'pTimestep' seconds.
602 internal void Step(float pTimestep) 740 internal void Step(float pTimestep)
603 { 741 {
604 if (!IsActive) return; 742 if (!IsActive) return;
605 743
744 ForgetKnownVehicleProperties();
745
606 MoveLinear(pTimestep); 746 MoveLinear(pTimestep);
607 MoveAngular(pTimestep); 747 MoveAngular(pTimestep);
608 748
609 LimitRotation(pTimestep); 749 LimitRotation(pTimestep);
610 750
611 // remember the position so next step we can limit absolute movement effects 751 // remember the position so next step we can limit absolute movement effects
612 m_lastPositionVector = Prim.ForcePosition; 752 m_lastPositionVector = VehiclePosition;
753
754 // If we forced the changing of some vehicle parameters, update the values and
755 // for the physics engine to note the changes so an UpdateProperties event will happen.
756 PushKnownChanged();
613 757
614 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 758 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
615 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); 759 Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
616 } 760 }
617 761
618 // Apply the effect of the linear motor. 762 // Apply the effect of the linear motor and other linear motions (like hover and float).
619 // Also does hover and float.
620 private void MoveLinear(float pTimestep) 763 private void MoveLinear(float pTimestep)
621 { 764 {
622 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep); 765 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
623 766
624 // Rotate new object velocity from vehicle relative to world coordinates 767 // The movement computed in the linear motor is relative to the vehicle
625 linearMotorContribution *= Prim.ForceOrientation; 768 // coordinates. Rotate the movement to world coordinates.
769 linearMotorContribution *= VehicleOrientation;
626 770
627 // ================================================================== 771 // ==================================================================
628 // Gravity and Buoyancy 772 // Gravity and Buoyancy
@@ -630,16 +774,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
630 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 774 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
631 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); 775 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
632 776
633 Vector3 pos = Prim.ForcePosition; 777 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep);
634 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
635
636 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
637 778
638 Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight); 779 Vector3 hoverContribution = ComputeLinearHover(pTimestep);
639 780
640 ComputeLinearBlockingEndPoint(pTimestep, ref pos); 781 ComputeLinearBlockingEndPoint(pTimestep);
641 782
642 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight); 783 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
643 784
644 // ================================================================== 785 // ==================================================================
645 Vector3 newVelocity = linearMotorContribution 786 Vector3 newVelocity = linearMotorContribution
@@ -667,42 +808,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
667 newVelocity = Vector3.Zero; 808 newVelocity = Vector3.Zero;
668 809
669 // ================================================================== 810 // ==================================================================
670 // Stuff new linear velocity into the vehicle 811 // Stuff new linear velocity into the vehicle.
671 Prim.ForceVelocity = newVelocity; 812 // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
672 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG 813 VehicleVelocity = newVelocity;
673 814
674 // Other linear forces are applied as forces. 815 // Other linear forces are applied as forces.
675 Vector3 totalDownForce = grav * m_vehicleMass; 816 Vector3 totalDownForce = grav * m_vehicleMass * pTimestep;
676 if (totalDownForce != Vector3.Zero) 817 if (totalDownForce != Vector3.Zero)
677 { 818 {
678 Prim.AddForce(totalDownForce, false); 819 Prim.AddForce(totalDownForce, false);
679 } 820 }
680 821
681 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", 822 VDetailLog("{0}, MoveLinear,done,newVel={1},totDown={2},linContrib={3},terrContrib={4},hoverContrib={5},limitContrib={6}",
682 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, 823 Prim.LocalID, newVelocity, totalDownForce,
683 newVelocity, Prim.Velocity, totalDownForce); 824 linearMotorContribution, terrainHeightContribution, hoverContribution, limitMotorUpContribution
825 );
684 826
685 } // end MoveLinear() 827 } // end MoveLinear()
686 828
687 public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight) 829 public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep)
688 { 830 {
689 Vector3 ret = Vector3.Zero; 831 Vector3 ret = Vector3.Zero;
690 // If below the terrain, move us above the ground a little. 832 // If below the terrain, move us above the ground a little.
691 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. 833 // TODO: Consider taking the rotated size of the object or possibly casting a ray.
692 // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. 834 if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
693 // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
694 // if (rotatedSize.Z < terrainHeight)
695 if (pos.Z < terrainHeight)
696 { 835 {
697 // TODO: correct position by applying force rather than forcing position. 836 // TODO: correct position by applying force rather than forcing position.
698 pos.Z = terrainHeight + 2; 837 VehiclePosition += new Vector3(0f, 0f, GetTerrainHeight(VehiclePosition) + 2f);
699 Prim.ForcePosition = pos; 838 VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
700 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
701 } 839 }
702 return ret; 840 return ret;
703 } 841 }
704 842
705 public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight) 843 public Vector3 ComputeLinearHover(float pTimestep)
706 { 844 {
707 Vector3 ret = Vector3.Zero; 845 Vector3 ret = Vector3.Zero;
708 846
@@ -713,11 +851,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
713 // We should hover, get the target height 851 // We should hover, get the target height
714 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) 852 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
715 { 853 {
716 m_VhoverTargetHeight = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos) + m_VhoverHeight; 854 m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
717 } 855 }
718 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) 856 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
719 { 857 {
720 m_VhoverTargetHeight = terrainHeight + m_VhoverHeight; 858 m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
721 } 859 }
722 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) 860 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
723 { 861 {
@@ -727,46 +865,44 @@ namespace OpenSim.Region.Physics.BulletSPlugin
727 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) 865 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
728 { 866 {
729 // If body is already heigher, use its height as target height 867 // If body is already heigher, use its height as target height
730 if (pos.Z > m_VhoverTargetHeight) 868 if (VehiclePosition.Z > m_VhoverTargetHeight)
731 m_VhoverTargetHeight = pos.Z; 869 m_VhoverTargetHeight = VehiclePosition.Z;
732 } 870 }
733 871
734 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) 872 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
735 { 873 {
736 if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) 874 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
737 { 875 {
876 Vector3 pos = VehiclePosition;
738 pos.Z = m_VhoverTargetHeight; 877 pos.Z = m_VhoverTargetHeight;
739 Prim.ForcePosition = pos; 878 VehiclePosition = pos;
740 } 879 }
741 } 880 }
742 else 881 else
743 { 882 {
744 float verticalError = pos.Z - m_VhoverTargetHeight; 883 // Error is positive if below the target and negative if above.
745 // RA: where does the 50 come from? 884 float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
746 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); 885 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
747 // Replace Vertical speed with correction figure if significant 886
748 if (verticalError > 0.01f) 887 // TODO: implement m_VhoverEfficiency correctly
888 if (Math.Abs(verticalError) > m_VhoverEfficiency)
749 { 889 {
750 ret = new Vector3(0f, 0f, verticalCorrectionVelocity); 890 ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
751 //KF: m_VhoverEfficiency is not yet implemented
752 }
753 else if (verticalError < -0.01)
754 {
755 ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
756 } 891 }
757 } 892 }
758 893
759 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", 894 VDetailLog("{0}, MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
760 Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight); 895 Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
761 } 896 }
762 897
763 return ret; 898 return ret;
764 } 899 }
765 900
766 public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos) 901 public bool ComputeLinearBlockingEndPoint(float pTimestep)
767 { 902 {
768 bool changed = false; 903 bool changed = false;
769 904
905 Vector3 pos = VehiclePosition;
770 Vector3 posChange = pos - m_lastPositionVector; 906 Vector3 posChange = pos - m_lastPositionVector;
771 if (m_BlockingEndPoint != Vector3.Zero) 907 if (m_BlockingEndPoint != Vector3.Zero)
772 { 908 {
@@ -797,8 +933,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
797 } 933 }
798 if (changed) 934 if (changed)
799 { 935 {
800 Prim.ForcePosition = pos; 936 VehiclePosition = pos;
801 VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", 937 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
802 Prim.LocalID, m_BlockingEndPoint, posChange, pos); 938 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
803 } 939 }
804 } 940 }
@@ -812,13 +948,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
812 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering 948 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
813 // when they are in mid jump. 949 // when they are in mid jump.
814 // TODO: this code is wrong. Also, what should it do for boats? 950 // TODO: this code is wrong. Also, what should it do for boats?
815 public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight) 951 public Vector3 ComputeLinearMotorUp(float pTimestep)
816 { 952 {
817 Vector3 ret = Vector3.Zero; 953 Vector3 ret = Vector3.Zero;
818 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 954 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
819 { 955 {
820 // If the vehicle is motoring into the sky, get it going back down. 956 // If the vehicle is motoring into the sky, get it going back down.
821 float distanceAboveGround = pos.Z - terrainHeight; 957 // float distanceAboveGround = pos.Z - Math.Max(GetTerrainHeight(pos), GetWaterLevel(pos));
958 float distanceAboveGround = VehiclePosition.Z - GetTerrainHeight(VehiclePosition);
822 if (distanceAboveGround > 1f) 959 if (distanceAboveGround > 1f)
823 { 960 {
824 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); 961 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
@@ -830,7 +967,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
830 // has a decay factor. This says this force should 967 // has a decay factor. This says this force should
831 // be computed with a motor. 968 // be computed with a motor.
832 // TODO: add interaction with banking. 969 // TODO: add interaction with banking.
833 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 970 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
834 Prim.LocalID, distanceAboveGround, ret); 971 Prim.LocalID, distanceAboveGround, ret);
835 } 972 }
836 return ret; 973 return ret;
@@ -839,36 +976,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
839 // ======================================================================= 976 // =======================================================================
840 // ======================================================================= 977 // =======================================================================
841 // Apply the effect of the angular motor. 978 // Apply the effect of the angular motor.
979 // The 'contribution' is how much angular correction velocity each function wants.
980 // All the contributions are added together and the orientation of the vehicle
981 // is changed by all the contributed corrections.
842 private void MoveAngular(float pTimestep) 982 private void MoveAngular(float pTimestep)
843 { 983 {
844 // m_angularMotorDirection // angular velocity requested by LSL motor 984 // The user wants how many radians per second angular change?
845 // m_angularMotorVelocity // current angular motor velocity (ramps up and down)
846 // m_angularMotorTimescale // motor angular velocity ramp up time
847 // m_angularMotorDecayTimescale // motor angular velocity decay rate
848 // m_angularFrictionTimescale // body angular velocity decay rate
849 // m_lastAngularVelocity // what was last applied to body
850
851 /*
852 if (m_angularMotorDirection.LengthSquared() > 0.0001)
853 {
854 Vector3 origVel = m_angularMotorVelocity;
855 Vector3 origDir = m_angularMotorDirection;
856
857 // new velocity += error / ( time to get there / step interval)
858 // requested direction - current vehicle direction
859 m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep);
860 // decay requested direction
861 m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
862
863 VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
864 Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
865 }
866 else
867 {
868 m_angularMotorVelocity = Vector3.Zero;
869 }
870 */
871
872 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep); 985 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
873 986
874 // ================================================================== 987 // ==================================================================
@@ -881,25 +994,47 @@ namespace OpenSim.Region.Physics.BulletSPlugin
881 { 994 {
882 angularMotorContribution.X = 0f; 995 angularMotorContribution.X = 0f;
883 angularMotorContribution.Y = 0f; 996 angularMotorContribution.Y = 0f;
884 VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution); 997 VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
885 } 998 }
886 999
887 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep); 1000 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
888 1001
889 Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep); 1002 Vector3 deflectionContribution = ComputeAngularDeflection();
890 1003
891 Vector3 bankingContribution = ComputeAngularBanking(pTimestep); 1004 Vector3 bankingContribution = ComputeAngularBanking(angularMotorContribution.Z);
892 1005
893 // ================================================================== 1006 // ==================================================================
894 m_lastVertAttractor = verticalAttractionContribution; 1007 m_lastVertAttractor = verticalAttractionContribution;
895 1008
896 // Sum velocities 1009 // Sum corrections
897 m_lastAngularVelocity = angularMotorContribution 1010 m_lastAngularCorrection = angularMotorContribution
898 + verticalAttractionContribution 1011 + verticalAttractionContribution
899 + deflectionContribution 1012 + deflectionContribution
900 + bankingContribution; 1013 + bankingContribution;
901 1014
902 // ================================================================== 1015 // ==================================================================
1016 // The correction is applied to the current orientation.
1017 if (!m_lastAngularCorrection.ApproxEquals(Vector3.Zero, 0.01f))
1018 {
1019 Vector3 scaledCorrection = m_lastAngularCorrection * pTimestep;
1020
1021 VehicleRotationalVelocity = scaledCorrection;
1022
1023 VDetailLog("{0}, MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5},scaledCorr={6}",
1024 Prim.LocalID,
1025 angularMotorContribution, verticalAttractionContribution,
1026 bankingContribution, deflectionContribution,
1027 m_lastAngularCorrection, scaledCorrection
1028 );
1029 }
1030 else
1031 {
1032 // The vehicle is not adding anything velocity wise.
1033 VehicleRotationalVelocity = Vector3.Zero;
1034 VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
1035 }
1036
1037 // ==================================================================
903 //Offset section 1038 //Offset section
904 if (m_linearMotorOffset != Vector3.Zero) 1039 if (m_linearMotorOffset != Vector3.Zero)
905 { 1040 {
@@ -927,53 +1062,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
927 torqueFromOffset.Z = 0; 1062 torqueFromOffset.Z = 0;
928 torqueFromOffset *= m_vehicleMass; 1063 torqueFromOffset *= m_vehicleMass;
929 Prim.ApplyTorqueImpulse(torqueFromOffset, true); 1064 Prim.ApplyTorqueImpulse(torqueFromOffset, true);
930 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); 1065 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
931 } 1066 }
932 1067
933 // ==================================================================
934 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
935 {
936 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
937 // TODO: zeroing is good but it also sets values in unmanaged code. Remove the stores when idle.
938 VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
939 Prim.ZeroAngularMotion(true);
940 }
941 else
942 {
943 // Apply to the body.
944 // The above calculates the absolute angular velocity needed. Angular velocity is massless.
945 // Since we are stuffing the angular velocity directly into the object, the computed
946 // velocity needs to be scaled by the timestep.
947 // Also remove any motion that is on the object so added motion is only from vehicle.
948 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
949 - Prim.ForceRotationalVelocity);
950 // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
951 Prim.ForceRotationalVelocity = applyAngularForce;
952
953 VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
954 Prim.LocalID,
955 angularMotorContribution, verticalAttractionContribution,
956 bankingContribution, deflectionContribution,
957 applyAngularForce, m_lastAngularVelocity
958 );
959 }
960 } 1068 }
961 1069
962 public Vector3 ComputeAngularVerticalAttraction(float pTimestep) 1070 public Vector3 ComputeAngularVerticalAttraction()
963 { 1071 {
964 Vector3 ret = Vector3.Zero; 1072 Vector3 ret = Vector3.Zero;
965 1073
966 // If vertical attaction timescale is reasonable and we applied an angular force last time... 1074 // If vertical attaction timescale is reasonable and we applied an angular force last time...
967 if (m_verticalAttractionTimescale < 500) 1075 if (m_verticalAttractionTimescale < 500)
968 { 1076 {
969 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
970 verticalError.Normalize();
971 m_verticalAttractionMotor.SetCurrent(verticalError);
972 m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
973 ret = m_verticalAttractionMotor.Step(pTimestep);
974 /*
975 // Take a vector pointing up and convert it from world to vehicle relative coords. 1077 // Take a vector pointing up and convert it from world to vehicle relative coords.
976 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; 1078 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
977 verticalError.Normalize(); 1079 verticalError.Normalize();
978 1080
979 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ) 1081 // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
@@ -991,63 +1093,70 @@ namespace OpenSim.Region.Physics.BulletSPlugin
991 } 1093 }
992 1094
993 // Y error means needed rotation around X axis and visa versa. 1095 // Y error means needed rotation around X axis and visa versa.
994 verticalAttractionContribution.X = verticalError.Y; 1096 ret.X = verticalError.Y;
995 verticalAttractionContribution.Y = - verticalError.X; 1097 ret.Y = - verticalError.X;
996 verticalAttractionContribution.Z = 0f; 1098 ret.Z = 0f;
997 1099
998 // scale by the time scale and timestep 1100 // scale by the time scale and timestep
999 Vector3 unscaledContrib = verticalAttractionContribution; 1101 Vector3 unscaledContrib = ret;
1000 verticalAttractionContribution /= m_verticalAttractionTimescale; 1102 ret /= m_verticalAttractionTimescale;
1001 verticalAttractionContribution *= pTimestep; 1103 // This returns the angular correction desired. Timestep is added later.
1104 // ret *= pTimestep;
1002 1105
1003 // apply efficiency 1106 // apply efficiency
1004 Vector3 preEfficiencyContrib = verticalAttractionContribution; 1107 Vector3 preEfficiencyContrib = ret;
1108 // TODO: implement efficiency.
1109 // Effenciency squared seems to give a more realistic effect
1005 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; 1110 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
1006 verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 1111 // ret *= efficencySquared;
1007 1112
1008 VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}", 1113 VDetailLog("{0}, MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
1009 Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib, 1114 Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
1010 m_verticalAttractionEfficiency, efficencySquared, 1115 m_verticalAttractionEfficiency, efficencySquared,
1011 verticalAttractionContribution); 1116 ret);
1012 */
1013
1014 } 1117 }
1015 return ret; 1118 return ret;
1016 } 1119 }
1017 1120
1018 public Vector3 ComputeAngularDeflection(float pTimestep) 1121 // Return the angular correction to correct the direction the vehicle is pointing to be
1122 // the direction is should want to be pointing.
1123 public Vector3 ComputeAngularDeflection()
1019 { 1124 {
1020 Vector3 ret = Vector3.Zero; 1125 Vector3 ret = Vector3.Zero;
1021 1126
1022 if (m_angularDeflectionEfficiency != 0) 1127 if (m_angularDeflectionEfficiency != 0)
1023 { 1128 {
1024 // Compute a scaled vector that points in the preferred axis (X direction) 1129 // Where the vehicle should want to point relative to the vehicle
1025 Vector3 scaledDefaultDirection = 1130 Vector3 preferredDirection = Vector3.UnitX * m_referenceFrame;
1026 new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
1027 // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
1028 // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
1029 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
1030 1131
1031 // Scale by efficiency and timescale 1132 // Where the vehicle is pointing relative to the vehicle.
1032 ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; 1133 Vector3 currentDirection = Vector3.UnitX * Quaternion.Add(VehicleOrientation, m_referenceFrame);
1033 1134
1034 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret); 1135 // Difference between where vehicle is pointing and where it should wish to point
1136 Vector3 directionCorrection = preferredDirection - currentDirection;
1035 1137
1036 // This deflection computation is not correct. 1138 // Scale the correction by recovery timescale and efficiency
1037 ret = Vector3.Zero; 1139 ret = directionCorrection * m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
1140
1141 VDetailLog("{0}, MoveAngular,Deflection,perfDir={1},currentDir={2},dirCorrection={3},ret={4}",
1142 Prim.LocalID, preferredDirection, currentDirection, directionCorrection, ret);
1038 } 1143 }
1039 return ret; 1144 return ret;
1040 } 1145 }
1041 1146
1042 public Vector3 ComputeAngularBanking(float pTimestep) 1147 // Return an angular change to tip the vehicle (around X axis) when turning (turned around Z).
1148 // Remembers the last banking value calculated and returns the difference needed this tick.
1149 // TurningFactor is rate going left or right (pos=left, neg=right, scale=0..1).
1150 public Vector3 ComputeAngularBanking(float turningFactor)
1043 { 1151 {
1044 Vector3 ret = Vector3.Zero; 1152 Vector3 ret = Vector3.Zero;
1153 Vector3 computedBanking = Vector3.Zero;
1045 1154
1046 if (m_bankingEfficiency != 0) 1155 if (m_bankingEfficiency != 0)
1047 { 1156 {
1048 Vector3 dir = Vector3.One * Prim.ForceOrientation; 1157 Vector3 currentDirection = Vector3.UnitX * VehicleOrientation;
1158
1049 float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1); 1159 float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
1050 //Changes which way it banks in and out of turns
1051 1160
1052 //Use the square of the efficiency, as it looks much more how SL banking works 1161 //Use the square of the efficiency, as it looks much more how SL banking works
1053 float effSquared = (m_bankingEfficiency * m_bankingEfficiency); 1162 float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
@@ -1055,58 +1164,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1055 effSquared *= -1; //Keep the negative! 1164 effSquared *= -1; //Keep the negative!
1056 1165
1057 float mix = Math.Abs(m_bankingMix); 1166 float mix = Math.Abs(m_bankingMix);
1058 if (m_angularMotorVelocity.X == 0) 1167 // TODO: Must include reference frame.
1059 { 1168 float forwardSpeed = VehicleVelocity.X;
1060 // The vehicle is stopped
1061 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
1062 {
1063 Vector3 axisAngle;
1064 float angle;
1065 parent.Orientation.GetAxisAngle(out axisAngle, out angle);
1066 Vector3 rotatedVel = parent.Velocity * parent.Orientation;
1067 if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
1068 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
1069 else
1070 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
1071 }*/
1072 }
1073 else
1074 {
1075 ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
1076 }
1077 1169
1078 //If they are colliding, we probably shouldn't shove the prim around... probably 1170 if (!Prim.IsColliding && forwardSpeed > mix)
1079 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
1080 { 1171 {
1081 float angVelZ = m_angularMotorVelocity.X * -1; 1172 computedBanking.X = ClampInRange(-3f, turningFactor * (effSquared * mult), 3f);
1082 /*if(angVelZ > mix)
1083 angVelZ = mix;
1084 else if(angVelZ < -mix)
1085 angVelZ = -mix;*/
1086 //This controls how fast and how far the banking occurs
1087 Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
1088 if (bankingRot.X > 3)
1089 bankingRot.X = 3;
1090 else if (bankingRot.X < -3)
1091 bankingRot.X = -3;
1092 bankingRot *= Prim.ForceOrientation;
1093 ret += bankingRot;
1094 } 1173 }
1095 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; 1174
1096 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}", 1175 // 'computedBanking' is now how much banking that should be happening.
1097 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, ret); 1176 ret = computedBanking - m_lastBanking;
1177
1178 // Scale the correction by timescale and efficiency
1179 ret /= m_bankingTimescale * m_bankingEfficiency;
1180
1181 VDetailLog("{0}, MoveAngular,Banking,computedB={1},lastB={2},bEff={3},effSq={4},mult={5},mix={6},banking={7}",
1182 Prim.LocalID, computedBanking, m_lastBanking, m_bankingEfficiency, effSquared, mult, mix, ret);
1098 } 1183 }
1184 m_lastBanking = computedBanking;
1099 return ret; 1185 return ret;
1100 } 1186 }
1101 1187
1102
1103 // This is from previous instantiations of XXXDynamics.cs. 1188 // This is from previous instantiations of XXXDynamics.cs.
1104 // Applies roll reference frame. 1189 // Applies roll reference frame.
1105 // TODO: is this the right way to separate the code to do this operation? 1190 // TODO: is this the right way to separate the code to do this operation?
1106 // Should this be in MoveAngular()? 1191 // Should this be in MoveAngular()?
1107 internal void LimitRotation(float timestep) 1192 internal void LimitRotation(float timestep)
1108 { 1193 {
1109 Quaternion rotq = Prim.ForceOrientation; 1194 Quaternion rotq = VehicleOrientation;
1110 Quaternion m_rot = rotq; 1195 Quaternion m_rot = rotq;
1111 if (m_RollreferenceFrame != Quaternion.Identity) 1196 if (m_RollreferenceFrame != Quaternion.Identity)
1112 { 1197 {
@@ -1134,12 +1219,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1134 } 1219 }
1135 if (rotq != m_rot) 1220 if (rotq != m_rot)
1136 { 1221 {
1137 Prim.ForceOrientation = m_rot; 1222 VehicleOrientation = m_rot;
1138 VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot); 1223 VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot);
1139 } 1224 }
1140 1225
1141 } 1226 }
1142 1227
1228 private float ClampInRange(float low, float val, float high)
1229 {
1230 return Math.Max(low, Math.Min(val, high));
1231 }
1232
1143 // Invoke the detailed logger and output something if it's enabled. 1233 // Invoke the detailed logger and output something if it's enabled.
1144 private void VDetailLog(string msg, params Object[] args) 1234 private void VDetailLog(string msg, params Object[] args)
1145 { 1235 {