/// <summary>
/// Check whether the ray originates in this intersection and exits.
/// If so, find the exit point.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public bool Leaves(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
float collRadius = Road.Generator.CollRadius;
if (collRadius > 0.0f)
{
Vector2 src2 = new Vector2(src.X, src.Y);
Vector2 src2toCenter = src2 - Node.Position2d;
float distSq = src2toCenter.LengthSquared();
float radSumSq = collRadius;
radSumSq *= radSumSq;
Vector2 cen2 = Node.Position2d;
if (distSq > radSumSq)
{
return(false);
}
hitBlock.CrossesPath = true;
Vector2 dst2 = new Vector2(dst.X, dst.Y);
/// Okay, look for intersection with circle.
float a = Vector2.DistanceSquared(dst2, src2);
if (a > 0.0f)
{
float b = 2.0f * Vector2.Dot(src2 - cen2, dst2 - src2);
float c = Vector2.DistanceSquared(src2, cen2) - radSumSq;
float det = b * b - 4.0f * a * c;
if (det >= 0)
{
float t = (float)((-b - Math.Sqrt(det)) / (2.0f * a));
if ((t >= 0.0f) && (t <= 1.0f))
{
Vector3 hitPoint = src + t * (dst - src);
hitBlock.Position = hitPoint;
hitBlock.Normal = Vector3.Normalize(new Vector3(
cen2.X - hitPoint.X,
cen2.Y - hitPoint.Y,
0.0f));
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = 0.0f;
return(true);
}
}
}
}
return(false);
}
private bool DegenerateBlocked(Vector3 src, ref Terrain.HitBlock hitBlock)
{
float pathHeight = 0.0f;
/// Handle unlikely degenerate input.
if (GetHeight(src, ref pathHeight))
{
hitBlock.Max = hitBlock.Min = pathHeight;
hitBlock.Position = new Vector3(src.X, src.Y, pathHeight);
hitBlock.Normal = Vector3.UnitZ;
hitBlock.BlockHeight = pathHeight;
hitBlock.CrossesPath = true;
return(true);
}
return(false);
}
/// <summary>
/// Line of sight test.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public static bool Blocked(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
bool ret = false;
foreach (Path path in Paths)
{
if (path.Road != null)
{
if (path.Road.Blocked(src, dst, ref hitBlock))
{
dst = hitBlock.Position;
ret = true;
}
}
}
return(ret);
}
/// <summary>
/// Find where the ray leaves the road.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public static bool LeavesRoad(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
bool ret = false;
foreach (Path path in Paths)
{
if (path.Road != null)
{
if (path.Road.LeavesRoad(src, dst, ref hitBlock))
{
/// This isn't quite right, really should be like
/// Road.LeavesRoad(), but might be close enough,
/// and is definitely cheaper.
src = hitBlock.Position;
ret = true;
}
}
}
return(ret);
}
public bool LeavesRoad(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
if (!BlocksTravel && !IsGround)
{
return(false);
}
bool leaves = false;
bool onPath = false;
do
{
onPath = false;
foreach (Section section in Sections)
{
if (!section.Abstain && section.Leaves(src, dst, ref hitBlock))
{
src = hitBlock.Position;
section.Abstain = true;
onPath = true;
leaves = true;
break;
}
}
foreach (Intersection isect in Intersections)
{
if (!isect.Abstain && isect.Leaves(src, dst, ref hitBlock))
{
src = hitBlock.Position;
isect.Abstain = true;
onPath = true;
leaves = true;
break;
}
}
}while (onPath);
return(leaves);
}
/// <summary>
/// Find the first (if any) wall blocking ray.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public bool Blocked(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
if (!BlocksTravel && !IsGround)
{
return(false);
}
/// Care about two cases:
/// a. src on path, dst off (dst possibly off end of world)
/// need point where leave path, and normal back in there.
/// b. ray src to dst passes through path
/// need first point hitting path, and normal out from there
const float kCloseTogether = Single.Epsilon;
if (Vector3.DistanceSquared(src, dst) <= kCloseTogether)
{
return(DegenerateBlocked(src, ref hitBlock));
}
bool hit = false;
foreach (Section section in Sections)
{
if (section.Blocked(src, dst, ref hitBlock))
{
dst = hitBlock.Position;
hit = true;
}
}
foreach (Intersection isect in Intersections)
{
if (isect.Blocked(src, dst, ref hitBlock))
{
dst = hitBlock.Position;
hit = true;
}
}
return(hit);
}
/// <summary>
/// Look for the exit point of ray from src to dst. Returns false of no
/// exit found. That can be because either src is not inside the sections bounds
/// or dst is inside.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public bool Leaves(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
Vector2 src2 = new Vector2(src.X, src.Y);
Vector2 src2sec = Vector2.Transform(src2, worldToSection);
float collWidth = Road.Generator.CollWidth;
if (!InsideRect(src2sec))
{
return(false);
}
hitBlock.CrossesPath = true;
Vector2 dst2 = new Vector2(dst.X, dst.Y);
Vector2 dst2sec = Vector2.Transform(dst2, worldToSection);
if (dst2sec.Y < -collWidth)
{
float t = (src2sec.Y + collWidth) / (src2sec.Y - dst2sec.Y);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
Vector3 hitPoint = src + t * (dst - src);
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
sectionToWorld.M21,
sectionToWorld.M22,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = 0.0f;
return(true);
}
}
else if (dst2sec.Y > collWidth)
{
float t = (src2sec.Y - collWidth) / (src2sec.Y - dst2sec.Y);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
Vector3 hitPoint = src + t * (dst - src);
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
-sectionToWorld.M21,
-sectionToWorld.M22,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = 0.0f;
return(true);
}
}
float halfEdge = edgeLength * 0.5f;
if (dst2sec.X < -halfEdge)
{
float t = (src2sec.X + halfEdge) / (src2sec.X - dst2sec.X);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
Vector3 hitPoint = src + t * (dst - src);
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
sectionToWorld.M11,
sectionToWorld.M12,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = 0.0f;
return(true);
}
}
else if (dst2sec.X > halfEdge)
{
float t = (src2sec.X - halfEdge) / (src2sec.X - dst2sec.X);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
Vector3 hitPoint = src + t * (dst - src);
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
-sectionToWorld.M11,
-sectionToWorld.M12,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = 0.0f;
return(true);
}
}
return(false);
}
/// Basic idea here is we'll transform the problem into the space where
/// the section of road is a rectangle, so all tests become trivial because
/// it's an axis aligned problem.
/// <summary>
///
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public bool Blocked(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
Vector2 src2 = new Vector2(src.X, src.Y);
Vector2 src2sec = Vector2.Transform(src2, worldToSection);
Vector2 dst2 = new Vector2(dst.X, dst.Y);
Vector2 dst2sec = Vector2.Transform(dst2, worldToSection);
float collWidth = Road.Generator.CollWidth;
if (InsideRect(src2sec))
{
hitBlock.CrossesPath = true;
/// src point is on the path.
float baseHeight = BaseHeight(src2);
Vector3 degNormal = GetNormal(src2);
float degCollisionHeight = baseHeight + Road.Generator.CollHeight;
float degCollisionBase = baseHeight + Road.Generator.CollBase;
float degHeightDist = Vector3.Dot(degNormal, new Vector3(src2.X, src2.Y, degCollisionHeight));
float degBaseDist = Vector3.Dot(degNormal, new Vector3(src2.X, src2.Y, degCollisionBase));
float startDist = Vector3.Dot(src, degNormal);
float endDist = Vector3.Dot(dst, degNormal);
if ((startDist > degHeightDist) && (endDist < degHeightDist))
{
float t = (startDist - degHeightDist) / (startDist - endDist);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.Position = src + t * (dst - src);
hitBlock.Normal = degNormal;
hitBlock.Min = Math.Min(src.Z, degCollisionHeight);
hitBlock.Max = Math.Max(src.Z, degCollisionHeight);
hitBlock.BlockHeight = degCollisionHeight;
return(true);
}
}
if (Road.Generator.PassUnder)
{
if ((startDist < degBaseDist) && (endDist > degBaseDist))
{
float t = (startDist - degBaseDist) / (startDist - endDist);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.Position = src + t * (dst - src);
hitBlock.Normal = -degNormal;
hitBlock.Min = Math.Min(src.Z, degBaseDist);
hitBlock.Max = Math.Max(src.Z, degBaseDist);
hitBlock.BlockHeight = degCollisionHeight;
return(true);
}
}
}
}
/// For the height of the wall, we'll appoximate it with the height
/// at the point where the ray from src to dst crosses the wall's center.
/// If src and dst are both on the same side of the wall, we'll approximate
/// with the center point between src and dst.
Vector2 cen2sec = CenterPoint(src2sec, dst2sec);
Vector2 cen2 = Vector2.Transform(cen2sec, sectionToWorld);
float cenBaseHeight = BaseHeight(cen2);
Vector3 cenNormal = GetNormal(cen2);
float cenCollisionHeight = cenBaseHeight + Road.Generator.CollHeight;
float cenCollisionBase = cenBaseHeight + Road.Generator.CollBase;
float cenHeightDist = Vector3.Dot(cenNormal, new Vector3(cen2.X, cen2.Y, cenCollisionHeight));
float cenBaseDist = Vector3.Dot(cenNormal, new Vector3(cen2.X, cen2.Y, cenCollisionBase));
/// Three interesting cases:
/// a) the ray pierces the top of the box.
/// b) the ray pierces the bottom of the box (with PassUnder)
/// c) the ray pierces the side of the box.
float srcDist = Vector3.Dot(src, cenNormal);
float dstDist = Vector3.Dot(dst, cenNormal);
if ((srcDist > cenHeightDist) && (dstDist < cenHeightDist))
{
/// The ray pierces the infinite plane of the box top, see if it hits the finite
/// box top, case a.
float t = (srcDist - cenHeightDist) / (srcDist - dstDist);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.Position = src + t * (dst - src);
hitBlock.Normal = cenNormal;
hitBlock.Min = Math.Min(src.Z, cenCollisionHeight);
hitBlock.Max = Math.Max(src.Z, cenCollisionHeight);
hitBlock.BlockHeight = cenCollisionHeight;
hitBlock.CrossesPath = true;
return(true);
}
}
if (Road.Generator.PassUnder)
{
if ((srcDist < cenBaseDist) && (dstDist > cenBaseDist))
{
/// The ray pierces the infinite plane of the box top, see if it hits the finite
/// box top, case a.
float t = (srcDist - cenBaseDist) / (srcDist - dstDist);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.Position = src + t * (dst - src);
hitBlock.Normal = -cenNormal;
hitBlock.Min = Math.Min(src.Z, cenCollisionBase);
hitBlock.Max = Math.Max(src.Z, cenCollisionBase);
hitBlock.BlockHeight = cenCollisionBase;
hitBlock.CrossesPath = true;
return(true);
}
}
}
/// Now case c, piercing the side of the box.
/// This is the last chance for a hit.
///
if (src2sec.Y < -collWidth)
{
if (dst2sec.Y > -collWidth)
{
float t = (src2sec.Y + collWidth) / (src2sec.Y - dst2sec.Y);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.CrossesPath = true;
Vector3 hitPoint = src + t * (dst - src);
bool hit = hitPoint.Z < cenCollisionHeight;
if (hit && Road.Generator.PassUnder)
{
hit = hitPoint.Z > cenCollisionBase;
}
if (hit)
{
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
-sectionToWorld.M21,
-sectionToWorld.M22,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = cenCollisionHeight;
return(true);
}
}
}
}
else if (src2sec.Y > collWidth)
{
if (dst2sec.Y < collWidth)
{
float t = (src2sec.Y - collWidth) / (src2sec.Y - dst2sec.Y);
Vector2 hit2sec = src2sec + t * (dst2sec - src2sec);
if (InsideRect(hit2sec))
{
hitBlock.CrossesPath = true;
Vector3 hitPoint = src + t * (dst - src);
bool hit = hitPoint.Z < cenCollisionHeight;
if (hit && Road.Generator.PassUnder)
{
hit = hitPoint.Z > cenCollisionBase;
}
if (hit)
{
hitBlock.Position = hitPoint;
hitBlock.Normal = new Vector3(
sectionToWorld.M21,
sectionToWorld.M22,
0.0f);
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = cenCollisionHeight;
return(true);
}
}
}
}
/// Note that we don't check hitting the ends of the box,
/// because we expect the intersection to catch those cases.
return(false);
}
/// <summary>
/// Check whether the ray passes through this intersection.
/// </summary>
/// <param name="src"></param>
/// <param name="dst"></param>
/// <param name="hitBlock"></param>
/// <returns></returns>
public bool Blocked(Vector3 src, Vector3 dst, ref Terrain.HitBlock hitBlock)
{
float collRadius = Road.Generator.CollRadius;
if (collRadius > 0.0f)
{
Vector2 src2 = new Vector2(src.X, src.Y);
Vector2 src2toCenter = src2 - Node.Position2d;
float distSq = src2toCenter.LengthSquared();
float radSumSq = collRadius;
radSumSq *= radSumSq;
Vector2 cen2 = Node.Position2d;
float baseHeight = BaseHeight(cen2);
float collTop = baseHeight + Road.Generator.CollEndHeight;
float collBot = baseHeight + Road.Generator.CollEndBase;
if (!Road.Generator.PassUnderEnd)
{
collBot = 0.0f;
}
Vector3 normal = Plane.Normal;
float srcDist = Vector3.Dot(normal, src);
float dstDist = Vector3.Dot(normal, dst);
float topDist = Vector3.Dot(normal, new Vector3(cen2, collTop));
float botDist = Vector3.Dot(normal, new Vector3(cen2, collBot));
if (distSq < radSumSq)
{
hitBlock.CrossesPath = true;
/// src is over the intersection, check for degenerate case.
if ((srcDist < topDist) && (srcDist > botDist))
{
hitBlock.Position = src;
hitBlock.Normal = Plane.Normal;
hitBlock.Min = hitBlock.Max = collTop;
hitBlock.BlockHeight = collTop;
return(true);
}
}
if ((srcDist > topDist) && (dstDist < topDist))
{
/// The ray crosses the collision top plane. See if it intersects
/// the collision top cap.
float t = (srcDist - topDist) / (srcDist - dstDist);
Vector3 hitPoint = src + t * (dst - src);
distSq = new Vector2(
cen2.X - hitPoint.X,
cen2.Y - hitPoint.Y).LengthSquared();
if (distSq < radSumSq)
{
hitBlock.Position = hitPoint;
hitBlock.Normal = Plane.Normal;
hitBlock.Min = collTop;
hitBlock.Max = src.Z;
hitBlock.BlockHeight = collTop;
return(true);
}
}
if (Road.Generator.PassUnderEnd)
{
if ((srcDist < botDist) && (dstDist > botDist))
{
/// The ray crosses the collision top plane. See if it intersects
/// the collision top cap.
float t = (srcDist - botDist) / (srcDist - dstDist);
Vector3 hitPoint = src + t * (dst - src);
distSq = new Vector2(
cen2.X - hitPoint.X,
cen2.Y - hitPoint.Y).LengthSquared();
if (distSq < radSumSq)
{
hitBlock.Position = hitPoint;
hitBlock.Normal = -Plane.Normal;
hitBlock.Min = collBot;
hitBlock.Max = src.Z;
hitBlock.BlockHeight = collTop;
return(true);
}
}
}
Vector2 dst2 = new Vector2(dst.X, dst.Y);
/// Okay, look for intersection with circle.
float a = Vector2.DistanceSquared(dst2, src2);
if (a > 0.0f)
{
float b = 2.0f * Vector2.Dot(src2 - cen2, dst2 - src2);
float c = Vector2.DistanceSquared(src2, cen2) - radSumSq;
float det = b * b - 4.0f * a * c;
if (det >= 0)
{
float t = (float)((-b - Math.Sqrt(det)) / (2.0f * a));
if ((t >= 0.0f) && (t <= 1.0f))
{
hitBlock.CrossesPath = true;
Vector3 hitPoint = src + t * (dst - src);
if ((hitPoint.Z < collTop) && (hitPoint.Z > collBot))
{
hitBlock.Position = hitPoint;
hitBlock.Normal = Vector3.Normalize(new Vector3(
hitPoint.X - cen2.X,
hitPoint.Y - cen2.Y,
0.0f));
hitBlock.Min = Math.Min(src.Z, hitPoint.Z);
hitBlock.Max = Math.Max(src.Z, hitPoint.Z);
hitBlock.BlockHeight = collTop;
return(true);
}
}
}
}
}
return(false);
}
