ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-05 17:05:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

several changed to ubode rays; remove some test probes not usefull, etc

Browse Source
This commit is contained in:
UbitUmarov
2022-08-18 00:23:38 +01:00
parent 4fae19164d
commit 6ebcae4885
5 changed files with 224 additions and 398 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs
View File

@@ -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<ContactResult> list);
public delegate void ProbeBoxCallback(List<ContactResult> list);
public delegate void ProbeSphereCallback(List<ContactResult> list);
public delegate void ProbePlaneCallback(List<ContactResult> 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);
/// <summary>
/// A unique identifying string for this instance of the physics engine.

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

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

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

@@ -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_<other> 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;

465
OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs
View File

@@ -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<ContactResult> m_contactResults = new List<ContactResult>();
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
/// <summary>
/// Process all queued raycast requests
/// </summary>
/// <returns>Time in MS the raycasts took to process.</returns>
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);
}
/// <summary>
/// Method that actually initiates the raycast with spaces
@@ -275,6 +190,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
/// <param name="req"></param>
///
[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<ContactResult> cresult = new List<ContactResult>();
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<ContactResult>());
}
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<ContactResult> cresult = new List<ContactResult>(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<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
lock (m_PendingRequests)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
}
((ProbePlaneCallback)req.callbackMethod)(cresult);
}
/// <summary>
/// Method that actually initiates the raycast with a geom
/// </summary>
/// <param name="req"></param>
[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;
}
}

113
OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs
View File

@@ -2464,119 +2464,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde
return ourResults;
}
public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
{
List<ContactResult> ourResults = null;
object SyncObject = new object();
ProbeBoxCallback retMethod = delegate(List<ContactResult> 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<ContactResult>();
}
if (ourResults == null)
return new List<ContactResult>();
return ourResults;
}
public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
{
List<ContactResult> ourResults = null;
object SyncObject = new object();
ProbeSphereCallback retMethod = delegate(List<ContactResult> 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<ContactResult>();
}
if (ourResults == null)
return new List<ContactResult>();
return ourResults;
}
public override List<ContactResult> 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<ContactResult> ourResults = null;
object SyncObject = new object();
ProbePlaneCallback retMethod = delegate(List<ContactResult> 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<ContactResult>();
}
if (ourResults == null)
return new List<ContactResult>();
return ourResults;
}
public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
{
Util.FireAndForget( delegate