AngMotor update 1
Kitto Flora
parent
0c5850237e
commit
8c20610293
|
|
@ -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_angularMotorDVel = Vector3.Zero;
|
||||
}
|
||||
|
||||
private void UpdateLinDecay()
|
||||
|
|
@ -612,53 +618,61 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
//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 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;
|
||||
|
||||
if (m_angularMotorApply > 0)
|
||||
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)
|
||||
{
|
||||
// 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);
|
||||
float decayfactor = m_angularMotorDecayTimescale/pTimestep;
|
||||
Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
|
||||
m_angularMotorDVel -= decayAmount;
|
||||
|
||||
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
|
||||
// velocity may still be acheived.
|
||||
if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
|
||||
{
|
||||
m_angularMotorDVel = Vector3.Zero;
|
||||
}
|
||||
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 (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;
|
||||
//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
|
||||
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);
|
||||
|
||||
// scaling appears better usingsquare-law
|
||||
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
|
||||
vertattr.X += bounce * angularVelocity.X;
|
||||
vertattr.Y += bounce * angularVelocity.Y;
|
||||
|
||||
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);
|
||||
|
||||
// m_lastVertAttractor = vertattr;
|
||||
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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue