aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
authorKitto Flora2010-12-11 21:04:03 +0000
committerKitto Flora2010-12-11 21:04:03 +0000
commit95a915efd88d859c3fc29576e599c6912522e6dc (patch)
tree51712bceec499260c47da4b5ed1c451bcc6da5c8 /OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
parentAllow floats in the list for llSetPayPrice and silently convert them to int, (diff)
downloadopensim-SC_OLD-95a915efd88d859c3fc29576e599c6912522e6dc.zip
opensim-SC_OLD-95a915efd88d859c3fc29576e599c6912522e6dc.tar.gz
opensim-SC_OLD-95a915efd88d859c3fc29576e599c6912522e6dc.tar.bz2
opensim-SC_OLD-95a915efd88d859c3fc29576e599c6912522e6dc.tar.xz
Fix border fence for physicals. Fix llRotLookAt() for Vehicles.
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs210
1 files changed, 59 insertions, 151 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 793774a..16e90ff 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -3001,12 +3001,9 @@ Console.WriteLine(" JointCreateFixed");
3001 // This is a temp patch until proper region crossing is developed. 3001 // This is a temp patch until proper region crossing is developed.
3002 3002
3003 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; 3003 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
3004 int fence = _parent_scene.geomRegionFence; 3004 float fence = _parent_scene.geomRegionFence;
3005 3005
3006 float border_limit = 0.05f; // original limit 3006 frcount++; // used to limit debug comment output
3007 if (fence == 1) border_limit = 0.5f; // bounce point
3008
3009 frcount++; // used to limit debug comment output
3010 if (frcount > 50) 3007 if (frcount > 50)
3011 frcount = 0; 3008 frcount = 0;
3012 3009
@@ -3049,35 +3046,34 @@ Console.WriteLine(" JointCreateFixed");
3049 3046
3050 // Check if outside region 3047 // Check if outside region
3051 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! 3048 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3052 if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) 3049 if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
3053 { 3050 {
3054 l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); 3051 l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
3055 outside = 1; 3052 outside = 1;
3056 } 3053 }
3057 3054
3058 if (l_position.X < border_limit) 3055 if (l_position.X < fence)
3059 { 3056 {
3060 l_position.X = border_limit; 3057 l_position.X = fence;
3061 outside = 2; 3058 outside = 2;
3062 } 3059 }
3063 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) 3060 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
3064 { 3061 {
3065 l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); 3062 l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
3066 outside = 3; 3063 outside = 3;
3067 } 3064 }
3068 3065
3069 if (l_position.Y < border_limit) 3066 if (l_position.Y < fence)
3070 { 3067 {
3071 l_position.Y = border_limit; 3068 l_position.Y = fence;
3072 outside = 4; 3069 outside = 4;
3073 } 3070 }
3074 3071
3075 if (outside > 0) 3072 if (outside > 0)
3076 { 3073 {
3077//Console.WriteLine(" fence = {0}",fence);
3078 3074
3079//Console.WriteLine("Border {0}", l_position); 3075//Console.WriteLine("Border {0} fence={1}", l_position, fence);
3080 if (fence == 1) // bounce object off boundary 3076 if (fence > 0.0f) // bounce object off boundary
3081 { 3077 {
3082 if (revcount == 0) 3078 if (revcount == 0)
3083 { 3079 {
@@ -3647,75 +3643,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3647 } 3643 }
3648 } // end PID MoveToTarget 3644 } // end PID MoveToTarget
3649 3645
3650 /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position.
3651 This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it.
3652
3653 //if (!d.BodyIsEnabled(Body))
3654 //d.BodySetForce(Body, 0f, 0f, 0f);
3655
3656 // no lock; for now it's only called from within Simulate()
3657
3658 // If the PID Controller isn't active then we set our force
3659 // calculating base velocity to the current position
3660
3661 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3662 {
3663 //PID_G = PID_G / m_PIDTau;
3664 m_PIDTau = 1;
3665 }
3666
3667 if ((PID_G - m_PIDTau) <= 0)
3668 {
3669 PID_G = m_PIDTau + 1;
3670 }
3671 //PidStatus = true;
3672
3673 // PhysicsVector vec = new PhysicsVector();
3674// d.Vector3 vel = d.BodyGetLinearVel(Body);
3675
3676 d.Vector3 pos = d.BodyGetPosition(Body);
3677 _target_velocity =
3678 new Vector3(
3679 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3680 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3681 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3682 );
3683
3684if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity);
3685 // if velocity is zero, use position control; otherwise, velocity control
3686
3687 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3688 {
3689 // keep track of where we stopped. No more slippin' & slidin'
3690
3691 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3692 // react to the physics scene by moving it's position.
3693 // Avatar to Avatar collisions
3694 // Prim to avatar collisions
3695
3696 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3697 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3698 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3699 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3700 d.BodySetLinearVel(Body, 0, 0, 0);
3701 d.BodyAddForce(Body, 0, 0, fz);
3702 // return;
3703 }
3704 else
3705 {
3706 _zeroFlag = false;
3707
3708 // We're flying and colliding with something
3709 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3710 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3711
3712 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3713
3714 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3715 }
3716 } // end if (m_usePID)
3717 End of old PID system */
3718
3719 3646
3720 /// Dynamics Hover =================================================================================== 3647 /// Dynamics Hover ===================================================================================
3721 // Hover PID Controller can only run if the PIDcontroller is not in use. 3648 // Hover PID Controller can only run if the PIDcontroller is not in use.
@@ -3802,51 +3729,6 @@ if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName
3802 } 3729 }
3803 } // end m_useHoverPID && !m_usePID 3730 } // end m_useHoverPID && !m_usePID
3804 3731
3805 /// Dynamics RotLookAt =================================================================================
3806 if (m_useAPID)
3807 {
3808 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3809 // Quaternion m_APIDTarget
3810 // float m_APIDStrength // From SL experiments, this is the time to get there
3811 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3812 // Also in SL the mass of the object has no effect on time to get there.
3813 // Factors:
3814 // get present body rotation
3815 float limit = 1.0f;
3816 float scaler = 50f; // adjusts damping time
3817 float RLAservo = 0f;
3818
3819 d.Quaternion rot = d.BodyGetQuaternion(Body);
3820 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3821 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3822 float diff_angle;
3823 Vector3 diff_axis;
3824 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3825 diff_axis.Normalize();
3826 if(diff_angle > 0.01f) // diff_angle is always +ve
3827 {
3828// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3829 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3830 rotforce = rotforce * rotq;
3831 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3832// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3833 // rotforce = rotforce * RLAservo * diff_angle ;
3834 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3835 RLAservo = timestep / m_APIDStrength * scaler;
3836 rotforce = rotforce * RLAservo * diff_angle ;
3837 /*
3838 if (m_angularEnable.X == 0)
3839 rotforce.X = 0;
3840 if (m_angularEnable.Y == 0)
3841 rotforce.Y = 0;
3842 if (m_angularEnable.Z == 0)
3843 rotforce.Z = 0;
3844 */
3845 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3846//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3847 }
3848//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3849 } // end m_useAPID
3850 3732
3851 /// Dynamics Apply Forces =================================================================================== 3733 /// Dynamics Apply Forces ===================================================================================
3852 fx *= m_mass; 3734 fx *= m_mass;
@@ -3889,29 +3771,55 @@ if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName
3889 d.BodyAddForce(Body, fx, fy, fz); 3771 d.BodyAddForce(Body, fx, fy, fz);
3890//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3772//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3891 } // end apply forces 3773 } // end apply forces
3892 } // end Dynamics 3774 } // end Vehicle/Dynamics
3775
3776 /// RotLookAt =================================================================================
3777 if (m_useAPID)
3778 {
3779 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3780 // Quaternion m_APIDTarget
3781 // float m_APIDStrength // From SL experiments, this is the time to get there
3782 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3783 // Also in SL the mass of the object has no effect on time to get there.
3784 // Factors:
3785 // get present body rotation
3786 float limit = 1.0f;
3787 float scaler = 50f; // adjusts damping time
3788 float RLAservo = 0f;
3789
3790 d.Quaternion rot = d.BodyGetQuaternion(Body);
3791 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3792 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3793 float diff_angle;
3794 Vector3 diff_axis;
3795 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3796 diff_axis.Normalize();
3797 if(diff_angle > 0.01f) // diff_angle is always +ve
3798 {
3799// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3800 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3801 rotforce = rotforce * rotq;
3802 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3803// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3804 // rotforce = rotforce * RLAservo * diff_angle ;
3805 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3806 RLAservo = timestep / m_APIDStrength * scaler;
3807 rotforce = rotforce * RLAservo * diff_angle ;
3808 /*
3809 if (m_angularEnable.X == 0)
3810 rotforce.X = 0;
3811 if (m_angularEnable.Y == 0)
3812 rotforce.Y = 0;
3813 if (m_angularEnable.Z == 0)
3814 rotforce.Z = 0;
3815 */
3816 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3817//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3818 }
3819//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3820 } // end m_useAPID
3893 3821
3894/* obsolete?
3895 else
3896 { // is not physical, or is not a body or is selected
3897 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
3898 _velocity.X = 0;
3899 _velocity.Y = 0;
3900 _velocity.Z = 0;
3901
3902 _acceleration.X = 0;
3903 _acceleration.Y = 0;
3904 _acceleration.Z = 0;
3905
3906 m_rotationalVelocity.X = 0;
3907 m_rotationalVelocity.Y = 0;
3908 m_rotationalVelocity.Z = 0;
3909 _zeroFlag = true;
3910 return;
3911 }
3912 */
3913 } // end root prims 3822 } // end root prims
3914
3915 } // end Move() 3823 } // end Move()
3916 } // end class 3824 } // end class
3917} 3825}