1 files changed, 136 insertions, 70 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
index 14d5caa..4eb3313 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -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 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_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,65 @@ 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 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 ");   
        
+//#### 
                // 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 1. In SL this also depends on attack rate! decay ~= 23/Attack.
+                float atk_decayfactor = 23.0f  / (m_angularMotorTimescale * pTimestep); 
+                m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
+                // Decay Angular Motor 2.
+                if (m_angularMotorDecayTimescale < 300.0f)
+                {
+                                float decayfactor = m_angularMotorDecayTimescale/pTimestep;                     // df = Dec / pts
+                    Vector3 decayAmount = (m_angularMotorDVel/decayfactor);                             // v-da = v-Dvel / df  = v-Dvel * pts / Dec
+                    m_angularMotorDVel -= decayAmount;                                                          // v-Dvel = v-Dvel - (v-Dvel / df  = v-Dvel * pts / Dec)
                
-                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
                        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 +687,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)
+                                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);       
+                        
-                                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 
+                                        // scaling appears better usingsquare-law
-                                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
+                                        float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
-                                vertattr.X =    verterr.Y;
+                                        float bounce = 1.0f - damped;  
-                                vertattr.Y =  - verterr.X;
+                                        // 0 = crit damp, 1 = bouncy
-                                vertattr.Z = 0f;
+                                        float oavz = angObjectVel.Z;   // retain z velocity
-                                
+                                        angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
-                                                                        // scaling appears better usingsquare-law
+                                        angObjectVel = angObjectVel + (vertattr * VAservo *  0.0667f * damped); // damped, good @ < 90.
-                                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);  
+                                        angObjectVel.Z = oavz;
-                                vertattr.X += bounce * angularVelocity.X;
+//if(frcount == 0) Console.WriteLine("VA+");                                    
-                                vertattr.Y += bounce * angularVelocity.Y;
+//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
                        
-        //              m_lastVertAttractor = vertattr;
+//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 = 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);
                        }
@@ -709,13 +776,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
        }

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 14d5caa..4eb3313 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -108,13 +108,14 @@ namespace OpenSim.Region.Physics.OdePlugin
108		108
109	//Angular properties	109	//Angular properties
110	private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor	110	private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
111	private int m_angularMotorApply = 0; // application frame counter	111
112	private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity	112	private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
113	private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate	113	private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
114	private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate	114	private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
115	private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate	115
		116	private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
		117	// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
116	private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body	118	private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
117	// private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
118		119
119	//Deflection properties	120	//Deflection properties
120	// private float m_angularDeflectionEfficiency = 0;	121	// private float m_angularDeflectionEfficiency = 0;
@@ -227,7 +228,7 @@ namespace OpenSim.Region.Physics.OdePlugin
227	break;	228	break;
228	case Vehicle.ANGULAR_MOTOR_DIRECTION:	229	case Vehicle.ANGULAR_MOTOR_DIRECTION:
229	m_angularMotorDirection = new Vector3(pValue, pValue, pValue);	230	m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
230	m_angularMotorApply = 10;	231	UpdateAngDecay();
231	break;	232	break;
232	case Vehicle.LINEAR_FRICTION_TIMESCALE:	233	case Vehicle.LINEAR_FRICTION_TIMESCALE:
233	m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);	234	m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -260,7 +261,7 @@ namespace OpenSim.Region.Physics.OdePlugin
260	if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;	261	if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
261	if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;	262	if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
262	if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;	263	if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
263	m_angularMotorApply = 10;	264	UpdateAngDecay();
264	break;	265	break;
265	case Vehicle.LINEAR_FRICTION_TIMESCALE:	266	case Vehicle.LINEAR_FRICTION_TIMESCALE:
266	m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);	267	m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -310,6 +311,7 @@ namespace OpenSim.Region.Physics.OdePlugin
310	m_linearMotorTimescale = 1000;	311	m_linearMotorTimescale = 1000;
311	m_linearMotorDecayTimescale = 120;	312	m_linearMotorDecayTimescale = 120;
312	m_angularMotorDirection = Vector3.Zero;	313	m_angularMotorDirection = Vector3.Zero;
		314	m_angularMotorDVel = Vector3.Zero;
