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"); }
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); }
// 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); }
// 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); }
// 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); }