/*
* Copyright (c) Contributors, http://opensimulator.org/
* See CONTRIBUTORS.TXT for a full list of copyright holders.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the OpenSimulator Project nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
using System;
using System.Collections.Generic;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Text;
using OpenSim.Framework;
using OpenSim.Region.PhysicsModules.SharedBase;
using OdeAPI;
using log4net;
using OpenMetaverse;
namespace OpenSim.Region.PhysicsModule.UbitOde
{
///
/// Processes raycast requests as ODE is in a state to be able to do them.
/// This ensures that it's thread safe and there will be no conflicts.
/// Requests get returned by a different thread then they were requested by.
///
public class ODERayCastRequestManager
{
///
/// Pending ray requests
///
protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue();
///
/// Scene that created this object.
///
private 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;
private const int MaxTimePerCallMS = 30;
///
/// ODE near callback delegate
///
private d.NearCallback nearCallback;
private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
private List m_contactResults = new List();
private RayFilterFlags CurrentRayFilter;
private int CurrentMaxCount;
public ODERayCastRequestManager(ODEScene pScene)
{
m_scene = pScene;
nearCallback = near;
ray = d.CreateRay(IntPtr.Zero, 1.0f);
d.GeomSetCategoryBits(ray, 0);
Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
d.GeomSetCategoryBits(Box, 0);
Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
d.GeomSetCategoryBits(Sphere, 0);
Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
d.GeomSetCategoryBits(Sphere, 0);
}
public void QueueRequest(ODERayRequest req)
{
if (req.Count == 0)
req.Count = DefaultMaxCount;
m_PendingRequests.Enqueue(req);
}
///
/// Process all queued raycast requests
///
/// Time in MS the raycasts took to process.
public int 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;
}
int time = Util.EnvironmentTickCount();
ODERayRequest req;
int closestHit;
int backfacecull;
CollisionCategories catflags;
while (m_PendingRequests.Dequeue(out req))
{
if (req.callbackMethod != null)
{
IntPtr geom = IntPtr.Zero;
if (req.actor != null)
{
if (m_scene.haveActor(req.actor))
{
if (req.actor is OdePrim)
geom = ((OdePrim)req.actor).prim_geom;
else if (req.actor is OdeCharacter)
geom = ((OdePrim)req.actor).prim_geom;
}
if (geom == IntPtr.Zero)
{
NoContacts(req);
continue;
}
}
CurrentRayFilter = req.filter;
CurrentMaxCount = req.Count;
CollisionContactGeomsPerTest = req.Count & 0xffff;
closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
if (req.callbackMethod is ProbeBoxCallback)
{
if (CollisionContactGeomsPerTest > 80)
CollisionContactGeomsPerTest = 80;
d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
d.Quaternion qtmp;
qtmp.X = req.orientation.X;
qtmp.Y = req.orientation.Y;
qtmp.Z = req.orientation.Z;
qtmp.W = req.orientation.W;
d.GeomSetQuaternion(Box, ref qtmp);
}
else if (req.callbackMethod is ProbeSphereCallback)
{
if (CollisionContactGeomsPerTest > 80)
CollisionContactGeomsPerTest = 80;
d.GeomSphereSetRadius(Sphere, req.length);
d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
}
else if (req.callbackMethod is ProbePlaneCallback)
{
if (CollisionContactGeomsPerTest > 80)
CollisionContactGeomsPerTest = 80;
d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
}
else
{
if (CollisionContactGeomsPerTest > 25)
CollisionContactGeomsPerTest = 25;
d.GeomRaySetLength(ray, req.length);
d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
d.GeomRaySetParams(ray, 0, backfacecull);
d.GeomRaySetClosestHit(ray, closestHit);
if (req.callbackMethod is RaycastCallback)
{
// if we only want one get only one per Collision pair saving memory
CurrentRayFilter |= RayFilterFlags.ClosestHit;
d.GeomRaySetClosestHit(ray, 1);
}
else
d.GeomRaySetClosestHit(ray, closestHit);
}
if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
unchecked
{
CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
}
if (geom == IntPtr.Zero)
{
// translate ray filter to Collision flags
catflags = 0;
if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
catflags |= CollisionCategories.VolumeDtc;
if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
catflags |= CollisionCategories.Phantom;
if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
catflags |= CollisionCategories.Character;
if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
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;
d.GeomSetCollideBits(Box, (uint)catflags);
d.GeomSetCategoryBits(Box, (uint)catflags);
doProbe(req, Box);
}
else if (req.callbackMethod is ProbeSphereCallback)
{
catflags |= CollisionCategories.Space;
d.GeomSetCollideBits(Sphere, (uint)catflags);
d.GeomSetCategoryBits(Sphere, (uint)catflags);
doProbe(req, Sphere);
}
else if (req.callbackMethod is ProbePlaneCallback)
{
catflags |= CollisionCategories.Space;
d.GeomSetCollideBits(Plane, (uint)catflags);
d.GeomSetCategoryBits(Plane, (uint)catflags);
doPlane(req,IntPtr.Zero);
}
else
{
d.GeomSetCollideBits(ray, (uint)catflags);
doSpaceRay(req);
}
}
}
else
{
// if we select a geom don't use filters
if (req.callbackMethod is ProbePlaneCallback)
{
d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
doPlane(req,geom);
}
else
{
d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
doGeomRay(req,geom);
}
}
}
if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
break;
}
lock (m_contactResults)
m_contactResults.Clear();
return Util.EnvironmentTickCountSubtract(time);
}
///
/// Method that actually initiates the raycast with spaces
///
///
///
private void NoContacts(ODERayRequest req)
{
if (req.callbackMethod is RaycastCallback)
{
((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);
}
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 void doSpaceRay(ODERayRequest req)
{
// Collide tests
if ((CurrentRayFilter & FilterActiveSpace) != 0)
{
d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
}
if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
d.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 > 30.0f)
d.GeomRaySetLength(ray, 30.0f);
d.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 cresult = new List(m_contactResults.Count);
lock (m_PendingRequests)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
}
((RayCallback)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 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)
{
d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
}
if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
}
else
{
d.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
///
///
private void doGeomRay(ODERayRequest req, IntPtr geom)
{
// Collide test
d.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 cresult = new List(m_contactResults.Count);
lock (m_PendingRequests)
{
cresult.AddRange(m_contactResults);
m_contactResults.Clear();
}
((RayCallback)req.callbackMethod)(cresult);
}
}
private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
{
IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
return false;
IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
return true;
}
// This is the standard Near. g1 is the ray
private void near(IntPtr space, IntPtr g1, IntPtr g2)
{
if (g2 == IntPtr.Zero || g1 == g2)
return;
if (m_contactResults.Count >= CurrentMaxCount)
return;
if (d.GeomIsSpace(g2))
{
try
{
d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
}
return;
}
int count = 0;
try
{
count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
}
catch (Exception e)
{
m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
return;
}
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);
if (p2 == null)
return;
switch (p2.PhysicsActorType)
{
case (int)ActorTypes.Prim:
RayFilterFlags thisFlags;
if (p2.IsPhysical)
thisFlags = RayFilterFlags.physical;
else
thisFlags = RayFilterFlags.nonphysical;
if (p2.Phantom)
thisFlags |= RayFilterFlags.phantom;
if (p2.IsVolumeDtc)
thisFlags |= RayFilterFlags.volumedtc;
if ((thisFlags & CurrentRayFilter) == 0)
return;
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;
}
d.ContactGeom curcontact = new d.ContactGeom();
// 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))
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);
if (m_contactResults.Count >= CurrentMaxCount)
return;
}
}
}
else
{
// keep only closest contact
ContactResult collisionresult = new ContactResult();
collisionresult.ConsumerID = ID;
collisionresult.Depth = float.MaxValue;
for (int i = 0; i < count; i++)
{
if (!GetCurContactGeom(i, ref curcontact))
break;
if (curcontact.depth < collisionresult.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;
}
}
if (collisionresult.Depth != float.MaxValue)
{
lock (m_contactResults)
m_contactResults.Add(collisionresult);
}
}
}
///
/// Dereference the creator scene so that it can be garbage collected if needed.
///
internal void Dispose()
{
m_scene = null;
if (ray != IntPtr.Zero)
{
d.GeomDestroy(ray);
ray = IntPtr.Zero;
}
if (Box != IntPtr.Zero)
{
d.GeomDestroy(Box);
Box = IntPtr.Zero;
}
if (Sphere != IntPtr.Zero)
{
d.GeomDestroy(Sphere);
Sphere = IntPtr.Zero;
}
if (Plane != IntPtr.Zero)
{
d.GeomDestroy(Plane);
Plane = IntPtr.Zero;
}
}
}
public struct ODERayRequest
{
public PhysicsActor actor;
public Vector3 Origin;
public Vector3 Normal;
public int Count;
public float length;
public object callbackMethod;
public RayFilterFlags filter;
public Quaternion orientation;
}
}