From 6e2cc98eacf48a24ae19433a3f6c2ba9693c5b9e Mon Sep 17 00:00:00 2001 From: Kitto Flora Date: Tue, 8 Jun 2010 16:15:16 -0400 Subject: Fixed llMoveToTarget() and Rotation Lock/Enable (ChODE/ODEPrim only) --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 230 ++++++++++++++++++++------ 1 file changed, 179 insertions(+), 51 deletions(-) (limited to 'OpenSim/Region/Physics/ChOdePlugin') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index fbe52c8..7ce01dc 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -64,11 +64,16 @@ namespace OpenSim.Region.Physics.OdePlugin private Vector3 m_taintVelocity; private Vector3 m_taintTorque; private Quaternion m_taintrot; - private Vector3 m_angularEnable = Vector3.One; // Current setting - private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL - - + private Vector3 m_rotateEnable = Vector3.One; // Current setting + private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL + private bool m_rotateEnableUpdate = false; + private Vector3 m_lockX; + private Vector3 m_lockY; + private Vector3 m_lockZ; private IntPtr Amotor = IntPtr.Zero; + private IntPtr AmotorX = IntPtr.Zero; + private IntPtr AmotorY = IntPtr.Zero; + private IntPtr AmotorZ = IntPtr.Zero; private Vector3 m_PIDTarget; private float m_PIDTau; @@ -729,7 +734,12 @@ namespace OpenSim.Region.Physics.OdePlugin public override float Buoyancy { get { return m_buoyancy; } - set { m_buoyancy = value; } +// set { m_buoyancy = value; } + set { + m_buoyancy = value; + + Console.WriteLine("m_buoyancy={0}", m_buoyancy); + } } public override void link(PhysicsActor obj) @@ -744,14 +754,18 @@ namespace OpenSim.Region.Physics.OdePlugin public override void LockAngularMotion(Vector3 axis) { - // reverse the zero/non zero values for ODE. + // This is actually ROTATION ENABLE, not a lock. + // default is <1,1,1> which is all enabled. + // The lock value is updated inside Move(), no point in using the taint system. + // OS 'm_taintAngularLock' etc change to m_rotateEnable. if (axis.IsFinite()) { axis.X = (axis.X > 0) ? 1f : 0f; axis.Y = (axis.Y > 0) ? 1f : 0f; axis.Z = (axis.Z > 0) ? 1f : 0f; m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - m_taintAngularLock = axis; + m_rotateEnableRequest = axis; + m_rotateEnableUpdate = true; } else { @@ -1391,10 +1405,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); - - if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) +/* obsolete + if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) changeAngularLock(timestep); - + */ } else { @@ -1402,15 +1416,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } - +/* obsolete private void changeAngularLock(float timestep) { if (_parent == null) { - m_angularEnable = m_taintAngularLock; + m_angularLock = m_taintAngularLock; + m_angularLockSet = true; } } - + */ private void changelink(float timestep) { // If the newly set parent is not null @@ -2991,7 +3006,7 @@ Console.WriteLine(" JointCreateFixed"); if (fence == 1) border_limit = 0.5f; // bounce point frcount++; // used to limit debug comment output - if (frcount > 10) + if (frcount > 50) frcount = 0; if(revcount > 0) revcount--; @@ -3200,8 +3215,91 @@ Console.WriteLine(" JointCreateFixed"); } m_lastposition = l_position; - /// End UpdatePositionAndVelocity insert - + /// End UpdatePositionAndVelocity insert + + + // Rotation lock ===================================== + if(m_rotateEnableUpdate) + { + // Snapshot current angles, set up Amotor(s) + m_rotateEnableUpdate = false; + m_rotateEnable = m_rotateEnableRequest; +Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; +Console.WriteLine("Old Amotor Destroyed"); + } + + if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) + { // not all are enabled + d.Quaternion r = d.BodyGetQuaternion(Body); + Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); + // extract the axes vectors + Vector3 vX = new Vector3(1f,0f,0f); + Vector3 vY = new Vector3(0f,1f,0f); + Vector3 vZ = new Vector3(0f,0f,1f); + vX = vX * locrot; + vY = vY * locrot; + vZ = vZ * locrot; + // snapshot the current angle vectors + m_lockX = vX; + m_lockY = vY; + m_lockZ = vZ; + // m_lockRot = locrot; + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + d.JointSetAMotorMode(Amotor, 0); // User mode?? +Console.WriteLine("New Amotor Created for {0}", m_primName); + + float axisnum = 3; // how many to lock + axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); + d.JointSetAMotorNumAxes(Amotor,(int)axisnum); +Console.WriteLine("AxisNum={0}",(int)axisnum); + + int i = 0; + + if (m_rotateEnable.X == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); +Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); + i++; + } + + if (m_rotateEnable.Y == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); +Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); + i++; + } + + if (m_rotateEnable.Z == 0) + { + d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); +Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); + i++; + } + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); + d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + } // else none are locked + } // end Rotation Update + + + // VEHICLE processing ========================================== if (m_type != Vehicle.TYPE_NONE) { // get body attitude @@ -3420,7 +3518,7 @@ Console.WriteLine(" JointCreateFixed"); //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - // Rotation Axis Disables: + /* // Rotation Axis Disables: if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) { if (m_angularEnable.X == 0) @@ -3430,7 +3528,7 @@ Console.WriteLine(" JointCreateFixed"); if (m_angularEnable.Z == 0) angObjectVel.Z = 0f; } - + */ angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation // Vertical attractor section @@ -3500,35 +3598,58 @@ Console.WriteLine(" JointCreateFixed"); } // end VEHICLES else { + // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here - /// Dynamics Angular Lock ======================================================================== - if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularEnable.X == 0) - avel2.X = 0; - if (m_angularEnable.Y == 0) - avel2.Y = 0; - if (m_angularEnable.Z == 0) - avel2.Z = 0; - d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); - } - - - /// Dynamics Buoyancy =============================================================================== + + /// Dynamics Buoyancy //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up // NB Prims in ODE are no subject to global gravity + // This should only affect gravity operations + float m_mass = CalculateMass(); + // calculate z-force due togravity on object. fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if (m_usePID) + if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. { -//if(frcount == 0) Console.WriteLine("PID " + m_primName); - // KF - this is for object MoveToTarget. - - //if (!d.BodyIsEnabled(Body)) + fz = 0; // llMoveToTarget ignores gravity. + // it also ignores mass of object, and any physical resting on it. + // Vector3 m_PIDTarget is where we are going + // float m_PIDTau is time to get there + fx = 0; + fy = 0; + d.Vector3 pos = d.BodyGetPosition(Body); + Vector3 error = new Vector3( + (m_PIDTarget.X - pos.X), + (m_PIDTarget.Y - pos.Y), + (m_PIDTarget.Z - pos.Z)); + if (error.ApproxEquals(Vector3.Zero,0.01f)) + { // Very close, Jump there and quit move + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + _target_velocity = Vector3.Zero; + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + else + { + float scale = 50.0f * timestep / m_PIDTau; + if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) + { + // Nearby, quit update of velocity + } + else + { // Far, calc damped velocity + _target_velocity = error * scale; + } + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // 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() @@ -3559,6 +3680,7 @@ Console.WriteLine(" JointCreateFixed"); (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)) @@ -3591,9 +3713,11 @@ Console.WriteLine(" JointCreateFixed"); 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 needs to be mutually exlusive to MoveTo PID controller + // Hover PID Controller can only run if the PIDcontroller is not in use. if (m_useHoverPID && !m_usePID) { //Console.WriteLine("Hover " + m_primName); @@ -3709,14 +3833,14 @@ Console.WriteLine(" JointCreateFixed"); // 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); } @@ -3763,11 +3887,12 @@ Console.WriteLine(" JointCreateFixed"); fy = nmin; d.BodyAddForce(Body, fx, fy, fz); //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - } - } - else - { // is not physical, or is not a body or is selected + } // end apply forces + } // end Dynamics + +/* 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; @@ -3781,8 +3906,11 @@ Console.WriteLine(" JointCreateFixed"); m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; _zeroFlag = true; - return; - } - } // end Move() + return; + } + */ + } // end root prims + + } // end Move() } // end class } -- cgit v1.1