diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs index 7ee6a976f5..c5e3b3c9d8 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs @@ -41,9 +41,6 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); public delegate void RayCallback(List list); - public delegate void ProbeBoxCallback(List list); - public delegate void ProbeSphereCallback(List list); - public delegate void ProbePlaneCallback(List list); public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation); public delegate void JointMoved(PhysicsJoint joint); @@ -95,7 +92,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract class PhysicsScene { -// private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + //private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); /// /// A unique identifying string for this instance of the physics engine. diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs index 2f3a5c6d48..acb5203155 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -1026,6 +1026,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] internal static extern void GeomiDestroy(IntPtr geom); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static void GeomDestroy(IntPtr geom) { NTotalGeoms--; @@ -1062,6 +1063,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector3 GeomGetOffsetPosition(IntPtr geom) { unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } @@ -1069,6 +1071,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Matrix3 GeomGetOffsetRotation(IntPtr geom) { unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } @@ -1076,22 +1079,27 @@ namespace OpenSim.Region.PhysicsModule.ubOde [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] internal extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static Vector3 GeomGetPosition(IntPtr geom) { unsafe { return *(GeomGetPositionUnsafe(geom)); } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) { - Vector3 vtmp = GeomGetPosition(geom); - return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); + unsafe + { + Vector3* vtmp = GeomGetPositionUnsafe(geom); + return new OMV.Vector3(vtmp->X, vtmp->Y, vtmp->Z); + } } [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] internal static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) { - Quaternion qtmp; - GeomCopyQuaternion(geom, out qtmp); + GeomCopyQuaternion(geom, out Quaternion qtmp); return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); } @@ -1239,24 +1247,30 @@ namespace OpenSim.Region.PhysicsModule.ubOde internal static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] internal static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomRaySetLength(IntPtr ray, dReal length); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomSetBody(IntPtr geom, IntPtr body); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomSetCategoryBits(IntPtr geom, uint bits); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + [MethodImpl(MethodImplOptions.AggressiveInlining)] internal static extern void GeomSetCollideBits(IntPtr geom, uint bits); [DllImport("ubode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs index 30469b758c..06d7196b5c 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -25,25 +25,10 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -//#define SPAM - using System; using System.Collections.Generic; using System.Reflection; +using System.Runtime.CompilerServices; using log4net; using OpenMetaverse; using OpenSim.Framework; diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs index b726c66b1f..165cc33d31 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs @@ -28,6 +28,7 @@ using System; using System.Collections.Generic; using System.Reflection; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; @@ -54,9 +55,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde private readonly ODEScene m_scene; IntPtr ray; // the ray. we only need one for our lifetime - IntPtr Sphere; - IntPtr Box; - IntPtr Plane; private int CollisionContactGeomsPerTest = 25; private const int DefaultMaxCount = 25; @@ -70,6 +68,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde private readonly List m_contactResults = new List(); private RayFilterFlags CurrentRayFilter; private int CurrentMaxCount; + ContactResult SharedCollisionResult = new ContactResult(); + SafeNativeMethods.ContactGeomClass Sharedcontact = new SafeNativeMethods.ContactGeomClass(); public ODERayCastRequestManager(ODEScene pScene) { @@ -77,14 +77,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde nearCallback = near; ray = SafeNativeMethods.CreateRay(IntPtr.Zero, 1.0f); SafeNativeMethods.GeomSetCategoryBits(ray, 0); - Box = SafeNativeMethods.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); - SafeNativeMethods.GeomSetCategoryBits(Box, 0); - Sphere = SafeNativeMethods.CreateSphere(IntPtr.Zero,1.0f); - SafeNativeMethods.GeomSetCategoryBits(Sphere, 0); - Plane = SafeNativeMethods.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); - SafeNativeMethods.GeomSetCategoryBits(Sphere, 0); } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public void QueueRequest(ODERayRequest req) { if (req.Count == 0) @@ -96,28 +91,21 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// /// Process all queued raycast requests /// - /// Time in MS the raycasts took to process. - public int ProcessQueuedRequests() + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public void ProcessQueuedRequests() { - - if (m_PendingRequests.Count <= 0) - return 0; - - if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) - // oops something got wrong or scene isn't ready still - { - m_PendingRequests.Clear(); - return 0; - } + if (m_PendingRequests.Count == 0) + return ; int time = Util.EnvironmentTickCount(); - - int closestHit; - int backfacecull; - CollisionCategories catflags; - while (m_PendingRequests.Dequeue(out ODERayRequest req)) { + if(req.length <= 0) + { + NoContacts(req); + continue; + } + IntPtr geom = IntPtr.Zero; if (req.actor != null) { @@ -137,70 +125,34 @@ namespace OpenSim.Region.PhysicsModule.ubOde CurrentRayFilter = req.filter; CurrentMaxCount = req.Count; + if (CurrentMaxCount > 25) + CurrentMaxCount = 25; - CollisionContactGeomsPerTest = req.Count & 0xffff; - - closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); - backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); - - if (req.callbackMethod is ProbeBoxCallback) + unchecked { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - SafeNativeMethods.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); - SafeNativeMethods.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); - SafeNativeMethods.Quaternion qtmp; - qtmp.X = req.orientation.X; - qtmp.Y = req.orientation.Y; - qtmp.Z = req.orientation.Z; - qtmp.W = req.orientation.W; - SafeNativeMethods.GeomSetQuaternion(Box, ref qtmp); - } - else if (req.callbackMethod is ProbeSphereCallback) - { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - - SafeNativeMethods.GeomSphereSetRadius(Sphere, req.length); - SafeNativeMethods.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); - } - else if (req.callbackMethod is ProbePlaneCallback) - { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - - SafeNativeMethods.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); + CollisionContactGeomsPerTest = ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) ? + CurrentMaxCount | (int)SafeNativeMethods.CONTACTS_UNIMPORTANT : CurrentMaxCount; } + int backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); + SafeNativeMethods.GeomRaySetParams(ray, 0, backfacecull); + + if (req.callbackMethod is RaycastCallback) + { + // if we only want one get only one per Collision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + SafeNativeMethods.GeomRaySetClosestHit(ray, 1); + } else { - if (CollisionContactGeomsPerTest > 25) - CollisionContactGeomsPerTest = 25; - - SafeNativeMethods.GeomRaySetLength(ray, req.length); - SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); - SafeNativeMethods.GeomRaySetParams(ray, 0, backfacecull); - - if (req.callbackMethod is RaycastCallback) - { - // if we only want one get only one per Collision pair saving memory - CurrentRayFilter |= RayFilterFlags.ClosestHit; - SafeNativeMethods.GeomRaySetClosestHit(ray, 1); - } - else - SafeNativeMethods.GeomRaySetClosestHit(ray, closestHit); + int closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); + SafeNativeMethods.GeomRaySetClosestHit(ray, closestHit); } - if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) - unchecked - { - CollisionContactGeomsPerTest |= (int)SafeNativeMethods.CONTACTS_UNIMPORTANT; - } - if (geom == IntPtr.Zero) { // translate ray filter to Collision flags - catflags = 0; + CollisionCategories catflags = 0; if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) catflags |= CollisionCategories.VolumeDtc; if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) @@ -211,53 +163,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde catflags |= CollisionCategories.Geom; if ((CurrentRayFilter & RayFilterFlags.land) != 0) catflags |= CollisionCategories.Land; - if ((CurrentRayFilter & RayFilterFlags.water) != 0) - catflags |= CollisionCategories.Water; if (catflags != 0) { - if (req.callbackMethod is ProbeBoxCallback) - { - catflags |= CollisionCategories.Space; - SafeNativeMethods.GeomSetCollideBits(Box, (uint)catflags); - SafeNativeMethods.GeomSetCategoryBits(Box, (uint)catflags); - doProbe(req, Box); - } - else if (req.callbackMethod is ProbeSphereCallback) - { - catflags |= CollisionCategories.Space; - SafeNativeMethods.GeomSetCollideBits(Sphere, (uint)catflags); - SafeNativeMethods.GeomSetCategoryBits(Sphere, (uint)catflags); - doProbe(req, Sphere); - } - else if (req.callbackMethod is ProbePlaneCallback) - { - catflags |= CollisionCategories.Space; - SafeNativeMethods.GeomSetCollideBits(Plane, (uint)catflags); - SafeNativeMethods.GeomSetCategoryBits(Plane, (uint)catflags); - doPlane(req,IntPtr.Zero); - } - else - { - SafeNativeMethods.GeomSetCollideBits(ray, (uint)catflags); - doSpaceRay(req); - } + SafeNativeMethods.GeomSetCollideBits(ray, (uint)catflags); + doSpaceRay(req); } } else { // if we select a geom don't use filters - - if (req.callbackMethod is ProbePlaneCallback) - { - SafeNativeMethods.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); - doPlane(req,geom); - } - else - { - SafeNativeMethods.GeomSetCollideBits(ray, (uint)CollisionCategories.All); - doGeomRay(req,geom); - } + SafeNativeMethods.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req,geom); } if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) @@ -266,8 +183,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde lock (m_contactResults) m_contactResults.Clear(); - - return Util.EnvironmentTickCountSubtract(time); } /// /// Method that actually initiates the raycast with spaces @@ -275,6 +190,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde /// /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] private void NoContacts(ODERayRequest req) { if (req.callbackMethod is RaycastCallback) @@ -282,30 +198,77 @@ namespace OpenSim.Region.PhysicsModule.ubOde ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); return; } - List cresult = new List(); - if (req.callbackMethod is RayCallback) - ((RayCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeBoxCallback) - ((ProbeBoxCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeSphereCallback) - ((ProbeSphereCallback)req.callbackMethod)(cresult); + ((RayCallback)req.callbackMethod)(new List()); } - private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom; -// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; - private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; + private const RayFilterFlags FilterActiveSpace = RayFilterFlags.physical | RayFilterFlags.LSLPhantom; + private const RayFilterFlags FilterStaticSpace = RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private void doSpaceRay(ODERayRequest req) { + /* + float endx; + float endy; + + if (req.Normal.X <= 0) + { + if (req.Origin.X <= 0) + { + NoContacts(req); + return; + } + + endx = req.Origin.X - req.length; + if (endx < 0) + { + req.length += endx; + endx = 0; + } + } + else + { + if (req.Origin.X < 0) + { + req.length += req.Origin.X; + if (req.length <= 0) + { + NoContacts(req); + return; + } + req.Origin.X = 0; + } + + endx = req.Origin.X + req.length; + if (endx < 0) + { + req.length += endx; + endx = 0; + } + } + */ + + SafeNativeMethods.GeomRaySetLength(ray, req.length); + SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + // Collide tests if ((CurrentRayFilter & FilterActiveSpace) != 0) - { SafeNativeMethods.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - SafeNativeMethods.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + + if ((CurrentRayFilter & RayFilterFlags.agent) != 0) + { + foreach(OdeCharacter chr in m_scene._charactersList) + { + if (m_contactResults.Count >= CurrentMaxCount) + break; + collideRayAvatar(chr); + } } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) SafeNativeMethods.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) { // current ode land to ray collisions is very bad @@ -366,67 +329,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } - private void doProbe(ODERayRequest req, IntPtr probe) - { - // Collide tests - if ((CurrentRayFilter & FilterActiveSpace) != 0) - { - SafeNativeMethods.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - SafeNativeMethods.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); - } - if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) - SafeNativeMethods.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); - if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) - SafeNativeMethods.SpaceCollide2(probe, m_scene.TerrainGeom, IntPtr.Zero, nearCallback); - - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - if (req.callbackMethod is ProbeBoxCallback) - ((ProbeBoxCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeSphereCallback) - ((ProbeSphereCallback)req.callbackMethod)(cresult); - } - - private void doPlane(ODERayRequest req,IntPtr geom) - { - // Collide tests - if (geom == IntPtr.Zero) - { - if ((CurrentRayFilter & FilterActiveSpace) != 0) - { - SafeNativeMethods.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - SafeNativeMethods.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); - } - if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) - SafeNativeMethods.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); - if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) - SafeNativeMethods.SpaceCollide2(Plane, m_scene.TerrainGeom, IntPtr.Zero, nearCallback); - } - else - { - SafeNativeMethods.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); - } - - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - - ((ProbePlaneCallback)req.callbackMethod)(cresult); - } - /// /// Method that actually initiates the raycast with a geom /// /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] private void doGeomRay(ODERayRequest req, IntPtr geom) { + SafeNativeMethods.GeomRaySetLength(ray, req.length); + SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + // Collide test SafeNativeMethods.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test @@ -472,10 +384,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde } } + [MethodImpl(MethodImplOptions.AggressiveInlining)] private bool GetCurContactGeom(int index, ref SafeNativeMethods.ContactGeomClass newcontactgeom) { IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; - if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) + if (index >= CollisionContactGeomsPerTest) return false; IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * SafeNativeMethods.ContactGeomClass.unmanagedSizeOf)); @@ -484,12 +397,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde } // This is the standard Near. g1 is the ray - private void near(IntPtr space, IntPtr g1, IntPtr g2) + private void near(IntPtr dummy, IntPtr g1, IntPtr g2) { - if (g2 == IntPtr.Zero || g1 == g2) + if (g2 == IntPtr.Zero) return; - if (m_contactResults.Count >= CurrentMaxCount) + if (m_contactResults.Count >= CurrentMaxCount) return; if (SafeNativeMethods.GeomIsSpace(g2)) @@ -518,21 +431,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde if (count == 0) return; -/* - uint cat1 = d.GeomGetCategoryBits(g1); - uint cat2 = d.GeomGetCategoryBits(g2); - uint col1 = d.GeomGetCollideBits(g1); - uint col2 = d.GeomGetCollideBits(g2); -*/ - uint ID = 0; - PhysicsActor p2 = null; - - m_scene.actor_name_map.TryGetValue(g2, out p2); + m_scene.actor_name_map.TryGetValue(g2, out PhysicsActor p2); if (p2 == null) return; + uint ID = 0; switch (p2.PhysicsActorType) { case (int)ActorTypes.Prim: @@ -556,53 +461,34 @@ namespace OpenSim.Region.PhysicsModule.ubOde ID = ((OdePrim)p2).LocalID; break; - case (int)ActorTypes.Agent: - - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) - return; - else - ID = ((OdeCharacter)p2).LocalID; - break; - case (int)ActorTypes.Ground: if ((CurrentRayFilter & RayFilterFlags.land) == 0) return; break; - case (int)ActorTypes.Water: - - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - break; - default: break; } - SafeNativeMethods.ContactGeomClass curcontact = new SafeNativeMethods.ContactGeomClass(); - // 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++) { - if (!GetCurContactGeom(i, ref curcontact)) + if (!GetCurContactGeom(i, ref Sharedcontact)) break; - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; lock (m_contactResults) { - m_contactResults.Add(collisionresult); + m_contactResults.Add(new ContactResult + { + ConsumerID = ID, + Pos = new Vector3(Sharedcontact.pos.X, Sharedcontact.pos.Y, Sharedcontact.pos.Z), + Normal = new Vector3(Sharedcontact.normal.X, Sharedcontact.normal.Y, Sharedcontact.normal.Z), + Depth = Sharedcontact.depth + }); if (m_contactResults.Count >= CurrentMaxCount) return; } @@ -611,31 +497,103 @@ namespace OpenSim.Region.PhysicsModule.ubOde else { // keep only closest contact - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Depth = float.MaxValue; + SharedCollisionResult.ConsumerID = ID; + SharedCollisionResult.Depth = float.MaxValue; for (int i = 0; i < count; i++) { - if (!GetCurContactGeom(i, ref curcontact)) + if (!GetCurContactGeom(i, ref Sharedcontact)) break; - if (curcontact.depth < collisionresult.Depth) + if (Sharedcontact.depth < SharedCollisionResult.Depth) { - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; + SharedCollisionResult.Pos.X = Sharedcontact.pos.X; + SharedCollisionResult.Pos.Y = Sharedcontact.pos.Y; + SharedCollisionResult.Pos.Z = Sharedcontact.pos.Z; + SharedCollisionResult.Normal.X = Sharedcontact.normal.X; + SharedCollisionResult.Normal.Y = Sharedcontact.normal.Y; + SharedCollisionResult.Normal.Z = Sharedcontact.normal.Z; + SharedCollisionResult.Depth = Sharedcontact.depth; } } - if (collisionresult.Depth != float.MaxValue) + if (SharedCollisionResult.Depth != float.MaxValue) { lock (m_contactResults) - m_contactResults.Add(collisionresult); + m_contactResults.Add(SharedCollisionResult); + } + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void collideRayAvatar(OdeCharacter chr) + { + if (chr.collider == IntPtr.Zero) + return; + + int count = 0; + try + { + count = SafeNativeMethods.CollidePtr(ray, chr.collider, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeomClass.unmanagedSizeOf); + if (count == 0) + return; + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + // 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++) + { + if (!GetCurContactGeom(i, ref Sharedcontact)) + break; + + lock (m_contactResults) + { + m_contactResults.Add(new ContactResult + { + ConsumerID = chr.LocalID, + Pos = new Vector3(Sharedcontact.pos.X, Sharedcontact.pos.Y, Sharedcontact.pos.Z), + Normal = new Vector3(Sharedcontact.normal.X, Sharedcontact.normal.Y, Sharedcontact.normal.Z), + Depth = Sharedcontact.depth + }); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + SharedCollisionResult.ConsumerID = chr.LocalID; + SharedCollisionResult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref Sharedcontact)) + break; + + if (Sharedcontact.depth < SharedCollisionResult.Depth) + { + SharedCollisionResult.Pos.X = Sharedcontact.pos.X; + SharedCollisionResult.Pos.Y = Sharedcontact.pos.Y; + SharedCollisionResult.Pos.Z = Sharedcontact.pos.Z; + SharedCollisionResult.Normal.X = Sharedcontact.normal.X; + SharedCollisionResult.Normal.Y = Sharedcontact.normal.Y; + SharedCollisionResult.Normal.Z = Sharedcontact.normal.Z; + SharedCollisionResult.Depth = Sharedcontact.depth; + } + } + + if (SharedCollisionResult.Depth != float.MaxValue) + { + lock (m_contactResults) + m_contactResults.Add(SharedCollisionResult); } } } @@ -650,33 +608,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde SafeNativeMethods.GeomDestroy(ray); ray = IntPtr.Zero; } - if (Box != IntPtr.Zero) - { - SafeNativeMethods.GeomDestroy(Box); - Box = IntPtr.Zero; - } - if (Sphere != IntPtr.Zero) - { - SafeNativeMethods.GeomDestroy(Sphere); - Sphere = IntPtr.Zero; - } - if (Plane != IntPtr.Zero) - { - SafeNativeMethods.GeomDestroy(Plane); - Plane = IntPtr.Zero; - } } } - public struct ODERayRequest + + public class ODERayRequest { - public PhysicsActor actor; public Vector3 Origin; public Vector3 Normal; + public object callbackMethod; + public PhysicsActor actor; public int Count; public float length; - public object callbackMethod; public RayFilterFlags filter; - public Quaternion orientation; } } diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs index d942d3fc25..f6fb7338af 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -2464,119 +2464,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde return ourResults; } - public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) - { - List ourResults = null; - object SyncObject = new object(); - - ProbeBoxCallback retMethod = delegate(List results) - { - lock (SyncObject) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - } - }; - - ODERayRequest req = new ODERayRequest - { - actor = null, - callbackMethod = retMethod, - Normal = size, - Origin = position, - orientation = orientation, - Count = Count, - filter = flags - }; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) - { - List ourResults = null; - object SyncObject = new object(); - - ProbeSphereCallback retMethod = delegate(List results) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - }; - - ODERayRequest req = new ODERayRequest - { - actor = null, - callbackMethod = retMethod, - length = radius, - Origin = position, - Count = Count, - filter = flags - }; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) - { - IntPtr geom = IntPtr.Zero;; - - if (actor != null) - { - if (actor is OdePrim) - geom = ((OdePrim)actor).m_prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).m_prim_geom; - } - - List ourResults = null; - object SyncObject = new object(); - - ProbePlaneCallback retMethod = delegate(List results) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - }; - - ODERayRequest req = new ODERayRequest - { - actor = null, - callbackMethod = retMethod, - length = plane.W, - Normal = new Vector3(plane.X, plane.Y, plane.Z), - Count = Count, - filter = flags - }; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) { Util.FireAndForget( delegate