ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-04 16:22:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

save a few ns on ubode

Browse Source
This commit is contained in:
UbitUmarov
2021-09-08 17:52:11 +01:00
parent 1e4799604f
commit 076c87914d
3 changed files with 86 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
View File

@@ -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.

81
OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs
View File

@@ -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);

61
OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
View File

@@ -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)
{