313	m_angularMotorTimescale = 1000;	315	m_angularMotorTimescale = 1000;
314	m_angularMotorDecayTimescale = 120;	316	m_angularMotorDecayTimescale = 120;
315	m_VhoverHeight = 0;	317	m_VhoverHeight = 0;
@@ -336,6 +338,7 @@ namespace OpenSim.Region.Physics.OdePlugin
336	m_linearMotorTimescale = 1;	338	m_linearMotorTimescale = 1;
337	m_linearMotorDecayTimescale = 60;	339	m_linearMotorDecayTimescale = 60;
338	m_angularMotorDirection = Vector3.Zero;	340	m_angularMotorDirection = Vector3.Zero;
		341	m_angularMotorDVel = Vector3.Zero;
339	m_angularMotorTimescale = 1;	342	m_angularMotorTimescale = 1;
340	m_angularMotorDecayTimescale = 0.8f;	343	m_angularMotorDecayTimescale = 0.8f;
341	m_VhoverHeight = 0;	344	m_VhoverHeight = 0;
@@ -363,6 +366,7 @@ namespace OpenSim.Region.Physics.OdePlugin
363	m_linearMotorTimescale = 5;	366	m_linearMotorTimescale = 5;
364	m_linearMotorDecayTimescale = 60;	367	m_linearMotorDecayTimescale = 60;
365	m_angularMotorDirection = Vector3.Zero;	368	m_angularMotorDirection = Vector3.Zero;
		369	m_angularMotorDVel = Vector3.Zero;
366	m_angularMotorTimescale = 4;	370	m_angularMotorTimescale = 4;
367	m_angularMotorDecayTimescale = 4;	371	m_angularMotorDecayTimescale = 4;
368	m_VhoverHeight = 0;	372	m_VhoverHeight = 0;
@@ -391,6 +395,7 @@ namespace OpenSim.Region.Physics.OdePlugin
391	m_linearMotorTimescale = 2;	395	m_linearMotorTimescale = 2;
392	m_linearMotorDecayTimescale = 60;	396	m_linearMotorDecayTimescale = 60;
393	m_angularMotorDirection = Vector3.Zero;	397	m_angularMotorDirection = Vector3.Zero;
		398	m_angularMotorDVel = Vector3.Zero;
394	m_angularMotorTimescale = 4;	399	m_angularMotorTimescale = 4;
395	m_angularMotorDecayTimescale = 4;	400	m_angularMotorDecayTimescale = 4;
396	m_VhoverHeight = 0;	401	m_VhoverHeight = 0;
@@ -417,6 +422,7 @@ namespace OpenSim.Region.Physics.OdePlugin
417	m_linearMotorTimescale = 5;	422	m_linearMotorTimescale = 5;
418	m_linearMotorDecayTimescale = 60;	423	m_linearMotorDecayTimescale = 60;
419	m_angularMotorDirection = Vector3.Zero;	424	m_angularMotorDirection = Vector3.Zero;
		425	m_angularMotorDVel = Vector3.Zero;
420	m_angularMotorTimescale = 6;	426	m_angularMotorTimescale = 6;
421	m_angularMotorDecayTimescale = 10;	427	m_angularMotorDecayTimescale = 10;
422	m_VhoverHeight = 5;	428	m_VhoverHeight = 5;
@@ -468,8 +474,8 @@ namespace OpenSim.Region.Physics.OdePlugin
468	m_lLinObjectVel = Vector3.Zero;	474	m_lLinObjectVel = Vector3.Zero;
469	m_wLinObjectVel = Vector3.Zero;	475	m_wLinObjectVel = Vector3.Zero;
470	m_angularMotorDirection = Vector3.Zero;	476	m_angularMotorDirection = Vector3.Zero;
471	m_angularMotorVelocity = Vector3.Zero;	477	m_lastAngularVelocity = Vector3.Zero;
472	m_lastAngularVelocity = Vector3.Zero;	478	m_angularMotorDVel = Vector3.Zero;
473	}	479	}
474		480
475	private void UpdateLinDecay()	481	private void UpdateLinDecay()
@@ -611,54 +617,65 @@ namespace OpenSim.Region.Physics.OdePlugin
611	d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);	617	d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
612	//if(frcount == 0) Console.WriteLine("Grav {0}", grav);	618	//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
613	} // end MoveLinear()	619	} // end MoveLinear()
		620
		621	private void UpdateAngDecay()
		622	{
		623	if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
		624	if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
		625	if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
		626	} // else let the motor decay on its own
