public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
{
ContactResult[] ourResults = null;
RayCallback retMethod = delegate(List<ContactResult> results)
{
ourResults = new ContactResult[results.Count];
results.CopyTo(ourResults, 0);
};
int waitTime = 0;
m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
while (ourResults == null && waitTime < 1000)
{
Thread.Sleep(1);
waitTime++;
}
if (ourResults == null)
return new List<ContactResult>();
return new List<ContactResult>(ourResults);
}
// This is the standard Near. Uses space AABBs to speed up detection.
private void near(IntPtr space, IntPtr g1, IntPtr g2)
{
//Don't test against heightfield Geom, or you'll be sorry!
/*
terminate called after throwing an instance of 'std::bad_alloc'
what(): std::bad_alloc
Stacktrace:
at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
fffff>
at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
0x00114>
at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
Native stacktrace:
mono [0x80d2a42]
[0xb7f5840c]
/lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
/usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
/usr/lib/libstdc++.so.6 [0xb45fa865]
/usr/lib/libstdc++.so.6 [0xb45fa8a2]
/usr/lib/libstdc++.so.6 [0xb45fa9da]
/usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
/usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
libode.so(dCollide+0x102) [0xb46571b2]
[0x95cfdec9]
[0x8ea07fe1]
[0xab260146]
libode.so [0xb465a5c4]
libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
libode.so(dSpaceCollide2+0x177) [0xb465ac67]
[0x95cf978e]
[0x8ea07945]
[0x95cf2bbc]
[0xab2787e7]
[0xab419fb3]
[0xab416657]
[0xab415bda]
[0xb609b08e]
mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
mono [0x81a2f0f]
mono [0x81d28b6]
mono [0x81ea2c6]
/lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
/lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
*/
// Exclude heightfield geom
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass ||
d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
return;
// Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
{
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
// Separating static prim geometry spaces.
// We'll be calling near recursivly if one
// of them is a space to find all of the
// contact points in the space
try
{
d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
}
catch (AccessViolationException)
{
MainConsole.Instance.Warn("[PHYSICS]: Unable to collide test a space");
return;
}
//Colliding a space or a geom with a space or a geom. so drill down
//Collide all geoms in each space..
//if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
//if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
return;
}
if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
return;
int count = 0;
try
{
if (g1 == g2)
return; // Can't collide with yourself
count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray,
d.ContactGeom.unmanagedSizeOf);
}
catch (SEHException)
{
MainConsole.Instance.Error(
"[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
}
catch (Exception e)
{
MainConsole.Instance.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e);
return;
}
PhysicsActor p1 = null;
if (g1 != IntPtr.Zero)
m_scene.actor_name_map.TryGetValue(g1, out p1);
// Loop over contacts, build results.
d.ContactGeom curContact = new d.ContactGeom();
for (int i = 0; i < count; i++)
{
if (!GetCurContactGeom(i, ref curContact))
break;
if (p1 != null)
{
if (p1 is WhiteCoreODEPrim)
{
ContactResult collisionresult = new ContactResult
{
ConsumerID = ((WhiteCoreODEPrim) p1).LocalID,
Pos =
new Vector3(curContact.pos.X, curContact.pos.Y,
curContact.pos.Z),
Depth = curContact.depth,
Normal =
new Vector3(curContact.normal.X,
curContact.normal.Y,
curContact.normal.Z)
};
lock (m_contactResults)
m_contactResults.Add(collisionresult);
}
}
}
}
ContactResult[] ObjectIntersection(Vector3 rayStart, Vector3 rayEnd, bool includePhysical, bool includeNonPhysical, bool includePhantom, int max)
{
List<ContactResult> contacts = World.PhysicsScene.RaycastWorld(rayStart, Vector3.Normalize(rayEnd - rayStart), Vector3.Distance(rayEnd, rayStart), max);
for (int i = 0; i < contacts.Count; i++)
{
ISceneEntity grp = World.GetGroupByPrim(contacts[i].ConsumerID);
if (grp == null || (!includePhysical && grp.RootChild.PhysActor.IsPhysical) ||
(!includeNonPhysical && !grp.RootChild.PhysActor.IsPhysical))
contacts.RemoveAt(i--);
}
if (includePhantom)
{
Ray ray = new Ray(rayStart, Vector3.Normalize(rayEnd - rayStart));
Vector3 ab = rayEnd - rayStart;
ISceneEntity[] objlist = World.Entities.GetEntities();
foreach (ISceneEntity group in objlist)
{
if (m_host.ParentEntity == group)
continue;
if (group.IsAttachment)
continue;
if (group.RootChild.PhysActor != null)
continue;
// Find the radius ouside of which we don't even need to hit test
float minX;
float maxX;
float minY;
float maxY;
float minZ;
float maxZ;
float radius = 0.0f;
group.GetAxisAlignedBoundingBoxRaw(out minX, out maxX, out minY, out maxY, out minZ, out maxZ);
if (Math.Abs(minX) > radius)
radius = Math.Abs(minX);
if (Math.Abs(minY) > radius)
radius = Math.Abs(minY);
if (Math.Abs(minZ) > radius)
radius = Math.Abs(minZ);
if (Math.Abs(maxX) > radius)
radius = Math.Abs(maxX);
if (Math.Abs(maxY) > radius)
radius = Math.Abs(maxY);
if (Math.Abs(maxZ) > radius)
radius = Math.Abs(maxZ);
radius = radius * 1.413f;
Vector3 ac = group.AbsolutePosition - rayStart;
// Vector3 bc = group.AbsolutePosition - rayEnd;
double d = Math.Abs(Vector3.Mag(Vector3.Cross(ab, ac)) / Vector3.Distance(rayStart, rayEnd));
// Too far off ray, don't bother
if (d > radius)
continue;
// Behind ray, drop
double d2 = Vector3.Dot(Vector3.Negate(ab), ac);
if (d2 > 0)
continue;
ray = new Ray(rayStart, Vector3.Normalize(rayEnd - rayStart));
EntityIntersection intersection = group.TestIntersection(ray, true, false);
// Miss.
if (!intersection.HitTF)
continue;
Vector3 b1 = new Vector3(minX, minY, minZ);
Vector3 b2 = new Vector3(maxX, maxY, maxZ);
//m_log.DebugFormat("[LLCASTRAY]: min<{0},{1},{2}>, max<{3},{4},{5}> = hitp<{6},{7},{8}>", b1.X,b1.Y,b1.Z,b2.X,b2.Y,b2.Z,intersection.ipoint.X,intersection.ipoint.Y,intersection.ipoint.Z);
if (!(intersection.ipoint.X >= b1.X && intersection.ipoint.X <= b2.X &&
intersection.ipoint.Y >= b1.Y && intersection.ipoint.Y <= b2.Y &&
intersection.ipoint.Z >= b1.Z && intersection.ipoint.Z <= b2.Z))
continue;
ContactResult result = new ContactResult();
result.ConsumerID = group.LocalId;
result.Depth = intersection.distance;
result.Normal = intersection.normal;
result.Pos = intersection.ipoint;
contacts.Add(result);
}
}
return contacts.ToArray();
}
private ContactResult? GroundIntersection(Vector3 rayStart, Vector3 rayEnd)
{
ITerrainChannel heightfield = World.RequestModuleInterface<ITerrainChannel>();
List<ContactResult> contacts = new List<ContactResult>();
double min = 2048.0;
double max = 0.0;
// Find the min and max of the heightfield
for (int x = 0; x < heightfield.Width; x++)
{
for (int y = 0; y < heightfield.Height; y++)
{
if (heightfield[x, y] > max)
max = heightfield[x, y];
if (heightfield[x, y] < min)
min = heightfield[x, y];
}
}
// A ray extends past rayEnd, but doesn't go back before
// rayStart. If the start is above the highest point of the ground
// and the ray goes up, we can't hit the ground. Ever.
if (rayStart.Z > max && rayEnd.Z >= rayStart.Z)
return null;
// Same for going down
if (rayStart.Z < min && rayEnd.Z <= rayStart.Z)
return null;
List<Tri> trilist = new List<Tri>();
// Create our triangle list
for (int x = 1; x < heightfield.Width; x++)
{
for (int y = 1; y < heightfield.Height; y++)
{
Tri t1 = new Tri();
Tri t2 = new Tri();
Vector3 p1 = new Vector3(x - 1, y - 1, heightfield[x - 1, y - 1]);
Vector3 p2 = new Vector3(x, y - 1, heightfield[x, y - 1]);
Vector3 p3 = new Vector3(x, y, heightfield[x, y]);
Vector3 p4 = new Vector3(x - 1, y, heightfield[x - 1, y]);
t1.p1 = p1;
t1.p2 = p2;
t1.p3 = p3;
t2.p1 = p3;
t2.p2 = p4;
t2.p3 = p1;
trilist.Add(t1);
trilist.Add(t2);
}
}
// Ray direction
Vector3 rayDirection = rayEnd - rayStart;
foreach (Tri t in trilist)
{
// Compute triangle plane normal and edges
Vector3 u = t.p2 - t.p1;
Vector3 v = t.p3 - t.p1;
Vector3 n = Vector3.Cross(u, v);
if (n == Vector3.Zero)
continue;
Vector3 w0 = rayStart - t.p1;
double a = -Vector3.Dot(n, w0);
double b = Vector3.Dot(n, rayDirection);
// Not intersecting the plane, or in plane (same thing)
// Ignoring this MAY cause the ground to not be detected
// sometimes
if (Math.Abs(b) < 0.000001)
continue;
double r = a / b;
// ray points away from plane
if (r < 0.0)
continue;
Vector3 ip = rayStart + Vector3.Multiply(rayDirection, (float)r);
float uu = Vector3.Dot(u, u);
float uv = Vector3.Dot(u, v);
float vv = Vector3.Dot(v, v);
Vector3 w = ip - t.p1;
float wu = Vector3.Dot(w, u);
float wv = Vector3.Dot(w, v);
float d = uv * uv - uu * vv;
float cs = (uv * wv - vv * wu) / d;
if (cs < 0 || cs > 1.0)
continue;
float ct = (uv * wu - uu * wv) / d;
if (ct < 0 || (cs + ct) > 1.0)
continue;
// Add contact point
ContactResult result = new ContactResult();
result.ConsumerID = 0;
result.Depth = Vector3.Distance(rayStart, ip);
result.Normal = n;
result.Pos = ip;
contacts.Add(result);
}
if (contacts.Count == 0)
return null;
contacts.Sort(delegate(ContactResult a, ContactResult b)
{
return a.Depth.CompareTo(b.Depth);
});
return contacts[0];
}
private ContactResult[] AvatarIntersection(Vector3 rayStart, Vector3 rayEnd)
{
List<ContactResult> contacts = new List<ContactResult>();
Vector3 ab = rayEnd - rayStart;
World.ForEachScenePresence(delegate(IScenePresence sp)
{
Vector3 ac = sp.AbsolutePosition - rayStart;
// Vector3 bc = sp.AbsolutePosition - rayEnd;
double d = Math.Abs(Vector3.Mag(Vector3.Cross(ab, ac)) / Vector3.Distance(rayStart, rayEnd));
if (d > 1.5)
return;
double d2 = Vector3.Dot(Vector3.Negate(ab), ac);
if (d2 > 0)
return;
double dp = Math.Sqrt(Vector3.Mag(ac) * Vector3.Mag(ac) - d * d);
Vector3 p = rayStart + Vector3.Divide(Vector3.Multiply(ab, (float)dp), Vector3.Mag(ab));
if (!InBoundingBox(sp, p))
return;
ContactResult result = new ContactResult();
result.ConsumerID = sp.LocalId;
result.Depth = Vector3.Distance(rayStart, p);
result.Normal = Vector3.Zero;
result.Pos = p;
contacts.Add(result);
});
return contacts.ToArray();
}
