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.GroundSpace, 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);
}
private void doProbe(ODERayRequest req, IntPtr probe)
{
// Collide tests
if ((CurrentRayFilter & FilterActiveSpace) != 0)
{
d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
}
if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
{
d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
}
if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
{
d.SpaceCollide2(probe, m_scene.GroundSpace, 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);
}
}
public void QueueRequest(ODERayRequest req)
{
if (req.Count == 0)
{
req.Count = DefaultMaxCount;
}
m_PendingRequests.Enqueue(req);
}
/// <summary>
/// Method that actually initiates the raycast with a geom
/// </summary>
/// <param name="req"></param>
private void doGeomRay(ODERayRequest req, IntPtr geom)
{
// Collide test
SafeNativeMethods.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
if (req.callbackMethod is RaycastCallback)
{
// Define default results
bool hitYN = false;
uint hitConsumerID = 0;
float distance = float.MaxValue;
Vector3 closestcontact = Vector3.Zero;
Vector3 snormal = Vector3.Zero;
// Find closest contact and object.
lock (m_contactResults)
{
foreach (ContactResult cResult in m_contactResults)
{
if (cResult.Depth < distance)
{
closestcontact = cResult.Pos;
hitConsumerID = cResult.ConsumerID;
distance = cResult.Depth;
snormal = cResult.Normal;
}
}
m_contactResults.Clear();
}
if (distance > 0 && distance < float.MaxValue)
{
hitYN = true;
}
((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
}
else
{
List <ContactResult> cresult = new List <ContactResult>(m_contactResults.Count);
lock (m_PendingRequests)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
}
((RayCallback)req.callbackMethod)(cresult);
}
}
/// <summary>
/// Method that actually initiates the raycast with spaces
/// </summary>
/// <param name="req"></param>
///
private void NoContacts(ODERayRequest req)
{
if (req.callbackMethod is RaycastCallback)
{
((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);
}
}
public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
{
if (retMethod != null)
{
ODERayRequest req = new ODERayRequest();
req.actor = null;
req.callbackMethod = retMethod;
req.length = length;
req.Normal = direction;
req.Origin = position;
req.Count = Count;
req.filter = RayFilterFlags.AllPrims;
m_rayCastManager.QueueRequest(req);
}
}
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).prim_geom;
else if (actor is OdeCharacter)
geom = ((OdePrim)actor).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();
req.actor = null;
req.callbackMethod = retMethod;
req.length = plane.W;
req.Normal.X = plane.X;
req.Normal.Y = plane.Y;
req.Normal.Z = plane.Z;
req.Count = Count;
req.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();
req.actor = null;
req.callbackMethod = retMethod;
req.length = radius;
req.Origin = position;
req.Count = Count;
req.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> 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();
req.actor = null;
req.callbackMethod = retMethod;
req.Normal = size;
req.Origin = position;
req.orientation = orientation;
req.Count = Count;
req.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> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
{
if (actor == null)
return new List<ContactResult>();
IntPtr geom;
if (actor is OdePrim)
geom = ((OdePrim)actor).prim_geom;
else if (actor is OdeCharacter)
geom = ((OdePrim)actor).prim_geom;
else
return new List<ContactResult>();
if (geom == IntPtr.Zero)
return new List<ContactResult>();
List<ContactResult> ourResults = null;
object SyncObject = new object();
RayCallback retMethod = delegate(List<ContactResult> results)
{
lock (SyncObject)
{
ourResults = results;
Monitor.PulseAll(SyncObject);
}
};
ODERayRequest req = new ODERayRequest();
req.actor = actor;
req.callbackMethod = retMethod;
req.length = length;
req.Normal = direction;
req.Origin = position;
req.Count = Count;
req.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 object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
{
object SyncObject = new object();
List<ContactResult> ourresults = new List<ContactResult>();
RayCallback retMethod = delegate(List<ContactResult> results)
{
lock (SyncObject)
{
ourresults = results;
Monitor.PulseAll(SyncObject);
}
};
ODERayRequest req = new ODERayRequest();
req.actor = null;
req.callbackMethod = retMethod;
req.length = length;
req.Normal = direction;
req.Origin = position;
req.Count = Count;
req.filter = filter;
lock (SyncObject)
{
m_rayCastManager.QueueRequest(req);
if (!Monitor.Wait(SyncObject, 500))
return null;
else
return ourresults;
}
}
private void doSpaceRay(ODERayRequest req)
{
// 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 & 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
// so for now limit its range badly
if (req.length > 60.0f)
{
Vector3 t = req.Normal * req.length;
float tmp = t.X * t.X + t.Y * t.Y;
if (tmp > 2500)
{
float tmp2 = req.length * req.length - tmp + 2500;
tmp2 = (float)Math.Sqrt(tmp2);
SafeNativeMethods.GeomRaySetLength(ray, tmp2);
}
}
SafeNativeMethods.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
}
if (req.callbackMethod is RaycastCallback)
{
// Define default results
bool hitYN = false;
uint hitConsumerID = 0;
float distance = float.MaxValue;
Vector3 closestcontact = Vector3.Zero;
Vector3 snormal = Vector3.Zero;
// Find closest contact and object.
lock (m_contactResults)
{
foreach (ContactResult cResult in m_contactResults)
{
if (cResult.Depth < distance)
{
closestcontact = cResult.Pos;
hitConsumerID = cResult.ConsumerID;
distance = cResult.Depth;
snormal = cResult.Normal;
}
}
m_contactResults.Clear();
}
if (distance > 0 && distance < float.MaxValue)
{
hitYN = true;
}
((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
}
else
{
List <ContactResult> cresult = new List <ContactResult>(m_contactResults.Count);
lock (m_PendingRequests)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
}
((RayCallback)req.callbackMethod)(cresult);
}
}