614		627
615	private void MoveAngular(float pTimestep)	628	private void MoveAngular(float pTimestep)
616	{	629	{
617	/*	630	/*
618	private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor	631	private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
619	private int m_angularMotorApply = 0; // application frame counter	632
620	private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)	633	private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
621	private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate	634	private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
622	private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate	635	private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
623	private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate	636
624	private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body	637	private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
		638	private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
625	*/	639	*/
626	//if(frcount == 0) Console.WriteLine("MoveAngular ");	640	//if(frcount == 0) Console.WriteLine("MoveAngular ");
627		641
		642	//####
628	// Get what the body is doing, this includes 'external' influences	643	// Get what the body is doing, this includes 'external' influences
629	d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);	644	d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
630	// Vector3 angularVelocity = Vector3.Zero;	645	Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
		646	//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
		647	// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
		648	// Vector3 initavel = angObjectVel;
		649	// Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
		650	float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep);
		651	m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
		652	// Decay Angular Motor 2.
		653	if (m_angularMotorDecayTimescale < 300.0f)
		654	{
		655	float decayfactor = m_angularMotorDecayTimescale/pTimestep; // df = Dec / pts
		656	Vector3 decayAmount = (m_angularMotorDVel/decayfactor); // v-da = v-Dvel / df = v-Dvel * pts / Dec
		657	m_angularMotorDVel -= decayAmount; // v-Dvel = v-Dvel - (v-Dvel / df = v-Dvel * pts / Dec)
631		658
632	if (m_angularMotorApply > 0)	659	if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
633	{	660	{
634	// ramp up to new value	661	m_angularMotorDVel = Vector3.Zero;
635	// current velocity += error / ( time to get there / step interval )	662	}
636	// requested speed - last motor speed	663	else
637	m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);	664	{
638	m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);	665	if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
639	m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);	666	if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
640		667	if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
641	m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected	668	}
642	// velocity may still be acheived.	669	} // end decay angular motor
643	}	670	//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
644	else
645	{
646	// no motor recently applied, keep the body velocity
647	/* m_angularMotorVelocity.X = angularVelocity.X;
648	m_angularMotorVelocity.Y = angularVelocity.Y;
649	m_angularMotorVelocity.Z = angularVelocity.Z; */
650
651	// and decay the velocity
652	m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
653	} // end motor section
654
655		671
		672	//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
656	// Vertical attractor section	673	// Vertical attractor section
657	Vector3 vertattr = Vector3.Zero;	674	Vector3 vertattr = Vector3.Zero;
658		675
659	if(m_verticalAttractionTimescale < 300)	676	if(m_verticalAttractionTimescale < 300)
660	{	677	{
661	float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep);	678	float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
662	// get present body rotation	679	// get present body rotation
663	d.Quaternion rot = d.BodyGetQuaternion(Body);	680	d.Quaternion rot = d.BodyGetQuaternion(Body);
664	Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);	681	Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
@@ -670,38 +687,88 @@ namespace OpenSim.Region.Physics.OdePlugin
670	// 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.	687	// 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.
671	// 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	688	// 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
672	// negative. Similar for tilt and \|.Y\|. .X and .Y must be modulated to prevent a stable inverted body.	689	// negative. Similar for tilt and \|.Y\|. .X and .Y must be modulated to prevent a stable inverted body.
		690
