Changes to vehicles code etc. Includes some debug aids to remove later.

OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs

@@ -629,36 +629,31 @@ namespace OpenSim.Region.Physics.OdePlugin
 
             Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
             Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame
-            d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame
+            d.Vector3 dtorque = new d.Vector3();
-
-            bool doathing = false;
 
             // linear motor
             if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
             {
                 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
-                if (tmpV.LengthSquared() > 1e-6f)
+                tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
+                tmpV *= rotq; // to world
+
+                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
+                    tmpV.Z = 0;
+
+                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
                 {
-                    tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep
+                    // have offset, do it now
-                    tmpV *= rotq; // to world
+                    tmpV *= rootPrim.Mass;
-
+                    d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
-                    if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
-                        tmpV.Z = 0;
-
-                    if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0)
-                    {
-                        // have offset, do it now
-                        tmpV *= rootPrim.Mass;
-                        d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
-                    }
-                    else
-                    {
-                        force.X += tmpV.X;
-                        force.Y += tmpV.Y;
-                        force.Z += tmpV.Z;
-                    }
                 }
-                m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale);
+                else
+                {
+                    force.X += tmpV.X;
+                    force.Y += tmpV.Y;
+                    force.Z += tmpV.Z;
+                }
+                m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
             }
             else
                 m_lmEfect = 0;
@@ -719,30 +714,35 @@ namespace OpenSim.Region.Physics.OdePlugin
             if (m_linearDeflectionEfficiency > 0)
             {
                 float len = curVel.Length();
-                Vector3 atAxis = refAtAxis;
+                Vector3 atAxis;
-                atAxis *= rotq; // at axis rotated to world 
+                atAxis = Xrot(rotq); // where are we pointing to
-                atAxis = Xrot(rotq);
+                atAxis *= len; // make it same size as world velocity vector
-                tmpV = atAxis * len;
+                tmpV = -atAxis; // oposite direction
-                tmpV -= curVel; // velocity error
+                atAxis -= curVel; // error to one direction
-                tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                len = atAxis.LengthSquared();
-                force.X += tmpV.X;
+                tmpV -= curVel; // error to oposite
-                force.Y += tmpV.Y;
+                float lens = tmpV.LengthSquared();
-                if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0)
+                if (len > 0.01 || lens > 0.01) // do nothing if close enougth
-                    force.Z += tmpV.Z;
+                {
+                    if (len < lens)
+                        tmpV = atAxis;
+
+                    tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
+                    force.X += tmpV.X;
+                    force.Y += tmpV.Y;
+                    if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
+                        force.Z += tmpV.Z;
+                }
             }
 
             // angular motor
             if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
             {
                 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
-                if (tmpV.LengthSquared() > 1e-6f)
+                tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
-                {
+                torque.X += tmpV.X;
-                    tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep
+                torque.Y += tmpV.Y;
-                    tmpV *= m_referenceFrame; // to object
+                torque.Z += tmpV.Z;
-                    dtorque.X += tmpV.X;
-                    dtorque.Y += tmpV.Y;
-                    dtorque.Z += tmpV.Z;
-                }
                 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
             }
             else
@@ -751,85 +751,64 @@ namespace OpenSim.Region.Physics.OdePlugin
             // angular friction
             if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
             {
-                tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X;
+                torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
-                tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y;
+                torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
-                tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z;
+                torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
-                tmpV *= m_referenceFrame; // to object
-                dtorque.X += tmpV.X;
-                dtorque.Y += tmpV.Y;
-                dtorque.Z += tmpV.Z;
             }
 
             // angular deflection
             if (m_angularDeflectionEfficiency > 0)
             {
-                doathing = false;
+                Vector3 dirv;
-                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE;
+                
-                tmpV.X = 0;
+                if (curLocalVel.X > 0.01f)
-                if (Math.Abs(curLocalVel.Z) > 0.01)
+                    dirv = curLocalVel;
-                {
+                else if (curLocalVel.X < -0.01f)
-                    tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp;
+                    // use oposite 
-                    doathing = true;
+                    dirv = -curLocalVel;
-                }
                 else
-                    tmpV.Y = 0;
-                if (Math.Abs(curLocalVel.Y) > 0.01)
                 {
-                    tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp;
+                    // make it fall into small positive x case
-                    doathing = true;
+                    dirv.X = 0.01f;
+                    dirv.Y = curLocalVel.Y;
+                    dirv.Z = curLocalVel.Z;
                 }
-                else
-                    tmpV.Z = 0;
 
-                if (doathing)
+                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
+
+                if (Math.Abs(dirv.Z) > 0.01)
                 {
-                    tmpV *= m_referenceFrame; // to object
+                    torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
-                    dtorque.X += tmpV.X;
+                }
-                    dtorque.Y += tmpV.Y;
+
-                    dtorque.Z += tmpV.Z;
+                if (Math.Abs(dirv.Y) > 0.01)
+                {
+                    torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
                 }
             }
 
             // vertical atractor
             if (m_verticalAttractionTimescale < 300)
             {
-                doathing = false;
                 float roll;
                 float pitch;
 
                 GetRollPitch(rotq, out roll, out pitch);
 
-
                 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
                 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
 
                 if (Math.Abs(roll) > 0.01) // roll
                 {
-                    tmpV.X = -roll * ftmp;
+                    torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2;
-                    tmpV.X -= curLocalAngVel.X * ftmp2;
-                    doathing = true;
-                }
-                else
-                {
-                    tmpV.X = 0;
                 }
 
                 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
                 {
-                    tmpV.Y = -pitch * ftmp;
+                    torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2;
-                    tmpV.Y -= curLocalAngVel.Y * ftmp2;
-                    doathing = true;
-                }
-                else
-                {
-                    tmpV.Y = 0;
                 }
 
-                tmpV.Z = 0;
+                if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01)
-
-                if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01)
-                    tmpV.Z = 0;
-                else
                 {
                     float broll = -roll * m_bankingEfficiency; ;
                     if (m_bankingMix != 0)
@@ -839,146 +818,10 @@ namespace OpenSim.Region.Physics.OdePlugin
                             broll *= ((1 - m_bankingMix) + vfact);
                     }
 
