OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

205
OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
View File

@ -108,13 +108,14 @@ namespace OpenSim.Region.Physics.OdePlugin
//Angular properties //Angular properties
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 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 Attack rate set by LSL
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
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_lastAngularVelocity = Vector3.Zero; // what was last applied to body
// private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
//Deflection properties //Deflection properties
// private float m_angularDeflectionEfficiency = 0; // private float m_angularDeflectionEfficiency = 0;
@ -227,7 +228,7 @@ namespace OpenSim.Region.Physics.OdePlugin
break; break;
case Vehicle.ANGULAR_MOTOR_DIRECTION: case Vehicle.ANGULAR_MOTOR_DIRECTION:
m_angularMotorDirection = new Vector3(pValue, pValue, pValue); m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
m_angularMotorApply = 10; UpdateAngDecay();
break; break;
case Vehicle.LINEAR_FRICTION_TIMESCALE: case Vehicle.LINEAR_FRICTION_TIMESCALE:
m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 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.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;
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; break;
case Vehicle.LINEAR_FRICTION_TIMESCALE: case Vehicle.LINEAR_FRICTION_TIMESCALE:
m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
@ -310,6 +311,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 1000; m_linearMotorTimescale = 1000;
m_linearMotorDecayTimescale = 120; m_linearMotorDecayTimescale = 120;
m_angularMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 1000; m_angularMotorTimescale = 1000;
m_angularMotorDecayTimescale = 120; m_angularMotorDecayTimescale = 120;
m_VhoverHeight = 0; m_VhoverHeight = 0;
@ -336,6 +338,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 1; m_linearMotorTimescale = 1;
m_linearMotorDecayTimescale = 60; m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 1; m_angularMotorTimescale = 1;
m_angularMotorDecayTimescale = 0.8f; m_angularMotorDecayTimescale = 0.8f;
m_VhoverHeight = 0; m_VhoverHeight = 0;
@ -363,6 +366,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 5; m_linearMotorTimescale = 5;
m_linearMotorDecayTimescale = 60; m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 4; m_angularMotorTimescale = 4;
m_angularMotorDecayTimescale = 4; m_angularMotorDecayTimescale = 4;
m_VhoverHeight = 0; m_VhoverHeight = 0;
@ -391,6 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 2; m_linearMotorTimescale = 2;
m_linearMotorDecayTimescale = 60; m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 4; m_angularMotorTimescale = 4;
m_angularMotorDecayTimescale = 4; m_angularMotorDecayTimescale = 4;
m_VhoverHeight = 0; m_VhoverHeight = 0;
@ -417,6 +422,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = 5; m_linearMotorTimescale = 5;
m_linearMotorDecayTimescale = 60; m_linearMotorDecayTimescale = 60;
m_angularMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero;
m_angularMotorDVel = Vector3.Zero;
m_angularMotorTimescale = 6; m_angularMotorTimescale = 6;
m_angularMotorDecayTimescale = 10; m_angularMotorDecayTimescale = 10;
m_VhoverHeight = 5; m_VhoverHeight = 5;
@ -468,8 +474,8 @@ namespace OpenSim.Region.Physics.OdePlugin
m_lLinObjectVel = Vector3.Zero; m_lLinObjectVel = Vector3.Zero;
m_wLinObjectVel = Vector3.Zero; m_wLinObjectVel = Vector3.Zero;
m_angularMotorDirection = 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() private void UpdateLinDecay()
@ -611,54 +617,62 @@ namespace OpenSim.Region.Physics.OdePlugin
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
//if(frcount == 0) Console.WriteLine("Grav {0}", grav); //if(frcount == 0) Console.WriteLine("Grav {0}", grav);
} // end MoveLinear() } // 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 void MoveAngular(float pTimestep)
{ {
/* /*
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 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 Attack rate set by LSL
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
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_angularMotorDVel = Vector3.Zero; // decayed angular motor
private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
*/ */
//if(frcount == 0) Console.WriteLine("MoveAngular "); //if(frcount == 0) Console.WriteLine("MoveAngular ");
//####
// Get what the body is doing, this includes 'external' influences // Get what the body is doing, this includes 'external' influences
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero; 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) if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
{ {
// ramp up to new value m_angularMotorDVel = Vector3.Zero;
// current velocity += error / ( time to get there / step interval ) }
// requested speed - last motor speed else
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); {
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); 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;
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected }
// velocity may still be acheived. } // end decay angular motor
} //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
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(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
// Vertical attractor section // Vertical attractor section
Vector3 vertattr = Vector3.Zero; Vector3 vertattr = Vector3.Zero;
if(m_verticalAttractionTimescale < 300) if(m_verticalAttractionTimescale < 300)
{ {
float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep); float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation // get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body); d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 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. // 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 // 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. // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f) 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.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y; verterr.Y = 2.0f - verterr.Y;
} }
// Error is 0 (no error) to +/- 2 (max error) verterr *= 0.5f;
// scale it by VAservo // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
verterr = verterr * VAservo;
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); //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 // Bank section tba
// Deflection section tba // Deflection section tba
//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
// Sum velocities m_lastAngularVelocity = angObjectVel;
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection /*
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{ {
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 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. 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 // 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); d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
} //end MoveAngular } //end MoveAngular
} }

19
OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
View File

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