673	if (verterr.Z < 0.0f)	691	if (verterr.Z < 0.0f)
674	{	692	{ // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to
		693	// vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
		694	//Console.WriteLine("InvertFlip");
675	verterr.X = 2.0f - verterr.X;	695	verterr.X = 2.0f - verterr.X;
676	verterr.Y = 2.0f - verterr.Y;	696	verterr.Y = 2.0f - verterr.Y;
677	}	697	}
678	// Error is 0 (no error) to +/- 2 (max error)	698	verterr *= 0.5f;
679	// scale it by VAservo	699	// verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
680	verterr = verterr * VAservo;	700
		701	if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) \|\| (verterr.Z < 0.49f))
		702	{
		703	//if(frcount == 0)
		704	// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
		705	// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
		706	vertattr.X = verterr.Y;
		707	vertattr.Y = - verterr.X;
		708	vertattr.Z = 0f;
681	//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);	709	//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
682		710
683	// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so	711	// scaling appears better usingsquare-law
684	// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.	712	float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
685	vertattr.X = verterr.Y;	713	float bounce = 1.0f - damped;
686	vertattr.Y = - verterr.X;	714	// 0 = crit damp, 1 = bouncy
687	vertattr.Z = 0f;	715	float oavz = angObjectVel.Z; // retain z velocity
688		716	angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
689	// scaling appears better usingsquare-law	717	angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90.
690	float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);	718	angObjectVel.Z = oavz;
691	vertattr.X += bounce * angularVelocity.X;	719	//if(frcount == 0) Console.WriteLine("VA+");
692	vertattr.Y += bounce * angularVelocity.Y;	720	//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
693		721	}
		722	else
		723	{
		724	// else error is very small
		725	angObjectVel.X = 0f;
		726	angObjectVel.Y = 0f;
		727	//if(frcount == 0) Console.WriteLine("VA0");
		728	}
694	} // else vertical attractor is off	729	} // else vertical attractor is off
		730	//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
		731
		732	if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) \|\| (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
		733	{ // if motor or object have motion
		734	if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
		735
		736	if (m_angularMotorTimescale < 300.0f)
		737	{
		738	Vector3 attack_error = m_angularMotorDVel - angObjectVel;
		739	float angfactor = m_angularMotorTimescale/pTimestep;
		740	Vector3 attackAmount = (attack_error/angfactor);
		741	angObjectVel += attackAmount;
		742	//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
		743	//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
		744	}
		745
		746	if (m_angularFrictionTimescale.X < 300.0f)
		747	{
		748	float fricfactor = m_angularFrictionTimescale.X / pTimestep;
		749	angObjectVel.X -= angObjectVel.X / fricfactor;
		750	}
		751	if (m_angularFrictionTimescale.Y < 300.0f)
		752	{
		753	float fricfactor = m_angularFrictionTimescale.Y / pTimestep;
		754	angObjectVel.Y -= angObjectVel.Y / fricfactor;
		755	}
		756	if (m_angularFrictionTimescale.Z < 300.0f)
		757	{
		758	float fricfactor = m_angularFrictionTimescale.Z / pTimestep;
		759	angObjectVel.Z -= angObjectVel.Z / fricfactor;
		760	Console.WriteLine("z fric");
		761	}
		762	} // else no signif. motion
695		763
696	// m_lastVertAttractor = vertattr;	764	//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
697
698	// Bank section tba	765	// Bank section tba
699	// Deflection section tba	766	// Deflection section tba
		767	//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
700		768
701	// Sum velocities	769	m_lastAngularVelocity = angObjectVel;
702	m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection	770	/*
703		771	if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
704	if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
705	{	772	{
706	if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);	773	if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
707	}	774	}
@@ -709,13 +776,12 @@ namespace OpenSim.Region.Physics.OdePlugin
709	{	776	{
710	m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.	777	m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
711	}	778	}
712		779	*/
713	// apply friction
714	Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
715	m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
716
717	// Apply to the body	780	// Apply to the body
		781	// Vector3 aInc = m_lastAngularVelocity - initavel;
		782	//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
718	d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);	783	d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
		784	//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
719		785
720	} //end MoveAngular	786	} //end MoveAngular
721	}	787	}