aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs296
1 files changed, 72 insertions, 224 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
index 363cbef..6b323fb 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -629,36 +629,31 @@ namespace OpenSim.Region.Physics.OdePlugin
629 629
630 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame 630 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
631 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame 631 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame
632 d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame 632 d.Vector3 dtorque = new d.Vector3();
633
634 bool doathing = false;
635 633
636 // linear motor 634 // linear motor
637 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) 635 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
638 { 636 {
639 tmpV = m_linearMotorDirection - curLocalVel; // velocity error 637 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
640 if (tmpV.LengthSquared() > 1e-6f) 638 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
641 { 639 tmpV *= rotq; // to world
642 tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep
643 tmpV *= rotq; // to world
644 640
645 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) 641 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
646 tmpV.Z = 0; 642 tmpV.Z = 0;
647 643
648 if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0) 644 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
649 { 645 {
650 // have offset, do it now 646 // have offset, do it now
651 tmpV *= rootPrim.Mass; 647 tmpV *= rootPrim.Mass;
652 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); 648 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
653 }
654 else
655 {
656 force.X += tmpV.X;
657 force.Y += tmpV.Y;
658 force.Z += tmpV.Z;
659 }
660 } 649 }
661 m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale); 650 else
651 {
652 force.X += tmpV.X;
653 force.Y += tmpV.Y;
654 force.Z += tmpV.Z;
655 }
656 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
662 } 657 }
663 else 658 else
664 m_lmEfect = 0; 659 m_lmEfect = 0;
@@ -719,30 +714,35 @@ namespace OpenSim.Region.Physics.OdePlugin
719 if (m_linearDeflectionEfficiency > 0) 714 if (m_linearDeflectionEfficiency > 0)
720 { 715 {
721 float len = curVel.Length(); 716 float len = curVel.Length();
722 Vector3 atAxis = refAtAxis; 717 Vector3 atAxis;
723 atAxis *= rotq; // at axis rotated to world 718 atAxis = Xrot(rotq); // where are we pointing to
724 atAxis = Xrot(rotq); 719 atAxis *= len; // make it same size as world velocity vector
725 tmpV = atAxis * len; 720 tmpV = -atAxis; // oposite direction
726 tmpV -= curVel; // velocity error 721 atAxis -= curVel; // error to one direction
727 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep 722 len = atAxis.LengthSquared();
728 force.X += tmpV.X; 723 tmpV -= curVel; // error to oposite
729 force.Y += tmpV.Y; 724 float lens = tmpV.LengthSquared();
730 if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0) 725 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
731 force.Z += tmpV.Z; 726 {
727 if (len < lens)
728 tmpV = atAxis;
729
730 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
731 force.X += tmpV.X;
732 force.Y += tmpV.Y;
733 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
734 force.Z += tmpV.Z;
735 }
732 } 736 }
733 737
734 // angular motor 738 // angular motor
735 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) 739 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
736 { 740 {
737 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error 741 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
738 if (tmpV.LengthSquared() > 1e-6f) 742 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
739 { 743 torque.X += tmpV.X;
740 tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep 744 torque.Y += tmpV.Y;
741 tmpV *= m_referenceFrame; // to object 745 torque.Z += tmpV.Z;
742 dtorque.X += tmpV.X;
743 dtorque.Y += tmpV.Y;
744 dtorque.Z += tmpV.Z;
745 }
746 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); 746 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
747 } 747 }
748 else 748 else
@@ -751,85 +751,64 @@ namespace OpenSim.Region.Physics.OdePlugin
751 // angular friction 751 // angular friction
752 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) 752 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
753 { 753 {
754 tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X; 754 torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
755 tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y; 755 torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
756 tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z; 756 torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
757 tmpV *= m_referenceFrame; // to object
758 dtorque.X += tmpV.X;
759 dtorque.Y += tmpV.Y;
760 dtorque.Z += tmpV.Z;
761 } 757 }
762 758
763 // angular deflection 759 // angular deflection
764 if (m_angularDeflectionEfficiency > 0) 760 if (m_angularDeflectionEfficiency > 0)
765 { 761 {
766 doathing = false; 762 Vector3 dirv;
767 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE; 763
768 tmpV.X = 0; 764 if (curLocalVel.X > 0.01f)
769 if (Math.Abs(curLocalVel.Z) > 0.01) 765 dirv = curLocalVel;
766 else if (curLocalVel.X < -0.01f)
767 // use oposite
768 dirv = -curLocalVel;
769 else
770 { 770 {
771 tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp; 771 // make it fall into small positive x case
772 doathing = true; 772 dirv.X = 0.01f;
773 dirv.Y = curLocalVel.Y;
774 dirv.Z = curLocalVel.Z;
773 } 775 }
774 else 776
775 tmpV.Y = 0; 777 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
776 if (Math.Abs(curLocalVel.Y) > 0.01) 778
779 if (Math.Abs(dirv.Z) > 0.01)
777 { 780 {
778 tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp; 781 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
779 doathing = true;
780 } 782 }
781 else
782 tmpV.Z = 0;
783 783
784 if (doathing) 784 if (Math.Abs(dirv.Y) > 0.01)
785 { 785 {
786 tmpV *= m_referenceFrame; // to object 786 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
787 dtorque.X += tmpV.X;
788 dtorque.Y += tmpV.Y;
789 dtorque.Z += tmpV.Z;
790 } 787 }
791 } 788 }
792 789
793 // vertical atractor 790 // vertical atractor
794 if (m_verticalAttractionTimescale < 300) 791 if (m_verticalAttractionTimescale < 300)
795 { 792 {
796 doathing = false;
797 float roll; 793 float roll;
798 float pitch; 794 float pitch;
799 795
800 GetRollPitch(rotq, out roll, out pitch); 796 GetRollPitch(rotq, out roll, out pitch);
801 797
802
803 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; 798 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
804 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; 799 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
805 800
806 if (Math.Abs(roll) > 0.01) // roll 801 if (Math.Abs(roll) > 0.01) // roll
807 { 802 {
808 tmpV.X = -roll * ftmp; 803 torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2;
809 tmpV.X -= curLocalAngVel.X * ftmp2;
810 doathing = true;
811 }
812 else
813 {
814 tmpV.X = 0;
815 } 804 }
816 805
817 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch 806 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
818 { 807 {
819 tmpV.Y = -pitch * ftmp; 808 torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2;
820 tmpV.Y -= curLocalAngVel.Y * ftmp2;
821 doathing = true;
822 }
823 else
824 {
825 tmpV.Y = 0;
826 } 809 }
827 810
828 tmpV.Z = 0; 811 if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01)
829
830 if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01)
831 tmpV.Z = 0;
832 else
833 { 812 {
834 float broll = -roll * m_bankingEfficiency; ; 813 float broll = -roll * m_bankingEfficiency; ;
835 if (m_bankingMix != 0) 814 if (m_bankingMix != 0)
@@ -839,146 +818,10 @@ namespace OpenSim.Region.Physics.OdePlugin
839 broll *= ((1 - m_bankingMix) + vfact); 818 broll *= ((1 - m_bankingMix) + vfact);
840 } 819 }
841 820
842 tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale; 821 torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale;
843 doathing = true;
844 }
845
846 if (doathing)
847 {
848
849 tmpV *= m_referenceFrame; // to object
850 dtorque.X += tmpV.X;
851 dtorque.Y += tmpV.Y;
852 dtorque.Z += tmpV.Z;
853 } 822 }
854 } 823 }
855 /*
856 d.Vector3 pos = d.BodyGetPosition(Body);
857 // 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);
858 Vector3 posChange = new Vector3();
859 posChange.X = pos.X - m_lastPositionVector.X;
860 posChange.Y = pos.Y - m_lastPositionVector.Y;
861 posChange.Z = pos.Z - m_lastPositionVector.Z;
862 double Zchange = Math.Abs(posChange.Z);
863 if (m_BlockingEndPoint != Vector3.Zero)
864 {
865 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
866 {
867 pos.X -= posChange.X + 1;
868 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
869 }
870 if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
871 {
872 pos.Y -= posChange.Y + 1;
873 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
874 }
875 if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
876 {
877 pos.Z -= posChange.Z + 1;
878 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
879 }
880 if (pos.X <= 0)
881 {
882 pos.X += posChange.X + 1;
883 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
884 }
885 if (pos.Y <= 0)
886 {
887 pos.Y += posChange.Y + 1;
888 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
889 }
890 }
891 if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
892 {
893 pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
894 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
895 }
896
897 }
898 if ((m_flags & (VehicleFlag.NO_X)) != 0)
899 {
900 m_dir.X = 0;
901 }
902 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
903 {
904 m_dir.Y = 0;
905 }
906 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
907 {
908 m_dir.Z = 0;
909 }
910
911
912 */
913 // angular part
914 /*
915
916 // Get what the body is doing, this includes 'external' influences
917/*
918 Vector3 angularVelocity = Vector3.Zero;
919
920 // Vertical attractor section
921 Vector3 vertattr = Vector3.Zero;
922
923 if (m_verticalAttractionTimescale < 300)
924 {
925 float VAservo = 0.2f / m_verticalAttractionTimescale;
926 // get present body rotation
927 // make a vector pointing up
928 Vector3 verterr = Vector3.Zero;
929 verterr.Z = 1.0f;
930 // rotate it to Body Angle
931 verterr = verterr * rotq;
932 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
933 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
934 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
935 if (verterr.Z < 0.0f)
936 {
937 verterr.X = 2.0f - verterr.X;
938 verterr.Y = 2.0f - verterr.Y;
939 }
940 // Error is 0 (no error) to +/- 2 (max error)
941 // scale it by VAservo
942 verterr = verterr * VAservo;
943//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
944
945 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
946 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
947 vertattr.X = verterr.Y;
948 vertattr.Y = - verterr.X;
949 vertattr.Z = 0f;
950
951 // scaling appears better usingsquare-law
952 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
953 vertattr.X += bounce * angularVelocity.X;
954 vertattr.Y += bounce * angularVelocity.Y;
955
956 } // else vertical attractor is off
957
958 // m_lastVertAttractor = vertattr;
959
960 // Bank section tba
961 // Deflection section tba
962
963 // Sum velocities
964 m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection
965 824
966 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
967 {
968 m_lastAngularVelocity.X = 0;
969 m_lastAngularVelocity.Y = 0;
970 }
971
972 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
973 {
974 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
975 }
976 else
977 {
978 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
979 }
980*/
981
982 d.Mass dmass; 825 d.Mass dmass;
983 d.BodyGetMass(Body,out dmass); 826 d.BodyGetMass(Body,out dmass);
984 827
@@ -988,8 +831,13 @@ namespace OpenSim.Region.Physics.OdePlugin
988 d.BodySetForce(Body, force.X, force.Y, force.Z); 831 d.BodySetForce(Body, force.X, force.Y, force.Z);
989 } 832 }
990 833
991 if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0) 834 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
992 { 835 {
836 torque *= m_referenceFrame; // to object frame
837 dtorque.X = torque.X;
838 dtorque.Y = torque.Y;
839 dtorque.Z = torque.Z;
840
993 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); 841 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
994 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame 842 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
995 } 843 }
generated by cgit v1.1 at 2024-12-26 15:06:19 +0000