aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
diff options
context:
space:
mode:
authorDiva Canto2012-12-04 08:08:58 -0800
committerDiva Canto2012-12-04 08:08:58 -0800
commita4821c5e01da394ba0968e6206a52f4db45cd7bd (patch)
tree68dae4719fe4100ce272ba7e9cd07018052c825b /OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
parentImprovement in HGInstantMessageService: account for the existence of an offli... (diff)
parentMerge branch 'master' of ssh://opensimulator.org/var/git/opensim (diff)
downloadopensim-SC-a4821c5e01da394ba0968e6206a52f4db45cd7bd.zip
opensim-SC-a4821c5e01da394ba0968e6206a52f4db45cd7bd.tar.gz
opensim-SC-a4821c5e01da394ba0968e6206a52f4db45cd7bd.tar.bz2
opensim-SC-a4821c5e01da394ba0968e6206a52f4db45cd7bd.tar.xz
Merge branch 'master' of ssh://opensimulator.org/var/git/opensim
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs522
1 files changed, 313 insertions, 209 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 74eb9ab..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,6 +442,7 @@ 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
@@ -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 778
636 Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight); 779 Vector3 hoverContribution = ComputeLinearHover(pTimestep);
637 780
638 Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight); 781 ComputeLinearBlockingEndPoint(pTimestep);
639 782
640 ComputeLinearBlockingEndPoint(pTimestep, ref pos); 783 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep);
641
642 Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
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.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,45 +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 }
871
733 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) 872 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
734 { 873 {
735 if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) 874 if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
736 { 875 {
876 Vector3 pos = VehiclePosition;
737 pos.Z = m_VhoverTargetHeight; 877 pos.Z = m_VhoverTargetHeight;
738 Prim.ForcePosition = pos; 878 VehiclePosition = pos;
739 } 879 }
740 } 880 }
741 else 881 else
742 { 882 {
743 float verticalError = pos.Z - m_VhoverTargetHeight; 883 // Error is positive if below the target and negative if above.
744 // RA: where does the 50 come from? 884 float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
745 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); 885 float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
746 // Replace Vertical speed with correction figure if significant 886
747 if (verticalError > 0.01f) 887 // TODO: implement m_VhoverEfficiency correctly
888 if (Math.Abs(verticalError) > m_VhoverEfficiency)
748 { 889 {
749 ret = new Vector3(0f, 0f, verticalCorrectionVelocity); 890 ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
750 //KF: m_VhoverEfficiency is not yet implemented
751 }
752 else if (verticalError < -0.01)
753 {
754 ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
755 } 891 }
756 } 892 }
757 893
758 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}",
759 Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight); 895 Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
760 } 896 }
761 897
762 return ret; 898 return ret;
763 } 899 }
764 900
765 public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos) 901 public bool ComputeLinearBlockingEndPoint(float pTimestep)
766 { 902 {
767 bool changed = false; 903 bool changed = false;
768 904
905 Vector3 pos = VehiclePosition;
769 Vector3 posChange = pos - m_lastPositionVector; 906 Vector3 posChange = pos - m_lastPositionVector;
770 if (m_BlockingEndPoint != Vector3.Zero) 907 if (m_BlockingEndPoint != Vector3.Zero)
771 { 908 {
@@ -796,32 +933,41 @@ namespace OpenSim.Region.Physics.BulletSPlugin
796 } 933 }
797 if (changed) 934 if (changed)
798 { 935 {
799 Prim.ForcePosition = pos; 936 VehiclePosition = pos;
800 VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", 937 VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
801 Prim.LocalID, m_BlockingEndPoint, posChange, pos); 938 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
802 } 939 }
803 } 940 }
804 return changed; 941 return changed;
805 } 942 }
806 943
807 public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight) 944 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
945 // Prevent ground vehicles from motoring into the sky.This flag has a subtle effect when
946 // used with conjunction with banking: the strength of the banking will decay when the
947 // vehicle no longer experiences collisions. The decay timescale is the same as
948 // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
949 // when they are in mid jump.
950 // TODO: this code is wrong. Also, what should it do for boats?
951 public Vector3 ComputeLinearMotorUp(float pTimestep)
808 { 952 {
809 Vector3 ret = Vector3.Zero; 953 Vector3 ret = Vector3.Zero;
810 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 954 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
811 { 955 {
812 // 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.
813 float distanceAboveGround = pos.Z - terrainHeight; 957 // float distanceAboveGround = pos.Z - Math.Max(GetTerrainHeight(pos), GetWaterLevel(pos));
958 float distanceAboveGround = VehiclePosition.Z - GetTerrainHeight(VehiclePosition);
814 if (distanceAboveGround > 1f) 959 if (distanceAboveGround > 1f)
815 { 960 {
816 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); 961 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
817 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); 962 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
818 ret = new Vector3(0, 0, -distanceAboveGround); 963 ret = new Vector3(0, 0, -distanceAboveGround);
819 } 964 }
820 // TODO: this calculation is all wrong. From the description at 965 // TODO: this calculation is wrong. From the description at
821 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce 966 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
822 // has a decay factor. This says this force should 967 // has a decay factor. This says this force should
823 // be computed with a motor. 968 // be computed with a motor.
824 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 969 // TODO: add interaction with banking.
970 VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
825 Prim.LocalID, distanceAboveGround, ret); 971 Prim.LocalID, distanceAboveGround, ret);
826 } 972 }
827 return ret; 973 return ret;
@@ -830,61 +976,63 @@ namespace OpenSim.Region.Physics.BulletSPlugin
830 // ======================================================================= 976 // =======================================================================
831 // ======================================================================= 977 // =======================================================================
832 // 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.
833 private void MoveAngular(float pTimestep) 982 private void MoveAngular(float pTimestep)
834 { 983 {
835 // m_angularMotorDirection // angular velocity requested by LSL motor 984 // The user wants how many radians per second angular change?
836 // m_angularMotorVelocity // current angular motor velocity (ramps up and down)
837 // m_angularMotorTimescale // motor angular velocity ramp up time
838 // m_angularMotorDecayTimescale // motor angular velocity decay rate
839 // m_angularFrictionTimescale // body angular velocity decay rate
840 // m_lastAngularVelocity // what was last applied to body
841
842 /*
843 if (m_angularMotorDirection.LengthSquared() > 0.0001)
844 {
845 Vector3 origVel = m_angularMotorVelocity;
846 Vector3 origDir = m_angularMotorDirection;
847
848 // new velocity += error / ( time to get there / step interval)
849 // requested direction - current vehicle direction
850 m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep);
851 // decay requested direction
852 m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
853
854 VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}",
855 Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
856 }
857 else
858 {
859 m_angularMotorVelocity = Vector3.Zero;
860 }
861 */
862
863 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep); 985 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
864 986
865 // ================================================================== 987 // ==================================================================
866 // NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement 988 // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
989 // This flag prevents linear deflection parallel to world z-axis. This is useful
990 // for preventing ground vehicles with large linear deflection, like bumper cars,
991 // from climbing their linear deflection into the sky.
992 // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
867 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) 993 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
868 { 994 {
869 angularMotorContribution.X = 0f; 995 angularMotorContribution.X = 0f;
870 angularMotorContribution.Y = 0f; 996 angularMotorContribution.Y = 0f;
871 VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution); 997 VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
872 } 998 }
873 999
874 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep); 1000 Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
875 1001
876 Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep); 1002 Vector3 deflectionContribution = ComputeAngularDeflection();
877 1003
878 Vector3 bankingContribution = ComputeAngularBanking(pTimestep); 1004 Vector3 bankingContribution = ComputeAngularBanking(angularMotorContribution.Z);
879 1005
880 // ================================================================== 1006 // ==================================================================
881 m_lastVertAttractor = verticalAttractionContribution; 1007 m_lastVertAttractor = verticalAttractionContribution;
882 1008
883 // Sum velocities 1009 // Sum corrections
884 m_lastAngularVelocity = angularMotorContribution 1010 m_lastAngularCorrection = angularMotorContribution
885 + verticalAttractionContribution 1011 + verticalAttractionContribution
886 + bankingContribution 1012 + deflectionContribution
887 + deflectionContribution; 1013 + bankingContribution;
1014
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 }
888 1036
889 // ================================================================== 1037 // ==================================================================
890 //Offset section 1038 //Offset section
@@ -914,52 +1062,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
914 torqueFromOffset.Z = 0; 1062 torqueFromOffset.Z = 0;
915 torqueFromOffset *= m_vehicleMass; 1063 torqueFromOffset *= m_vehicleMass;
916 Prim.ApplyTorqueImpulse(torqueFromOffset, true); 1064 Prim.ApplyTorqueImpulse(torqueFromOffset, true);
917 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); 1065 VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
918 } 1066 }
919 1067
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
928 {
929 // Apply to the body.
930 // The above calculates the absolute angular velocity needed. Angular velocity is massless.
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 } 1068 }
947 1069
948 public Vector3 ComputeAngularVerticalAttraction(float pTimestep) 1070 public Vector3 ComputeAngularVerticalAttraction()
949 { 1071 {
950 Vector3 ret = Vector3.Zero; 1072 Vector3 ret = Vector3.Zero;
951 1073
952 // 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...
953 if (m_verticalAttractionTimescale < 500) 1075 if (m_verticalAttractionTimescale < 500)
954 { 1076 {
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. 1077 // Take a vector pointing up and convert it from world to vehicle relative coords.
962 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; 1078 Vector3 verticalError = Vector3.UnitZ * VehicleOrientation;
963 verticalError.Normalize(); 1079 verticalError.Normalize();
964 1080
965 // 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)
@@ -977,63 +1093,70 @@ namespace OpenSim.Region.Physics.BulletSPlugin
977 } 1093 }
978 1094
979 // Y error means needed rotation around X axis and visa versa. 1095 // Y error means needed rotation around X axis and visa versa.
980 verticalAttractionContribution.X = verticalError.Y; 1096 ret.X = verticalError.Y;
981 verticalAttractionContribution.Y = - verticalError.X; 1097 ret.Y = - verticalError.X;
982 verticalAttractionContribution.Z = 0f; 1098 ret.Z = 0f;
983 1099
984 // scale by the time scale and timestep 1100 // scale by the time scale and timestep
985 Vector3 unscaledContrib = verticalAttractionContribution; 1101 Vector3 unscaledContrib = ret;
986 verticalAttractionContribution /= m_verticalAttractionTimescale; 1102 ret /= m_verticalAttractionTimescale;
987 verticalAttractionContribution *= pTimestep; 1103 // This returns the angular correction desired. Timestep is added later.
1104 // ret *= pTimestep;
988 1105
989 // apply efficiency 1106 // apply efficiency
990 Vector3 preEfficiencyContrib = verticalAttractionContribution; 1107 Vector3 preEfficiencyContrib = ret;
1108 // TODO: implement efficiency.
1109 // Effenciency squared seems to give a more realistic effect
991 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; 1110 float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
992 verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 1111 // ret *= efficencySquared;
993 1112
994 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}",
995 Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib, 1114 Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
996 m_verticalAttractionEfficiency, efficencySquared, 1115 m_verticalAttractionEfficiency, efficencySquared,
997 verticalAttractionContribution); 1116 ret);
998 */
999
1000 } 1117 }
1001 return ret; 1118 return ret;
1002 } 1119 }
1003 1120
1004 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()
1005 { 1124 {
1006 Vector3 ret = Vector3.Zero; 1125 Vector3 ret = Vector3.Zero;
1007 1126
1008 if (m_angularDeflectionEfficiency != 0) 1127 if (m_angularDeflectionEfficiency != 0)
1009 { 1128 {
1010 // Compute a scaled vector that points in the preferred axis (X direction) 1129 // Where the vehicle should want to point relative to the vehicle
1011 Vector3 scaledDefaultDirection = 1130 Vector3 preferredDirection = Vector3.UnitX * m_referenceFrame;
1012 new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
1013 // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
1014 // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
1015 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
1016 1131
1017 // Scale by efficiency and timescale 1132 // Where the vehicle is pointing relative to the vehicle.
1018 ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; 1133 Vector3 currentDirection = Vector3.UnitX * Quaternion.Add(VehicleOrientation, m_referenceFrame);
1019 1134
1020 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;
1021 1137
1022 // This deflection computation is not correct. 1138 // Scale the correction by recovery timescale and efficiency
1023 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);
1024 } 1143 }
1025 return ret; 1144 return ret;
1026 } 1145 }
1027 1146
1028 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)
1029 { 1151 {
1030 Vector3 ret = Vector3.Zero; 1152 Vector3 ret = Vector3.Zero;
1153 Vector3 computedBanking = Vector3.Zero;
1031 1154
1032 if (m_bankingEfficiency != 0) 1155 if (m_bankingEfficiency != 0)
1033 { 1156 {
1034 Vector3 dir = Vector3.One * Prim.ForceOrientation; 1157 Vector3 currentDirection = Vector3.UnitX * VehicleOrientation;
1158
1035 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);
1036 //Changes which way it banks in and out of turns
1037 1160
1038 //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
1039 float effSquared = (m_bankingEfficiency * m_bankingEfficiency); 1162 float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
@@ -1041,58 +1164,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1041 effSquared *= -1; //Keep the negative! 1164 effSquared *= -1; //Keep the negative!
1042 1165
1043 float mix = Math.Abs(m_bankingMix); 1166 float mix = Math.Abs(m_bankingMix);
1044 if (m_angularMotorVelocity.X == 0) 1167 // TODO: Must include reference frame.
1045 { 1168 float forwardSpeed = VehicleVelocity.X;
1046 // The vehicle is stopped
1047 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
1048 {
1049 Vector3 axisAngle;
1050 float angle;
1051 parent.Orientation.GetAxisAngle(out axisAngle, out angle);
1052 Vector3 rotatedVel = parent.Velocity * parent.Orientation;
1053 if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
1054 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
1055 else
1056 m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
1057 }*/
1058 }
1059 else
1060 {
1061 ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
1062 }
1063 1169
1064 //If they are colliding, we probably shouldn't shove the prim around... probably 1170 if (!Prim.IsColliding && forwardSpeed > mix)
1065 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
1066 { 1171 {
1067 float angVelZ = m_angularMotorVelocity.X * -1; 1172 computedBanking.X = ClampInRange(-3f, turningFactor * (effSquared * mult), 3f);
1068 /*if(angVelZ > mix)
1069 angVelZ = mix;
1070 else if(angVelZ < -mix)
1071 angVelZ = -mix;*/
1072 //This controls how fast and how far the banking occurs
1073 Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
1074 if (bankingRot.X > 3)
1075 bankingRot.X = 3;
1076 else if (bankingRot.X < -3)
1077 bankingRot.X = -3;
1078 bankingRot *= Prim.ForceOrientation;
1079 ret += bankingRot;
1080 } 1173 }
1081 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; 1174
1082 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.
1083 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);
1084 } 1183 }
1184 m_lastBanking = computedBanking;
1085 return ret; 1185 return ret;
1086 } 1186 }
1087 1187
1088
1089 // This is from previous instantiations of XXXDynamics.cs. 1188 // This is from previous instantiations of XXXDynamics.cs.
1090 // Applies roll reference frame. 1189 // Applies roll reference frame.
1091 // 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?
1092 // Should this be in MoveAngular()? 1191 // Should this be in MoveAngular()?
1093 internal void LimitRotation(float timestep) 1192 internal void LimitRotation(float timestep)
1094 { 1193 {
1095 Quaternion rotq = Prim.ForceOrientation; 1194 Quaternion rotq = VehicleOrientation;
1096 Quaternion m_rot = rotq; 1195 Quaternion m_rot = rotq;
1097 if (m_RollreferenceFrame != Quaternion.Identity) 1196 if (m_RollreferenceFrame != Quaternion.Identity)
1098 { 1197 {
@@ -1120,12 +1219,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1120 } 1219 }
1121 if (rotq != m_rot) 1220 if (rotq != m_rot)
1122 { 1221 {
1123 Prim.ForceOrientation = m_rot; 1222 VehicleOrientation = m_rot;
1124 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);
1125 } 1224 }
1126 1225
1127 } 1226 }
1128 1227
1228 private float ClampInRange(float low, float val, float high)
1229 {
1230 return Math.Max(low, Math.Min(val, high));
1231 }
1232
1129 // Invoke the detailed logger and output something if it's enabled. 1233 // Invoke the detailed logger and output something if it's enabled.
1130 private void VDetailLog(string msg, params Object[] args) 1234 private void VDetailLog(string msg, params Object[] args)
1131 { 1235 {