From 95a915efd88d859c3fc29576e599c6912522e6dc Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Sat, 11 Dec 2010 21:04:03 +0000 Subject: [PATCH] Fix border fence for physicals. Fix llRotLookAt() for Vehicles. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 210 +++++------------- .../Region/Physics/ChOdePlugin/OdePlugin.cs | 4 +- 2 files changed, 61 insertions(+), 153 deletions(-) diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 793774a7c6..16e90fffff 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3001,12 +3001,9 @@ Console.WriteLine(" JointCreateFixed"); // This is a temp patch until proper region crossing is developed. int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; - int fence = _parent_scene.geomRegionFence; + float fence = _parent_scene.geomRegionFence; - float border_limit = 0.05f; // original limit - if (fence == 1) border_limit = 0.5f; // bounce point - - frcount++; // used to limit debug comment output + frcount++; // used to limit debug comment output if (frcount > 50) frcount = 0; @@ -3049,35 +3046,34 @@ Console.WriteLine(" JointCreateFixed"); // Check if outside region // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! - if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit)) + if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) { - l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit); + l_position.X = ((float)_parent_scene.WorldExtents.X - fence); outside = 1; } - if (l_position.X < border_limit) + if (l_position.X < fence) { - l_position.X = border_limit; + l_position.X = fence; outside = 2; } - if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit)) + if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) { - l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit); + l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); outside = 3; } - if (l_position.Y < border_limit) + if (l_position.Y < fence) { - l_position.Y = border_limit; + l_position.Y = fence; outside = 4; } if (outside > 0) { -//Console.WriteLine(" fence = {0}",fence); -//Console.WriteLine("Border {0}", l_position); - if (fence == 1) // bounce object off boundary +//Console.WriteLine("Border {0} fence={1}", l_position, fence); + if (fence > 0.0f) // bounce object off boundary { if (revcount == 0) { @@ -3647,75 +3643,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); } } // end PID MoveToTarget - /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position. - This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it. - - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; - - // PhysicsVector vec = new PhysicsVector(); -// d.Vector3 vel = d.BodyGetLinearVel(Body); - - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - -if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity); - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - // return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end if (m_usePID) - End of old PID system */ - /// Dynamics Hover =================================================================================== // 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 } } // end m_useHoverPID && !m_usePID - /// Dynamics RotLookAt ================================================================================= - if (m_useAPID) - { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: - // get present body rotation - float limit = 1.0f; - float scaler = 50f; // adjusts damping time - float RLAservo = 0f; - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; - float diff_angle; - Vector3 diff_axis; - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); - diff_axis.Normalize(); - if(diff_angle > 0.01f) // diff_angle is always +ve - { -// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate -// RLAservo = timestep / m_APIDStrength * m_mass * scaler; - // rotforce = rotforce * RLAservo * diff_angle ; - // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); - RLAservo = timestep / m_APIDStrength * scaler; - rotforce = rotforce * RLAservo * diff_angle ; - /* - if (m_angularEnable.X == 0) - rotforce.X = 0; - if (m_angularEnable.Y == 0) - rotforce.Y = 0; - if (m_angularEnable.Z == 0) - rotforce.Z = 0; - */ - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); - } // end m_useAPID /// Dynamics Apply Forces =================================================================================== fx *= m_mass; @@ -3889,29 +3771,55 @@ if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName d.BodyAddForce(Body, fx, fy, fz); //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); } // end apply forces - } // end Dynamics + } // end Vehicle/Dynamics + + /// RotLookAt ================================================================================= + if (m_useAPID) + { + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: + // get present body rotation + float limit = 1.0f; + float scaler = 50f; // adjusts damping time + float RLAservo = 0f; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; + float diff_angle; + Vector3 diff_axis; + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); + diff_axis.Normalize(); + if(diff_angle > 0.01f) // diff_angle is always +ve + { +// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if(diff_angle > limit) diff_angle = limit; // cap the rotate rate +// RLAservo = timestep / m_APIDStrength * m_mass * scaler; + // rotforce = rotforce * RLAservo * diff_angle ; + // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); + RLAservo = timestep / m_APIDStrength * scaler; + rotforce = rotforce * RLAservo * diff_angle ; + /* + if (m_angularEnable.X == 0) + rotforce.X = 0; + if (m_angularEnable.Y == 0) + rotforce.Y = 0; + if (m_angularEnable.Z == 0) + rotforce.Z = 0; + */ + d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); +//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } +//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + } // end m_useAPID -/* obsolete? - else - { // is not physical, or is not a body or is selected - // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - return; - } - */ } // end root prims - } // end Move() } // end class } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 86f989302b..e1989e2481 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -224,7 +224,7 @@ namespace OpenSim.Region.Physics.OdePlugin public float bodyPIDG = 25; public int geomCrossingFailuresBeforeOutofbounds = 5; - public int geomRegionFence = 0; + public float geomRegionFence = 0.0f; public float bodyMotorJointMaxforceTensor = 2; @@ -448,7 +448,7 @@ namespace OpenSim.Region.Physics.OdePlugin geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); - geomRegionFence = physicsconfig.GetInt("region_border_fence", 0); + geomRegionFence = physicsconfig.GetFloat("region_border_fence", 0.0f); geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);