-                    tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale;
+                    torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale;
-                    doathing = true;
-                }
-
-                if (doathing)
-                {
-
-                    tmpV *= m_referenceFrame; // to object
-                    dtorque.X += tmpV.X;
-                    dtorque.Y += tmpV.Y;
-                    dtorque.Z += tmpV.Z;
                 }
             }
-            /*
-                        d.Vector3 pos = d.BodyGetPosition(Body);
-            //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
-                        Vector3 posChange = new Vector3();
-                        posChange.X = pos.X - m_lastPositionVector.X;
-                        posChange.Y = pos.Y - m_lastPositionVector.Y;
-                        posChange.Z = pos.Z - m_lastPositionVector.Z;
-                        double Zchange = Math.Abs(posChange.Z);
-                        if (m_BlockingEndPoint != Vector3.Zero)
-                        {
-                            if (pos.X >= (m_BlockingEndPoint.X - (float)1))
-                            {
-                                pos.X -= posChange.X + 1;
-                                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                            }
-                            if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
-                            {
-                                pos.Y -= posChange.Y + 1;
-                                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                            }
-                            if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
-                            {
-                                pos.Z -= posChange.Z + 1;
-                                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                            }
-                            if (pos.X <= 0)
-                            {
-                                pos.X += posChange.X + 1;
-                                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                            }
-                            if (pos.Y <= 0)
-                            {
-                                pos.Y += posChange.Y + 1;
-                                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                            }
-                        }
-                        if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
-                        {
-                            pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
-                            d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
-                        }
-
-                        }
-                        if ((m_flags & (VehicleFlag.NO_X)) != 0)
-                        {
-                            m_dir.X = 0;
-                        }
-                        if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-                        {
-                            m_dir.Y = 0;
-                        }
-                        if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-                        {
-                            m_dir.Z = 0;
-                        }
-
-
-            */
-            // angular part
-            /*
-
-            // Get what the body is doing, this includes 'external' influences
-/*
-            Vector3 angularVelocity = Vector3.Zero;
-
-            // Vertical attractor section
-            Vector3 vertattr = Vector3.Zero;
-
-            if (m_verticalAttractionTimescale < 300)
-            {
-                float VAservo = 0.2f / m_verticalAttractionTimescale;
-                // get present body rotation
-                // make a vector pointing up
-                Vector3 verterr = Vector3.Zero;
-                verterr.Z = 1.0f;
-                // rotate it to Body Angle
-                verterr = verterr * rotq;
-                // 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)
-                {
-                    verterr.X = 2.0f - verterr.X;
-                    verterr.Y = 2.0f - verterr.Y;
-                }
-                // Error is 0 (no error) to +/- 2 (max error)
-                // scale it by VAservo
-                verterr = verterr * VAservo;
-//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;
-
-            // Bank section tba
-            // Deflection section tba
-
-            // Sum velocities
-            m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection
             
-            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
-            {
-                m_lastAngularVelocity.X = 0;
-                m_lastAngularVelocity.Y = 0;
-            }
-
-            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
-            {
-                if (!d.BodyIsEnabled (Body))  d.BodyEnable (Body);
-            }
-            else
-            {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-            }
-*/
-
             d.Mass dmass;
             d.BodyGetMass(Body,out dmass);
 
@@ -988,8 +831,13 @@ namespace OpenSim.Region.Physics.OdePlugin
                 d.BodySetForce(Body, force.X, force.Y, force.Z);
             }
 
