private void DoWork() { m_mesher.ExpireFileCache(); ODEPhysRepData nextRep; while (m_running) { workQueue.TryTake(out nextRep, -1); if (!m_running) { return; } if (nextRep == null) { continue; } if (m_scene.haveActor(nextRep.actor)) { switch (nextRep.comand) { case meshWorkerCmnds.changefull: case meshWorkerCmnds.changeshapetype: case meshWorkerCmnds.changesize: GetMesh(nextRep); if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) { m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); } break; case meshWorkerCmnds.getmesh: DoRepDataGetMesh(nextRep); break; } } } }
private void DoWork(object rep) { ODEPhysRepData nextRep = rep as ODEPhysRepData; if (m_running && nextRep != null && m_scene.haveActor(nextRep.actor)) { switch (nextRep.comand) { case meshWorkerCmnds.changefull: case meshWorkerCmnds.changeshapetype: case meshWorkerCmnds.changesize: GetMesh(nextRep); if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) { m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); } break; case meshWorkerCmnds.getmesh: DoRepDataGetMesh(nextRep); break; } } }
public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) { if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) { PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); return; } IntPtr geom = ((OdePrim)actor).prim_geom; Vector3 geopos = SafeNativeMethods.GeomGetPositionOMV(geom); Quaternion geomOri = SafeNativeMethods.GeomGetQuaternionOMV(geom); // Vector3 geopos = actor.Position; // Quaternion geomOri = actor.Orientation; Quaternion geomInvOri = Quaternion.Conjugate(geomOri); Quaternion ori = Quaternion.Identity; Vector3 rayDir = geopos + offset - avCameraPosition; float raylen = rayDir.Length(); if (raylen < 0.001f) { PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); return; } float t = 1 / raylen; rayDir.X *= t; rayDir.Y *= t; rayDir.Z *= t; raylen += 30f; // focal point may be far List <ContactResult> rayResults; rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); if (rayResults.Count == 0) { /* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim * d.AABB aabb; * d.GeomGetAABB(geom, out aabb); * offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); * ori = geomInvOri; * offset *= geomInvOri; * PhysicsSitResponse(1, actor.LocalID, offset, ori); */ PhysicsSitResponse(0, actor.LocalID, offset, ori); return; } int status = 1; offset = rayResults[0].Pos - geopos; SafeNativeMethods.GeomClassID geoclass = SafeNativeMethods.GeomGetClass(geom); if (geoclass == SafeNativeMethods.GeomClassID.SphereClass) { float r = SafeNativeMethods.GeomSphereGetRadius(geom); offset.Normalize(); offset *= r; RotAroundZ(offset.X, offset.Y, ref ori); if (r < 0.4f) { offset = new Vector3(0, 0, r); } else { if (offset.Z < 0.4f) { t = offset.Z; float rsq = r * r; t = 1.0f / (rsq - t * t); offset.X *= t; offset.Y *= t; offset.Z = 0.4f; t = rsq - 0.16f; offset.X *= t; offset.Y *= t; } else if (r > 0.8f && offset.Z > 0.8f * r) { status = 3; avOffset.X = -avOffset.X; avOffset.Z *= 1.6f; } } offset += avOffset * ori; ori = geomInvOri * ori; offset *= geomInvOri; PhysicsSitResponse(status, actor.LocalID, offset, ori); return; } Vector3 norm = rayResults[0].Normal; if (norm.Z < -0.4f) { PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); return; } float SitNormX = -rayDir.X; float SitNormY = -rayDir.Y; Vector3 pivot = geopos + offset; float edgeNormalX = norm.X; float edgeNormalY = norm.Y; float edgeDirX = -rayDir.X; float edgeDirY = -rayDir.Y; Vector3 edgePos = rayResults[0].Pos; float edgeDist = float.MaxValue; bool foundEdge = false; if (norm.Z < 0.5f) { float rayDist = 4.0f; for (int i = 0; i < 6; i++) { pivot.X -= 0.01f * norm.X; pivot.Y -= 0.01f * norm.Y; pivot.Z -= 0.01f * norm.Z; rayDir.X = -norm.X * norm.Z; rayDir.Y = -norm.Y * norm.Z; rayDir.Z = 1.0f - norm.Z * norm.Z; rayDir.Normalize(); rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); if (rayResults.Count == 0) { break; } if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) { rayDist -= rayResults[0].Depth; if (rayDist < 0f) { break; } pivot = rayResults[0].Pos; norm = rayResults[0].Normal; edgeNormalX = norm.X; edgeNormalY = norm.Y; edgeDirX = -rayDir.X; edgeDirY = -rayDir.Y; } else { foundEdge = true; edgePos = rayResults[0].Pos; break; } } if (!foundEdge) { PhysicsSitResponse(0, actor.LocalID, offset, ori); return; } avOffset.X *= 0.5f; } else if (norm.Z > 0.866f) { float toCamBaseX = avCameraPosition.X - pivot.X; float toCamBaseY = avCameraPosition.Y - pivot.Y; float toCamX = toCamBaseX; float toCamY = toCamBaseY; for (int j = 0; j < 4; j++) { float rayDist = 1.0f; float curEdgeDist = 0.0f; for (int i = 0; i < 3; i++) { pivot.Z -= 0.01f; rayDir.X = toCamX; rayDir.Y = toCamY; rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; rayDir.Normalize(); rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); if (rayResults.Count == 0) { break; } curEdgeDist += rayResults[0].Depth; if (rayResults[0].Normal.Z > 0.5f) { rayDist -= rayResults[0].Depth; if (rayDist < 0f) { break; } pivot = rayResults[0].Pos; norm = rayResults[0].Normal; } else { foundEdge = true; if (curEdgeDist < edgeDist) { edgeDist = curEdgeDist; edgeNormalX = rayResults[0].Normal.X; edgeNormalY = rayResults[0].Normal.Y; edgeDirX = rayDir.X; edgeDirY = rayDir.Y; edgePos = rayResults[0].Pos; } break; } } if (foundEdge && edgeDist < 0.2f) { break; } pivot = geopos + offset; switch (j) { case 0: toCamX = -toCamBaseY; toCamY = toCamBaseX; break; case 1: toCamX = toCamBaseY; toCamY = -toCamBaseX; break; case 2: toCamX = -toCamBaseX; toCamY = -toCamBaseY; break; default: break; } } if (!foundEdge) { avOffset.X = -avOffset.X; avOffset.Z *= 1.6f; RotAroundZ(SitNormX, SitNormY, ref ori); offset += avOffset * ori; ori = geomInvOri * ori; offset *= geomInvOri; PhysicsSitResponse(3, actor.LocalID, offset, ori); return; } avOffset.X *= 0.5f; } SitNormX = edgeNormalX; SitNormY = edgeNormalY; if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) { SitNormX = -SitNormX; SitNormY = -SitNormY; } RotAroundZ(SitNormX, SitNormY, ref ori); offset = edgePos + avOffset * ori; offset -= geopos; ori = geomInvOri * ori; offset *= geomInvOri; PhysicsSitResponse(1, actor.LocalID, offset, ori); return; }
/// <summary> /// Process all queued raycast requests /// </summary> /// <returns>Time in MS the raycasts took to process.</returns> 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; } SafeNativeMethods.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); SafeNativeMethods.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); SafeNativeMethods.Quaternion qtmp; qtmp.X = req.orientation.X; qtmp.Y = req.orientation.Y; qtmp.Z = req.orientation.Z; qtmp.W = req.orientation.W; SafeNativeMethods.GeomSetQuaternion(Box, ref qtmp); } else if (req.callbackMethod is ProbeSphereCallback) { if (CollisionContactGeomsPerTest > 80) { CollisionContactGeomsPerTest = 80; } SafeNativeMethods.GeomSphereSetRadius(Sphere, req.length); SafeNativeMethods.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); } else if (req.callbackMethod is ProbePlaneCallback) { if (CollisionContactGeomsPerTest > 80) { CollisionContactGeomsPerTest = 80; } SafeNativeMethods.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); } else { if (CollisionContactGeomsPerTest > 25) { CollisionContactGeomsPerTest = 25; } SafeNativeMethods.GeomRaySetLength(ray, req.length); SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); SafeNativeMethods.GeomRaySetParams(ray, 0, backfacecull); if (req.callbackMethod is RaycastCallback) { // if we only want one get only one per Collision pair saving memory CurrentRayFilter |= RayFilterFlags.ClosestHit; SafeNativeMethods.GeomRaySetClosestHit(ray, 1); } else { SafeNativeMethods.GeomRaySetClosestHit(ray, closestHit); } } if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) { unchecked { CollisionContactGeomsPerTest |= (int)SafeNativeMethods.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; SafeNativeMethods.GeomSetCollideBits(Box, (uint)catflags); SafeNativeMethods.GeomSetCategoryBits(Box, (uint)catflags); doProbe(req, Box); } else if (req.callbackMethod is ProbeSphereCallback) { catflags |= CollisionCategories.Space; SafeNativeMethods.GeomSetCollideBits(Sphere, (uint)catflags); SafeNativeMethods.GeomSetCategoryBits(Sphere, (uint)catflags); doProbe(req, Sphere); } else if (req.callbackMethod is ProbePlaneCallback) { catflags |= CollisionCategories.Space; SafeNativeMethods.GeomSetCollideBits(Plane, (uint)catflags); SafeNativeMethods.GeomSetCategoryBits(Plane, (uint)catflags); doPlane(req, IntPtr.Zero); } else { SafeNativeMethods.GeomSetCollideBits(ray, (uint)catflags); doSpaceRay(req); } } } else { // if we select a geom don't use filters if (req.callbackMethod is ProbePlaneCallback) { SafeNativeMethods.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); doPlane(req, geom); } else { SafeNativeMethods.GeomSetCollideBits(ray, (uint)CollisionCategories.All); doGeomRay(req, geom); } } } if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) { break; } } lock (m_contactResults) m_contactResults.Clear(); return(Util.EnvironmentTickCountSubtract(time)); }