2 files changed, 43 insertions, 26 deletions

diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index bfdb90f..85aac93 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -71,8 +71,8 @@ namespace OpenSim.Region.Physics.OdePlugin
         private static float PID_D = 3020.0f;
         private static float PID_P = 7000.0f;
         private static float POSTURE_SERVO = 10000.0f;
-        public static float CAPSULE_RADIUS = 0.5f;
-        public float CAPSULE_LENGTH = 0.79f;
+        public static float CAPSULE_RADIUS = 0.37f;
+        public float CAPSULE_LENGTH = 2.140599f;
         private bool flying = false;
         private bool m_iscolliding = false;
         private bool m_iscollidingGround = false;
@@ -326,12 +326,14 @@ namespace OpenSim.Region.Physics.OdePlugin
                 lock (OdeScene.OdeLock)
                 {
                     d.JointDestroy(Amotor);
+                    
                     PhysicsVector SetSize = value;
                     float prevCapsule = CAPSULE_LENGTH;
                     float capsuleradius = CAPSULE_RADIUS;
-                    capsuleradius = 0.2f;
+                    //capsuleradius = 0.2f;
 
                     CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z*0.43f))); // subtract 43% of the size
+                    OpenSim.Framework.Console.MainLog.Instance.Verbose("SIZE", CAPSULE_LENGTH.ToString());
                     d.BodyDestroy(Body);
                     d.GeomDestroy(Shell);
                     AvatarGeomAndBodyCreation(_position.X, _position.Y,
@@ -351,6 +353,7 @@ namespace OpenSim.Region.Physics.OdePlugin
         /// <param name="npositionZ"></param>
         private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
         {
+            
             int dAMotorEuler = 1;
             Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
             d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
@@ -360,7 +363,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.BodySetMass(Body, ref ShellMass);
 
             // 90 Stand up on the cap of the capped cyllinder
-            d.RFromAxisAndAngle(out m_StandUpRotation, 1, 0, 0, (float)(Math.PI / 2));
+            d.RFromAxisAndAngle(out m_StandUpRotation, 1, 0, 1, (float)(Math.PI / 2));
 
 
             d.GeomSetRotation(Shell, ref m_StandUpRotation);
@@ -395,21 +398,8 @@ namespace OpenSim.Region.Physics.OdePlugin
             d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
             d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 3800000f);
 
-
-            // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
-            // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you 
-            // change appearance and when you enter the simulator
-            // After this routine is done, the amotor stabilizes much quicker
-            d.Vector3 feet;
-            d.Vector3 head;
-            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
-            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
-            float posture = head.Z - feet.Z;
-
-            // restoring force proportional to lack of posture:
-            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);
+            //standupStraight();
+            
             
             
         }
@@ -428,6 +418,27 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
         }
 
+        private void standupStraight()
+        {
+
+            // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
+            // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you 
+            // change appearance and when you enter the simulator
+            // After this routine is done, the amotor stabilizes much quicker
+            d.Vector3 feet;
+            d.Vector3 head;
+            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
+            d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
+            float posture = head.Z - feet.Z;
+
+            // restoring force proportional to lack of posture:
+            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);
+        }
+
         public override PhysicsVector Force
         {
             get { return new PhysicsVector(_target_velocity.X, _target_velocity.Y, _target_velocity.Z); }
@@ -467,7 +478,12 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override Quaternion Orientation
         {
             get { return Quaternion.Identity; }
-            set { }
+            set {
+                //Matrix3 or = Orientation.ToRotationMatrix();
+                //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
+                //d.BodySetRotation(Body, ref ord);
+
+            }
         }
 
         public override PhysicsVector Acceleration
@@ -505,6 +521,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (!collidelock)
             {
                 d.BodyAddForce(Body, force.X, force.Y, force.Z);
+                //standupStraight();
             }
         }
 
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 47839ca..4045add 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -174,7 +174,7 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
 
-                d.WorldSetGravity(world, 0.0f, 0.0f, -10.0f);
+                d.WorldSetGravity(world, 0.0f, 0.0f, -9.8f);
                 d.WorldSetAutoDisableFlag(world, false);
                 d.WorldSetContactSurfaceLayer(world, 0.001f);
                 
@@ -336,8 +336,8 @@ namespace OpenSim.Region.Physics.OdePlugin
 
                     if (contacts[i].depth >= 0.08f)
                     {
-                        /*  This is disabled at the moment only because it needs more tweaking
-                            It will eventually be uncommented
+                          //This is disabled at the moment only because it needs more tweaking
+                            //It will eventually be uncommented
                          
                         if (contacts[i].depth >= 1.00f)
                         {
@@ -363,7 +363,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                             else
                             {
                                 
-                                contacts[i].depth = 0.0000000f;
+                                //contacts[i].depth = 0.0000000f;
                             }
                             if (p1.PhysicsActorType == (int) ActorTypes.Agent)
                             {
@@ -378,10 +378,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             else
                             {
                                 
-                                contacts[i].depth = 0.0000000f;
+                                //contacts[i].depth = 0.0000000f;
                             }
                         }
-                         */
+                         
 
                         // If you interpenetrate a prim with another prim
                         if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)