* Return of the avatar wobble.

1 files changed, 43 insertions, 35 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 9a37a9e..08524a7 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -73,6 +73,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         private static float POSTURE_SERVO = 10000.0f;
         public static float CAPSULE_RADIUS = 0.37f;
         public float CAPSULE_LENGTH = 2.140599f;
+        private float m_tensor = 3800000f;
         private bool flying = false;
         private bool m_iscolliding = false;
         private bool m_iscollidingGround = false;
@@ -105,10 +106,10 @@ namespace OpenSim.Region.Physics.OdePlugin
             _position = pos;
             _acceleration = new PhysicsVector();
             _parent_scene = parent_scene;
-
+           
             m_StandUpRotation =
-                new d.Matrix3(0.8184158f, -0.5744568f, -0.0139677f, 0.5744615f, 0.8185215f, -0.004074608f, 0.01377355f,
-                              -0.004689182f, 0.9998941f);
+                new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
+                              0.5f);
 
             for (int i = 0; i < 11; i++)
             {
@@ -117,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             lock (OdeScene.OdeLock)
             {
-                AvatarGeomAndBodyCreation(pos.X, pos.Y, pos.Z);
+                AvatarGeomAndBodyCreation(pos.X, pos.Y, pos.Z, m_tensor);
             }
             m_name = avName;
             parent_scene.geom_name_map[Shell] = avName;
@@ -325,7 +326,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 m_pidControllerActive = true;
                 lock (OdeScene.OdeLock)
                 {
-                    d.JointDestroy(Amotor);
+                    //d.JointDestroy(Amotor);
                     
                     PhysicsVector SetSize = value;
                     float prevCapsule = CAPSULE_LENGTH;
@@ -337,7 +338,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                     d.BodyDestroy(Body);
                     d.GeomDestroy(Shell);
                     AvatarGeomAndBodyCreation(_position.X, _position.Y,
-                                      _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule)*2));
+                                      _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
                     Velocity = new PhysicsVector(0f, 0f, 0f);
                     
                 }
@@ -351,7 +352,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="npositionX"></param>
         /// <param name="npositionY"></param>
         /// <param name="npositionZ"></param>
-        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
+        private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
         {
             
             int dAMotorEuler = 1;
@@ -361,44 +362,49 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
 
             d.BodySetMass(Body, ref ShellMass);
-
+            d.Matrix3 m_caprot;
             // 90 Stand up on the cap of the capped cyllinder
-            d.RFromAxisAndAngle(out m_StandUpRotation, 1, 0, 1, (float)(Math.PI / 2));
+            //d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
 
 
-            d.GeomSetRotation(Shell, ref m_StandUpRotation);
-            d.BodySetRotation(Body, ref m_StandUpRotation);
+            //d.GeomSetRotation(Shell, ref m_caprot);
+            //d.BodySetRotation(Body, ref m_caprot);
 
             d.GeomSetBody(Shell, Body);
 
 
             // The purpose of the AMotor here is to keep the avatar's physical 
             // surrogate from rotating while moving
-            Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
-            d.JointAttach(Amotor, Body, IntPtr.Zero);
-            d.JointSetAMotorMode(Amotor, dAMotorEuler);
-            d.JointSetAMotorNumAxes(Amotor, 3);
-            d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
-            d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
-            d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
-            d.JointSetAMotorAngle(Amotor, 0, 0);
-            d.JointSetAMotorAngle(Amotor, 1, 0);
-            d.JointSetAMotorAngle(Amotor, 2, 0);
+            //Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
+            //d.JointAttach(Amotor, Body, IntPtr.Zero);
+            ///d.JointSetAMotorMode(Amotor, dAMotorEuler);
+            //d.JointSetAMotorNumAxes(Amotor, 3);
+            //d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
+            //d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
+            //d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
+            //d.JointSetAMotorAngle(Amotor, 0, 0);
+            //d.JointSetAMotorAngle(Amotor, 1, 0);
+            //d.JointSetAMotorAngle(Amotor, 2, 0);
             
             // These lowstops and high stops are effectively (no wiggle room)
-            d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
 
             // Fudge factor is 1f by default, we're setting it to 0.  We don't want it to Fudge or the 
             // capped cyllinder will fall over
-            d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
-            d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 3800000f);
-
-            standupStraight();
+            //d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
+            //d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
+            
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //d.QfromR(
+            //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
+            // 
+            //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICSAV", "Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+            //standupStraight();
             
             
             
@@ -435,8 +441,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             float servo = (2.5f - posture) * POSTURE_SERVO;
             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
             d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
-            d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
-            OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICSAV", "Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
+            //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
+            //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICSAV", "Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
         }
 
         public override PhysicsVector Force
@@ -521,7 +527,9 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (!collidelock)
             {
                 d.BodyAddForce(Body, force.X, force.Y, force.Z);
-                //standupStraight();
+                //d.BodySetRotation(Body, ref m_StandUpRotation);
+                standupStraight();
+                
             }
         }
 
@@ -713,7 +721,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             lock (OdeScene.OdeLock)
             {
                 // Kill the Amotor
-                d.JointDestroy(Amotor);
+                //d.JointDestroy(Amotor);
 
                 //kill the Geometry
                 d.GeomDestroy(Shell);