From cd037cd2ea6566686ba8b479588280222714a641 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 19 Oct 2022 23:22:21 +0100 Subject: [PATCH] a few changtes to ubode --- OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs | 29 +-- .../PhysicsModules/ubOde/ODECharacter.cs | 211 +++++++-------- .../ubOde/ODERayCastRequestManager.cs | 244 ++++++++---------- .../Region/PhysicsModules/ubOde/ODEScene.cs | 210 ++++++++++++--- 4 files changed, 391 insertions(+), 303 deletions(-) diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs index 554b251eae..2b63a6ad6b 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -242,12 +242,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal SurfaceParameters surface; internal ContactGeom geom; internal Vector3 fdir1; - internal static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); } + internal static readonly int SizeOfContact = Marshal.SizeOf(typeof(Contact)); [StructLayout(LayoutKind.Sequential)] - //needed for Contact only, rest should use the class internal struct ContactGeom { @@ -258,20 +257,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal IntPtr g2; internal int side1; internal int side2; - internal static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); } - /* - [StructLayout(LayoutKind.Sequential)] - internal struct GeomClass - { - internal int bytes; - internal GetColliderFnFn collider; - internal GetAABBFn aabb; - internal AABBTestFn aabb_test; - internal GeomDtorFn dtor; - } - */ + internal static readonly int SizeOfContactGeom = Marshal.SizeOf(typeof(ContactGeom)); [StructLayout(LayoutKind.Sequential)] internal struct JointFeedback @@ -541,7 +529,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde unsafe { Vector3* vtmp = BodyGetAngularVelUnsafe(body); - return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + return Unsafe.As(ref *vtmp); } } @@ -551,7 +539,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde unsafe { Vector3* vtmp = BodyGetAngularVelUnsafe(body); - return new OMV.Vector3(0, 0, (float)Math.Round(vtmp->Z, 3)); + return Unsafe.As(ref *vtmp); } } @@ -593,7 +581,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde unsafe { Vector3* vtmp = BodyGetLinearVelUnsafe(body); - return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + return Unsafe.As(ref *vtmp); } } @@ -619,7 +607,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde unsafe { Vector3* vtmp = BodyGetPositionUnsafe(body); - return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + return Unsafe.As(ref *vtmp); } } @@ -1076,7 +1064,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde unsafe { Vector3* vtmp = GeomGetPositionUnsafe(geom); - return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + return Unsafe.As(ref *vtmp); } } @@ -1142,8 +1130,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde dReal width, dReal depth, int widthSamples, int depthSamples, dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] internal static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, dReal width, dReal depth, int widthSamples, int depthSamples, @@ -1169,7 +1155,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] internal static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); - [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] internal static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, dReal sampleSize, int widthSamples, int depthSamples, diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs index 53c6b63720..f5775de228 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -1028,7 +1028,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde } //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z - public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) + public static void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) { float sin = 2.0f * rot.Z * rot.W; float cos = rot.W * rot.W - rot.Z * rot.Z; @@ -1037,20 +1037,20 @@ namespace OpenSim.Region.PhysicsModule.ubOde x = tx * cos - y * sin; y = tx * sin + y * cos; } - public void RotateXYonZ(ref float x, ref float y, float sin, float cos) + public static void RotateXYonZ(ref float x, ref float y, float sin, float cos) { float tx = x; x = tx * cos - y * sin; y = tx * sin + y * cos; } - public void invRotateXYonZ(ref float x, ref float y, float sin, float cos) + public static void invRotateXYonZ(ref float x, ref float y, float sin, float cos) { float tx = x; x = tx * cos + y * sin; y = -tx * sin + y * cos; } - public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) + public static void invRotateXYonZ(ref float x, ref float y, in Quaternion rot) { float sin = -2.0f * rot.Z * rot.W; float cos = rot.W * rot.W - rot.Z * rot.Z; @@ -1061,129 +1061,116 @@ namespace OpenSim.Region.PhysicsModule.ubOde } [MethodImpl(MethodImplOptions.AggressiveInlining)] - internal bool Collide(IntPtr me, IntPtr other, bool reverse, ref SafeNativeMethods.ContactGeom contact, + internal bool Collide(IntPtr other, bool reverse, ref SafeNativeMethods.ContactGeom contact, ref SafeNativeMethods.ContactGeom altContact, ref bool useAltcontact, ref bool feetcollision) { feetcollision = false; useAltcontact = false; - //if (me == capsule) - if (me == collider) + Vector3 offset; + + float h = contact.pos.Z - _position.Z; + offset.Z = h - feetOff; + + offset.X = contact.pos.X - _position.X; + offset.Y = contact.pos.Y - _position.Y; + + SafeNativeMethods.GeomClassID gtype = SafeNativeMethods.GeomGetClass(other); + if (gtype == SafeNativeMethods.GeomClassID.CapsuleClass) { - Vector3 offset; + Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); + float r = roff.X * roff.X / AvaAvaSizeXsq; + r += (roff.Y * roff.Y) / AvaAvaSizeYsq; + if (r > 1.0f) + return false; - float h = contact.pos.Z - _position.Z; - offset.Z = h - feetOff; + float dp = 1.0f - MathF.Sqrt(r); + if (dp > 0.05f) + dp = 0.05f; - offset.X = contact.pos.X - _position.X; - offset.Y = contact.pos.Y - _position.Y; + contact.depth = dp; - SafeNativeMethods.GeomClassID gtype = SafeNativeMethods.GeomGetClass(other); - if (gtype == SafeNativeMethods.GeomClassID.CapsuleClass) + if (offset.Z < 0) { - Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); - float r = roff.X * roff.X / AvaAvaSizeXsq; - r += (roff.Y * roff.Y) / AvaAvaSizeYsq; - if (r > 1.0f) - return false; - - float dp = 1.0f - MathF.Sqrt(r); - if (dp > 0.05f) - dp = 0.05f; - - contact.depth = dp; - - if (offset.Z < 0) + feetcollision = true; + if (h < boneOff) { - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } + m_collideNormal = Unsafe.As(ref contact.normal); + IsColliding = true; } - return true; - } - - if (gtype == SafeNativeMethods.GeomClassID.SphereClass && SafeNativeMethods.GeomGetBody(other) != IntPtr.Zero) - { - if (SafeNativeMethods.GeomSphereGetRadius(other) < 0.5) - return true; - } - - if (offset.Z > 0 || contact.normal.Z > 0.35f) - { - if (offset.Z <= 0) - { - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } - } - return true; - } - - if (m_flying) - return true; - - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } - - useAltcontact = true; - - offset.Z -= 0.2f; - - offset.Normalize(); - - float tdp = contact.depth; - float t = offset.X; - t = MathF.Abs(t); - if (t > 1e-6) - { - tdp /= t; - tdp *= contact.normal.X; - } - else - tdp *= 10; - - if (tdp > 0.25f) - tdp = 0.25f; - - altContact.pos = contact.pos; - //altContact.g1 = contact.g1; - //altContact.g2 = contact.g2; - //altContact.side1 = contact.side1; - //altContact.side2 = contact.side2; - - altContact.depth = tdp; - - if (reverse) - { - altContact.normal.X = offset.X; - altContact.normal.Y = offset.Y; - altContact.normal.Z = offset.Z; - } - else - { - altContact.normal.X = -offset.X; - altContact.normal.Y = -offset.Y; - altContact.normal.Z = -offset.Z; } return true; } - return false; + + if (gtype == SafeNativeMethods.GeomClassID.SphereClass && SafeNativeMethods.GeomGetBody(other) != IntPtr.Zero) + { + if (SafeNativeMethods.GeomSphereGetRadius(other) < 0.5) + return true; + } + + if (offset.Z > 0 || contact.normal.Z > 0.35f) + { + if (offset.Z <= 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal = Unsafe.As(ref contact.normal); + IsColliding = true; + } + } + return true; + } + + if (m_flying) + return true; + + feetcollision = true; + if (h < boneOff) + { + m_collideNormal = Unsafe.As(ref contact.normal); + IsColliding = true; + } + + useAltcontact = true; + + offset.Z -= 0.2f; + + offset.Normalize(); + + float tdp = contact.depth; + float t = offset.X; + t = MathF.Abs(t); + if (t > 1e-6) + { + tdp /= t; + tdp *= contact.normal.X; + } + else + tdp *= 10; + + if (tdp > 0.25f) + tdp = 0.25f; + + altContact.pos = contact.pos; + //altContact.g1 = contact.g1; + //altContact.g2 = contact.g2; + //altContact.side1 = contact.side1; + //altContact.side2 = contact.side2; + + altContact.depth = tdp; + + if (reverse) + { + altContact.normal = Unsafe.As(ref offset); + } + else + { + altContact.normal.X = -offset.X; + altContact.normal.Y = -offset.Y; + altContact.normal.Z = -offset.Z; + } + return true; } /// diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs index 0d55ebd9eb..63cf3377b4 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs @@ -27,6 +27,7 @@ using System; using System.Collections.Generic; +using System.Collections.Concurrent; using System.Reflection; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; @@ -47,7 +48,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// /// Pending ray requests /// - protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); + protected ConcurrentQueue m_PendingRequests = new(); /// /// Scene that created this object. @@ -58,7 +59,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde IntPtr ray; // the ray. we only need one for our lifetime private int CollisionContactGeomsPerTest = 25; - private const int DefaultMaxCount = 25; + private const int ResultsMaxCount = 25; + private const int DefaultResultsMaxCount = 25; private const int MaxTimePerCallMS = 30; /// @@ -66,10 +68,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// private readonly SafeNativeMethods.NearCallback nearCallback; private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - private readonly List m_contactResults = new List(); + private readonly List m_contactResults = new(ResultsMaxCount); + private readonly object m_contactResultsLock = new(); private RayFilterFlags CurrentRayFilter; private int CurrentMaxCount; - ContactResult SharedCollisionResult = new ContactResult(); + ContactResult SharedCollisionResult = new(); public ODERayCastRequestManager(ODEScene pScene) { @@ -84,7 +87,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde public void QueueRequest(ODERayRequest req) { if (req.Count == 0) - req.Count = DefaultMaxCount; + req.Count = DefaultResultsMaxCount; m_PendingRequests.Enqueue(req); } @@ -95,11 +98,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde [MethodImpl(MethodImplOptions.AggressiveInlining)] public void ProcessQueuedRequests() { - if (m_PendingRequests.Count == 0) + if (m_PendingRequests.IsEmpty) return ; int time = Util.EnvironmentTickCount(); - while (m_PendingRequests.Dequeue(out ODERayRequest req)) + while (m_PendingRequests.TryDequeue(out ODERayRequest req)) { if(req.length <= 0) { @@ -112,10 +115,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde { if (m_scene.haveActor(req.actor)) { - if (req.actor is OdePrim) - geom = ((OdePrim)req.actor).m_prim_geom; - else if (req.actor is OdeCharacter) - geom = ((OdeCharacter)req.actor).collider; + if (req.actor is OdePrim prim) + geom = prim.m_prim_geom; + else if (req.actor is OdeCharacter ch) + geom = ch.collider; } if (geom == IntPtr.Zero) { @@ -182,7 +185,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde break; } - lock (m_contactResults) + lock (m_contactResultsLock) m_contactResults.Clear(); } /// @@ -192,15 +195,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void NoContacts(ODERayRequest req) + private static void NoContacts(ODERayRequest req) { - if (req.callbackMethod is RaycastCallback) - { - ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); - return; - } - if (req.callbackMethod is RayCallback) - ((RayCallback)req.callbackMethod)(new List()); + if (req.callbackMethod is RaycastCallback callback) + callback(false, Vector3.Zero, 0, 0, Vector3.Zero); + + else if (req.callbackMethod is RayCallback raycallback) + raycallback(new List()); } private const RayFilterFlags FilterActiveSpace = RayFilterFlags.physical | RayFilterFlags.LSLPhantom; @@ -259,11 +260,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde if ((CurrentRayFilter & RayFilterFlags.agent) != 0) { - foreach(OdeCharacter chr in m_scene._charactersList) + foreach(OdeCharacter chr in CollectionsMarshal.AsSpan(m_scene._charactersList)) { if (m_contactResults.Count >= CurrentMaxCount) break; - collideRayAvatar(chr); + CollideRayAvatar(chr); } } @@ -284,12 +285,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde tmp2 = (float)Math.Sqrt(tmp2); SafeNativeMethods.GeomRaySetLength(ray, tmp2); } - } - collideRayTerrain(m_scene.TerrainGeom); + CollideRayTerrain(m_scene.TerrainGeom); } - if (req.callbackMethod is RaycastCallback) + if (req.callbackMethod is RaycastCallback callback) { // Define default results bool hitYN = false; @@ -299,7 +299,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde Vector3 snormal = Vector3.Zero; // Find closest contact and object. - lock (m_contactResults) + lock (m_contactResultsLock) { foreach (ContactResult cResult in m_contactResults) { @@ -316,12 +316,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (distance > 0 && distance < float.MaxValue) hitYN = true; - ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + callback(hitYN, closestcontact, hitConsumerID, distance, snormal); } else { - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) + List cresult = new(m_contactResults.Count); + lock (m_contactResultsLock) { cresult.AddRange(m_contactResults); m_contactResults.Clear(); @@ -343,7 +343,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde // Collide test SafeNativeMethods.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test - if (req.callbackMethod is RaycastCallback) + if (req.callbackMethod is RaycastCallback callback) { // Define default results bool hitYN = false; @@ -353,7 +353,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde Vector3 snormal = Vector3.Zero; // Find closest contact and object. - lock (m_contactResults) + lock (m_contactResultsLock) { foreach (ContactResult cResult in m_contactResults) { @@ -371,12 +371,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (distance > 0 && distance < float.MaxValue) hitYN = true; - ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + callback(hitYN, closestcontact, hitConsumerID, distance, snormal); } else { - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) + List cresult = new(m_contactResults.Count); + lock (m_contactResultsLock) { cresult.AddRange(m_contactResults); m_contactResults.Clear(); @@ -407,10 +407,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde return; } - int count = 0; + int count; try { - count = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); } catch (Exception e) { @@ -423,52 +423,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_scene.actor_name_map.TryGetValue(g2, out PhysicsActor p2); - if (p2 == null) + if (p2 is null || p2.PhysicsActorType != (int)ActorTypes.Prim) return; - uint ID = 0; - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Prim: + RayFilterFlags thisFlags = p2.IsPhysical ? RayFilterFlags.physical : RayFilterFlags.nonphysical; - RayFilterFlags thisFlags; + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; - if (p2.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; - if (p2.Phantom) - thisFlags |= RayFilterFlags.phantom; + if ((thisFlags & CurrentRayFilter) == 0) + return; - if (p2.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; - - if ((thisFlags & CurrentRayFilter) == 0) - return; - - ID = ((OdePrim)p2).LocalID; - break; - - default: - return; -// break; - } + uint ID = ((OdePrim)p2).LocalID; // closestHit for now only works for meshs, so must do it for others if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) { // Loop all contacts, build results. - for (int i = 0; i < count; i++) + for (int i = 0; i < count; ++i) { - lock (m_contactResults) + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i]; + lock (m_contactResultsLock) { m_contactResults.Add(new ContactResult { ConsumerID = ID, - Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z), - Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z), - Depth = m_contacts[i].depth + Pos = Unsafe.As(ref curCtg.pos), + Normal = Unsafe.As(ref curCtg.normal), + Depth = curCtg.depth }); if (m_contactResults.Count >= CurrentMaxCount) return; @@ -478,41 +463,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde else { // keep only closest contact - SharedCollisionResult.ConsumerID = ID; - SharedCollisionResult.Depth = float.MaxValue; + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0]; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + float depth = curCtg.depth; + SharedCollisionResult.Depth = depth; - for (int i = 0; i < count; i++) + for (int i = 1; i < count; ++i) { - if (m_contacts[i].depth < SharedCollisionResult.Depth) + curCtg = ref m_contacts[i]; + if (curCtg.depth < depth) { - SharedCollisionResult.Pos.X = m_contacts[i].pos.X; - SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y; - SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z; - SharedCollisionResult.Normal.X = m_contacts[i].normal.X; - SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y; - SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z; - SharedCollisionResult.Depth = m_contacts[i].depth; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + depth = curCtg.depth; + SharedCollisionResult.Depth = depth; } } - if (SharedCollisionResult.Depth != float.MaxValue) - { - lock (m_contactResults) - m_contactResults.Add(SharedCollisionResult); - } + SharedCollisionResult.ConsumerID = ID; + lock (m_contactResultsLock) + m_contactResults.Add(SharedCollisionResult); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void collideRayAvatar(OdeCharacter chr) + private void CollideRayAvatar(OdeCharacter chr) { - if (chr.collider == IntPtr.Zero) - return; - - int count = 0; + int count; try { - count = SafeNativeMethods.CollidePtr(ray, chr.collider, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(ray, chr.collider, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); if (count == 0) return; } @@ -529,14 +510,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde // Loop all contacts, build results. for (int i = 0; i < count; i++) { - lock (m_contactResults) + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i]; + lock (m_contactResultsLock) { m_contactResults.Add(new ContactResult { ConsumerID = id, - Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z), - Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z), - Depth = m_contacts[i].depth + Pos = Unsafe.As(ref curCtg.pos), + Normal = Unsafe.As(ref curCtg.normal), + Depth = curCtg.depth }); if (m_contactResults.Count >= CurrentMaxCount) return; @@ -546,41 +528,40 @@ namespace OpenSim.Region.PhysicsModule.ubOde else { // keep only closest contact - SharedCollisionResult.ConsumerID = chr.LocalID; - SharedCollisionResult.Depth = float.MaxValue; + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0]; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + float depth = curCtg.depth; + SharedCollisionResult.Depth = depth; - for (int i = 0; i < count; i++) + for (int i = 1; i < count; ++i) { - if (m_contacts[i].depth < SharedCollisionResult.Depth) + curCtg = ref m_contacts[i]; + if (curCtg.depth < depth) { - SharedCollisionResult.Pos.X = m_contacts[i].pos.X; - SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y; - SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z; - SharedCollisionResult.Normal.X = m_contacts[i].normal.X; - SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y; - SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z; - SharedCollisionResult.Depth = m_contacts[i].depth; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + depth = curCtg.depth; + SharedCollisionResult.Depth = depth; } } - if (SharedCollisionResult.Depth != float.MaxValue) - { - lock (m_contactResults) - m_contactResults.Add(SharedCollisionResult); - } + SharedCollisionResult.ConsumerID = chr.LocalID; + lock (m_contactResultsLock) + m_contactResults.Add(SharedCollisionResult); } } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void collideRayTerrain(IntPtr terrain) + private void CollideRayTerrain(IntPtr terrain) { if (terrain == IntPtr.Zero) return; - int count = 0; + int count; try { - count = SafeNativeMethods.CollidePtr(ray, terrain, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(ray, terrain, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); if (count == 0) return; } @@ -596,14 +577,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde // Loop all contacts, build results. for (int i = 0; i < count; i++) { - lock (m_contactResults) + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[i]; + lock (m_contactResultsLock) { m_contactResults.Add(new ContactResult { ConsumerID = 0, - Pos = new Vector3(m_contacts[i].pos.X, m_contacts[i].pos.Y, m_contacts[i].pos.Z), - Normal = new Vector3(m_contacts[i].normal.X, m_contacts[i].normal.Y, m_contacts[i].normal.Z), - Depth = m_contacts[i].depth + Pos = Unsafe.As(ref curCtg.pos), + Normal = Unsafe.As(ref curCtg.normal), + Depth = curCtg.depth }); if (m_contactResults.Count >= CurrentMaxCount) return; @@ -613,28 +595,27 @@ namespace OpenSim.Region.PhysicsModule.ubOde else { // keep only closest contact - SharedCollisionResult.ConsumerID = 0; - SharedCollisionResult.Depth = float.MaxValue; + ref SafeNativeMethods.ContactGeom curCtg = ref m_contacts[0]; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + float depth = curCtg.depth; + SharedCollisionResult.Depth = depth; - for (int i = 0; i < count; i++) + for (int i = 1; i < count; ++i) { - if (m_contacts[i].depth < SharedCollisionResult.Depth) + curCtg = ref m_contacts[i]; + if (curCtg.depth < depth) { - SharedCollisionResult.Pos.X = m_contacts[i].pos.X; - SharedCollisionResult.Pos.Y = m_contacts[i].pos.Y; - SharedCollisionResult.Pos.Z = m_contacts[i].pos.Z; - SharedCollisionResult.Normal.X = m_contacts[i].normal.X; - SharedCollisionResult.Normal.Y = m_contacts[i].normal.Y; - SharedCollisionResult.Normal.Z = m_contacts[i].normal.Z; - SharedCollisionResult.Depth = m_contacts[i].depth; + SharedCollisionResult.Pos = Unsafe.As(ref curCtg.pos); + SharedCollisionResult.Normal = Unsafe.As(ref curCtg.normal); + depth = curCtg.depth; + SharedCollisionResult.Depth = depth; } } - if (SharedCollisionResult.Depth != float.MaxValue) - { - lock (m_contactResults) - m_contactResults.Add(SharedCollisionResult); - } + SharedCollisionResult.ConsumerID = 0; + lock (m_contactResultsLock) + m_contactResults.Add(SharedCollisionResult); } } @@ -651,7 +632,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } - public class ODERayRequest { public Vector3 Origin; diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs index 5816ffe2dd..cf531d2211 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -474,7 +474,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde m_contactsHandler = GCHandle.Alloc(m_contacts, GCHandleType.Pinned); ContactgeomsArray = m_contactsHandler.AddrOfPinnedObject(); - GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * SafeNativeMethods.Contact.unmanagedSizeOf); + GlobalContactsArray = Marshal.AllocHGlobal((maxContactJoints + 100) * SafeNativeMethods.SizeOfContact); contactSharedForJoints.geom.g1 = IntPtr.Zero; contactSharedForJoints.geom.g2 = IntPtr.Zero; @@ -546,7 +546,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde contactSharedForJoints.geom.pos = contactGeom.pos; contactSharedForJoints.geom.normal = contactGeom.normal; - IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.Contact.unmanagedSizeOf)); + IntPtr contact = new(GlobalContactsArray.ToInt64() + (Int64)(ContactJointCount * SafeNativeMethods.SizeOfContact)); Marshal.StructureToPtr(contactSharedForJoints, contact, false); return SafeNativeMethods.JointCreateContactPtr(world, JointContactGroup, contact); } @@ -578,7 +578,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde // contact points in the space try { - SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, NearCallback); + SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, DefaultNearCallback); } catch (AccessViolationException) { @@ -598,10 +598,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) { int cflags = unchecked((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT)); - count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); } else - count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); } catch (SEHException) { @@ -642,7 +642,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde curctc0.depth, false ); - collision_accounting_events(p1, p2, ref volDepthContact); + Collision_accounting_events(p1, p2, ref volDepthContact); return; } @@ -786,7 +786,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (dop1ava) { - if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))) + if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))) { if (p2.PhysicsActorType == (int)ActorTypes.Agent) { @@ -801,7 +801,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde } else if (dop2ava) { - if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))) + if ((((OdeCharacter)p2).Collide(g1, true, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))) { if (p1.PhysicsActorType == (int)ActorTypes.Agent) { @@ -831,28 +831,169 @@ namespace OpenSim.Region.PhysicsModule.ubOde { maxDepth = curctc.depth; maxDepthContact.PenetrationDepth = maxDepth; - maxDepthContact.Position.X = curctc.pos.X; - maxDepthContact.Position.Y = curctc.pos.Y; - maxDepthContact.Position.Z = curctc.pos.Z; + maxDepthContact.Position = Unsafe.As(ref curctc.pos); maxDepthContact.CharacterFeet = FeetCollision; } if (curctc.depth < minDepth) { minDepth = curctc.depth; - maxDepthContact.SurfaceNormal.X = curctc.normal.X; - maxDepthContact.SurfaceNormal.Y = curctc.normal.Y; - maxDepthContact.SurfaceNormal.Z = curctc.normal.Z; + maxDepthContact.SurfaceNormal = Unsafe.As(ref curctc.normal); } } } if (ncontacts > 0) { - collision_accounting_events(p1, p2, ref maxDepthContact); + Collision_accounting_events(p1, p2, ref maxDepthContact); } } - + + private void CharPrimNearCallback(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + if (ContactJointCount >= maxContactJoints) + return; + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + //if (SafeNativeMethods.GeomIsSpace(g1) || SafeNativeMethods.GeomIsSpace(g2)) + if (SafeNativeMethods.GeomIsSpace(g2)) + { + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, CharPrimNearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + } + return; + } + + // Figure out how many contact points we have + int count = 0; + try + { + if (SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) + { + int cflags = unchecked((int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT)); + count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); + } + else + count = SafeNativeMethods.CollidePtr(g1, g2, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); + } + catch (SEHException) + { + m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); + //ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + return; + } + + // contacts done + if (count == 0) + return; + + // try get physical actors + if (!actor_name_map.TryGetValue(g1, out PhysicsActor p1)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); + return; + } + + if (!actor_name_map.TryGetValue(g2, out PhysicsActor p2)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + return; + } + + // do volume detection case + if (p2.IsVolumeDtc) + { + ref SafeNativeMethods.ContactGeom curctc0 = ref m_contacts[0]; + ContactPoint volDepthContact = new( + new Vector3(curctc0.pos.X, curctc0.pos.Y, curctc0.pos.Z), + new Vector3(curctc0.normal.X, curctc0.normal.Y, curctc0.normal.Z), + curctc0.depth, false + ); + + Collision_accounting_events(p1, p2, ref volDepthContact); + return; + } + + if(p2.PhysicsActorType != (int)ActorTypes.Prim) + return; + + IntPtr Joint; + bool FeetCollision = false; + int ncontacts = 0; + + ContactPoint accountContact = new(); + + float minDepth = float.MaxValue; + float maxDepth = float.MinValue; + + contactSharedForJoints.surface.mu = 0; + contactSharedForJoints.surface.bounce = 0; + + bool useAltcontact; + + IntPtr b1 = SafeNativeMethods.GeomGetBody(g1); + IntPtr b2 = SafeNativeMethods.GeomGetBody(g2); + + for (int i = 0; i < count; ++i) + { + ref SafeNativeMethods.ContactGeom curctc = ref m_contacts[i]; + useAltcontact = false; + + if ((((OdeCharacter)p1).Collide(g2, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision))) + { + if(p2.rootVelocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + + Joint = useAltcontact ? + CreateContacJoint(ref altWorkContact, false) : + CreateContacJoint(ref curctc, false); + if (Joint == IntPtr.Zero) + break; + + SafeNativeMethods.JointAttach(Joint, b1, b2); + + ncontacts++; + + if (curctc.depth > maxDepth) + { + maxDepth = curctc.depth; + accountContact.PenetrationDepth = maxDepth; + accountContact.Position = Unsafe.As(ref curctc.pos); + accountContact.CharacterFeet = FeetCollision; + } + + if (curctc.depth < minDepth) + { + minDepth = curctc.depth; + accountContact.SurfaceNormal = Unsafe.As(ref curctc.normal); + } + } + } + + if (ncontacts > 0) + { + Collision_accounting_events(p1, p2, ref accountContact); + } + } + [MethodImpl(MethodImplOptions.AggressiveInlining)] private void CollideCharChar(OdeCharacter p1, OdeCharacter p2) { @@ -863,7 +1004,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde int count = 0; try { - count = SafeNativeMethods.CollidePtr(p1.collider, p2.collider, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf); + count = SafeNativeMethods.CollidePtr(p1.collider, p2.collider, contactsPerCollision, ContactgeomsArray, SafeNativeMethods.SizeOfContactGeom); } catch (SEHException) { @@ -885,7 +1026,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde bool FeetCollision = false; int ncontacts = 0; - ContactPoint maxDepthContact = new(); + ContactPoint maccountContact = new(); float minDepth = float.MaxValue; float maxDepth = float.MinValue; @@ -900,15 +1041,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde ref SafeNativeMethods.ContactGeom curctc = ref m_contacts[i]; useAltcontact = false; - if (p1.Collide(p1.collider, p2.collider, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)) + if (p1.Collide(p2.collider, false, ref curctc, ref altWorkContact, ref useAltcontact, ref FeetCollision)) { p1.CollidingObj = true; p1.CollidingObj = true; - if (useAltcontact) - Joint = CreateContacJoint(ref altWorkContact, false); - else - Joint = CreateContacJoint(ref curctc, false); + Joint = useAltcontact ? + CreateContacJoint(ref altWorkContact, false) : + CreateContacJoint(ref curctc, false); if (Joint == IntPtr.Zero) break; @@ -919,30 +1059,26 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (curctc.depth > maxDepth) { maxDepth = curctc.depth; - maxDepthContact.PenetrationDepth = maxDepth; - maxDepthContact.Position.X = curctc.pos.X; - maxDepthContact.Position.Y = curctc.pos.Y; - maxDepthContact.Position.Z = curctc.pos.Z; - maxDepthContact.CharacterFeet = FeetCollision; + maccountContact.PenetrationDepth = maxDepth; + maccountContact.Position = Unsafe.As(ref curctc.pos); + maccountContact.CharacterFeet = FeetCollision; } if (curctc.depth < minDepth) { minDepth = curctc.depth; - maxDepthContact.SurfaceNormal.X = curctc.normal.X; - maxDepthContact.SurfaceNormal.Y = curctc.normal.Y; - maxDepthContact.SurfaceNormal.Z = curctc.normal.Z; + maccountContact.SurfaceNormal = Unsafe.As(ref curctc.normal); } } } if (ncontacts > 0) { - collision_accounting_events(p1, p2, ref maxDepthContact); + Collision_accounting_events(p1, p2, ref maccountContact); } } - private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact) + private static void Collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ref ContactPoint contact) { // update actors collision score @@ -1054,8 +1190,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (chr.Body != IntPtr.Zero && chr.collider != IntPtr.Zero) { - SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, NearCallback); - SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, NearCallback); + SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback); + SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback); } } } @@ -1073,8 +1209,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde chr.CollidingObj = false; // do colisions with static space - SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, NearCallback); - SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, NearCallback); + SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, CharPrimNearCallback); + SafeNativeMethods.SpaceCollide2(chr.collider, ActiveSpace, IntPtr.Zero, CharPrimNearCallback); float mx = chr._AABB2D.minx; float Mx = chr._AABB2D.maxx;