From 076c87914d809ec26e03244bb577c002ae791950 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Sep 2021 17:52:11 +0100 Subject: [PATCH] save a few ns on ubode --- .../Framework/Scenes/SceneObjectPart.cs | 4 +- OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs | 81 ++++++++++++++++--- .../Region/PhysicsModules/ubOde/ODEPrim.cs | 61 ++++---------- 3 files changed, 86 insertions(+), 60 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 53c71aa474..3149a6d9cf 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -3450,9 +3450,9 @@ namespace OpenSim.Region.Framework.Scenes } } - private const float ROTATION_TOLERANCE = 0.01f; + private const float ROTATION_TOLERANCE = 0.002f; private const float VELOCITY_TOLERANCE = 0.1f; - private const float ANGVELOCITY_TOLERANCE = 0.005f; + private const float ANGVELOCITY_TOLERANCE = 0.002f; private const float POSITION_TOLERANCE = 0.05f; // I don't like this, but I suppose it's necessary private const double TIME_MS_TOLERANCE = 250.0; //llSetPos has a 200ms delay. This should NOT be 3 seconds. diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs index ee22a70b22..a3cbf3f375 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -294,16 +294,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde { internal Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) { - M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; - M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; - M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + M00 = m00; M10 = m10; M20 = m20; M30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; M31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; M32 = 0.0f; } - internal dReal M00, M10, M20; - private dReal _m30; - internal dReal M01, M11, M21; - private dReal _m31; - internal dReal M02, M12, M22; - private dReal _m32; + internal dReal M00, M10, M20, M30; + internal dReal M01, M11, M21, M31; + internal dReal M02, M12, M22, M32; } [StructLayout(LayoutKind.Sequential)] @@ -373,6 +370,67 @@ namespace OpenSim.Region.PhysicsModule.ubOde } #endregion + public static void RfromQ(ref Matrix3 matrix, ref OMV.Quaternion rot) + { + float x2 = rot.X + rot.X; + float y2 = rot.Y + rot.Y; + float z2 = rot.Z + rot.Z; + + float yy2 = rot.Y * y2; + float zz2 = rot.Z * z2; + matrix.M00 = 1.0f - yy2 - zz2; + float xy2 = rot.X * y2; + float wz2 = rot.W * z2; + matrix.M10 = xy2 - wz2; + float xz2 = rot.X * z2; + float wy2 = rot.W * y2; + matrix.M20 = xz2 + wy2; + matrix.M30 = 0f; + + matrix.M01 = xy2 + wz2; + float xx2 = rot.X * x2; + matrix.M11 = 1.0f - xx2 - zz2; + float yz2 = rot.Y * z2; + float wx2 = rot.W * x2; + matrix.M21 = yz2 - wx2; + matrix.M31 = 0f; + + matrix.M02 = xz2 - wy2; + matrix.M12 = yz2 + wx2; + matrix.M22 = 1.0f - xx2 - yy2; + matrix.M32 = 0f; + } + + public static void RfromQ(ref Matrix3 matrix, ref OMV.Vector4 rot) + { + float x2 = rot.X + rot.X; + float y2 = rot.Y + rot.Y; + float z2 = rot.Z + rot.Z; + + float yy2 = rot.Y * y2; + float zz2 = rot.Z * z2; + matrix.M00 = 1.0f - yy2 - zz2; + float xy2 = rot.X * y2; + float wz2 = rot.W * z2; + matrix.M10 = xy2 - wz2; + float xz2 = rot.X * z2; + float wy2 = rot.W * y2; + matrix.M20 = xz2 + wy2; + matrix.M30 = 0f; + + matrix.M01 = xy2 + wz2; + float xx2 = rot.X * x2; + matrix.M11 = 1.0f - xx2 - zz2; + float yz2 = rot.Y * z2; + float wx2 = rot.W * x2; + matrix.M21 = yz2 - wx2; + matrix.M31 = 0f; + + matrix.M02 = xz2 - wy2; + matrix.M12 = yz2 + wx2; + matrix.M22 = 1.0f - xx2 - yy2; + matrix.M32 = 0f; + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] internal static extern int AllocateODEDataForThread(uint ODEInitFlags); @@ -559,7 +617,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] internal static extern IntPtr dBodyGetNextGeom(IntPtr Geom); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] internal static extern bool BodyIsEnabled(IntPtr body); @@ -1791,8 +1848,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] internal static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] - internal static extern void RfromQ(out Matrix3 R, ref Quaternion q); + //[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + //internal static extern void RfromQ(out Matrix3 R, ref Quaternion q); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] internal static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs index 121a8ce937..26f3ffe35d 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -496,13 +496,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999) { SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3(); - SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion(); - - inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X; - inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y; - inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z; - inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W; - SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot); + SafeNativeMethods.RfromQ(ref inertiarotmat, ref m_fakeInertiaOverride.InertiaRotation); SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat); } @@ -579,14 +573,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde } else if (prim_geom != IntPtr.Zero) { - SafeNativeMethods.Quaternion dq; - SafeNativeMethods.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - + Quaternion q = SafeNativeMethods.GeomGetQuaternionOMV(prim_geom); Vector3 Ptot = m_OBBOffset * q; dtmp = SafeNativeMethods.GeomGetPosition(prim_geom); Ptot.X += dtmp.X; @@ -1903,20 +1890,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde Body = SafeNativeMethods.BodyCreate(_parent_scene.world); - SafeNativeMethods.Matrix3 mymat = new SafeNativeMethods.Matrix3(); - SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion(); - SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { }; - - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - SafeNativeMethods.RfromQ(out mymat, ref myrot); - // set the body rotation + SafeNativeMethods.Matrix3 mymat = new SafeNativeMethods.Matrix3(); + SafeNativeMethods.RfromQ(ref mymat, ref _orientation); SafeNativeMethods.BodySetRotation(Body, ref mymat); - if(noInertiaOverride) + SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { }; + + + if (noInertiaOverride) { objdmass = primdMass; SafeNativeMethods.MassRotate(ref objdmass, ref mymat); @@ -1926,7 +1908,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (childrenPrim.Count > 0) { SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3(); - SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion(); SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { }; Vector3 rcm; @@ -1944,11 +1925,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde continue; } - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - SafeNativeMethods.RfromQ(out mat, ref quat); + SafeNativeMethods.RfromQ(ref mat, ref prm._orientation); // fix prim colision cats @@ -2004,11 +1981,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde if(noInertiaOverride) { SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.X = -myrot.X; - myrot.Y = -myrot.Y; - myrot.Z = -myrot.Z; + Quaternion mr = Quaternion.Conjugate(_orientation); - SafeNativeMethods.RfromQ(out mymat, ref myrot); + SafeNativeMethods.RfromQ(ref mymat, ref mr); SafeNativeMethods.MassRotate(ref objdmass, ref mymat); SafeNativeMethods.BodySetMass(Body, ref objdmass); @@ -2029,13 +2004,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999) { SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3(); - SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion(); - - inertiarot.X = m_InertiaOverride.InertiaRotation.X; - inertiarot.Y = m_InertiaOverride.InertiaRotation.Y; - inertiarot.Z = m_InertiaOverride.InertiaRotation.Z; - inertiarot.W = m_InertiaOverride.InertiaRotation.W; - SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot); + SafeNativeMethods.RfromQ(ref inertiarotmat, ref m_InertiaOverride.InertiaRotation); SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat); } SafeNativeMethods.BodySetMass(Body, ref objdmass); @@ -2048,9 +2017,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.BodySetAutoDisableFlag(Body, true); SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); - SafeNativeMethods.BodySetAutoDisableAngularThreshold(Body, 0.05f); - SafeNativeMethods.BodySetAutoDisableLinearThreshold(Body, 0.05f); - SafeNativeMethods.BodySetDamping(Body, .004f, .001f); + SafeNativeMethods.BodySetAutoDisableAngularThreshold(Body, 0.001f); + SafeNativeMethods.BodySetAutoDisableLinearThreshold(Body, 0.01f); + SafeNativeMethods.BodySetDamping(Body, .002f, .0005f); if (m_targetSpace != IntPtr.Zero) {