Fixed llMoveToTarget() and Rotation Lock/Enable (ChODE/ODEPrim only)
Kitto Flora
parent
74d62901c8
commit
6e2cc98eac
|
|
@ -64,11 +64,16 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
private Vector3 m_taintVelocity;
|
private Vector3 m_taintVelocity;
|
||||||
private Vector3 m_taintTorque;
|
private Vector3 m_taintTorque;
|
||||||
private Quaternion m_taintrot;
|
private Quaternion m_taintrot;
|
||||||
private Vector3 m_angularEnable = Vector3.One; // Current setting
|
private Vector3 m_rotateEnable = Vector3.One; // Current setting
|
||||||
private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL
|
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 Amotor = IntPtr.Zero;
|
||||||
|
private IntPtr AmotorX = IntPtr.Zero;
|
||||||
|
private IntPtr AmotorY = IntPtr.Zero;
|
||||||
|
private IntPtr AmotorZ = IntPtr.Zero;
|
||||||
|
|
||||||
private Vector3 m_PIDTarget;
|
private Vector3 m_PIDTarget;
|
||||||
private float m_PIDTau;
|
private float m_PIDTau;
|
||||||
|
|
@ -729,7 +734,12 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
public override float Buoyancy
|
public override float Buoyancy
|
||||||
{
|
{
|
||||||
get { return m_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)
|
public override void link(PhysicsActor obj)
|
||||||
|
|
@ -744,14 +754,18 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
|
|
||||||
public override void LockAngularMotion(Vector3 axis)
|
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())
|
if (axis.IsFinite())
|
||||||
{
|
{
|
||||||
axis.X = (axis.X > 0) ? 1f : 0f;
|
axis.X = (axis.X > 0) ? 1f : 0f;
|
||||||
axis.Y = (axis.Y > 0) ? 1f : 0f;
|
axis.Y = (axis.Y > 0) ? 1f : 0f;
|
||||||
axis.Z = (axis.Z > 0) ? 1f : 0f;
|
axis.Z = (axis.Z > 0) ? 1f : 0f;
|
||||||
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
|
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
|
||||||
m_taintAngularLock = axis;
|
m_rotateEnableRequest = axis;
|
||||||
|
m_rotateEnableUpdate = true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
@ -1391,10 +1405,10 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
|
|
||||||
if (m_taintCollidesWater != m_collidesWater)
|
if (m_taintCollidesWater != m_collidesWater)
|
||||||
changefloatonwater(timestep);
|
changefloatonwater(timestep);
|
||||||
|
/* obsolete
|
||||||
if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f))
|
if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
|
||||||
changeAngularLock(timestep);
|
changeAngularLock(timestep);
|
||||||
|
*/
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
|
@ -1402,15 +1416,16 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* obsolete
|
||||||
private void changeAngularLock(float timestep)
|
private void changeAngularLock(float timestep)
|
||||||
{
|
{
|
||||||
if (_parent == null)
|
if (_parent == null)
|
||||||
{
|
{
|
||||||
m_angularEnable = m_taintAngularLock;
|
m_angularLock = m_taintAngularLock;
|
||||||
|
m_angularLockSet = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
private void changelink(float timestep)
|
private void changelink(float timestep)
|
||||||
{
|
{
|
||||||
// If the newly set parent is not null
|
// If the newly set parent is not null
|
||||||
|
|
@ -2991,7 +3006,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
if (fence == 1) border_limit = 0.5f; // bounce point
|
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 > 10)
|
if (frcount > 50)
|
||||||
frcount = 0;
|
frcount = 0;
|
||||||
|
|
||||||
if(revcount > 0) revcount--;
|
if(revcount > 0) revcount--;
|
||||||
|
|
@ -3202,6 +3217,89 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
|
|
||||||
/// 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)
|
if (m_type != Vehicle.TYPE_NONE)
|
||||||
{
|
{
|
||||||
// get body attitude
|
// get body attitude
|
||||||
|
|
@ -3420,7 +3518,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
|
//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.ApproxEquals(Vector3.One, 0.003f))
|
||||||
{
|
{
|
||||||
if (m_angularEnable.X == 0)
|
if (m_angularEnable.X == 0)
|
||||||
|
|
@ -3430,7 +3528,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
if (m_angularEnable.Z == 0)
|
if (m_angularEnable.Z == 0)
|
||||||
angObjectVel.Z = 0f;
|
angObjectVel.Z = 0f;
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
|
angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
|
||||||
|
|
||||||
// Vertical attractor section
|
// Vertical attractor section
|
||||||
|
|
@ -3500,35 +3598,58 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
} // end VEHICLES
|
} // end VEHICLES
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
// Dyamics (NON-'VEHICLES') are dealt with here ================================================================
|
||||||
|
|
||||||
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
|
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.
|
//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
|
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
|
||||||
// NB Prims in ODE are no subject to global gravity
|
// NB Prims in ODE are no subject to global gravity
|
||||||
|
// This should only affect gravity operations
|
||||||
|
|
||||||
float m_mass = CalculateMass();
|
float m_mass = CalculateMass();
|
||||||
|
// calculate z-force due togravity on object.
|
||||||
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
|
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);
|
fz = 0; // llMoveToTarget ignores gravity.
|
||||||
// KF - this is for object MoveToTarget.
|
// 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
|
||||||
|
|
||||||
//if (!d.BodyIsEnabled(Body))
|
/* 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);
|
//d.BodySetForce(Body, 0f, 0f, 0f);
|
||||||
|
|
||||||
// no lock; for now it's only called from within Simulate()
|
// 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)
|
(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 velocity is zero, use position control; otherwise, velocity control
|
||||||
|
|
||||||
if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
|
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);
|
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
|
||||||
}
|
}
|
||||||
} // end if (m_usePID)
|
} // end if (m_usePID)
|
||||||
|
End of old PID system */
|
||||||
|
|
||||||
|
|
||||||
/// Dynamics Hover ===================================================================================
|
/// 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)
|
if (m_useHoverPID && !m_usePID)
|
||||||
{
|
{
|
||||||
//Console.WriteLine("Hover " + m_primName);
|
//Console.WriteLine("Hover " + m_primName);
|
||||||
|
|
@ -3709,14 +3833,14 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
// d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
|
// d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
|
||||||
RLAservo = timestep / m_APIDStrength * scaler;
|
RLAservo = timestep / m_APIDStrength * scaler;
|
||||||
rotforce = rotforce * RLAservo * diff_angle ;
|
rotforce = rotforce * RLAservo * diff_angle ;
|
||||||
|
/*
|
||||||
if (m_angularEnable.X == 0)
|
if (m_angularEnable.X == 0)
|
||||||
rotforce.X = 0;
|
rotforce.X = 0;
|
||||||
if (m_angularEnable.Y == 0)
|
if (m_angularEnable.Y == 0)
|
||||||
rotforce.Y = 0;
|
rotforce.Y = 0;
|
||||||
if (m_angularEnable.Z == 0)
|
if (m_angularEnable.Z == 0)
|
||||||
rotforce.Z = 0;
|
rotforce.Z = 0;
|
||||||
|
*/
|
||||||
d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
|
d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
|
||||||
//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
|
//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
|
||||||
}
|
}
|
||||||
|
|
@ -3763,11 +3887,12 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
fy = nmin;
|
fy = nmin;
|
||||||
d.BodyAddForce(Body, fx, fy, fz);
|
d.BodyAddForce(Body, fx, fy, fz);
|
||||||
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
|
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
|
||||||
}
|
} // end apply forces
|
||||||
}
|
} // end Dynamics
|
||||||
}
|
|
||||||
else
|
/* obsolete?
|
||||||
{ // is not physical, or is not a body or is selected
|
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
|
// from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
|
||||||
_velocity.X = 0;
|
_velocity.X = 0;
|
||||||
_velocity.Y = 0;
|
_velocity.Y = 0;
|
||||||
|
|
@ -3781,8 +3906,11 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
m_rotationalVelocity.Y = 0;
|
m_rotationalVelocity.Y = 0;
|
||||||
m_rotationalVelocity.Z = 0;
|
m_rotationalVelocity.Z = 0;
|
||||||
_zeroFlag = true;
|
_zeroFlag = true;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
} // end Move()
|
*/
|
||||||
|
} // end root prims
|
||||||
|
|
||||||
|
} // end Move()
|
||||||
} // end class
|
} // end class
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue