public bool TestCollision(MovingObject mo, ref Vector3 displacement, CollisionParms parms) { // Find the minimum step size for the assembly of parts float stepSize = mo.StepSize(displacement); return(TestCollision(mo, stepSize, ref displacement, parms)); }
// Test all the parts of a moving object to see if they collide with // any obstacle public bool CollideAfterAddingDisplacement(MovingObject mo, Vector3 step, CollisionParms parms) { for (int i = 0; i < mo.parts.Count; i++) { MovingPart movingPart = mo.parts[i]; collisionTimeStamp++; movingPart.AddDisplacement(step); SphereTreeNode s = sphereTree.FindSmallestContainer(movingPart.shape, movingPart.sphere); //MaybeDumpSphereTree(); movingPart.sphere = s; partCalls++; if (s.TestSphereCollision(movingPart.shape, collisionTimeStamp, ref collisionTestCount, parms)) { // We hit something, so back out the displacements // added to the parts tested so far for (int j = 0; j <= i; j++) { MovingPart displacedPart = mo.parts[j]; displacedPart.AddDisplacement(-step); //MaybeDumpSphereTree(); } return(true); } } return(false); }
public bool ShapeCollides(CollisionShape shape, CollisionParms parms) { collisionTimeStamp++; SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null); return(s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms)); }
public bool ShapeCollides(CollisionShape shape) { CollisionParms parms = new CollisionParms(); parms.genNormals = false; return(ShapeCollides(shape, parms)); }
private bool StepTowardCollision(MovingObject mo, Vector3 displacement, int nsteps, Vector3 step, CollisionParms parms) { Vector3 accumulatedSteps = Vector3.Zero; for (int i = 0; i < nsteps; i++) { Vector3 nextStep; if (i == nsteps - 1) { nextStep = displacement - accumulatedSteps; } else { nextStep = step; } if (CollideAfterAddingDisplacement(mo, nextStep, parms)) { topLevelCollisions++; return(true); } accumulatedSteps += nextStep; } // No collision! parms.obstacle = null; return(false); }
// Test for collision with any object in this sphere public void FindIntersectingShapes(CollisionShape part, List <CollisionShape> shapes, ulong timeStamp, ref int collisionTestCount, CollisionParms parms) { if (containedShape != null) { collisionTestCount++; parms.swapped = false; if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)containedShape.ShapeType()] (part, containedShape, parms)) { shapes.Add(containedShape); } else { containedShape.timeStamp = timeStamp; } } // Iterate over the shapes in this sphere and not wholly // contained in a subsphere foreach (CollisionShape obstacle in intersectingShapes) { if (obstacle.timeStamp == timeStamp) { continue; } collisionTestCount++; parms.swapped = false; if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)obstacle.ShapeType()] (part, obstacle, parms)) { shapes.Add(obstacle); } else { obstacle.timeStamp = timeStamp; } } // Now iterate over subspheres for (int i = 0; i < SphereTreeNode.childCount; i++) { SphereTreeNode cs = children[i]; if (cs == null) { continue; } // Skip any sphere that doesn't overlap the part in question if (!cs.SphereOverlap(part)) { continue; } cs.FindIntersectingShapes(part, shapes, timeStamp, ref collisionTestCount, parms); } }
public CollisionShape FindClosestCollision(CollisionShape cap, Vector3 start, Vector3 end, ref Vector3 intersection) { collisionTimeStamp++; SphereTreeNode s = sphereTree.FindSmallestContainer(cap, null); CollisionParms parms = new CollisionParms(); parms.genNormals = false; List <CollisionShape> shapes = new List <CollisionShape>(); s.FindIntersectingShapes(cap, shapes, collisionTimeStamp, ref collisionTestCount, parms); intersection = Vector3.Zero; if (shapes.Count == 0) { return(null); } else { collisionTimeStamp++; return(FindClosestIntersectingShape(shapes, start, end, ref intersection)); } }
public static bool TestCollisionSphereOBB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionSphere x1 = (CollisionSphere)s1; CollisionOBB x2 = (CollisionOBB)s2; Vector3 closest; if (TestSphereOBB(x1, x2, out closest)) { Vector3 n = s1.center - closest; parms.SetNormPart(n); // ??? This isn't the correct value of the normal parms.SetNormObstacle(-n); return true; } else return false; }
////////////////////////////////////////////////////////////////////// // // The per-shape-pair collision predicates. Since collisions // themselves are much less frequent than the tests for // collisions, all the effort is in making the tests fast, and // if we have to repeat the same calculations in the event of // a collision, it's still a net savings if it makes the tests // cheaper // ////////////////////////////////////////////////////////////////////// public static bool TestCollisionSphereSphere(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionSphere x1 = (CollisionSphere)s1; CollisionSphere x2 = (CollisionSphere)s2; Vector3 nS2 = s2.center - s1.center; if (x1.radius + x2.radius > nS2.Length) { Vector3 n = s2.center - s1.center; parms.SetNormPart(n); parms.SetNormObstacle(-n); return true; } else return false; }
// Test for collision with any object in this sphere public bool TestSphereCollision(CollisionShape part, ulong timeStamp, ref int collisionTestCount, CollisionParms parms) { if (containedShape != null) { collisionTestCount++; parms.swapped = false; if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)containedShape.ShapeType()] (part, containedShape, parms)) { parms.part = part; parms.obstacle = containedShape; return true; } else { containedShape.timeStamp = timeStamp; } } // Iterate over the shapes in this sphere and not wholly // contained in a subsphere foreach (CollisionShape obstacle in intersectingShapes) { if (obstacle.timeStamp == timeStamp) continue; collisionTestCount++; parms.swapped = false; if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)obstacle.ShapeType()] (part, obstacle, parms)) { parms.part = part; parms.obstacle = obstacle; return true; } else { obstacle.timeStamp = timeStamp; } } // Now iterate over subspheres for (int i=0; i<SphereTreeNode.childCount; i++) { SphereTreeNode cs = children[i]; if (cs == null) continue; // Skip any sphere that doesn't overlap the part in question if (!cs.SphereOverlap(part)) continue; if (cs.TestSphereCollision(part, timeStamp, ref collisionTestCount, parms)) return true; } return false; }
public static bool TestCollisionSphereCapsule(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionSphere x1 = (CollisionSphere)s1; CollisionCapsule x2 = (CollisionCapsule)s2; float rSum = x1.radius + x2.capRadius; if (SqDistPointSegment(x2.bottomcenter, x2.topcenter, x1.center) < rSum * rSum) { float t; Vector3 d; ClosestPtPointSegment(x1.center, x2.bottomcenter, x2.topcenter, out t, out d); Vector3 n = d - x1.center; parms.SetNormPart(n); parms.SetNormObstacle(-n); return true; } else return false; }
// Test all the parts of a moving object to see if they collide with // any obstacle public bool CollideAfterAddingDisplacement(MovingObject mo, Vector3 step, CollisionParms parms) { for (int i=0; i<mo.parts.Count; i++) { MovingPart movingPart = mo.parts[i]; collisionTimeStamp++; movingPart.AddDisplacement(step); SphereTreeNode s = sphereTree.FindSmallestContainer(movingPart.shape, movingPart.sphere); //MaybeDumpSphereTree(); movingPart.sphere = s; partCalls++; if (s.TestSphereCollision(movingPart.shape, collisionTimeStamp, ref collisionTestCount, parms)) { // We hit something, so back out the displacements // added to the parts tested so far for (int j=0; j<=i; j++) { MovingPart displacedPart = mo.parts[j]; displacedPart.AddDisplacement (-step); //MaybeDumpSphereTree(); } return true; } } return false; }
private bool StepTowardCollision(MovingObject mo, Vector3 displacement, int nsteps, Vector3 step, CollisionParms parms) { Vector3 accumulatedSteps = Vector3.Zero; for (int i=0; i<nsteps; i++) { Vector3 nextStep; if (i == nsteps - 1) nextStep = displacement - accumulatedSteps; else nextStep = step; if (CollideAfterAddingDisplacement(mo, nextStep, parms)) { topLevelCollisions++; return true; } accumulatedSteps += nextStep; } // No collision! parms.obstacle = null; return false; }
// The is the top-level collision detection function. // // The MovingObject doesn't intersect anything now; would it // do so if the vector displacement is applied? // // If we do have a collision, we "creep up" on the object we collide // with until we're within stepsize of the minimum dimension of the // moving part // public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms) { topLevelCalls++; parms.Initialize(); if (mo.parts.Count == 0) return false; // Remember where the first part started Vector3 start = mo.parts[0].shape.center; Vector3 originalDisplacement = displacement; float len = displacement.Length; //MaybeDumpSphereTree(); int nsteps = (int)Math.Ceiling(len / stepSize); Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize/len)); // Try to step the whole way if (!StepTowardCollision(mo, displacement, nsteps, step, parms)) { displacement = Vector3.Zero; return false; } // Otherwise, we hit something. Step toward it // The minimum distance float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle), stepSize * MinFractionToObstacle); len = step.Length; nsteps = (int)Math.Ceiling(len/smallStepSize); Vector3 smallStep = step * (smallStepSize/len); bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms); displacement = originalDisplacement - (mo.parts[0].shape.center - start); return hit; }
public bool ShapeCollides(CollisionShape shape, CollisionParms parms) { collisionTimeStamp++; SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null); return s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms); }
public static bool TestCapsuleCapsule(CollisionCapsule c1, CollisionCapsule c2, CollisionParms parms) { // Compute (squared) distance between the inner structures of the capsules float s, t; Vector3 p1, p2; float d = ClosestPtSegmentSegment(c1.bottomcenter, c1.topcenter, c2.bottomcenter, c2.topcenter, out s, out t, out p1, out p2); float r = c1.capRadius + c2.capRadius; Vector3 n = p2 - p1; parms.SetNormPart(n); parms.SetNormObstacle(-n); return d < r * r; }
public static bool TestCapsuleOBB(CollisionCapsule a, CollisionOBB b, CollisionParms parms) { SegOBBParams obbParms = new SegOBBParams(true); Segment s = Segment.SegmentFromStartAndEnd(a.bottomcenter, a.topcenter); if (SqrDistSegOBB(s, b, obbParms) < a.capRadius * a.capRadius) { // If parms.pfLParam == 0, closest is bottomcenter; if == 1, // closest is topcenter. Vector3 d = a.bottomcenter + obbParms.pfLParam * s.direction; // pfBVec is relative to the center of the box, but in box // coords. Vector3 f = b.center; for (int i=0; i<3; i++) f += obbParms.pfBVec[i] * b.axes[i]; Vector3 n = f - d; parms.SetNormPart(n); parms.SetNormObstacle(-n); if (MO.DoLog) { MO.Log(" TestCapsuleOBB: pfLParam {0}, pfBVec {1}", obbParms.pfLParam, obbParms.pfBVec); MO.Log(" TestCapsuleOBB: d {0}, f {1}", f, d); MO.Log(" -n {0}", -n); } return true; } else return false; }
public static bool TestCapsuleAABB(CollisionCapsule a, CollisionAABB b, CollisionParms parms) { // Convert the AABB into an OBB, then run the OBBOBB test. // Not the most efficient algorithm but it gets the job // done. CollisionOBB newB = new CollisionOBB(b.center, UnitBasisVectors, (b.max - b.min) * .5f); return TestCapsuleOBB(a, newB, parms); }
private void MoveToObject(StreamWriter stream, CollisionAPI API, CollisionShape collisionShape, CollisionShape movingShape) { stream.Write("\n\n\nEntering MoveToObject\n"); // Create a MovingObject, and add movingShape to it MovingObject mo = new MovingObject(); API.AddPartToMovingObject(mo, movingShape); // Move movingObject 1 foot at a time toward the sphere Vector3 toShape = collisionShape.center - movingShape.center; stream.Write(string.Format("movingShape {0}\n", movingShape.ToString())); stream.Write(string.Format("collisionShape {0}\nDisplacement Vector {1}\n", collisionShape.ToString(), toShape.ToString())); // We'll certainly get there before steps expires int steps = (int)Math.Ceiling(toShape.Length); // 1 foot step in the right direction Vector3 stepVector = toShape.ToNormalized(); stream.Write(string.Format("Steps {0}, stepVector {1}\n", steps, stepVector.ToString())); bool hitIt = false; // Loop til we smack into something CollisionParms parms = new CollisionParms(); for (int i=0; i<steps; i++) { // Move 1 foot; if returns true, we hit something hitIt = (API.TestCollision (mo, stepVector, parms)); stream.Write(string.Format("i = {0}, hitIt = {1}, movingShape.center = {2}\n", i, hitIt, movingShape.center.ToString())); if (hitIt) { stream.Write(string.Format("collidingPart {0}\nblockingObstacle {1}\n, normPart {2}, normObstacle {3}\n", parms.part.ToString(), parms.obstacle.ToString(), parms.normPart.ToString(), parms.normObstacle.ToString())); return; } stream.Write("\n"); } Debug.Assert(hitIt, "Didn't hit the obstacle"); }
// Keep the code below, commented out, because it is useful // for debugging near plane issues // private static int nearPlaneCollisionCount = 0; // private static int nearPlaneNodeId = 99999; // private static List<RenderedNode>[] nearPlaneRenderedNodes = new List<RenderedNode>[4] { // new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>(), new List<RenderedNode>() }; // private static CollisionCapsule[] nearPlaneCapsules = new CollisionCapsule[4]; // // protected void CreateNearPlaneCapsules(Camera camera, // Vector3 cameraPosition, // Vector3 playerHeadPosition) // { // // Frame the clip plane with 4 narrow capsules // // Start by finding the center of the near plane // Vector3 toPlayer = (playerHeadPosition - cameraPosition).ToNormalized(); // Vector3 center = cameraPosition + (1.02f * camera.Near) * toPlayer; // float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f); // float tanThetaY = MathUtil.Tan(thetaY); // float tanThetaX = tanThetaY * camera.AspectRatio; // Vector3 right = tanThetaX * camera.Near * (camera.Right.ToNormalized()); // Vector3 up = tanThetaY * camera.Near * (camera.Up.ToNormalized()); // Vector3 corner1 = center + right - up; // Vector3 corner2 = center + right + up; // Vector3 corner3 = center - right + up; // Vector3 corner4 = center - right - up; // nearPlaneCapsules[0] = new CollisionCapsule(corner1, corner2, Client.OneMeter * .001f); // nearPlaneCapsules[1] = new CollisionCapsule(corner2, corner3, Client.OneMeter * .001f); // nearPlaneCapsules[2] = new CollisionCapsule(corner3, corner4, Client.OneMeter * .001f); // nearPlaneCapsules[3] = new CollisionCapsule(corner4, corner1, Client.OneMeter * .001f); // // RenderedNode.Scene = sceneManager; // for (int i=0; i<4; i++) { // RenderedNode.RemoveNodeRendering(nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]); // RenderedNode.CreateRenderedNodes(nearPlaneCapsules[i], nearPlaneNodeId + i, ref nearPlaneRenderedNodes[i]); // } // } protected float FindAcceptableCameraPosition(Camera camera, Vector3 cameraPos, Vector3 cameraTarget) { //Logger.Log(0, "FindAcceptableCameraPosition cameraPos {0}, cameraTarget {1}", cameraPos, cameraTarget); // If there is no CollisionAPI object, punt CollisionAPI api = client.WorldManager.CollisionHelper; if (api == null) return (cameraPos - cameraTarget).Length; // Start by finding the point at which a narrow capsule // from the camera to the player intersects. If it // doesn't, then set the point to the cameraPos. // Make a very narrow capsule between the camera and the player's head CollisionCapsule cap = new CollisionCapsule(cameraPos, cameraTarget, Client.OneMeter * .001f); // 1 millimeter Vector3 newCameraPos = cameraPos; Vector3 unitTowardCamera = (newCameraPos - cameraTarget).ToNormalized(); // Test for a collision, setting intersection if there is one. Vector3 intersection = Vector3.Zero; if (api.FindClosestCollision(cap, cameraTarget, cameraPos, ref intersection) != null) { // There is A collision - - set the new camera pos to // be the point of intersection newCameraPos = intersection - unitTowardCamera * camera.Near; //Logger.Log(0, "FindAcceptableCameraPosition: Thin cap collides at {0}", newCameraPos); } // Move a near-plane OBB forward from the point of // intersection until we find a place where the near plane // doesn't intersect any collision volumes Vector3 center = newCameraPos - camera.Near * unitTowardCamera; float thetaY = MathUtil.DegreesToRadians(camera.FOVy * 0.5f); float tanThetaY = MathUtil.Tan(thetaY); float tanThetaX = tanThetaY * camera.AspectRatio; // Compute the OBB axes and extents Vector3[] axes = new Vector3[3]; Vector3 extents = Vector3.Zero; // The axis perpendicular to the near plane float obbDepth = 100f; // 100 mm thick axes[0] = -unitTowardCamera; axes[1] = -axes[0].Cross(Vector3.UnitY).ToNormalized(); axes[2] = -axes[1].Cross(axes[0]).ToNormalized(); extents[0] = obbDepth; extents[1] = camera.Near * tanThetaX; extents[2] = camera.Near * tanThetaY; float len = (newCameraPos - cameraTarget).Length; float startingLen = len; CollisionOBB obb = new CollisionOBB(center, axes, extents); CollisionParms parms = new CollisionParms(); while (len >= minThirdPersonDistance) { obb.center = cameraTarget + unitTowardCamera * len; bool collides = api.ShapeCollides(obb, parms); //Logger.Log(0, "FindAcceptableCameraPosition len {0}, obb center {1}, collides {2}", len, obb.center, collides); if (!collides) break; //client.Write(string.Format("OBB collides; len {0}", len)); len -= obbDepth; } // Len is less than the camera min distance, so just // return it //Logger.Log(0, "FindAcceptableCameraPosition: starting len {0} final len {1}", startingLen, len); return len; }
// This function swaps the arguments so we only have to // implement the intersection function once public static bool TestCollisionSwapper(CollisionShape s1, CollisionShape s2, CollisionParms parms) { parms.swapped = true; return TestCollisionFunctions[(int)s2.ShapeType(), (int)s1.ShapeType()] (s2, s1, parms); }
public bool ShapeCollides(CollisionShape shape) { CollisionParms parms = new CollisionParms(); parms.genNormals = false; return ShapeCollides(shape, parms); }
public static bool TestOBBOBB(CollisionOBB a, CollisionOBB b, CollisionParms parms) { if (MO.DoLog) MO.Log("TestOBBOBB Entering: a {0} b {1}", a, b); if (TestOBBOBBInternal(a, b)) { Vector3 n = b.center - a.center; // ??? Not right parms.SetNormPart(n); parms.SetNormObstacle(-n); if (MO.DoLog) MO.Log("TestOBBOBB Collided: n {0}", n); return true; } else return false; }
// We only call this if both the collisionManager and the collider exist private static Vector3 FindMobNodeDisplacement(MobNode mobNode, CollisionParms parms, Vector3 desiredDisplacement, out bool collided) { Vector3 start = mobNode.Position; Vector3 pos = start + desiredDisplacement; Vector3 displacement = desiredDisplacement; MovingObject mo = mobNode.Collider; Vector3 moStart = mo.parts[0].shape.center; if (MO.DoLog) { MO.Log(" moStart = {0}, start = {1}", moStart, start); MO.Log(" pos = {0}, displacement = {1}", pos, displacement); TraceMOBottom(mo, " On entry to FindMobNodeDisplacement"); } collided = false; if (collisionManager.TestCollision(mo, ref displacement, parms)) { collided = true; if (MO.DoLog) { MO.Log(" Collision when moving object {0} from {1} to {2}", parms.part.handle, start, pos); NowColliding(mo, " After first TestCollision in FindMobNodeDisplacement"); TraceObstacle(parms.obstacle); MO.Log(" Before collision moved {0}", desiredDisplacement - displacement); } // Decide if the normals are such that we want // to slide along the obstacle Vector3 remainingDisplacement = displacement; Vector3 norm1 = parms.normObstacle.ToNormalized(); if (DecideToSlide(mo, start + displacement, parms, ref remainingDisplacement)) { if (MO.DoLog) MO.Log(" After DecideToSlide, remainingDisplacement {0}", remainingDisplacement); // We have to test the displacement if (collisionManager.TestCollision(mo, ref remainingDisplacement, parms)) { if (MO.DoLog) { NowColliding(mo, " After first try TestCollision"); MO.Log(" Slid into obstacle on the first try; remainingDisplacement = {0}", remainingDisplacement); TraceObstacle(parms.obstacle); } if (remainingDisplacement.LengthSquared > 0) { Vector3 norm2 = parms.normObstacle.ToNormalized(); // Find the cross product of the of norm1 and // norm2, and dot with displacement. If // negative, reverse. Vector3 newDir = norm1.Cross(norm2); float len = newDir.Dot(remainingDisplacement); if (len < 0) { newDir = - newDir; len = - len; } Vector3 slidingDisplacement = len * newDir; Vector3 originalSlidingDisplacement = slidingDisplacement; if (MO.DoLog) { MO.Log(" norm1 = {0}, norm2 = {1}, len = {2}", norm1, norm2, len); MO.Log(" Cross product slidingDisplacement is {0}", slidingDisplacement); } if (collisionManager.TestCollision(mo, ref slidingDisplacement, parms)) { if (MO.DoLog) { NowColliding(mo, " After second try TestCollision"); MO.Log(" Slid into obstacle on the second try; slidingDisplacement = {0}", slidingDisplacement); } } else if (MO.DoLog) MO.Log(" Didn't slide into obstacle on the second try"); remainingDisplacement -= (originalSlidingDisplacement - slidingDisplacement); } } } else remainingDisplacement = displacement; if (MO.DoLog) MO.Log(" Before checking hop, remainingDisplacement is {0}", remainingDisplacement); if (remainingDisplacement.Length > 30f) { // Try to hop over the obstacle Vector3 c = remainingDisplacement; mo.AddDisplacement(new Vector3(0f, CollisionAPI.HopOverThreshold * Client.OneMeter, 0f)); if (MO.DoLog) { TraceMOBottom(mo, " Before trying to hop"); MO.Log(" remainingDisplacement {0}", remainingDisplacement); } if (collisionManager.TestCollision(mo, ref remainingDisplacement, parms)) { if (MO.DoLog) { MO.Log(" Even after hopping up {0} meters, can't get over obstacle; disp {1}", CollisionAPI.HopOverThreshold, remainingDisplacement); } c = c - remainingDisplacement; c.y = 0; c += new Vector3(0f, CollisionAPI.HopOverThreshold * Client.OneMeter, 0f); if (MO.DoLog) MO.Log(" After failed hop, subtracting {0}", c); mo.AddDisplacement(- c); } else if (MO.DoLog) { MO.Log(" Hopping up {0} meters got us over obstacle; disp {1}", CollisionAPI.HopOverThreshold, remainingDisplacement); TraceMOBottom(mo, " After hopping"); } NowColliding(mo, " After hopping"); } } Vector3 moPos = mo.parts[0].shape.center; pos = start + moPos - moStart; if (MO.DoLog) { MO.Log(" mo location = {0}, moPos - moStart {1}", moPos, moPos - moStart); NowColliding(mo, " Leaving FindMobNodeDisplacement"); MO.Log(" pos = {0}", pos); } return pos; }
public CollisionShape FindClosestCollision(CollisionShape cap, Vector3 start, Vector3 end, ref Vector3 intersection) { collisionTimeStamp++; SphereTreeNode s = sphereTree.FindSmallestContainer(cap, null); CollisionParms parms = new CollisionParms(); parms.genNormals = false; List<CollisionShape> shapes = new List<CollisionShape>(); s.FindIntersectingShapes(cap, shapes, collisionTimeStamp, ref collisionTestCount, parms); intersection = Vector3.Zero; if (shapes.Count == 0) return null; else { collisionTimeStamp++; return FindClosestIntersectingShape(shapes, start, end, ref intersection); } }
// Move the desired displacement, limited by hitting an // obstacle. Then, if we're not already at the terrain level, // "fall" until we are either at the terrain level, or hit an // obstacle public static Vector3 MoveMobNode(MobNode mobNode, Vector3 requestedDisplacement, Client client) { // Logger.Log(0, "MoveMobNode oid {0} requestedDisplacement {1}", mobNode.Oid, requestedDisplacement); // log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1}, followTerrain {2}, position {3}, disp {4}", // mobNode.Oid, mobNode.Name, mobNode.FollowTerrain, mobNode.Position, requestedDisplacement); Vector3 start = mobNode.Position; MovingObject mo = mobNode.Collider; bool collided = false; // Zero the y coordinate of displacement, because it seems // that it can be arbitrarily large Vector3 desiredDisplacement = requestedDisplacement; if (mobNode.FollowTerrain) desiredDisplacement.y = 0; if (desiredDisplacement.LengthSquared <= float.Epsilon) return start; if (MO.DoLog) MO.Log("MoveMobNode called with mobNode {0} at {1}, disp of {2}", mobNode.Oid, start, requestedDisplacement); if (collisionManager == null) { log.Info("MoveMobNode: returning because collisionManager isn't initialized"); return start + desiredDisplacement; } if (mo == null || mo.parts.Count == 0) { if (MO.DoLog) MO.Log("MoveMobNode returning because no collision volume for node"); return start + requestedDisplacement; } if (mobNode is Player && NowColliding(mo, "Testing player collision on entry")) { if (client.MillisecondsStuckBeforeGotoStuck != 0) { if (!playerStuck) { stuckGotoTime = DateTime.Now.AddMilliseconds(client.MillisecondsStuckBeforeGotoStuck); playerStuck = true; } else if (DateTime.Now >= stuckGotoTime) { // We issue the goto command to move us out of the // collision volume client.Write("Executing /stuck command because player has been in a collision volume for " + client.MillisecondsStuckBeforeGotoStuck + " milliseconds"); client.NetworkHelper.SendTargettedCommand(client.Player.Oid, "/stuck"); playerStuck = false; return start; } } } else playerStuck = false; // If we haven't completed setup to this extent, just give up CollisionParms parms = new CollisionParms(); Vector3 pos = FindMobNodeDisplacement(mobNode, parms, desiredDisplacement, out collided); // log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1}, mob node position {2}, displacement {3}", // mobNode.Oid, mobNode.Name, pos, requestedDisplacement); float h = worldManager.GetHeightAt(pos); // If we're already below ground level, just set our // level to ground level. This will have to be modified // if we deal with caves if (pos.y - h < 0) { // log.DebugFormat("MoveMobNode: mobNode oid {0}, name {1} below terrain level", mobNode.Oid, mobNode.Name); mo.AddDisplacement(new Vector3(0f, h - pos.y, 0f)); pos.y = h; if (MO.DoLog && (pos.y - h) < -.001 * Client.OneMeter) MO.Log(string.Format(" MobNode at {0} is below ground height {1}!", pos, h)); } // else { if (mobNode.FollowTerrain) { // NowColliding(mo, " Before falling loop"); // Fall toward the terrain or an obstacle, whichever comes // first float step = mo.StepSize(new Vector3(0, h, 0)); while (true) { if (Math.Abs(pos.y - h) < CollisionAPI.VerticalTerrainThreshold * Client.OneMeter) { mo.AddDisplacement(new Vector3(0f, h - pos.y, 0f)); pos.y = h; break; } else { float dy = -Math.Min(pos.y - h, step); Vector3 displacement = new Vector3(0, dy, 0); Vector3 cd = displacement; if (MO.DoLog) { MO.Log(" Testing for collision falling {0}", dy); TraceMOBottom(mo, " Before falling"); } if (collisionManager.TestCollision(mo, ref displacement, parms)) { if (MO.DoLog) { TraceMOBottom(mo, " After TestCollision after falling"); NowColliding(mo, " After TestCollision after falling"); MO.Log(" Collision when object {0} falls from {1} to {2}", parms.part.handle, pos, pos + cd); TraceObstacle(parms.obstacle); MO.Log(" Adding dy {0} - displacement.y {1} to pos {2}", dy, displacement.y, pos); } pos.y += dy - displacement.y; break; } if (MO.DoLog) MO.Log(" Didn't collide falling; dy {0}, pos {1}", dy, pos); pos.y += dy; } } } else { if (MO.DoLog) MO.Log(" Not falling because mobNode {0} doesn't have FollowTerrain", mobNode.Oid); } // } if (MO.DoLog) { NowColliding(mo, " Leaving MoveMobNode"); MO.Log("MoveMobNode returning pos {0}", pos); MO.Log(""); } if (collided) log.DebugFormat("MoveMobNode collided: mobNode oid {0}, name {1}, orig pos {2}, displacement {3}, new pos {4}", mobNode.Oid, mobNode.Name, start, requestedDisplacement, pos); else log.DebugFormat("MoveMobNode didn't collide: mobNode oid {0}, name {1}, orig pos {2}, displacement {3}, new pos {4}", mobNode.Oid, mobNode.Name, start, requestedDisplacement, pos); return pos; }
public static bool TestCollisionAABBAABB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionAABB x1 = (CollisionAABB)s1; CollisionAABB x2 = (CollisionAABB)s2; if (TestAABBAABB(x1, x2)) { Vector3 n = x2.center - x1.center; parms.SetNormPart(n); // ??? This isn't the correct value of the normal parms.SetNormObstacle(-n); return true; } else return false; }
public bool TestCollision(MovingObject mo, ref Vector3 displacement, CollisionParms parms) { // Find the minimum step size for the assembly of parts float stepSize = mo.StepSize(displacement); return TestCollision(mo, stepSize, ref displacement, parms); }
// The is the top-level collision detection function. // // The MovingObject doesn't intersect anything now; would it // do so if the vector displacement is applied? // // If we do have a collision, we "creep up" on the object we collide // with until we're within stepsize of the minimum dimension of the // moving part // public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms) { topLevelCalls++; parms.Initialize(); if (mo.parts.Count == 0) { return(false); } // Remember where the first part started Vector3 start = mo.parts[0].shape.center; Vector3 originalDisplacement = displacement; float len = displacement.Length; //MaybeDumpSphereTree(); int nsteps = (int)Math.Ceiling(len / stepSize); Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize / len)); // Try to step the whole way if (!StepTowardCollision(mo, displacement, nsteps, step, parms)) { displacement = Vector3.Zero; return(false); } // Otherwise, we hit something. Step toward it // The minimum distance float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle), stepSize * MinFractionToObstacle); len = step.Length; nsteps = (int)Math.Ceiling(len / smallStepSize); Vector3 smallStep = step * (smallStepSize / len); bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms); displacement = originalDisplacement - (mo.parts[0].shape.center - start); return(hit); }
public static bool TestCollisionAABBOBB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionAABB x1 = (CollisionAABB)s1; CollisionOBB x2 = (CollisionOBB)s2; // Convert the AABB into an OBB, then run the OBBOBB test. // Not the most efficient algorithm but it gets the job // done. CollisionOBB newx1 = new CollisionOBB(x1.center, UnitBasisVectors, (x1.max - x1.min) * .5f); return TestOBBOBB(newx1, x2, parms); }
// Decide, based on the normal to the collision object, if the // moving object can slide across the obstacle, and if it can, // return the updated displacement. This displacement may in fact // run into _another_ obstacle, however, so the call must again // run the collision test. private static bool DecideToSlide(MovingObject mo, Vector3 mobNodePosition, CollisionParms parms, ref Vector3 displacement) { Vector3 normDisplacement = displacement.ToNormalized(); Vector3 normObstacle = parms.normObstacle.ToNormalized(); if (MO.DoLog) { MO.Log(" DecideToSlide: normObstacle {0}, normDisplacement {1}", normObstacle.ToString(), normDisplacement.ToString()); MO.Log(" DecideToSlide: displacement {0}", displacement); } // First we find the angle between the normal and the // direction of travel, and reject the displacement if // it's too small float slideAngle = (NormalizeAngle((float)Math.Acos((double)normDisplacement.Dot(normObstacle))) - .5f * (float)Math.PI); if (Math.Abs(slideAngle) > CollisionAPI.MinSlideAngle) { if (MO.DoLog) MO.Log(" After collision, displacement {0}, won't slide because slideAngle {1} > minSlideAngle {2}", displacement.ToString(), slideAngle, CollisionAPI.MinSlideAngle); displacement = Vector3.Zero; return false; } // Then we find the angle with the y axis, and reject the // displacement if it's too steep float verticalAngle = NormalizeAngle((float)Math.Acos((double)normDisplacement[1])); if (Math.Abs(verticalAngle) > CollisionAPI.MaxVerticalAngle) { if (MO.DoLog) MO.Log(" After collision, displacement {0}, won't slide because verticalAngle {1} <= maxVerticalAngle {2}", displacement.ToString(), verticalAngle, CollisionAPI.MaxVerticalAngle); displacement = Vector3.Zero; return false; } // Else, we can slide, so return a displacement that // points in the direction we're sliding, and has length // equal to a constant times the displacement length // Rotate displacement so that it's 90 degress from the // obstacle normal Vector3 cross = normObstacle.Cross(normDisplacement); Quaternion q = Quaternion.FromAngleAxis(.5f * (float)Math.PI, cross); Matrix4 transform = q.ToRotationMatrix(); Vector3 transformedNorm = transform * normObstacle.ToNormalized(); float len = displacement.Length; displacement = transformedNorm * len; // Vector3 alignedPart = normObstacle * (normObstacle.Dot(displacement)); // displacement -= alignedPart; Vector3 p = mobNodePosition + displacement; float h = worldManager.GetHeightAt(p); // If sliding would put us below ground, limit the displacement if (h > p.y) { if (MO.DoLog) MO.Log(" Sliding up because terrain height is {0} is higher than projected mobNode height {1}", h, p.y); displacement.y += h - p.y; } if (MO.DoLog) { MO.Log(" Exiting DecideToSlide, sliding displacement {0}, slideAngle {1}, verticalAngle {2}", displacement.ToString(), slideAngle, verticalAngle); MO.Log(" Exiting DecideToSlide, cross product {0}, quaternion {1}, transformedNorm {2}", cross, q, transformedNorm); // MO.Log(" Exiting DecideToSlide, alignedPart {0}", alignedPart); } return true; }
public static bool TestCollisionCapsuleCapsule(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionCapsule x1 = (CollisionCapsule)s1; CollisionCapsule x2 = (CollisionCapsule)s2; return TestCapsuleCapsule(x1, x2, parms); }
private static bool NowColliding(MovingObject mo, string description) { CollisionParms parms = new CollisionParms(); bool colliding = collisionManager.CollideAfterAddingDisplacement(mo, Vector3.Zero, parms); CollisionShape obstacle = mo.parts[0].shape; CollisionCapsule moCapsule = null; if (obstacle is CollisionCapsule) moCapsule = (CollisionCapsule)obstacle; string rest = (moCapsule != null ? string.Format("mo bottom is {0}", moCapsule.bottomcenter - new Vector3(0f, moCapsule.capRadius, 0f)) : string.Format("obstacle {0}", obstacle)); if (MO.DoLog) MO.Log("{0}, now colliding = {1}; {2}", description, colliding, rest); log.DebugFormat("{0}, now colliding = {1}; {2}", description, colliding, rest); return colliding; }
public static bool TestCollisionOBBOBB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionOBB x1 = (CollisionOBB)s1; CollisionOBB x2 = (CollisionOBB)s2; return TestOBBOBB(x1, x2, parms); }
///<summary> /// Run the grid traversal algorithm, pushing the cells /// around the model ///</summary> protected void TraverseGridCells() { cellsProcessed = 0; cellsOnCVs = 0; Stopwatch collisionStopwatch = new Stopwatch(); int collisionCount = 0; // Create the CollisionParms object CollisionParms parms = new CollisionParms(); // Set the big step size to 5% of the box height; this // will make the small step size .5% of the box height. // For a box height of 1.8 meters, this is .009m, or 9mm // Since the grid resolution is .25 of model width, and // for the human model, that width is .5m, the cell width // is .125m or 125mm. So .009 / .125 = .072, so the // maximum variation in slope due exclusively to the step // size is 7.2% float stepSize = boxHeight * .05f; // Iterate over work list items until there aren't any while (workList.Count > 0) { CellLocation loc = workList[0]; workList.RemoveAt(0); GridCell cell = FindCellAtHeight(loc); if (cell != null) // Skip, because it's already been visited continue; // Position the moving object over the cell SetMovingBoxOverCell(loc); // If we're above terrain level, we need to drop the // cell. If we're at or below terrain level, we just // mark the cell as supported by the terrain float distanceToTerrain = movingBox.min.y - terrainLevel; if (distanceToTerrain <= 0f) { loc.height = terrainLevel; if (FindCellAtHeight(loc) != null) continue; cell = new GridCell(loc); cell.status = CellStatus.OverTerrain; } else { // Now drop it until it hits a collision object, or // it's at terrain level. Note: this means that we // can't have "basements" until we find a way to have // a non-constant terrain level Vector3 displacement = new Vector3(0, -distanceToTerrain, 0); collisionCount++; collisionStopwatch.Start(); bool hit = collisionAPI.TestCollision(movingObject, stepSize, ref displacement, parms); collisionStopwatch.Stop(); float oldHeight = loc.height; loc.height = movingBox.min.y; if (FindCellAtHeight(loc) != null) continue; cell = new GridCell(loc); if (hit) { // We hit a collision object - - if it's below // us, then set the height accordingly. If // it's not below us, mark the cell as inaccessible. if (displacement.y != -distanceToTerrain) { cell.status = CellStatus.OverCV; cell.supportingShape = parms.obstacle; cellsOnCVs++; } else { loc.height = oldHeight; if (FindCellAtHeight(loc) != null) continue; cell.loc.height = oldHeight; cell.status = CellStatus.Inaccessible; } } else { loc.height = terrainLevel; cell.loc.height = terrainLevel; cell.status = CellStatus.OverTerrain; if (FindCellAtHeight(loc) != null) continue; } } // Add the cell to the grid, now that we know its // actual height cellsProcessed++; grid[loc.xCell, loc.zCell].Add(cell); if (cell.status == CellStatus.Inaccessible) continue; // Now add the neighbors to the work list, if they // haven't already been visited for (GridDirection dir = GridDirection.PlusX; dir <= GridDirection.MinusZ; dir++) { int neighborX = loc.xCell + XIncrement[(int)dir]; int neighborZ = loc.zCell + ZIncrement[(int)dir]; // If the neighbor is outside the grid, ignore it if (neighborX < 0 || neighborX >= xCount || neighborZ < 0 || neighborZ >= zCount) continue; // Test to see if it exists; if so, it's been visited CellLocation neighborLoc = new CellLocation(neighborX, neighborZ, cell.loc.height, loc); GridCell neighborCell = FindCellAtHeight(neighborLoc); // If it doesn't exist, add it to the work queue if (neighborCell == null) workList.Add(neighborLoc); //AddToWorkList(neighborLoc); } } DumpCurrentTime(string.Format("Processing {0} box drops, {1} collision tests, took {2} ms", collisionCount, collisionAPI.partCalls, collisionStopwatch.ElapsedMilliseconds)); DumpGrid("Prefiltered Grid", GridDumpKind.Cells, false); }
public static bool TestCollisionSphereAABB(CollisionShape s1, CollisionShape s2, CollisionParms parms) { CollisionSphere x1 = (CollisionSphere)s1; CollisionAABB x2 = (CollisionAABB)s2; float d = SqDistPointAABB(x1.center, x2); if (d < Square(x1.radius)) { Vector3 n; ClosestPtPointAABB(x1.center, x2, out n); n -= x1.center; parms.SetNormPart(n); // ??? This isn't the correct value of the normal parms.SetNormObstacle(-n); return true; } else return false; }