aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs256
1 files changed, 145 insertions, 111 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index e8894f7..01c3f81 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -180,6 +180,7 @@ namespace OpenSim.Region.Physics.OdePlugin
180 internal int m_material = (int)Material.Wood; 180 internal int m_material = (int)Material.Wood;
181 181
182 private int frcount = 0; // Used to limit dynamics debug output to 182 private int frcount = 0; // Used to limit dynamics debug output to
183 private int revcount = 0; // Reverse motion while > 0
183 184
184 private IntPtr m_body = IntPtr.Zero; 185 private IntPtr m_body = IntPtr.Zero;
185 186
@@ -1888,22 +1889,19 @@ namespace OpenSim.Region.Physics.OdePlugin
1888 1889
1889 public void changemove(float timestep) 1890 public void changemove(float timestep)
1890 { 1891 {
1891//Console.WriteLine("changemove for {0}", m_primName ); 1892//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position );
1892
1893 if (m_isphysical) 1893 if (m_isphysical)
1894 { 1894 {
1895//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); 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! 1896// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1897 if (!m_taintremove && !childPrim) 1897 if (!m_taintremove && !childPrim)
1898 { 1898 {
1899//Console.WriteLine("physOK");
1900 if (Body == IntPtr.Zero) 1899 if (Body == IntPtr.Zero)
1901 enableBody(); 1900 enableBody();
1902 //Prim auto disable after 20 frames, 1901 //Prim auto disable after 20 frames,
1903 //if you move it, re-enable the prim manually. 1902 //if you move it, re-enable the prim manually.
1904 if (_parent != null) 1903 if (_parent != null)
1905 { 1904 {
1906//Console.WriteLine("physChild");
1907 if (m_linkJoint != IntPtr.Zero) 1905 if (m_linkJoint != IntPtr.Zero)
1908 { 1906 {
1909 d.JointDestroy(m_linkJoint); 1907 d.JointDestroy(m_linkJoint);
@@ -1912,7 +1910,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1912 } 1910 }
1913 if (Body != IntPtr.Zero) 1911 if (Body != IntPtr.Zero)
1914 { 1912 {
1915//Console.WriteLine("physNotIPZ");
1916 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 1913 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1917 1914
1918 if (_parent != null) 1915 if (_parent != null)
@@ -1945,7 +1942,6 @@ Console.WriteLine(" JointCreateFixed");
1945 } 1942 }
1946 else 1943 else
1947 { 1944 {
1948//Console.WriteLine("NONphys");
1949 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 1945 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1950 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1946 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1951 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1947 _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -2382,9 +2378,7 @@ Console.WriteLine(" JointCreateFixed");
2382 if (IsPhysical) 2378 if (IsPhysical)
2383 { 2379 {
2384 if (Body != IntPtr.Zero) 2380 if (Body != IntPtr.Zero)
2385 {
2386 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); 2381 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2387 }
2388 } 2382 }
2389 2383
2390 //resetCollisionAccounting(); 2384 //resetCollisionAccounting();
@@ -2394,31 +2388,9 @@ Console.WriteLine(" JointCreateFixed");
2394 2388
2395 public void UpdatePositionAndVelocity() 2389 public void UpdatePositionAndVelocity()
2396 { 2390 {
2397 return; // moved to the Move() method 2391 return; // moved to the Move () method
2398 }
2399/* No one uses this?
2400 public Matrix4 FromDMass(d.Mass pMass)
2401 {
2402 Matrix4 obj;
2403 obj.M11 = pMass.I.M00;
2404 obj.M12 = pMass.I.M01;
2405 obj.M13 = pMass.I.M02;
2406 obj.M14 = 0;
2407 obj.M21 = pMass.I.M10;
2408 obj.M22 = pMass.I.M11;
2409 obj.M23 = pMass.I.M12;
2410 obj.M24 = 0;
2411 obj.M31 = pMass.I.M20;
2412 obj.M32 = pMass.I.M21;
2413 obj.M33 = pMass.I.M22;
2414 obj.M34 = 0;
2415 obj.M41 = 0;
2416 obj.M42 = 0;
2417 obj.M43 = 0;
2418 obj.M44 = 1;
2419 return obj;
2420 } 2392 }
2421*/ 2393
2422 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2394 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2423 { 2395 {
2424 obj.I.M00 = pMat[0, 0]; 2396 obj.I.M00 = pMat[0, 0];
@@ -2654,15 +2626,15 @@ Console.WriteLine(" JointCreateFixed");
2654 // m_angularDeflectionEfficiency = pValue; 2626 // m_angularDeflectionEfficiency = pValue;
2655 break; 2627 break;
2656 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: 2628 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2657 if (pValue < 0.01f) pValue = 0.01f; 2629 if (pValue < 0.1f) pValue = 0.1f;
2658 // m_angularDeflectionTimescale = pValue; 2630 // m_angularDeflectionTimescale = pValue;
2659 break; 2631 break;
2660 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: 2632 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2661 if (pValue < 0.01f) pValue = 0.01f; 2633 if (pValue < 0.3f) pValue = 0.3f;
2662 m_angularMotorDecayTimescale = pValue; 2634 m_angularMotorDecayTimescale = pValue;
2663 break; 2635 break;
2664 case Vehicle.ANGULAR_MOTOR_TIMESCALE: 2636 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2665 if (pValue < 0.01f) pValue = 0.01f; 2637 if (pValue < 0.3f) pValue = 0.3f;
2666 m_angularMotorTimescale = pValue; 2638 m_angularMotorTimescale = pValue;
2667 break; 2639 break;
2668 case Vehicle.BANKING_EFFICIENCY: 2640 case Vehicle.BANKING_EFFICIENCY:
@@ -2691,7 +2663,7 @@ Console.WriteLine(" JointCreateFixed");
2691 m_VhoverHeight = pValue; 2663 m_VhoverHeight = pValue;
2692 break; 2664 break;
2693 case Vehicle.HOVER_TIMESCALE: 2665 case Vehicle.HOVER_TIMESCALE:
2694 if (pValue < 0.01f) pValue = 0.01f; 2666 if (pValue < 0.1f) pValue = 0.1f;
2695 m_VhoverTimescale = pValue; 2667 m_VhoverTimescale = pValue;
2696 break; 2668 break;
2697 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: 2669 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
@@ -2703,11 +2675,11 @@ Console.WriteLine(" JointCreateFixed");
2703 // m_linearDeflectionTimescale = pValue; 2675 // m_linearDeflectionTimescale = pValue;
2704 break; 2676 break;
2705 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: 2677 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2706 if (pValue < 0.01f) pValue = 0.01f; 2678 if (pValue < 0.3f) pValue = 0.3f;
2707 m_linearMotorDecayTimescale = pValue; 2679 m_linearMotorDecayTimescale = pValue;
2708 break; 2680 break;
2709 case Vehicle.LINEAR_MOTOR_TIMESCALE: 2681 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2710 if (pValue < 0.01f) pValue = 0.01f; 2682 if (pValue < 0.1f) pValue = 0.1f;
2711 m_linearMotorTimescale = pValue; 2683 m_linearMotorTimescale = pValue;
2712 break; 2684 break;
2713 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 2685 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
@@ -2716,7 +2688,7 @@ Console.WriteLine(" JointCreateFixed");
2716 m_verticalAttractionEfficiency = pValue; 2688 m_verticalAttractionEfficiency = pValue;
2717 break; 2689 break;
2718 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: 2690 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2719 if (pValue < 0.01f) pValue = 0.01f; 2691 if (pValue < 0.1f) pValue = 0.1f;
2720 m_verticalAttractionTimescale = pValue; 2692 m_verticalAttractionTimescale = pValue;
2721 break; 2693 break;
2722 2694
@@ -2732,6 +2704,7 @@ Console.WriteLine(" JointCreateFixed");
2732 UpdateAngDecay(); 2704 UpdateAngDecay();
2733 break; 2705 break;
2734 case Vehicle.LINEAR_FRICTION_TIMESCALE: 2706 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2707 if (pValue < 0.1f) pValue = 0.1f;
2735 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 2708 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2736 break; 2709 break;
2737 case Vehicle.LINEAR_MOTOR_DIRECTION: 2710 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2771,6 +2744,9 @@ Console.WriteLine(" JointCreateFixed");
2771 UpdateAngDecay(); 2744 UpdateAngDecay();
2772 break; 2745 break;
2773 case Vehicle.LINEAR_FRICTION_TIMESCALE: 2746 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2747 if (pValue.X < 0.1f) pValue.X = 0.1f;
2748 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2749 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2774 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 2750 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2775 break; 2751 break;
2776 case Vehicle.LINEAR_MOTOR_DIRECTION: 2752 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2967,7 +2943,7 @@ Console.WriteLine(" JointCreateFixed");
2967 2943
2968 internal void Halt() 2944 internal void Halt()
2969 { // Kill all motions, when non-physical 2945 { // Kill all motions, when non-physical
2970 m_linearMotorDirection = Vector3.Zero; 2946 // m_linearMotorDirection = Vector3.Zero;
2971 m_lLinMotorDVel = Vector3.Zero; 2947 m_lLinMotorDVel = Vector3.Zero;
2972 m_lLinObjectVel = Vector3.Zero; 2948 m_lLinObjectVel = Vector3.Zero;
2973 m_wLinObjectVel = Vector3.Zero; 2949 m_wLinObjectVel = Vector3.Zero;
@@ -2979,16 +2955,22 @@ Console.WriteLine(" JointCreateFixed");
2979 2955
2980 private void UpdateLinDecay() 2956 private void UpdateLinDecay()
2981 { 2957 {
2982 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; 2958// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
2983 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 2959// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2984 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 2960// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2961 m_lLinMotorDVel.X = m_linearMotorDirection.X;
2962 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2963 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2985 } // else let the motor decay on its own 2964 } // else let the motor decay on its own
2986 2965
2987 private void UpdateAngDecay() 2966 private void UpdateAngDecay()
2988 { 2967 {
2989 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; 2968// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
2990 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; 2969// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2991 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; 2970// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2971 m_angularMotorDVel.X = m_angularMotorDirection.X;
2972 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2973 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2992 } // else let the motor decay on its own 2974 } // else let the motor decay on its own
2993 2975
2994 public void Move(float timestep) 2976 public void Move(float timestep)
@@ -2996,44 +2978,114 @@ Console.WriteLine(" JointCreateFixed");
2996 float fx = 0; 2978 float fx = 0;
2997 float fy = 0; 2979 float fy = 0;
2998 float fz = 0; 2980 float fz = 0;
2981 Vector3 linvel; // velocity applied, including any reversal
2982 int outside = 0;
2983
2984 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
2985 // This is a temp patch until proper region crossing is developed.
2986
2987 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
2988 int fence = _parent_scene.geomRegionFence;
2989
2990 float border_limit = 0.05f; // original limit
2991 if (fence == 1) border_limit = 0.5f; // bounce point
2999 2992
3000 frcount++; // used to limit debug comment output 2993 frcount++; // used to limit debug comment output
3001 if (frcount > 100) 2994 if (frcount > 10)
3002 frcount = 0; 2995 frcount = 0;
3003 2996
3004 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. 2997 if(revcount > 0) revcount--;
2998
2999 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims.
3005 { 3000 {
3006 3001 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3007// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3008 bool lastZeroFlag = _zeroFlag; // was it stopped 3002 bool lastZeroFlag = _zeroFlag; // was it stopped
3003
3009 d.Vector3 vec = d.BodyGetPosition(Body); 3004 d.Vector3 vec = d.BodyGetPosition(Body);
3010 d.Quaternion ori = d.BodyGetQuaternion(Body);
3011 d.Vector3 vel = d.BodyGetLinearVel(Body);
3012 // d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3013 d.Vector3 torque = d.BodyGetTorque(Body);
3014 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3015 Vector3 l_position = Vector3.Zero; 3005 Vector3 l_position = Vector3.Zero;
3016 Quaternion l_orientation = Quaternion.Identity;
3017
3018 m_lastposition = _position;
3019 m_lastorientation = _orientation;
3020
3021 l_position.X = vec.X; 3006 l_position.X = vec.X;
3022 l_position.Y = vec.Y; 3007 l_position.Y = vec.Y;
3023 l_position.Z = vec.Z; 3008 l_position.Z = vec.Z;
3024 l_orientation.X = ori.X; 3009 m_lastposition = _position;
3025 l_orientation.Y = ori.Y; 3010 _position = l_position;
3026 l_orientation.Z = ori.Z; 3011
3027 l_orientation.W = ori.W; 3012 d.Quaternion ori = d.BodyGetQuaternion(Body);
3013 // Quaternion l_orientation = Quaternion.Identity;
3014 _orientation.X = ori.X;
3015 _orientation.Y = ori.Y;
3016 _orientation.Z = ori.Z;
3017 _orientation.W = ori.W;
3018 m_lastorientation = _orientation;
3019
3020 d.Vector3 vel = d.BodyGetLinearVel(Body);
3021 m_lastVelocity = _velocity;
3022 _velocity.X = vel.X;
3023 _velocity.Y = vel.Y;
3024 _velocity.Z = vel.Z;
3025 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3026
3027 d.Vector3 torque = d.BodyGetTorque(Body);
3028 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3029
3030 base.RequestPhysicsterseUpdate();
3031
3028//Console.WriteLine("Move {0} at {1}", m_primName, l_position); 3032//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3029 3033
3030 // Check if outside region horizontally 3034 // Check if outside region
3031 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || 3035 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3032 l_position.X < 0f || 3036 if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit))
3033 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || 3037 {
3034 l_position.Y < 0f) 3038 l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit);
3039 outside = 1;
3040 }
3041
3042 if (l_position.X < border_limit)
3043 {
3044 l_position.X = border_limit;
3045 outside = 2;
3046 }
3047 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit))
3048 {
3049 l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit);
3050 outside = 3;
3051 }
3052
3053 if (l_position.Y < border_limit)
3035 { 3054 {
3036 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) 3055 l_position.Y = border_limit;
3056 outside = 4;
3057 }
3058
3059 if (outside > 0)
3060 {
3061//Console.WriteLine(" fence = {0}",fence);
3062
3063//Console.WriteLine("Border {0}", l_position);
3064 if (fence == 1) // bounce object off boundary
3065 {
3066 if (revcount == 0)
3067 {
3068 if (outside < 3)
3069 {
3070 _velocity.X = -_velocity.X;
3071 }
3072 else
3073 {
3074 _velocity.Y = -_velocity.Y;
3075 }
3076 if (m_type != Vehicle.TYPE_NONE) Halt();
3077 _position = l_position;
3078 m_taintposition = _position;
3079 m_lastVelocity = _velocity;
3080 _acceleration = Vector3.Zero;
3081 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3082 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
3083 base.RequestPhysicsterseUpdate();
3084
3085 revcount = 25; // wait for object to move away from border
3086 }
3087 } // else old crossing mode
3088 else if (m_crossingfailures < failureLimit)
3037 { // keep trying to cross? 3089 { // keep trying to cross?
3038 _position = l_position; 3090 _position = l_position;
3039 //_parent_scene.remActivePrim(this); 3091 //_parent_scene.remActivePrim(this);
@@ -3043,9 +3095,12 @@ Console.WriteLine(" JointCreateFixed");
3043 else 3095 else
3044 { // Too many tries 3096 { // Too many tries
3045 if (_parent == null) base.RaiseOutOfBounds(l_position); 3097 if (_parent == null) base.RaiseOutOfBounds(l_position);
3098//Console.WriteLine("ROOB 2");
3099
3046 return; // Dont process any other motion? 3100 return; // Dont process any other motion?
3047 } 3101 } // end various methods
3048 } // end outside region horizontally 3102 } // end outside region horizontally
3103
3049 3104
3050 if (l_position.Z < 0) 3105 if (l_position.Z < 0)
3051 { 3106 {
@@ -3057,6 +3112,8 @@ Console.WriteLine(" JointCreateFixed");
3057 3112
3058 //IsPhysical = false; 3113 //IsPhysical = false;
3059 if (_parent == null) base.RaiseOutOfBounds(_position); 3114 if (_parent == null) base.RaiseOutOfBounds(_position);
3115//Console.WriteLine("ROOB 3");
3116
3060 3117
3061 _acceleration.X = 0; // This stuff may stop client display but it has no 3118 _acceleration.X = 0; // This stuff may stop client display but it has no
3062 _acceleration.Y = 0; // effect on the object in phys engine! 3119 _acceleration.Y = 0; // effect on the object in phys engine!
@@ -3081,10 +3138,9 @@ Console.WriteLine(" JointCreateFixed");
3081 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3138 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3082 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3139 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3083 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 3140 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3084 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large 3141 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large
3085 { 3142 {
3086 _zeroFlag = true; 3143 _zeroFlag = true;
3087//Console.WriteLine("ZFT 2");
3088 m_throttleUpdates = false; 3144 m_throttleUpdates = false;
3089 } 3145 }
3090 else 3146 else
@@ -3104,10 +3160,7 @@ Console.WriteLine(" JointCreateFixed");
3104 _acceleration.X = 0; 3160 _acceleration.X = 0;
3105 _acceleration.Y = 0; 3161 _acceleration.Y = 0;
3106 _acceleration.Z = 0; 3162 _acceleration.Z = 0;
3107 //_orientation.w = 0f; 3163
3108 //_orientation.X = 0f;
3109 //_orientation.Y = 0f;
3110 //_orientation.Z = 0f;
3111 m_rotationalVelocity.X = 0; 3164 m_rotationalVelocity.X = 0;
3112 m_rotationalVelocity.Y = 0; 3165 m_rotationalVelocity.Y = 0;
3113 m_rotationalVelocity.Z = 0; 3166 m_rotationalVelocity.Z = 0;
@@ -3132,26 +3185,6 @@ Console.WriteLine(" JointCreateFixed");
3132 base.RequestPhysicsterseUpdate(); 3185 base.RequestPhysicsterseUpdate();
3133 } 3186 }
3134 } 3187 }
3135
3136 m_lastVelocity = _velocity;
3137
3138 _position = l_position;
3139
3140 _velocity.X = vel.X;
3141 _velocity.Y = vel.Y;
3142 _velocity.Z = vel.Z;
3143// Why 2 calcs???
3144// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3145// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3146// _velocity.Y - m_lastVelocity.Y / 0.1f,
3147// _velocity.Z - m_lastVelocity.Z / 0.1f);
3148
3149 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3150
3151 _orientation.X = ori.X;
3152 _orientation.Y = ori.Y;
3153 _orientation.Z = ori.Z;
3154 _orientation.W = ori.W;
3155 m_lastUpdateSent = false; 3188 m_lastUpdateSent = false;
3156 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) 3189 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3157 { 3190 {
@@ -3167,11 +3200,8 @@ Console.WriteLine(" JointCreateFixed");
3167 } 3200 }
3168 m_lastposition = l_position; 3201 m_lastposition = l_position;
3169 3202
3170 /// End of old UpdatePositionAndVelocity insert 3203 /// End UpdatePositionAndVelocity insert
3171 3204
3172//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3173// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3174// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3175 if (m_type != Vehicle.TYPE_NONE) 3205 if (m_type != Vehicle.TYPE_NONE)
3176 { 3206 {
3177 // get body attitude 3207 // get body attitude
@@ -3202,12 +3232,13 @@ Console.WriteLine(" JointCreateFixed");
3202 { 3232 {
3203 m_lLinMotorDVel = Vector3.Zero; 3233 m_lLinMotorDVel = Vector3.Zero;
3204 } 3234 }
3205 else 3235
3236 /* else
3206 { 3237 {
3207 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; 3238 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3208 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; 3239 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3209 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; 3240 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3210 } 3241 } */
3211 } // end linear motor decay 3242 } // end linear motor decay
3212 3243
3213 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3244 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
@@ -3235,7 +3266,6 @@ Console.WriteLine(" JointCreateFixed");
3235 if (m_linearFrictionTimescale.Z < 300.0f) 3266 if (m_linearFrictionTimescale.Z < 300.0f)
3236 { 3267 {
3237 float fricfactor = m_linearFrictionTimescale.Z / timestep; 3268 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3238//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
3239 float fricZ = m_lLinObjectVel.Z / fricfactor; 3269 float fricZ = m_lLinObjectVel.Z / fricfactor;
3240 m_lLinObjectVel.Z -= fricZ; 3270 m_lLinObjectVel.Z -= fricZ;
3241 } 3271 }
@@ -3296,14 +3326,17 @@ Console.WriteLine(" JointCreateFixed");
3296 } 3326 }
3297 } 3327 }
3298 else 3328 else
3299 { // not hovering, Gravity rules 3329 { // not hovering
3300 m_wLinObjectVel.Z = vel_now.Z; 3330 if (m_wLinObjectVel.Z == 0f)
3331 { // Gravity rules
3332 m_wLinObjectVel.Z = vel_now.Z;
3333 } // else the motor has it
3301 } 3334 }
3302 3335 linvel = m_wLinObjectVel;
3303 3336
3304 // Vehicle Linear Motion done ======================================= 3337 // Vehicle Linear Motion done =======================================
3305 // Apply velocity 3338 // Apply velocity
3306 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z); 3339 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3307 // apply gravity force 3340 // apply gravity force
3308 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3341 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3309//if(frcount == 0) Console.WriteLine("Grav {0}", grav); 3342//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
@@ -3626,9 +3659,10 @@ Console.WriteLine(" JointCreateFixed");
3626 // react to the physics scene by moving it's position. 3659 // react to the physics scene by moving it's position.
3627 // Avatar to Avatar collisions 3660 // Avatar to Avatar collisions
3628 // Prim to avatar collisions 3661 // Prim to avatar collisions
3662 d.Vector3 dlinvel = vel;
3629 3663
3630 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3664 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3631 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 3665 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3632 d.BodyAddForce(Body, 0, 0, fz); 3666 d.BodyAddForce(Body, 0, 0, fz);
3633 //KF this prevents furthur motions return; 3667 //KF this prevents furthur motions return;
3634 } 3668 }
generated by cgit v1.1 at 2024-12-19 00:55:05 +0000