aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs1114
1 files changed, 600 insertions, 514 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index fbe52c8..5f802d0 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -64,11 +64,16 @@ namespace OpenSim.Region.Physics.OdePlugin
64 private Vector3 m_taintVelocity; 64 private Vector3 m_taintVelocity;
65 private Vector3 m_taintTorque; 65 private Vector3 m_taintTorque;
66 private Quaternion m_taintrot; 66 private Quaternion m_taintrot;
67 private Vector3 m_angularEnable = Vector3.One; // Current setting 67 private Vector3 m_rotateEnable = Vector3.One; // Current setting
68 private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL 68 private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL
69 69 private bool m_rotateEnableUpdate = false;
70 70 private Vector3 m_lockX;
71 private Vector3 m_lockY;
72 private Vector3 m_lockZ;
71 private IntPtr Amotor = IntPtr.Zero; 73 private IntPtr Amotor = IntPtr.Zero;
74 private IntPtr AmotorX = IntPtr.Zero;
75 private IntPtr AmotorY = IntPtr.Zero;
76 private IntPtr AmotorZ = IntPtr.Zero;
72 77
73 private Vector3 m_PIDTarget; 78 private Vector3 m_PIDTarget;
74 private float m_PIDTau; 79 private float m_PIDTau;
@@ -80,6 +85,7 @@ namespace OpenSim.Region.Physics.OdePlugin
80 private float m_APIDStrength = 0.5f; 85 private float m_APIDStrength = 0.5f;
81 private float m_APIDDamping = 0.5f; 86 private float m_APIDDamping = 0.5f;
82 private bool m_useAPID = false; 87 private bool m_useAPID = false;
88 private float m_APIDdamper = 1.0f;
83 89
84 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 90 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
85 // do not confuse with VEHICLE HOVER 91 // do not confuse with VEHICLE HOVER
@@ -126,6 +132,7 @@ namespace OpenSim.Region.Physics.OdePlugin
126 //public GCHandle gc; 132 //public GCHandle gc;
127 private CollisionLocker ode; 133 private CollisionLocker ode;
128 134
135 private bool m_meshfailed = false;
129 private bool m_taintforce = false; 136 private bool m_taintforce = false;
130 private bool m_taintaddangularforce = false; 137 private bool m_taintaddangularforce = false;
131 private Vector3 m_force; 138 private Vector3 m_force;
@@ -137,7 +144,7 @@ namespace OpenSim.Region.Physics.OdePlugin
137 private OdeScene _parent_scene; 144 private OdeScene _parent_scene;
138 public IntPtr m_targetSpace = IntPtr.Zero; 145 public IntPtr m_targetSpace = IntPtr.Zero;
139 public IntPtr prim_geom; 146 public IntPtr prim_geom;
140 public IntPtr prev_geom; 147// public IntPtr prev_geom;
141 public IntPtr _triMeshData; 148 public IntPtr _triMeshData;
142 149
143 private IntPtr _linkJointGroup = IntPtr.Zero; 150 private IntPtr _linkJointGroup = IntPtr.Zero;
@@ -249,8 +256,9 @@ namespace OpenSim.Region.Physics.OdePlugin
249 256
250 257
251 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 258 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
252 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 259 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
253 { 260 {
261 m_localID = localid;
254 ode = dode; 262 ode = dode;
255 if (!pos.IsFinite()) 263 if (!pos.IsFinite())
256 { 264 {
@@ -269,7 +277,7 @@ namespace OpenSim.Region.Physics.OdePlugin
269 277
270 278
271 prim_geom = IntPtr.Zero; 279 prim_geom = IntPtr.Zero;
272 prev_geom = IntPtr.Zero; 280// prev_geom = IntPtr.Zero;
273 281
274 if (!pos.IsFinite()) 282 if (!pos.IsFinite())
275 { 283 {
@@ -538,6 +546,8 @@ namespace OpenSim.Region.Physics.OdePlugin
538 if (value.IsFinite()) 546 if (value.IsFinite())
539 { 547 {
540 _velocity = value; 548 _velocity = value;
549 if (_velocity.ApproxEquals(Vector3.Zero,0.001f))
550 _acceleration = Vector3.Zero;
541 551
542 m_taintVelocity = value; 552 m_taintVelocity = value;
543 _parent_scene.AddPhysicsActorTaint(this); 553 _parent_scene.AddPhysicsActorTaint(this);
@@ -655,7 +665,14 @@ namespace OpenSim.Region.Physics.OdePlugin
655 665
656 public override Vector3 Acceleration // client updates read data via here 666 public override Vector3 Acceleration // client updates read data via here
657 { 667 {
658 get { return _acceleration; } 668 get
669 {
670 if (_zeroFlag)
671 {
672 return Vector3.Zero;
673 }
674 return _acceleration;
675 }
659 } 676 }
660 677
661 678
@@ -744,14 +761,18 @@ namespace OpenSim.Region.Physics.OdePlugin
744 761
745 public override void LockAngularMotion(Vector3 axis) 762 public override void LockAngularMotion(Vector3 axis)
746 { 763 {
747 // reverse the zero/non zero values for ODE. 764 // This is actually ROTATION ENABLE, not a lock.
765 // default is <1,1,1> which is all enabled.
766 // The lock value is updated inside Move(), no point in using the taint system.
767 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
748 if (axis.IsFinite()) 768 if (axis.IsFinite())
749 { 769 {
750 axis.X = (axis.X > 0) ? 1f : 0f; 770 axis.X = (axis.X > 0) ? 1f : 0f;
751 axis.Y = (axis.Y > 0) ? 1f : 0f; 771 axis.Y = (axis.Y > 0) ? 1f : 0f;
752 axis.Z = (axis.Z > 0) ? 1f : 0f; 772 axis.Z = (axis.Z > 0) ? 1f : 0f;
753 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); 773 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
754 m_taintAngularLock = axis; 774 m_rotateEnableRequest = axis;
775 m_rotateEnableUpdate = true;
755 } 776 }
756 else 777 else
757 { 778 {
@@ -762,13 +783,26 @@ namespace OpenSim.Region.Physics.OdePlugin
762 783
763 public void SetGeom(IntPtr geom) 784 public void SetGeom(IntPtr geom)
764 { 785 {
765 prev_geom = prim_geom; 786 if(prim_geom != IntPtr.Zero)
787 {
788 // Remove any old entries
789//string tPA;
790//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
791//Console.WriteLine("**** Remove {0}", tPA);
792 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
793 if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
794 d.GeomDestroy(prim_geom);
795 }
796
766 prim_geom = geom; 797 prim_geom = geom;
767//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 798//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
768 if (prim_geom != IntPtr.Zero) 799 if (prim_geom != IntPtr.Zero)
769 { 800 {
801 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
802 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
770 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 803 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
771 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 804 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
805//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
772 } 806 }
773 807
774 if (childPrim) 808 if (childPrim)
@@ -856,300 +890,285 @@ namespace OpenSim.Region.Physics.OdePlugin
856 890
857 private float CalculateMass() 891 private float CalculateMass()
858 { 892 {
859 float volume = 0; 893 float volume = _size.X * _size.Y * _size.Z; // default
860 894 float tmp;
861 // No material is passed to the physics engines yet.. soo..
862 // we're using the m_density constant in the class definition
863 895
864 float returnMass = 0; 896 float returnMass = 0;
865 897 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
898 float hollowVolume = hollowAmount * hollowAmount;
899
866 switch (_pbs.ProfileShape) 900 switch (_pbs.ProfileShape)
867 { 901 {
868 case ProfileShape.Square: 902 case ProfileShape.Square:
869 // Profile Volume 903 // default box
870 904
871 volume = _size.X*_size.Y*_size.Z; 905 if (_pbs.PathCurve == (byte)Extrusion.Straight)
906 {
907 if (hollowAmount > 0.0)
908 {
909 switch (_pbs.HollowShape)
910 {
911 case HollowShape.Square:
912 case HollowShape.Same:
913 break;
872 914
873 // If the user has 'hollowed out' 915 case HollowShape.Circle:
874 // ProfileHollow is one of those 0 to 50000 values :P
875 // we like percentages better.. so turning into a percentage
876 916
877 if (((float) _pbs.ProfileHollow/50000f) > 0.0) 917 hollowVolume *= 0.78539816339f;
878 { 918 break;
879 float hollowAmount = (float) _pbs.ProfileHollow/50000f; 919
920 case HollowShape.Triangle:
921
922 hollowVolume *= (0.5f * .5f);
923 break;
880 924
881 // calculate the hollow volume by it's shape compared to the prim shape 925 default:
882 float hollowVolume = 0; 926 hollowVolume = 0;
883 switch (_pbs.HollowShape) 927 break;
928 }
929 volume *= (1.0f - hollowVolume);
930 }
931 }
932
933 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
884 { 934 {
885 case HollowShape.Square: 935 //a tube
886 case HollowShape.Same: 936
887 // Cube Hollow volume calculation 937 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
888 float hollowsizex = _size.X*hollowAmount; 938 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
889 float hollowsizey = _size.Y*hollowAmount; 939 volume -= volume*tmp*tmp;
890 float hollowsizez = _size.Z*hollowAmount; 940
891 hollowVolume = hollowsizex*hollowsizey*hollowsizez; 941 if (hollowAmount > 0.0)
892 break; 942 {
893 943 hollowVolume *= hollowAmount;
894 case HollowShape.Circle: 944
895 // Hollow shape is a perfect cyllinder in respect to the cube's scale 945 switch (_pbs.HollowShape)
896 // Cyllinder hollow volume calculation 946 {
897 float hRadius = _size.X/2; 947 case HollowShape.Square:
898 float hLength = _size.Z; 948 case HollowShape.Same:
899 949 break;
900 // pi * r2 * h 950
901 hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount); 951 case HollowShape.Circle:
902 break; 952 hollowVolume *= 0.78539816339f;;
903 953 break;
904 case HollowShape.Triangle: 954
905 // Equilateral Triangular Prism volume hollow calculation 955 case HollowShape.Triangle:
906 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y 956 hollowVolume *= 0.5f * 0.5f;
907 957 break;
908 float aLength = _size.Y; 958 default:
909 // 1/2 abh 959 hollowVolume = 0;
910 hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount); 960 break;
911 break; 961 }
912 962 volume *= (1.0f - hollowVolume);
913 default: 963 }
914 hollowVolume = 0;
915 break;
916 } 964 }
917 volume = volume - hollowVolume;
918 }
919 965
920 break; 966 break;
967
921 case ProfileShape.Circle: 968 case ProfileShape.Circle:
922 if (_pbs.PathCurve == (byte)Extrusion.Straight)
923 {
924 // Cylinder
925 float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
926 float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
927 969
928 // Approximating the cylinder's irregularity. 970 if (_pbs.PathCurve == (byte)Extrusion.Straight)
929 if (volume1 > volume2)
930 {
931 volume = (float)volume1 - (volume1 - volume2);
932 }
933 else if (volume2 > volume1)
934 { 971 {
935 volume = (float)volume2 - (volume2 - volume1); 972 volume *= 0.78539816339f; // elipse base
973
974 if (hollowAmount > 0.0)
975 {
976 switch (_pbs.HollowShape)
977 {
978 case HollowShape.Same:
979 case HollowShape.Circle:
980 break;
981
982 case HollowShape.Square:
983 hollowVolume *= 0.5f * 2.5984480504799f;
984 break;
985
986 case HollowShape.Triangle:
987 hollowVolume *= .5f * 1.27323954473516f;
988 break;
989
990 default:
991 hollowVolume = 0;
992 break;
993 }
994 volume *= (1.0f - hollowVolume);
995 }
936 } 996 }
937 else 997
998 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
938 { 999 {
939 // Regular cylinder 1000 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
940 volume = volume1; 1001 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
941 } 1002 volume *= (1.0f - tmp * tmp);
942 } 1003
943 else 1004 if (hollowAmount > 0.0)
944 { 1005 {
945 // We don't know what the shape is yet, so use default
946 volume = _size.X * _size.Y * _size.Z;
947 }
948 // If the user has 'hollowed out'
949 // ProfileHollow is one of those 0 to 50000 values :P
950 // we like percentages better.. so turning into a percentage
951 1006
952 if (((float)_pbs.ProfileHollow / 50000f) > 0.0) 1007 // calculate the hollow volume by it's shape compared to the prim shape
953 { 1008 hollowVolume *= hollowAmount;
954 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
955 1009
956 // calculate the hollow volume by it's shape compared to the prim shape 1010 switch (_pbs.HollowShape)
957 float hollowVolume = 0; 1011 {
958 switch (_pbs.HollowShape) 1012 case HollowShape.Same:
959 { 1013 case HollowShape.Circle:
960 case HollowShape.Same: 1014 break;
961 case HollowShape.Circle: 1015
962 // Hollow shape is a perfect cyllinder in respect to the cube's scale 1016 case HollowShape.Square:
963 // Cyllinder hollow volume calculation 1017 hollowVolume *= 0.5f * 2.5984480504799f;
964 float hRadius = _size.X / 2; 1018 break;
965 float hLength = _size.Z; 1019
966 1020 case HollowShape.Triangle:
967 // pi * r2 * h 1021 hollowVolume *= .5f * 1.27323954473516f;
968 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); 1022 break;
969 break; 1023
970 1024 default:
971 case HollowShape.Square: 1025 hollowVolume = 0;
972 // Cube Hollow volume calculation 1026 break;
973 float hollowsizex = _size.X * hollowAmount; 1027 }
974 float hollowsizey = _size.Y * hollowAmount; 1028 volume *= (1.0f - hollowVolume);
975 float hollowsizez = _size.Z * hollowAmount; 1029 }
976 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
977 break;
978
979 case HollowShape.Triangle:
980 // Equilateral Triangular Prism volume hollow calculation
981 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
982
983 float aLength = _size.Y;
984 // 1/2 abh
985 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
986 break;
987
988 default:
989 hollowVolume = 0;
990 break;
991 } 1030 }
992 volume = volume - hollowVolume;
993 }
994 break; 1031 break;
995 1032
996 case ProfileShape.HalfCircle: 1033 case ProfileShape.HalfCircle:
997 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1034 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
998 { 1035 {
999 if (_size.X == _size.Y && _size.Y == _size.Z) 1036 volume *= 0.52359877559829887307710723054658f;
1000 {
1001 // regular sphere
1002 // v = 4/3 * pi * r^3
1003 float sradius3 = (float)Math.Pow((_size.X / 2), 3);
1004 volume = (float)((4f / 3f) * Math.PI * sradius3);
1005 }
1006 else
1007 {
1008 // we treat this as a box currently
1009 volume = _size.X * _size.Y * _size.Z;
1010 }
1011 }
1012 else
1013 {
1014 // We don't know what the shape is yet, so use default
1015 volume = _size.X * _size.Y * _size.Z;
1016 } 1037 }
1017 break; 1038 break;
1018 1039
1019 case ProfileShape.EquilateralTriangle: 1040 case ProfileShape.EquilateralTriangle:
1020 /*
1021 v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
1022 1041
1023 // seed mesh 1042 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1024 Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); 1043 {
1025 Vertex PM = new Vertex(+0.5f, 0f, 0.0f); 1044 volume *= 0.32475953f;
1026 Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); 1045
1027 */ 1046 if (hollowAmount > 0.0)
1028 float xA = -0.25f * _size.X; 1047 {
1029 float yA = -0.45f * _size.Y;
1030 1048
1031 float xB = 0.5f * _size.X; 1049 // calculate the hollow volume by it's shape compared to the prim shape
1032 float yB = 0; 1050 switch (_pbs.HollowShape)
1051 {
1052 case HollowShape.Same:
1053 case HollowShape.Triangle:
1054 hollowVolume *= .25f;
1055 break;
1033 1056
1034 float xC = -0.25f * _size.X; 1057 case HollowShape.Square:
1035 float yC = 0.45f * _size.Y; 1058 hollowVolume *= 0.499849f * 3.07920140172638f;
1059 break;
1036 1060
1037 volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); 1061 case HollowShape.Circle:
1062 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1063 // Cyllinder hollow volume calculation
1038 1064
1039 // If the user has 'hollowed out' 1065 hollowVolume *= 0.1963495f * 3.07920140172638f;
1040 // ProfileHollow is one of those 0 to 50000 values :P 1066 break;
1041 // we like percentages better.. so turning into a percentage
1042 float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
1043 if (((float)fhollowFactor / 50000f) > 0.0)
1044 {
1045 float hollowAmount = (float)fhollowFactor / 50000f;
1046 1067
1047 // calculate the hollow volume by it's shape compared to the prim shape 1068 default:
1048 float hollowVolume = 0; 1069 hollowVolume = 0;
1049 switch (_pbs.HollowShape) 1070 break;
1050 { 1071 }
1051 case HollowShape.Same: 1072 volume *= (1.0f - hollowVolume);
1052 case HollowShape.Triangle: 1073 }
1053 // Equilateral Triangular Prism volume hollow calculation
1054 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1055
1056 float aLength = _size.Y;
1057 // 1/2 abh
1058 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1059 break;
1060
1061 case HollowShape.Square:
1062 // Cube Hollow volume calculation
1063 float hollowsizex = _size.X * hollowAmount;
1064 float hollowsizey = _size.Y * hollowAmount;
1065 float hollowsizez = _size.Z * hollowAmount;
1066 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1067 break;
1068
1069 case HollowShape.Circle:
1070 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1071 // Cyllinder hollow volume calculation
1072 float hRadius = _size.X / 2;
1073 float hLength = _size.Z;
1074
1075 // pi * r2 * h
1076 hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
1077 break;
1078
1079 default:
1080 hollowVolume = 0;
1081 break;
1082 } 1074 }
1083 volume = volume - hollowVolume; 1075 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1084 } 1076 {
1085 break; 1077 volume *= 0.32475953f;
1078 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1079 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1080 volume *= (1.0f - tmp * tmp);
1086 1081
1087 default: 1082 if (hollowAmount > 0.0)
1088 // we don't have all of the volume formulas yet so 1083 {
1089 // use the common volume formula for all
1090 volume = _size.X*_size.Y*_size.Z;
1091 break;
1092 }
1093 1084
1094 // Calculate Path cut effect on volume 1085 hollowVolume *= hollowAmount;
1095 // Not exact, in the triangle hollow example
1096 // They should never be zero or less then zero..
1097 // we'll ignore it if it's less then zero
1098 1086
1099 // ProfileEnd and ProfileBegin are values 1087 switch (_pbs.HollowShape)
1100 // from 0 to 50000 1088 {
1089 case HollowShape.Same:
1090 case HollowShape.Triangle:
1091 hollowVolume *= .25f;
1092 break;
1101 1093
1102 // Turning them back into percentages so that I can cut that percentage off the volume 1094 case HollowShape.Square:
1095 hollowVolume *= 0.499849f * 3.07920140172638f;
1096 break;
1103 1097
1104 float PathCutEndAmount = _pbs.ProfileEnd; 1098 case HollowShape.Circle:
1105 float PathCutStartAmount = _pbs.ProfileBegin;
1106 if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
1107 {
1108 float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
1109 1099
1110 // Check the return amount for sanity 1100 hollowVolume *= 0.1963495f * 3.07920140172638f;
1111 if (pathCutAmount >= 0.99f) 1101 break;
1112 pathCutAmount = 0.99f;
1113 1102
1114 volume = volume - (volume*pathCutAmount); 1103 default:
1115 } 1104 hollowVolume = 0;
1116 UInt16 taperX = _pbs.PathScaleX; 1105 break;
1117 UInt16 taperY = _pbs.PathScaleY; 1106 }
1118 float taperFactorX = 0; 1107 volume *= (1.0f - hollowVolume);
1119 float taperFactorY = 0; 1108 }
1109 }
1110 break;
1120 1111
1121 // Mass = density * volume 1112 default:
1122 if (taperX != 100) 1113 break;
1123 {
1124 if (taperX > 100)
1125 {
1126 taperFactorX = 1.0f - ((float)taperX / 200);
1127 //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
1128 }
1129 else
1130 {
1131 taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
1132 //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
1133 } 1114 }
1134 volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
1135 }
1136 1115
1137 if (taperY != 100) 1116
1138 { 1117
1139 if (taperY > 100) 1118 float taperX1;
1119 float taperY1;
1120 float taperX;
1121 float taperY;
1122 float pathBegin;
1123 float pathEnd;
1124 float profileBegin;
1125 float profileEnd;
1126
1127 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1140 { 1128 {
1141 taperFactorY = 1.0f - ((float)taperY / 200); 1129 taperX1 = _pbs.PathScaleX * 0.01f;
1142 //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString()); 1130 if (taperX1 > 1.0f)
1131 taperX1 = 2.0f - taperX1;
1132 taperX = 1.0f - taperX1;
1133
1134 taperY1 = _pbs.PathScaleY * 0.01f;
1135 if (taperY1 > 1.0f)
1136 taperY1 = 2.0f - taperY1;
1137 taperY = 1.0f - taperY1;
1143 } 1138 }
1144 else 1139 else
1145 { 1140 {
1146 taperFactorY = 1.0f - ((100 - (float)taperY) / 100); 1141 taperX = _pbs.PathTaperX * 0.01f;
1147 //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString()); 1142 if (taperX < 0.0f)
1143 taperX = -taperX;
1144 taperX1 = 1.0f - taperX;
1145
1146 taperY = _pbs.PathTaperY * 0.01f;
1147 if (taperY < 0.0f)
1148 taperY = -taperY;
1149 taperY1 = 1.0f - taperY;
1150
1148 } 1151 }
1149 volume = (float)volume * ((taperFactorY / 3f) + 0.001f); 1152
1150 } 1153
1151 returnMass = m_density*volume; 1154 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1152 if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1155
1156 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1157 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1158 volume *= (pathEnd - pathBegin);
1159
1160// this is crude aproximation
1161 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1162 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1163 volume *= (profileEnd - profileBegin);
1164
1165 returnMass = m_density * volume;
1166
1167 if (returnMass <= 0)
1168 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1169// else if (returnMass > _parent_scene.maximumMassObject)
1170// returnMass = _parent_scene.maximumMassObject;
1171
1153 1172
1154 1173
1155 1174
@@ -1280,7 +1299,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1280 disableBody(); 1299 disableBody();
1281 } 1300 }
1282 } 1301 }
1283
1284 IntPtr vertices, indices; 1302 IntPtr vertices, indices;
1285 int vertexCount, indexCount; 1303 int vertexCount, indexCount;
1286 int vertexStride, triStride; 1304 int vertexStride, triStride;
@@ -1304,10 +1322,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1304 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1322 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1305 try 1323 try
1306 { 1324 {
1307 if (prim_geom == IntPtr.Zero) 1325 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer
1308 { 1326 // {
1309 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 1327 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1310 } 1328 // }
1311 } 1329 }
1312 catch (AccessViolationException) 1330 catch (AccessViolationException)
1313 { 1331 {
@@ -1336,8 +1354,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1336 if (prim_geom != IntPtr.Zero) 1354 if (prim_geom != IntPtr.Zero)
1337 { 1355 {
1338 if (!_position.ApproxEquals(m_taintposition, 0f)) 1356 if (!_position.ApproxEquals(m_taintposition, 0f))
1357 {
1339 changemove(timestep); 1358 changemove(timestep);
1340 1359 }
1341 if (m_taintrot != _orientation) 1360 if (m_taintrot != _orientation)
1342 { 1361 {
1343 if(childPrim && IsPhysical) // For physical child prim... 1362 if(childPrim && IsPhysical) // For physical child prim...
@@ -1357,8 +1376,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1357 // 1376 //
1358 1377
1359 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 1378 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1379 {
1360 changePhysicsStatus(timestep); 1380 changePhysicsStatus(timestep);
1361 // 1381 }//
1362 1382
1363 if (!_size.ApproxEquals(m_taintsize,0f)) 1383 if (!_size.ApproxEquals(m_taintsize,0f))
1364 changesize(timestep); 1384 changesize(timestep);
@@ -1391,10 +1411,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1391 1411
1392 if (m_taintCollidesWater != m_collidesWater) 1412 if (m_taintCollidesWater != m_collidesWater)
1393 changefloatonwater(timestep); 1413 changefloatonwater(timestep);
1394 1414/* obsolete
1395 if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) 1415 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1396 changeAngularLock(timestep); 1416 changeAngularLock(timestep);
1397 1417 */
1398 } 1418 }
1399 else 1419 else
1400 { 1420 {
@@ -1402,15 +1422,16 @@ namespace OpenSim.Region.Physics.OdePlugin
1402 } 1422 }
1403 } 1423 }
1404 1424
1405 1425/* obsolete
1406 private void changeAngularLock(float timestep) 1426 private void changeAngularLock(float timestep)
1407 { 1427 {
1408 if (_parent == null) 1428 if (_parent == null)
1409 { 1429 {
1410 m_angularEnable = m_taintAngularLock; 1430 m_angularLock = m_taintAngularLock;
1431 m_angularLockSet = true;
1411 } 1432 }
1412 } 1433 }
1413 1434 */
1414 private void changelink(float timestep) 1435 private void changelink(float timestep)
1415 { 1436 {
1416 // If the newly set parent is not null 1437 // If the newly set parent is not null
@@ -1468,6 +1489,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1468 if (Body == IntPtr.Zero) 1489 if (Body == IntPtr.Zero)
1469 { 1490 {
1470 Body = d.BodyCreate(_parent_scene.world); 1491 Body = d.BodyCreate(_parent_scene.world);
1492 // disconnect from world gravity so we can apply buoyancy
1493 d.BodySetGravityMode (Body, false);
1494
1471 setMass(); 1495 setMass();
1472 } 1496 }
1473 if (Body != IntPtr.Zero) 1497 if (Body != IntPtr.Zero)
@@ -1508,7 +1532,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1508 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1532 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1509 continue; 1533 continue;
1510 } 1534 }
1511//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1512 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1535 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1513 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1536 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1514 1537
@@ -1551,9 +1574,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1551 m_collisionCategories |= CollisionCategories.Body; 1574 m_collisionCategories |= CollisionCategories.Body;
1552 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1575 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1553 1576
1554//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1555 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1577 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1556//Console.WriteLine(" Post GeomSetCategoryBits 2");
1557 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1578 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1558 1579
1559 1580
@@ -1708,6 +1729,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1708 { 1729 {
1709 disableBodySoft(); 1730 disableBodySoft();
1710 } 1731 }
1732 if (Body != IntPtr.Zero)
1733 {
1734 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1735 d.BodySetForce(Body, 0f, 0f, 0f);
1736 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1737 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1738 }
1739
1711 } 1740 }
1712 else 1741 else
1713 { 1742 {
@@ -1728,12 +1757,18 @@ namespace OpenSim.Region.Physics.OdePlugin
1728 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1757 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1729 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1758 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1730 } 1759 }
1760 if (Body != IntPtr.Zero)
1761 {
1762 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1763 d.BodySetForce(Body, 0f, 0f, 0f);
1764 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1765 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1766 }
1767
1731 if (m_isphysical) 1768 if (m_isphysical)
1732 { 1769 {
1733 if (Body != IntPtr.Zero) 1770 if (Body != IntPtr.Zero)
1734 { 1771 {
1735 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1736 d.BodySetForce(Body, 0, 0, 0);
1737 enableBodySoft(); 1772 enableBodySoft();
1738 } 1773 }
1739 } 1774 }
@@ -1758,23 +1793,21 @@ namespace OpenSim.Region.Physics.OdePlugin
1758 1793
1759 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1794 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1760 { 1795 {
1761//Console.WriteLine("CreateGeom:"); 1796 if (_mesh != null) // Special - make mesh
1762 if (_mesh != null)
1763 { 1797 {
1764 setMesh(_parent_scene, _mesh); 1798 setMesh(_parent_scene, _mesh);
1765 } 1799 }
1766 else 1800 else // not a mesh
1767 { 1801 {
1768 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) 1802 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile??
1769 { 1803 {
1770 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) 1804 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size
1771 { 1805 {
1772 if (((_size.X / 2f) > 0f)) 1806 if (((_size.X / 2f) > 0f)) // Has size
1773 { 1807 {
1774 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1808 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1775 try 1809 try
1776 { 1810 {
1777//Console.WriteLine(" CreateGeom 1");
1778 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); 1811 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1779 } 1812 }
1780 catch (AccessViolationException) 1813 catch (AccessViolationException)
@@ -1789,7 +1822,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1789 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1822 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1790 try 1823 try
1791 { 1824 {
1792//Console.WriteLine(" CreateGeom 2");
1793 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1825 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1794 } 1826 }
1795 catch (AccessViolationException) 1827 catch (AccessViolationException)
@@ -1800,12 +1832,11 @@ namespace OpenSim.Region.Physics.OdePlugin
1800 } 1832 }
1801 } 1833 }
1802 } 1834 }
1803 else 1835 else // not equi-size
1804 { 1836 {
1805 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1837 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1806 try 1838 try
1807 { 1839 {
1808//Console.WriteLine(" CreateGeom 3");
1809 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1840 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1810 } 1841 }
1811 catch (AccessViolationException) 1842 catch (AccessViolationException)
@@ -1817,12 +1848,11 @@ namespace OpenSim.Region.Physics.OdePlugin
1817 } 1848 }
1818 } 1849 }
1819 1850
1820 else 1851 else // not special profile
1821 { 1852 {
1822 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1853 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1823 try 1854 try
1824 { 1855 {
1825//Console.WriteLine(" CreateGeom 4");
1826 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1856 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1827 } 1857 }
1828 catch (AccessViolationException) 1858 catch (AccessViolationException)
@@ -1845,12 +1875,20 @@ namespace OpenSim.Region.Physics.OdePlugin
1845 1875
1846 m_targetSpace = targetspace; 1876 m_targetSpace = targetspace;
1847 1877
1848 if (_mesh == null) 1878 if (_mesh == null && m_meshfailed == false)
1849 { 1879 {
1850 if (_parent_scene.needsMeshing(_pbs)) 1880 if (_parent_scene.needsMeshing(_pbs))
1851 { 1881 {
1852 // Don't need to re-enable body.. it's done in SetMesh 1882 // Don't need to re-enable body.. it's done in SetMesh
1853 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); 1883 try
1884 {
1885 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1886 }
1887 catch
1888 {
1889 //Don't continuously try to mesh prims when meshing has failed
1890 m_meshfailed = true;
1891 }
1854 // createmesh returns null when it's a shape that isn't a cube. 1892 // createmesh returns null when it's a shape that isn't a cube.
1855 // m_log.Debug(m_localID); 1893 // m_log.Debug(m_localID);
1856 } 1894 }
@@ -1859,7 +1897,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1859 1897
1860 lock (_parent_scene.OdeLock) 1898 lock (_parent_scene.OdeLock)
1861 { 1899 {
1862//Console.WriteLine("changeadd 1");
1863 CreateGeom(m_targetSpace, _mesh); 1900 CreateGeom(m_targetSpace, _mesh);
1864 1901
1865 if (prim_geom != IntPtr.Zero) 1902 if (prim_geom != IntPtr.Zero)
@@ -1879,9 +1916,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1879 } 1916 }
1880 } 1917 }
1881 1918
1882 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
1883 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
1884
1885 changeSelectedStatus(timestep); 1919 changeSelectedStatus(timestep);
1886 1920
1887 m_taintadd = false; 1921 m_taintadd = false;
@@ -1889,10 +1923,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1889 1923
1890 public void changemove(float timestep) 1924 public void changemove(float timestep)
1891 { 1925 {
1892//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position );
1893 if (m_isphysical) 1926 if (m_isphysical)
1894 { 1927 {
1895//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1896// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! 1928// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1897 if (!m_taintremove && !childPrim) 1929 if (!m_taintremove && !childPrim)
1898 { 1930 {
@@ -1918,7 +1950,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1918 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1950 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1919 { 1951 {
1920// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 1952// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1921Console.WriteLine(" JointCreateFixed"); 1953Console.WriteLine("ODEPrim JointCreateFixed !!!");
1922 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1954 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1923 d.JointAttach(m_linkJoint, Body, odParent.Body); 1955 d.JointAttach(m_linkJoint, Body, odParent.Body);
1924 d.JointSetFixed(m_linkJoint); 1956 d.JointSetFixed(m_linkJoint);
@@ -2030,23 +2062,7 @@ Console.WriteLine(" JointCreateFixed");
2030 { 2062 {
2031 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) 2063 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2032 { 2064 {
2033 2065 _mesh = null;
2034
2035 if (prim_geom != IntPtr.Zero)
2036 {
2037 try
2038 {
2039 d.GeomDestroy(prim_geom);
2040 prim_geom = IntPtr.Zero;
2041 _mesh = null;
2042 }
2043 catch (System.AccessViolationException)
2044 {
2045 prim_geom = IntPtr.Zero;
2046 m_log.Error("[PHYSICS]: PrimGeom dead");
2047 }
2048 }
2049//Console.WriteLine("changePhysicsStatus for " + m_primName );
2050 changeadd(2f); 2066 changeadd(2f);
2051 } 2067 }
2052 if (childPrim) 2068 if (childPrim)
@@ -2105,12 +2121,10 @@ Console.WriteLine(" JointCreateFixed");
2105 _parent_scene.waitForSpaceUnlock(m_targetSpace); 2121 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2106 d.SpaceRemove(m_targetSpace, prim_geom); 2122 d.SpaceRemove(m_targetSpace, prim_geom);
2107 } 2123 }
2108 d.GeomDestroy(prim_geom);
2109 prim_geom = IntPtr.Zero;
2110 // we don't need to do space calculation because the client sends a position update also. 2124 // we don't need to do space calculation because the client sends a position update also.
2111 2125
2112 // Construction of new prim 2126 // Construction of new prim
2113 if (_parent_scene.needsMeshing(_pbs)) 2127 if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false)
2114 { 2128 {
2115 float meshlod = _parent_scene.meshSculptLOD; 2129 float meshlod = _parent_scene.meshSculptLOD;
2116 2130
@@ -2120,11 +2134,17 @@ Console.WriteLine(" JointCreateFixed");
2120 2134
2121 IMesh mesh = null; 2135 IMesh mesh = null;
2122 2136
2123 if (_parent_scene.needsMeshing(_pbs)) 2137 try
2124 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2138 {
2139 if (_parent_scene.needsMeshing(_pbs))
2140 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2141 }
2142 catch
2143 {
2144 m_meshfailed = true;
2145 }
2125 2146
2126 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2147 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2127//Console.WriteLine("changesize 1");
2128 CreateGeom(m_targetSpace, mesh); 2148 CreateGeom(m_targetSpace, mesh);
2129 2149
2130 2150
@@ -2132,7 +2152,6 @@ Console.WriteLine(" JointCreateFixed");
2132 else 2152 else
2133 { 2153 {
2134 _mesh = null; 2154 _mesh = null;
2135//Console.WriteLine("changesize 2");
2136 CreateGeom(m_targetSpace, _mesh); 2155 CreateGeom(m_targetSpace, _mesh);
2137 } 2156 }
2138 2157
@@ -2208,38 +2227,35 @@ Console.WriteLine(" JointCreateFixed");
2208 disableBody(); 2227 disableBody();
2209 } 2228 }
2210 } 2229 }
2211 try 2230
2212 { 2231
2213 d.GeomDestroy(prim_geom);
2214 }
2215 catch (System.AccessViolationException)
2216 {
2217 prim_geom = IntPtr.Zero;
2218 m_log.Error("[PHYSICS]: PrimGeom dead");
2219 }
2220 prim_geom = IntPtr.Zero;
2221 // we don't need to do space calculation because the client sends a position update also. 2232 // we don't need to do space calculation because the client sends a position update also.
2222 if (_size.X <= 0) _size.X = 0.01f; 2233 if (_size.X <= 0) _size.X = 0.01f;
2223 if (_size.Y <= 0) _size.Y = 0.01f; 2234 if (_size.Y <= 0) _size.Y = 0.01f;
2224 if (_size.Z <= 0) _size.Z = 0.01f; 2235 if (_size.Z <= 0) _size.Z = 0.01f;
2225 // Construction of new prim 2236 // Construction of new prim
2226 2237
2227 if (_parent_scene.needsMeshing(_pbs)) 2238 if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false)
2228 { 2239 {
2229 // Don't need to re-enable body.. it's done in SetMesh 2240 // Don't need to re-enable body.. it's done in SetMesh
2230 float meshlod = _parent_scene.meshSculptLOD; 2241 float meshlod = _parent_scene.meshSculptLOD;
2231 2242
2232 if (IsPhysical) 2243 if (IsPhysical)
2233 meshlod = _parent_scene.MeshSculptphysicalLOD; 2244 meshlod = _parent_scene.MeshSculptphysicalLOD;
2234 2245 try
2235 IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2246 {
2247 IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2248 CreateGeom(m_targetSpace, mesh);
2249 }
2250 catch
2251 {
2252 m_meshfailed = true;
2253 }
2236 // createmesh returns null when it doesn't mesh. 2254 // createmesh returns null when it doesn't mesh.
2237 CreateGeom(m_targetSpace, mesh);
2238 } 2255 }
2239 else 2256 else
2240 { 2257 {
2241 _mesh = null; 2258 _mesh = null;
2242//Console.WriteLine("changeshape");
2243 CreateGeom(m_targetSpace, null); 2259 CreateGeom(m_targetSpace, null);
2244 } 2260 }
2245 2261
@@ -2314,6 +2330,7 @@ Console.WriteLine(" JointCreateFixed");
2314 return; 2330 return;
2315 } 2331 }
2316 d.BodyEnable(Body); 2332 d.BodyEnable(Body);
2333
2317 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); 2334 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2318 } 2335 }
2319 m_forcelist.Clear(); 2336 m_forcelist.Clear();
@@ -2421,7 +2438,7 @@ Console.WriteLine(" JointCreateFixed");
2421 { 2438 {
2422 if (CollisionEventsThisFrame == null) 2439 if (CollisionEventsThisFrame == null)
2423 CollisionEventsThisFrame = new CollisionEventUpdate(); 2440 CollisionEventsThisFrame = new CollisionEventUpdate();
2424 CollisionEventsThisFrame.addCollider(CollidedWith, contact); 2441 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
2425 } 2442 }
2426 2443
2427 public void SendCollisions() 2444 public void SendCollisions()
@@ -2985,13 +3002,10 @@ Console.WriteLine(" JointCreateFixed");
2985 // This is a temp patch until proper region crossing is developed. 3002 // This is a temp patch until proper region crossing is developed.
2986 3003
2987 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; 3004 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
2988 int fence = _parent_scene.geomRegionFence; 3005 float fence = _parent_scene.geomRegionFence;
2989 3006
2990 float border_limit = 0.05f; // original limit 3007 frcount++; // used to limit debug comment output
2991 if (fence == 1) border_limit = 0.5f; // bounce point 3008 if (frcount > 50)
2992
2993 frcount++; // used to limit debug comment output
2994 if (frcount > 10)
2995 frcount = 0; 3009 frcount = 0;
2996 3010
2997 if(revcount > 0) revcount--; 3011 if(revcount > 0) revcount--;
@@ -3033,35 +3047,34 @@ Console.WriteLine(" JointCreateFixed");
3033 3047
3034 // Check if outside region 3048 // Check if outside region
3035 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! 3049 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3036 if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) 3050 if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
3037 { 3051 {
3038 l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); 3052 l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
3039 outside = 1; 3053 outside = 1;
3040 } 3054 }
3041 3055
3042 if (l_position.X < border_limit) 3056 if (l_position.X < fence)
3043 { 3057 {
3044 l_position.X = border_limit; 3058 l_position.X = fence;
3045 outside = 2; 3059 outside = 2;
3046 } 3060 }
3047 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) 3061 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
3048 { 3062 {
3049 l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); 3063 l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
3050 outside = 3; 3064 outside = 3;
3051 } 3065 }
3052 3066
3053 if (l_position.Y < border_limit) 3067 if (l_position.Y < fence)
3054 { 3068 {
3055 l_position.Y = border_limit; 3069 l_position.Y = fence;
3056 outside = 4; 3070 outside = 4;
3057 } 3071 }
3058 3072
3059 if (outside > 0) 3073 if (outside > 0)
3060 { 3074 {
3061//Console.WriteLine(" fence = {0}",fence);
3062 3075
3063//Console.WriteLine("Border {0}", l_position); 3076//Console.WriteLine("Border {0} fence={1}", l_position, fence);
3064 if (fence == 1) // bounce object off boundary 3077 if (fence > 0.0f) // bounce object off boundary
3065 { 3078 {
3066 if (revcount == 0) 3079 if (revcount == 0)
3067 { 3080 {
@@ -3095,8 +3108,6 @@ Console.WriteLine(" JointCreateFixed");
3095 else 3108 else
3096 { // Too many tries 3109 { // Too many tries
3097 if (_parent == null) base.RaiseOutOfBounds(l_position); 3110 if (_parent == null) base.RaiseOutOfBounds(l_position);
3098//Console.WriteLine("ROOB 2");
3099
3100 return; // Dont process any other motion? 3111 return; // Dont process any other motion?
3101 } // end various methods 3112 } // end various methods
3102 } // end outside region horizontally 3113 } // end outside region horizontally
@@ -3112,7 +3123,6 @@ Console.WriteLine(" JointCreateFixed");
3112 3123
3113 //IsPhysical = false; 3124 //IsPhysical = false;
3114 if (_parent == null) base.RaiseOutOfBounds(_position); 3125 if (_parent == null) base.RaiseOutOfBounds(_position);
3115//Console.WriteLine("ROOB 3");
3116 3126
3117 3127
3118 _acceleration.X = 0; // This stuff may stop client display but it has no 3128 _acceleration.X = 0; // This stuff may stop client display but it has no
@@ -3122,6 +3132,7 @@ Console.WriteLine(" JointCreateFixed");
3122 _velocity.X = 0; 3132 _velocity.X = 0;
3123 _velocity.Y = 0; 3133 _velocity.Y = 0;
3124 _velocity.Z = 0; 3134 _velocity.Z = 0;
3135 m_lastVelocity = Vector3.Zero;
3125 m_rotationalVelocity.X = 0; 3136 m_rotationalVelocity.X = 0;
3126 m_rotationalVelocity.Y = 0; 3137 m_rotationalVelocity.Y = 0;
3127 m_rotationalVelocity.Z = 0; 3138 m_rotationalVelocity.Z = 0;
@@ -3135,10 +3146,12 @@ Console.WriteLine(" JointCreateFixed");
3135 } // end neg Z check 3146 } // end neg Z check
3136 3147
3137 // Is it moving? 3148 // Is it moving?
3138 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3149 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3139 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3150 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3140 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 3151 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3141 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large 3152 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3153 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3154 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly
3142 { 3155 {
3143 _zeroFlag = true; 3156 _zeroFlag = true;
3144 m_throttleUpdates = false; 3157 m_throttleUpdates = false;
@@ -3155,15 +3168,20 @@ Console.WriteLine(" JointCreateFixed");
3155 { // Its stopped 3168 { // Its stopped
3156 _velocity.X = 0.0f; 3169 _velocity.X = 0.0f;
3157 _velocity.Y = 0.0f; 3170 _velocity.Y = 0.0f;
3158 _velocity.Z = 0.0f; 3171 // _velocity.Z = 0.0f;
3159 3172
3160 _acceleration.X = 0; 3173 _acceleration.X = 0;
3161 _acceleration.Y = 0; 3174 _acceleration.Y = 0;
3162 _acceleration.Z = 0; 3175 // _acceleration.Z = 0;
3163 3176
3164 m_rotationalVelocity.X = 0; 3177 m_rotationalVelocity.X = 0;
3165 m_rotationalVelocity.Y = 0; 3178 m_rotationalVelocity.Y = 0;
3166 m_rotationalVelocity.Z = 0; 3179 m_rotationalVelocity.Z = 0;
3180 // Stop it in the phys engine
3181 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3182 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3183 d.BodySetForce(Body, 0f, 0f, 0f);
3184
3167 if (!m_lastUpdateSent) 3185 if (!m_lastUpdateSent)
3168 { 3186 {
3169 m_throttleUpdates = false; 3187 m_throttleUpdates = false;
@@ -3200,8 +3218,91 @@ Console.WriteLine(" JointCreateFixed");
3200 } 3218 }
3201 m_lastposition = l_position; 3219 m_lastposition = l_position;
3202 3220
3203 /// End UpdatePositionAndVelocity insert 3221 /// End UpdatePositionAndVelocity insert
3204 3222
3223
3224 // Rotation lock =====================================
3225 if(m_rotateEnableUpdate)
3226 {
3227 // Snapshot current angles, set up Amotor(s)
3228 m_rotateEnableUpdate = false;
3229 m_rotateEnable = m_rotateEnableRequest;
3230//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3231
3232 if (Amotor != IntPtr.Zero)
3233 {
3234 d.JointDestroy(Amotor);
3235 Amotor = IntPtr.Zero;
3236//Console.WriteLine("Old Amotor Destroyed");
3237 }
3238
3239 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3240 { // not all are enabled
3241 d.Quaternion r = d.BodyGetQuaternion(Body);
3242 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3243 // extract the axes vectors
3244 Vector3 vX = new Vector3(1f,0f,0f);
3245 Vector3 vY = new Vector3(0f,1f,0f);
3246 Vector3 vZ = new Vector3(0f,0f,1f);
3247 vX = vX * locrot;
3248 vY = vY * locrot;
3249 vZ = vZ * locrot;
3250 // snapshot the current angle vectors
3251 m_lockX = vX;
3252 m_lockY = vY;
3253 m_lockZ = vZ;
3254 // m_lockRot = locrot;
3255 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3256 d.JointAttach(Amotor, Body, IntPtr.Zero);
3257 d.JointSetAMotorMode(Amotor, 0); // User mode??
3258//Console.WriteLine("New Amotor Created for {0}", m_primName);
3259
3260 float axisnum = 3; // how many to lock
3261 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3262 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3263//Console.WriteLine("AxisNum={0}",(int)axisnum);
3264
3265 int i = 0;
3266
3267 if (m_rotateEnable.X == 0)
3268 {
3269 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3270//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3271 i++;
3272 }
3273
3274 if (m_rotateEnable.Y == 0)
3275 {
3276 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3277//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3278 i++;
3279 }
3280
3281 if (m_rotateEnable.Z == 0)
3282 {
3283 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3284//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3285 i++;
3286 }
3287
3288 // These lowstops and high stops are effectively (no wiggle room)
3289 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3290 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3291 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3292 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3293 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3294 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3295 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
3296 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
3297 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
3298 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3299 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3300 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3301 } // else none are locked
3302 } // end Rotation Update
3303
3304
3305 // VEHICLE processing ==========================================
3205 if (m_type != Vehicle.TYPE_NONE) 3306 if (m_type != Vehicle.TYPE_NONE)
3206 { 3307 {
3207 // get body attitude 3308 // get body attitude
@@ -3213,7 +3314,6 @@ Console.WriteLine(" JointCreateFixed");
3213 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame 3314 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3214 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); 3315 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3215 m_lLinObjectVel = vel_now * irotq; 3316 m_lLinObjectVel = vel_now * irotq;
3216
3217 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate 3317 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3218 { 3318 {
3219 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) 3319 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
@@ -3339,7 +3439,7 @@ Console.WriteLine(" JointCreateFixed");
3339 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); 3439 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3340 // apply gravity force 3440 // apply gravity force
3341 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3441 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3342//if(frcount == 0) Console.WriteLine("Grav {0}", grav); 3442//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3343 // end MoveLinear() 3443 // end MoveLinear()
3344 3444
3345 3445
@@ -3420,7 +3520,7 @@ Console.WriteLine(" JointCreateFixed");
3420//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); 3520//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3421 3521
3422 3522
3423 // Rotation Axis Disables: 3523 /* // Rotation Axis Disables:
3424 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3524 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3425 { 3525 {
3426 if (m_angularEnable.X == 0) 3526 if (m_angularEnable.X == 0)
@@ -3430,7 +3530,7 @@ Console.WriteLine(" JointCreateFixed");
3430 if (m_angularEnable.Z == 0) 3530 if (m_angularEnable.Z == 0)
3431 angObjectVel.Z = 0f; 3531 angObjectVel.Z = 0f;
3432 } 3532 }
3433 3533 */
3434 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation 3534 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3435 3535
3436 // Vertical attractor section 3536 // Vertical attractor section
@@ -3500,100 +3600,58 @@ Console.WriteLine(" JointCreateFixed");
3500 } // end VEHICLES 3600 } // end VEHICLES
3501 else 3601 else
3502 { 3602 {
3603 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3604
3503 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 3605 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3504 // NON-'VEHICLES' are dealt with here 3606
3505 /// Dynamics Angular Lock ======================================================================== 3607 /// Dynamics Buoyancy
3506 if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3507 {
3508 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
3509 if (m_angularEnable.X == 0)
3510 avel2.X = 0;
3511 if (m_angularEnable.Y == 0)
3512 avel2.Y = 0;
3513 if (m_angularEnable.Z == 0)
3514 avel2.Z = 0;
3515 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
3516 }
3517
3518
3519 /// Dynamics Buoyancy ===============================================================================
3520 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. 3608 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3521 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 3609 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3522 // NB Prims in ODE are no subject to global gravity 3610 // NB Prims in ODE are no subject to global gravity
3611 // This should only affect gravity operations
3612
3523 float m_mass = CalculateMass(); 3613 float m_mass = CalculateMass();
3614 // calculate z-force due togravity on object.
3524 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass 3615 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3525 3616 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3526 if (m_usePID)
3527 { 3617 {
3528//if(frcount == 0) Console.WriteLine("PID " + m_primName); 3618 fz = 0; // llMoveToTarget ignores gravity.
3529 // KF - this is for object MoveToTarget. 3619 // it also ignores mass of object, and any physical resting on it.
3530 3620 // Vector3 m_PIDTarget is where we are going
3531 //if (!d.BodyIsEnabled(Body)) 3621 // float m_PIDTau is time to get there
3532 //d.BodySetForce(Body, 0f, 0f, 0f); 3622 fx = 0;
3533 3623 fy = 0;
3534 // no lock; for now it's only called from within Simulate()
3535
3536 // If the PID Controller isn't active then we set our force
3537 // calculating base velocity to the current position
3538
3539 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3540 {
3541 //PID_G = PID_G / m_PIDTau;
3542 m_PIDTau = 1;
3543 }
3544
3545 if ((PID_G - m_PIDTau) <= 0)
3546 {
3547 PID_G = m_PIDTau + 1;
3548 }
3549 //PidStatus = true;
3550
3551 // PhysicsVector vec = new PhysicsVector();
3552// d.Vector3 vel = d.BodyGetLinearVel(Body);
3553
3554 d.Vector3 pos = d.BodyGetPosition(Body); 3624 d.Vector3 pos = d.BodyGetPosition(Body);
3555 _target_velocity = 3625 Vector3 error = new Vector3(
3556 new Vector3( 3626 (m_PIDTarget.X - pos.X),
3557 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 3627 (m_PIDTarget.Y - pos.Y),
3558 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 3628 (m_PIDTarget.Z - pos.Z));
3559 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 3629 if (error.ApproxEquals(Vector3.Zero,0.01f))
3560 ); 3630 { // Very close, Jump there and quit move
3561 3631
3562 // if velocity is zero, use position control; otherwise, velocity control
3563
3564 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3565 {
3566 // keep track of where we stopped. No more slippin' & slidin'
3567
3568 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3569 // react to the physics scene by moving it's position.
3570 // Avatar to Avatar collisions
3571 // Prim to avatar collisions
3572
3573 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3574 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3575 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3576 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 3632 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3577 d.BodySetLinearVel(Body, 0, 0, 0); 3633 _target_velocity = Vector3.Zero;
3578 d.BodyAddForce(Body, 0, 0, fz); 3634 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3579 // return; 3635 d.BodySetForce(Body, 0f, 0f, 0f);
3580 } 3636 }
3581 else 3637 else
3582 { 3638 {
3583 _zeroFlag = false; 3639 float scale = 50.0f * timestep / m_PIDTau;
3584 3640 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3585 // We're flying and colliding with something 3641 {
3586 fx = ((_target_velocity.X) - vel.X) * (PID_D); 3642 // Nearby, quit update of velocity
3587 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 3643 }
3588 3644 else
3589 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 3645 { // Far, calc damped velocity
3590 3646 _target_velocity = error * scale;
3591 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 3647 }
3592 } 3648 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3593 } // end if (m_usePID) 3649 }
3650 } // end PID MoveToTarget
3594 3651
3652
3595 /// Dynamics Hover =================================================================================== 3653 /// Dynamics Hover ===================================================================================
3596 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 3654 // Hover PID Controller can only run if the PIDcontroller is not in use.
3597 if (m_useHoverPID && !m_usePID) 3655 if (m_useHoverPID && !m_usePID)
3598 { 3656 {
3599//Console.WriteLine("Hover " + m_primName); 3657//Console.WriteLine("Hover " + m_primName);
@@ -3662,11 +3720,10 @@ Console.WriteLine(" JointCreateFixed");
3662 // Avatar to Avatar collisions 3720 // Avatar to Avatar collisions
3663 // Prim to avatar collisions 3721 // Prim to avatar collisions
3664 d.Vector3 dlinvel = vel; 3722 d.Vector3 dlinvel = vel;
3665
3666 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3723 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3667 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); 3724 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3668 d.BodyAddForce(Body, 0, 0, fz); 3725 d.BodyAddForce(Body, 0, 0, fz);
3669 //KF this prevents furthur motions return; 3726 //KF this prevents furthur motions return;
3670 } 3727 }
3671 else 3728 else
3672 { 3729 {
@@ -3677,57 +3734,11 @@ Console.WriteLine(" JointCreateFixed");
3677 } 3734 }
3678 } // end m_useHoverPID && !m_usePID 3735 } // end m_useHoverPID && !m_usePID
3679 3736
3680 /// Dynamics RotLookAt =================================================================================
3681 if (m_useAPID)
3682 {
3683 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3684 // Quaternion m_APIDTarget
3685 // float m_APIDStrength // From SL experiments, this is the time to get there
3686 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3687 // Also in SL the mass of the object has no effect on time to get there.
3688 // Factors:
3689 // get present body rotation
3690 float limit = 1.0f;
3691 float scaler = 50f; // adjusts damping time
3692 float RLAservo = 0f;
3693
3694 d.Quaternion rot = d.BodyGetQuaternion(Body);
3695 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3696 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3697 float diff_angle;
3698 Vector3 diff_axis;
3699 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3700 diff_axis.Normalize();
3701 if(diff_angle > 0.01f) // diff_angle is always +ve
3702 {
3703// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3704 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3705 rotforce = rotforce * rotq;
3706 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3707// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3708 // rotforce = rotforce * RLAservo * diff_angle ;
3709 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3710 RLAservo = timestep / m_APIDStrength * scaler;
3711 rotforce = rotforce * RLAservo * diff_angle ;
3712
3713 if (m_angularEnable.X == 0)
3714 rotforce.X = 0;
3715 if (m_angularEnable.Y == 0)
3716 rotforce.Y = 0;
3717 if (m_angularEnable.Z == 0)
3718 rotforce.Z = 0;
3719
3720 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3721//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3722 }
3723//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3724 } // end m_useAPID
3725 3737
3726 /// Dynamics Apply Forces =================================================================================== 3738 /// Dynamics Apply Forces ===================================================================================
3727 fx *= m_mass; 3739 fx *= m_mass;
3728 fy *= m_mass; 3740 fy *= m_mass;
3729 //fz *= m_mass; 3741 //fz *= m_mass;
3730
3731 fx += m_force.X; 3742 fx += m_force.X;
3732 fy += m_force.Y; 3743 fy += m_force.Y;
3733 fz += m_force.Z; 3744 fz += m_force.Z;
@@ -3744,7 +3755,7 @@ Console.WriteLine(" JointCreateFixed");
3744 // this appears to re-enable it incase the surface it is upon vanishes, 3755 // this appears to re-enable it incase the surface it is upon vanishes,
3745 // and the body should fall again. 3756 // and the body should fall again.
3746 d.BodySetLinearVel(Body, 0f, 0f, 0f); 3757 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3747 d.BodySetForce(Body, 0, 0, 0); 3758 d.BodySetForce(Body, 0f, 0f, 0f);
3748 enableBodySoft(); 3759 enableBodySoft();
3749 } 3760 }
3750 3761
@@ -3762,27 +3773,102 @@ Console.WriteLine(" JointCreateFixed");
3762 if (fy < nmin) 3773 if (fy < nmin)
3763 fy = nmin; 3774 fy = nmin;
3764 d.BodyAddForce(Body, fx, fy, fz); 3775 d.BodyAddForce(Body, fx, fy, fz);
3765//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3776 } // end apply forces
3766 } 3777 } // end Vehicle/Dynamics
3767 } 3778
3768 } 3779 /// RotLookAt / LookAt =================================================================================
3769 else 3780 if (m_useAPID)
3770 { // is not physical, or is not a body or is selected 3781 {
3771 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating 3782 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3772 _velocity.X = 0; 3783 // Quaternion m_APIDTarget
3773 _velocity.Y = 0; 3784 // float m_APIDStrength // From SL experiments, this is the time to get there
3774 _velocity.Z = 0; 3785 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3775 3786 // Also in SL the mass of the object has no effect on time to get there.
3776 _acceleration.X = 0; 3787 // Factors:
3777 _acceleration.Y = 0; 3788 // get present body rotation
3778 _acceleration.Z = 0; 3789 float limit = 1.0f;
3779 3790 float rscaler = 50f; // adjusts rotation damping time
3780 m_rotationalVelocity.X = 0; 3791 float lscaler = 10f; // adjusts linear damping time in llLookAt
3781 m_rotationalVelocity.Y = 0; 3792 float RLAservo = 0f;
3782 m_rotationalVelocity.Z = 0; 3793 Vector3 diff_axis;
3783 _zeroFlag = true; 3794 float diff_angle;
3784 return; 3795 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3785 } 3796 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3786 } // end Move() 3797 Quaternion rtarget = new Quaternion();
3798
3799 if(m_APIDTarget.W == -99.9f)
3800 {
3801 // this is really a llLookAt(), x,y,z is the target vector
3802 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
3803 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3804 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3805 float twopi = 2.0f * (float)Math.PI;
3806 Vector3 dir = target - _position;
3807 dir.Normalize();
3808 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3809 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3810 float terot = (float)Math.Atan2(dir.Z, txy);
3811 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
3812 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3813 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3814 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
3815 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X));
3816 float roll = (float)Math.Atan2(ra, rb);
3817 float errorz = tzrot - ozrot;
3818 if(errorz > (float)Math.PI) errorz -= twopi;
3819 else if(errorz < -(float)Math.PI) errorz += twopi;
3820 float errory = oerot - terot;
3821 if(errory > (float)Math.PI) errory -= twopi;
3822 else if(errory < -(float)Math.PI) errory += twopi;
3823 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3824 if(diff_angle > 0.01f * m_APIDdamper)
3825 {
3826 m_APIDdamper = 1.0f;
3827 RLAservo = timestep / m_APIDStrength * rscaler;
3828 errorz *= RLAservo;
3829 errory *= RLAservo;
3830 error.X = -roll * 8.0f;
3831 error.Y = errory;
3832 error.Z = errorz;
3833 error *= rotq;
3834 d.BodySetAngularVel (Body, error.X, error.Y, error.Z);
3835 }
3836 else
3837 {
3838 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3839 m_APIDdamper = 2.0f;
3840 }
3841 }
3842 else
3843 {
3844 // this is a llRotLookAt()
3845 rtarget = m_APIDTarget;
3846
3847 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
3848 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
3849//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3850
3851 // diff_axis.Normalize(); it already is!
3852 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3853 {
3854 m_APIDdamper = 1.0f;
3855 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3856 rotforce = rotforce * rotq;
3857 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3858 RLAservo = timestep / m_APIDStrength * lscaler;
3859 rotforce = rotforce * RLAservo * diff_angle ;
3860 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3861//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3862 }
3863 else
3864 { // close enough
3865 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3866 m_APIDdamper = 2.0f;
3867 }
3868 } // end llLookAt/llRotLookAt
3869//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3870 } // end m_useAPID
3871 } // end root prims
3872 } // end Move()
3787 } // end class 3873 } // end class
3788} 3874}
generated by cgit v1.1 at 2025-01-23 00:32:57 +0000