aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin
diff options
context:
space:
mode:
authorRobert Adams2012-11-25 19:06:53 -0800
committerRobert Adams2012-11-25 20:04:32 -0800
commit4c077a06947fe879fb02849f7ed7c4ec5358366f (patch)
tree8c0de9f17eeadbfdfb3a0d6e2b33b7d6ba1c385a /OpenSim/Region/Physics/BulletSPlugin
parentBulletSim: add ToString override to BSVMotor. (diff)
downloadopensim-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.cs129
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