diff options
author | Robert Adams | 2012-11-27 10:02:29 -0800 |
---|---|---|
committer | Robert Adams | 2012-11-27 10:04:00 -0800 |
commit | 8e459a03467ffa45145f90ea764854deaf2615ed (patch) | |
tree | d60781c77f6d15bcf1de05aab8c5126855eaccc8 /OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | |
parent | BulletSim: fix terrain mesh generation for problem with regions that have une... (diff) | |
download | opensim-SC_OLD-8e459a03467ffa45145f90ea764854deaf2615ed.zip opensim-SC_OLD-8e459a03467ffa45145f90ea764854deaf2615ed.tar.gz opensim-SC_OLD-8e459a03467ffa45145f90ea764854deaf2615ed.tar.bz2 opensim-SC_OLD-8e459a03467ffa45145f90ea764854deaf2615ed.tar.xz |
BulletSim: reorganize linear movement routine into separate subroutines enabling external calibration routines and unit tests.
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 137 |
1 files changed, 79 insertions, 58 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index eb4d06a..74eb9ab 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | |||
@@ -630,13 +630,64 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
630 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | 630 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; |
631 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); | 631 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); |
632 | 632 | ||
633 | // Current vehicle position | ||
634 | Vector3 pos = Prim.ForcePosition; | 633 | Vector3 pos = Prim.ForcePosition; |
634 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); | ||
635 | |||
636 | Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight); | ||
637 | |||
638 | Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight); | ||
639 | |||
640 | ComputeLinearBlockingEndPoint(pTimestep, ref pos); | ||
641 | |||
642 | Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight); | ||
643 | |||
644 | // ================================================================== | ||
645 | Vector3 newVelocity = linearMotorContribution | ||
646 | + terrainHeightContribution | ||
647 | + hoverContribution | ||
648 | + limitMotorUpContribution; | ||
649 | |||
650 | // If not changing some axis, reduce out velocity | ||
651 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | ||
652 | newVelocity.X = 0; | ||
653 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | ||
654 | newVelocity.Y = 0; | ||
655 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
656 | newVelocity.Z = 0; | ||
657 | |||
658 | // ================================================================== | ||
659 | // Clamp REALLY high or low velocities | ||
660 | float newVelocityLengthSq = newVelocity.LengthSquared(); | ||
661 | if (newVelocityLengthSq > 1e6f) | ||
662 | { | ||
663 | newVelocity /= newVelocity.Length(); | ||
664 | newVelocity *= 1000f; | ||
665 | } | ||
666 | else if (newVelocityLengthSq < 1e-6f) | ||
667 | newVelocity = Vector3.Zero; | ||
635 | 668 | ||
636 | // ================================================================== | 669 | // ================================================================== |
637 | Vector3 terrainHeightContribution = Vector3.Zero; | 670 | // Stuff new linear velocity into the vehicle |
671 | Prim.ForceVelocity = newVelocity; | ||
672 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | ||
673 | |||
674 | // Other linear forces are applied as forces. | ||
675 | Vector3 totalDownForce = grav * m_vehicleMass; | ||
676 | if (totalDownForce != Vector3.Zero) | ||
677 | { | ||
678 | Prim.AddForce(totalDownForce, false); | ||
679 | } | ||
680 | |||
681 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", | ||
682 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, | ||
683 | newVelocity, Prim.Velocity, totalDownForce); | ||
684 | |||
685 | } // end MoveLinear() | ||
686 | |||
687 | public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight) | ||
688 | { | ||
689 | Vector3 ret = Vector3.Zero; | ||
638 | // If below the terrain, move us above the ground a little. | 690 | // If below the terrain, move us above the ground a little. |
639 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); | ||
640 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. | 691 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. |
641 | // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. | 692 | // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. |
642 | // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; | 693 | // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; |
@@ -648,10 +699,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
648 | Prim.ForcePosition = pos; | 699 | Prim.ForcePosition = pos; |
649 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); | 700 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); |
650 | } | 701 | } |
702 | return ret; | ||
703 | } | ||
704 | |||
705 | public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight) | ||
706 | { | ||
707 | Vector3 ret = Vector3.Zero; | ||
651 | 708 | ||
652 | // ================================================================== | ||
653 | Vector3 hoverContribution = Vector3.Zero; | ||
654 | // Check if hovering | ||
655 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped | 709 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped |
656 | // m_VhoverTimescale: time to achieve height | 710 | // m_VhoverTimescale: time to achieve height |
657 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | 711 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) |
@@ -692,24 +746,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
692 | // Replace Vertical speed with correction figure if significant | 746 | // Replace Vertical speed with correction figure if significant |
693 | if (verticalError > 0.01f) | 747 | if (verticalError > 0.01f) |
694 | { | 748 | { |
695 | hoverContribution = new Vector3(0f, 0f, verticalCorrectionVelocity); | 749 | ret = new Vector3(0f, 0f, verticalCorrectionVelocity); |
696 | //KF: m_VhoverEfficiency is not yet implemented | 750 | //KF: m_VhoverEfficiency is not yet implemented |
697 | } | 751 | } |
698 | else if (verticalError < -0.01) | 752 | else if (verticalError < -0.01) |
699 | { | 753 | { |
700 | hoverContribution = new Vector3(0f, 0f, -verticalCorrectionVelocity); | 754 | ret = new Vector3(0f, 0f, -verticalCorrectionVelocity); |
701 | } | 755 | } |
702 | } | 756 | } |
703 | 757 | ||
704 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", | 758 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", |
705 | Prim.LocalID, pos, hoverContribution, m_VhoverHeight, m_VhoverTargetHeight); | 759 | Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight); |
706 | } | 760 | } |
707 | 761 | ||
708 | // ================================================================== | 762 | return ret; |
763 | } | ||
764 | |||
765 | public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos) | ||
766 | { | ||
767 | bool changed = false; | ||
768 | |||
709 | Vector3 posChange = pos - m_lastPositionVector; | 769 | Vector3 posChange = pos - m_lastPositionVector; |
710 | if (m_BlockingEndPoint != Vector3.Zero) | 770 | if (m_BlockingEndPoint != Vector3.Zero) |
711 | { | 771 | { |
712 | bool changed = false; | ||
713 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) | 772 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) |
714 | { | 773 | { |
715 | pos.X -= posChange.X + 1; | 774 | pos.X -= posChange.X + 1; |
@@ -742,9 +801,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
742 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); | 801 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); |
743 | } | 802 | } |
744 | } | 803 | } |
804 | return changed; | ||
805 | } | ||
745 | 806 | ||
746 | // ================================================================== | 807 | public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight) |
747 | Vector3 limitMotorUpContribution = Vector3.Zero; | 808 | { |
809 | Vector3 ret = Vector3.Zero; | ||
748 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) | 810 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) |
749 | { | 811 | { |
750 | // If the vehicle is motoring into the sky, get it going back down. | 812 | // If the vehicle is motoring into the sky, get it going back down. |
@@ -753,58 +815,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
753 | { | 815 | { |
754 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); | 816 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); |
755 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); | 817 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); |
756 | limitMotorUpContribution = new Vector3(0, 0, -distanceAboveGround); | 818 | ret = new Vector3(0, 0, -distanceAboveGround); |
757 | } | 819 | } |
758 | // TODO: this calculation is all wrong. From the description at | 820 | // TODO: this calculation is all wrong. From the description at |
759 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce | 821 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce |
760 | // has a decay factor. This says this force should | 822 | // has a decay factor. This says this force should |
761 | // be computed with a motor. | 823 | // be computed with a motor. |
762 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", | 824 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", |
763 | Prim.LocalID, distanceAboveGround, limitMotorUpContribution); | 825 | Prim.LocalID, distanceAboveGround, ret); |
764 | } | 826 | } |
765 | 827 | return ret; | |
766 | // ================================================================== | 828 | } |
767 | Vector3 newVelocity = linearMotorContribution | ||
768 | + terrainHeightContribution | ||
769 | + hoverContribution | ||
770 | + limitMotorUpContribution; | ||
771 | |||
772 | // If not changing some axis, reduce out velocity | ||
773 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | ||
774 | newVelocity.X = 0; | ||
775 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | ||
776 | newVelocity.Y = 0; | ||
777 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
778 | newVelocity.Z = 0; | ||
779 | |||
780 | // ================================================================== | ||
781 | // Clamp REALLY high or low velocities | ||
782 | float newVelocityLengthSq = newVelocity.LengthSquared(); | ||
783 | if (newVelocityLengthSq > 1e6f) | ||
784 | { | ||
785 | newVelocity /= newVelocity.Length(); | ||
786 | newVelocity *= 1000f; | ||
787 | } | ||
788 | else if (newVelocityLengthSq < 1e-6f) | ||
789 | newVelocity = Vector3.Zero; | ||
790 | |||
791 | // ================================================================== | ||
792 | // Stuff new linear velocity into the vehicle | ||
793 | Prim.ForceVelocity = newVelocity; | ||
794 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | ||
795 | |||
796 | // Other linear forces are applied as forces. | ||
797 | Vector3 totalDownForce = grav * m_vehicleMass; | ||
798 | if (totalDownForce != Vector3.Zero) | ||
799 | { | ||
800 | Prim.AddForce(totalDownForce, false); | ||
801 | } | ||
802 | |||
803 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", | ||
804 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, | ||
805 | newVelocity, Prim.Velocity, totalDownForce); | ||
806 | |||
807 | } // end MoveLinear() | ||
808 | 829 | ||
809 | // ======================================================================= | 830 | // ======================================================================= |
810 | // ======================================================================= | 831 | // ======================================================================= |