From 5c23b4aa56a9de1baceb833ecba67f66274dca49 Mon Sep 17 00:00:00 2001
From: Kitto Flora
Date: Thu, 29 Apr 2010 13:50:11 -0400
Subject: Fix vertical attractor

---
 OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 84 ++++++++++++++-------------
 1 file changed, 43 insertions(+), 41 deletions(-)

(limited to 'OpenSim/Region/Physics')

diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 01c3f81..fbe52c8 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -3395,6 +3395,44 @@ Console.WriteLine(" JointCreateFixed");
 
 //if(frcount == 0) Console.WriteLine("VA = {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/timestep;
+			                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);        	
+		                }
+                
+				        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
+				        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
+				        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
+					} // 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);        	
+			
+	
+					// Rotation Axis Disables:
+				    if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
+			        {
+		                if (m_angularEnable.X == 0)
+			            	angObjectVel.X = 0f;
+			            if (m_angularEnable.Y == 0)
+			            	angObjectVel.Y = 0f;
+			            if (m_angularEnable.Z == 0)
+			            	angObjectVel.Z = 0f;
+			        }        
+			        
+					angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
+					
 		            // Vertical attractor section
 					Vector3 vertattr = Vector3.Zero;
             
@@ -3419,8 +3457,9 @@ Console.WriteLine(" JointCreateFixed");
 						}
 						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))
+						Vector3 xyav = angObjectVel;
+						xyav.Z = 0.0f;
+						if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
 						{
 							// 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.
@@ -3452,46 +3491,9 @@ Console.WriteLine(" JointCreateFixed");
 					} // 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/timestep;
-			                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);        	
-		                }
-                
-				        angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
-				        angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
-				        angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
-					} // 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);        	
-			
+					
 					m_lastAngularVelocity = angObjectVel;
-
-				    if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
-			        {
-		                if (m_angularEnable.X == 0)
-			            	m_lastAngularVelocity.X = 0f;
-			            if (m_angularEnable.Y == 0)
-			            	m_lastAngularVelocity.Y = 0f;
-			            if (m_angularEnable.Z == 0)
-			            	m_lastAngularVelocity.Z = 0f;
-			        }        
-					// Apply to the body
-// Vector3 aInc = m_lastAngularVelocity - initavel;
-//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);	
-					m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
-
+					// apply Angular Velocity to body
 					d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);        	
 		            
-- 
cgit v1.1