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. void NearSpace(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 ODEPrim) { ContactResult collisionresult = new ContactResult { ConsumerID = ((ODEPrim) 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); } } } }
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, (float)heightfield[x - 1, y - 1]); Vector3 p2 = new Vector3(x, y - 1, (float)heightfield[x, y - 1]); Vector3 p3 = new Vector3(x, y, (float)heightfield[x, y]); Vector3 p4 = new Vector3(x - 1, y, (float)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[] 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[] 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), (float)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(); }