diff options
author | Robert Adams | 2012-11-25 19:06:53 -0800 |
---|---|---|
committer | Robert Adams | 2012-11-25 20:04:32 -0800 |
commit | 4c077a06947fe879fb02849f7ed7c4ec5358366f (patch) | |
tree | 8c0de9f17eeadbfdfb3a0d6e2b33b7d6ba1c385a /OpenSim/Region/Physics/BulletSPlugin | |
parent | BulletSim: add ToString override to BSVMotor. (diff) | |
download | opensim-SC-4c077a06947fe879fb02849f7ed7c4ec5358366f.zip opensim-SC-4c077a06947fe879fb02849f7ed7c4ec5358366f.tar.gz opensim-SC-4c077a06947fe879fb02849f7ed7c4ec5358366f.tar.bz2 opensim-SC-4c077a06947fe879fb02849f7ed7c4ec5358366f.tar.xz |
BulletSim: organize MoveLinear code for understandability. Make LIMIT_MOTOR_UP contribution a velocity and not a force.
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin')
-rw-r--r-- | OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 129 |
1 files changed, 40 insertions, 89 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index bf8a004..7757584 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | |||
@@ -84,7 +84,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
84 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time | 84 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time |
85 | private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center | 85 | private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center |
86 | private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL | 86 | private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL |
87 | private Vector3 m_newVelocity = Vector3.Zero; // velocity computed to be applied to body | ||
88 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; | 87 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; |
89 | private float m_linearMotorDecayTimescale = 0; | 88 | private float m_linearMotorDecayTimescale = 0; |
90 | private float m_linearMotorTimescale = 0; | 89 | private float m_linearMotorTimescale = 0; |
@@ -577,15 +576,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
577 | { | 576 | { |
578 | if (!IsActive) return; | 577 | if (!IsActive) return; |
579 | 578 | ||
580 | // DEBUG | ||
581 | // Because Bullet does apply forces to the vehicle, our last computed | ||
582 | // linear and angular velocities are not what is happening now. | ||
583 | // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity; | ||
584 | // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep; | ||
585 | // m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time | ||
586 | // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG: | ||
587 | // END DEBUG | ||
588 | |||
589 | MoveLinear(pTimestep); | 579 | MoveLinear(pTimestep); |
590 | // Commented out for debug | 580 | // Commented out for debug |
591 | MoveAngular(pTimestep); | 581 | MoveAngular(pTimestep); |
@@ -612,67 +602,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
612 | // Also does hover and float. | 602 | // Also does hover and float. |
613 | private void MoveLinear(float pTimestep) | 603 | private void MoveLinear(float pTimestep) |
614 | { | 604 | { |
615 | /* | 605 | Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep); |
616 | // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates | ||
617 | // m_lastLinearVelocityVector is the current speed we are moving in that direction | ||
618 | if (m_linearMotorDirection.LengthSquared() > 0.001f) | ||
619 | { | ||
620 | Vector3 origDir = m_linearMotorDirection; // DEBUG | ||
621 | Vector3 origVel = m_lastLinearVelocityVector; // DEBUG | ||
622 | // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison | ||
623 | Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG | ||
624 | |||
625 | // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete | ||
626 | Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep; | ||
627 | m_lastLinearVelocityVector += addAmount; | ||
628 | |||
629 | float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep; | ||
630 | m_linearMotorDirection *= (1f - decayFactor); | ||
631 | |||
632 | // Rotate new object velocity from vehicle relative to world coordinates | ||
633 | m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation; | ||
634 | |||
635 | // Apply friction for next time | ||
636 | Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep; | ||
637 | m_lastLinearVelocityVector *= (Vector3.One - frictionFactor); | ||
638 | |||
639 | VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}", | ||
640 | Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, | ||
641 | m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity); | ||
642 | } | ||
643 | else | ||
644 | { | ||
645 | // if what remains of direction is very small, zero it. | ||
646 | m_linearMotorDirection = Vector3.Zero; | ||
647 | m_lastLinearVelocityVector = Vector3.Zero; | ||
648 | m_newVelocity = Vector3.Zero; | ||
649 | |||
650 | VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); | ||
651 | } | ||
652 | */ | ||
653 | |||
654 | m_newVelocity = m_linearMotor.Step(pTimestep); | ||
655 | 606 | ||
656 | // Rotate new object velocity from vehicle relative to world coordinates | 607 | // Rotate new object velocity from vehicle relative to world coordinates |
657 | m_newVelocity *= Prim.ForceOrientation; | 608 | linearMotorContribution *= Prim.ForceOrientation; |
658 | |||
659 | // m_newVelocity is velocity computed from linear motor in world coordinates | ||
660 | 609 | ||
610 | // ================================================================== | ||
661 | // Gravity and Buoyancy | 611 | // Gravity and Buoyancy |
662 | // There is some gravity, make a gravity force vector that is applied after object velocity. | 612 | // There is some gravity, make a gravity force vector that is applied after object velocity. |
663 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | 613 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; |
664 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); | 614 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); |
665 | 615 | ||
666 | /* | 616 | // Current vehicle position |
667 | * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ... | ||
668 | // Preserve the current Z velocity | ||
669 | Vector3 vel_now = m_prim.Velocity; | ||
670 | m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity | ||
671 | */ | ||
672 | |||
673 | Vector3 pos = Prim.ForcePosition; | 617 | Vector3 pos = Prim.ForcePosition; |
674 | // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); | ||
675 | 618 | ||
619 | // ================================================================== | ||
620 | Vector3 terrainHeightContribution = Vector3.Zero; | ||
676 | // If below the terrain, move us above the ground a little. | 621 | // If below the terrain, move us above the ground a little. |
677 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); | 622 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); |
678 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. | 623 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. |
@@ -687,6 +632,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
687 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); | 632 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); |
688 | } | 633 | } |
689 | 634 | ||
635 | // ================================================================== | ||
636 | Vector3 hoverContribution = Vector3.Zero; | ||
690 | // Check if hovering | 637 | // Check if hovering |
691 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped | 638 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped |
692 | // m_VhoverTimescale: time to achieve height | 639 | // m_VhoverTimescale: time to achieve height |
@@ -726,24 +673,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
726 | // RA: where does the 50 come from? | 673 | // RA: where does the 50 come from? |
727 | float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); | 674 | float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); |
728 | // Replace Vertical speed with correction figure if significant | 675 | // Replace Vertical speed with correction figure if significant |
729 | if (Math.Abs(verticalError) > 0.01f) | 676 | if (verticalError > 0.01f) |
730 | { | 677 | { |
731 | m_newVelocity.Z += verticalCorrectionVelocity; | 678 | hoverContribution = new Vector3(0f, 0f, verticalCorrectionVelocity); |
732 | //KF: m_VhoverEfficiency is not yet implemented | 679 | //KF: m_VhoverEfficiency is not yet implemented |
733 | } | 680 | } |
734 | else if (verticalError < -0.01) | 681 | else if (verticalError < -0.01) |
735 | { | 682 | { |
736 | m_newVelocity.Z -= verticalCorrectionVelocity; | 683 | hoverContribution = new Vector3(0f, 0f, -verticalCorrectionVelocity); |
737 | } | ||
738 | else | ||
739 | { | ||
740 | m_newVelocity.Z = 0f; | ||
741 | } | 684 | } |
742 | } | 685 | } |
743 | 686 | ||
744 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight); | 687 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", |
688 | Prim.LocalID, pos, hoverContribution, m_VhoverHeight, m_VhoverTargetHeight); | ||
745 | } | 689 | } |
746 | 690 | ||
691 | // ================================================================== | ||
747 | Vector3 posChange = pos - m_lastPositionVector; | 692 | Vector3 posChange = pos - m_lastPositionVector; |
748 | if (m_BlockingEndPoint != Vector3.Zero) | 693 | if (m_BlockingEndPoint != Vector3.Zero) |
749 | { | 694 | { |
@@ -781,60 +726,66 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
781 | } | 726 | } |
782 | } | 727 | } |
783 | 728 | ||
784 | #region downForce | 729 | // ================================================================== |
785 | Vector3 downForce = Vector3.Zero; | 730 | Vector3 limitMotorUpContribution = Vector3.Zero; |
786 | |||
787 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) | 731 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) |
788 | { | 732 | { |
789 | // If the vehicle is motoring into the sky, get it going back down. | 733 | // If the vehicle is motoring into the sky, get it going back down. |
790 | // Is this an angular force or both linear and angular?? | ||
791 | float distanceAboveGround = pos.Z - terrainHeight; | 734 | float distanceAboveGround = pos.Z - terrainHeight; |
792 | if (distanceAboveGround > 2f) | 735 | if (distanceAboveGround > 1f) |
793 | { | 736 | { |
794 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); | 737 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); |
795 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); | 738 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); |
796 | downForce = new Vector3(0, 0, -distanceAboveGround); | 739 | limitMotorUpContribution = new Vector3(0, 0, -distanceAboveGround); |
797 | } | 740 | } |
798 | // TODO: this calculation is all wrong. From the description at | 741 | // TODO: this calculation is all wrong. From the description at |
799 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce | 742 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce |
800 | // has a decay factor. This says this force should | 743 | // has a decay factor. This says this force should |
801 | // be computed with a motor. | 744 | // be computed with a motor. |
802 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", | 745 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", |
803 | Prim.LocalID, distanceAboveGround, downForce); | 746 | Prim.LocalID, distanceAboveGround, limitMotorUpContribution); |
804 | } | 747 | } |
805 | #endregion // downForce | 748 | |
749 | // ================================================================== | ||
750 | Vector3 newVelocity = linearMotorContribution | ||
751 | + terrainHeightContribution | ||
752 | + hoverContribution | ||
753 | + limitMotorUpContribution; | ||
806 | 754 | ||
807 | // If not changing some axis, reduce out velocity | 755 | // If not changing some axis, reduce out velocity |
808 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | 756 | if ((m_flags & (VehicleFlag.NO_X)) != 0) |
809 | m_newVelocity.X = 0; | 757 | newVelocity.X = 0; |
810 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | 758 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) |
811 | m_newVelocity.Y = 0; | 759 | newVelocity.Y = 0; |
812 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | 760 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) |
813 | m_newVelocity.Z = 0; | 761 | newVelocity.Z = 0; |
814 | 762 | ||
763 | // ================================================================== | ||
815 | // Clamp REALLY high or low velocities | 764 | // Clamp REALLY high or low velocities |
816 | float newVelocityLengthSq = m_newVelocity.LengthSquared(); | 765 | float newVelocityLengthSq = newVelocity.LengthSquared(); |
817 | if (newVelocityLengthSq > 1e6f) | 766 | if (newVelocityLengthSq > 1e6f) |
818 | { | 767 | { |
819 | m_newVelocity /= m_newVelocity.Length(); | 768 | newVelocity /= newVelocity.Length(); |
820 | m_newVelocity *= 1000f; | 769 | newVelocity *= 1000f; |
821 | } | 770 | } |
822 | else if (newVelocityLengthSq < 1e-6f) | 771 | else if (newVelocityLengthSq < 1e-6f) |
823 | m_newVelocity = Vector3.Zero; | 772 | newVelocity = Vector3.Zero; |
824 | 773 | ||
774 | // ================================================================== | ||
825 | // Stuff new linear velocity into the vehicle | 775 | // Stuff new linear velocity into the vehicle |
826 | Prim.ForceVelocity = m_newVelocity; | 776 | Prim.ForceVelocity = newVelocity; |
827 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | 777 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG |
828 | 778 | ||
829 | Vector3 totalDownForce = downForce + grav; | 779 | // Other linear forces are applied as forces. |
780 | Vector3 totalDownForce = grav * m_vehicleMass; | ||
830 | if (totalDownForce != Vector3.Zero) | 781 | if (totalDownForce != Vector3.Zero) |
831 | { | 782 | { |
832 | Prim.AddForce(totalDownForce * m_vehicleMass, false); | 783 | Prim.AddForce(totalDownForce, false); |
833 | // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false); | ||
834 | } | 784 | } |
835 | 785 | ||
836 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", | 786 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", |
837 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce); | 787 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, |
788 | newVelocity, Prim.Velocity, totalDownForce); | ||
838 | 789 | ||
839 | } // end MoveLinear() | 790 | } // end MoveLinear() |
840 | 791 | ||