-            if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0)
+            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
             {
+                torque *= m_referenceFrame; // to object frame
+                dtorque.X = torque.X;
+                dtorque.Y = torque.Y;
+                dtorque.Z = torque.Z;
+
                 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
                 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
             }

OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs

@@ -1198,7 +1198,7 @@ namespace OpenSim.Region.Physics.OdePlugin
             }
             Body = IntPtr.Zero;
             hasOOBoffsetFromMesh = false;
-//            CalcPrimBodyData();
+            CalcPrimBodyData();
         }
 
         private void ChildSetGeom(OdePrim odePrim)
@@ -1223,7 +1223,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    /*
                     if (m_targetSpace != _parent_scene.ActiveSpace)
                     {
                         m_targetSpace = _parent_scene.ActiveSpace;
@@ -1238,7 +1237,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                         }
                         d.SpaceAdd(m_targetSpace, prim_geom);
                     }
-                     */
                     d.GeomEnable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomEnable(prm.prim_geom);
@@ -1256,7 +1254,6 @@ namespace OpenSim.Region.Physics.OdePlugin
             {
                 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
                 {
-                    /*
                     if (m_targetSpace == _parent_scene.ActiveSpace)
                     {
                         foreach (OdePrim prm in childrenPrim)
@@ -1270,7 +1267,6 @@ namespace OpenSim.Region.Physics.OdePlugin
                         d.SpaceRemove(m_targetSpace, prim_geom);
                         m_targetSpace = IntPtr.Zero;
                     }
-                    */
                     d.GeomDisable(prim_geom);
                     foreach (OdePrim prm in childrenPrim)
                         d.GeomDisable(prm.prim_geom);

OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs

@@ -59,6 +59,7 @@ namespace OdeAPI
 	{
 		public static dReal Infinity = dReal.MaxValue;
         public static int NTotalBodies = 0;
+        public static int NTotalGeoms = 0;
 
 		#region Flags and Enumerations
 
@@ -420,7 +421,7 @@ namespace OdeAPI
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
 		public static extern IntPtr BodyiCreate(IntPtr world);
-		public static IntPtr BodyCreate(IntPtr world)
+        public static IntPtr BodyCreate(IntPtr world)
         {
             NTotalBodies++;
             return BodyiCreate(world);
@@ -689,22 +690,52 @@ namespace OdeAPI
 		public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz);
+        public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
+		public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
+        {
+            NTotalGeoms++;
+            return CreateiBox(space, lx, ly, lz);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length);
+        public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCapsule(space, radius, length);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+		public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
+        public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
+        {
+            NTotalGeoms++;
+            return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length);
+		public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
+        public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiCylinder(space, radius, length);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable);
+        public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
+        public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
+        {
+            NTotalGeoms++;
+            return CreateiHeightfield(space, data, bPlaceable);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateGeom(int classnum);
+		public static extern IntPtr CreateiGeom(int classnum);
+        public static IntPtr CreateGeom(int classnum)
+        {
+            NTotalGeoms++;
+            return CreateiGeom(classnum);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
 		public static extern int CreateGeomClass(ref GeomClass classptr);
@@ -713,19 +744,39 @@ namespace OdeAPI
 		public static extern IntPtr CreateGeomTransform(IntPtr space);
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
+        public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
+        {
+            NTotalGeoms++;
+            return CreateiPlane(space, a, b, c, d);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateRay(IntPtr space, dReal length);
+		public static extern IntPtr CreateiRay(IntPtr space, dReal length);
+        public static IntPtr CreateRay(IntPtr space, dReal length)
+        {
+            NTotalGeoms++;
+            return CreateiRay(space, length);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateSphere(IntPtr space, dReal radius);
+		public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
+        public static IntPtr CreateSphere(IntPtr space, dReal radius)
+        {
+            NTotalGeoms++;
+            return CreateiSphere(space, radius);
+        }
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
-		public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, 
+		public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, 
 			TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
-
+        public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
-		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
+            TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
+        {
+            NTotalGeoms++;
+            return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
+        }
+        [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
 		public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
@@ -798,7 +849,13 @@ namespace OdeAPI
 		public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
-		public static extern void GeomDestroy(IntPtr geom);
+		public static extern void GeomiDestroy(IntPtr geom);
+        public static void GeomDestroy(IntPtr geom)
+        {
+            NTotalGeoms--;
+            GeomiDestroy(geom);
+        }
+
 
 		[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
 		public static extern void GeomDisable(IntPtr geom);

OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs

@@ -1860,6 +1860,7 @@ namespace OpenSim.Region.Physics.OdePlugin
                 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
                 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
                 int nbodies = d.NTotalBodies;
+                int ngeoms = d.NTotalGeoms;
 
                 // Finished with all sim stepping. If requested, dump world state to file for debugging.
                 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?