AngMotor update 1

avinationmerge
Kitto Flora 2010-02-02 13:28:42 -05:00
parent 0c5850237e
commit 8c20610293
2 changed files with 144 additions and 80 deletions

View File

@ -108,13 +108,14 @@ namespace OpenSim.Region.Physics.OdePlugin
//Angular properties
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
// private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
//Deflection properties
// private float m_angularDeflectionEfficiency = 0;
@ -227,7 +228,7 @@ namespace OpenSim.Region.Physics.OdePlugin
break;
case Vehicle.ANGULAR_MOTOR_DIRECTION:
m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
m_angularMotorApply = 10;
UpdateAngDecay();
break;
case Vehicle.LINEAR_FRICTION_TIMESCALE:
m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
@ -260,7 +261,7 @@ namespace OpenSim.Region.Physics.OdePlugin
if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
m_angularMotorApply = 10;
UpdateAngDecay();
break;
case Vehicle.LINEAR_FRICTION_TIMESCALE:
m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
@ -310,6 +311,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 1000;
m_linearMotorDecayTimescale = 120;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 1000;
m_angularMotorDecayTimescale = 120;
m_VhoverHeight = 0;
@ -336,6 +338,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 1;
m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 1;
m_angularMotorDecayTimescale = 0.8f;
m_VhoverHeight = 0;
@ -363,6 +366,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 5;
m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 4;
m_angularMotorDecayTimescale = 4;
m_VhoverHeight = 0;
@ -391,6 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 2;
m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 4;
m_angularMotorDecayTimescale = 4;
m_VhoverHeight = 0;
@ -417,6 +422,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 5;
m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 6;
m_angularMotorDecayTimescale = 10;
m_VhoverHeight = 5;
@ -468,8 +474,8 @@ namespace OpenSim.Region.Physics.OdePlugin
m_lLinObjectVel = Vector3.Zero;
m_wLinObjectVel = Vector3.Zero;
m_angularMotorDirection = Vector3.Zero;
m_angularMotorVelocity = Vector3.Zero;
m_lastAngularVelocity = Vector3.Zero;
m_lastAngularVelocity = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
}
private void UpdateLinDecay()
@ -611,54 +617,62 @@ namespace OpenSim.Region.Physics.OdePlugin
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
} // end MoveLinear()
private void UpdateAngDecay()
{
if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
} // else let the motor decay on its own
private void MoveAngular(float pTimestep)
{
/*
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
*/
//if(frcount == 0) Console.WriteLine("MoveAngular ");
//####
// Get what the body is doing, this includes 'external' influences
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero;
d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
// Vector3 initavel = angObjectVel;
// Decay Angular Motor
if (m_angularMotorDecayTimescale < 300.0f)
{
float decayfactor = m_angularMotorDecayTimescale/pTimestep;
Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
m_angularMotorDVel -= decayAmount;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / ( time to get there / step interval )
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
{
m_angularMotorDVel = Vector3.Zero;
}
else
{
if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
}
} // end decay angular motor
//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if(m_verticalAttractionTimescale < 300)
{
float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep);
float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
@ -670,38 +684,88 @@ namespace OpenSim.Region.Physics.OdePlugin
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
{ // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to
// vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
//Console.WriteLine("InvertFlip");
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
verterr *= 0.5f;
// verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
{
//if(frcount == 0)
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// scaling appears better usingsquare-law
float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
float bounce = 1.0f - damped;
// 0 = crit damp, 1 = bouncy
float oavz = angObjectVel.Z; // retain z velocity
angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90.
angObjectVel.Z = oavz;
//if(frcount == 0) Console.WriteLine("VA+");
//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
}
else
{
// else error is very small
angObjectVel.X = 0f;
angObjectVel.Y = 0f;
//if(frcount == 0) Console.WriteLine("VA0");
}
} // else vertical attractor is off
//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
{ // if motor or object have motion
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
if (m_angularMotorTimescale < 300.0f)
{
Vector3 attack_error = m_angularMotorDVel - angObjectVel;
float angfactor = m_angularMotorTimescale/pTimestep;
Vector3 attackAmount = (attack_error/angfactor);
angObjectVel += attackAmount;
//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
}
if (m_angularFrictionTimescale.X < 300.0f)
{
float fricfactor = m_angularFrictionTimescale.X / pTimestep;
angObjectVel.X -= angObjectVel.X / fricfactor;
}
if (m_angularFrictionTimescale.Y < 300.0f)
{
float fricfactor = m_angularFrictionTimescale.Y / pTimestep;
angObjectVel.Y -= angObjectVel.Y / fricfactor;
}
if (m_angularFrictionTimescale.Z < 300.0f)
{
float fricfactor = m_angularFrictionTimescale.Z / pTimestep;
angObjectVel.Z -= angObjectVel.Z / fricfactor;
Console.WriteLine("z fric");
}
} // else no signif. motion
//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
// Bank section tba
// Deflection section tba
//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
m_lastAngularVelocity = angObjectVel;
/*
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
{
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
}
@ -709,13 +773,12 @@ namespace OpenSim.Region.Physics.OdePlugin
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
*/
// Apply to the body
// Vector3 aInc = m_lastAngularVelocity - initavel;
//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
} //end MoveAngular
}

View File

@ -2593,14 +2593,14 @@ Console.WriteLine(" JointCreateFixed");
{
get
{
Vector3 pv = Vector3.Zero;
/* Vector3 pv = Vector3.Zero;
if (_zeroFlag)
return pv;
m_lastUpdateSent = false;
if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
return pv;
*/
return m_rotationalVelocity;
}
set
@ -2827,14 +2827,15 @@ Console.WriteLine(" JointCreateFixed");
_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
if (_velocity.ApproxEquals(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ?????
// {
// m_rotationalVelocity = pv;/
// }
// else
// {
m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
}
// }
//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
_orientation.X = ori.X;