private bool GetCurContactGeom(int index, ref SafeNativeMethods.ContactGeom newcontactgeom)
{
IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
{
return(false);
}
IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * SafeNativeMethods.ContactGeom.unmanagedSizeOf));
newcontactgeom = (SafeNativeMethods.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(SafeNativeMethods.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 (SafeNativeMethods.GeomIsSpace(g2))
{
try
{
SafeNativeMethods.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 = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.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;
}
SafeNativeMethods.ContactGeom curcontact = new SafeNativeMethods.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);
}
}
}
