public static bool Intersect(Bounding.Cylinder a, Vector2 b, out object intersection) // CylHeight implemented
{
intersection = null;
float dx = b.X - a.Position.X, dy = b.Y - a.Position.Y;
return(dx * dx + dy * dy <= a.Radius2);
}
public static bool Intersect(BoundingBox a, Bounding.Cylinder b, out object intersection) // CylHeight implemented
{
intersection = null;
// check horizontal intersection
Vector3 center = a.Minimum + (a.Maximum - a.Minimum) / 2f;
float dx = center.X - b.Position.X;
float dy = center.Y - b.Position.Y;
float d = (float)System.Math.Sqrt((double)(dx * dx + dy * dy));
if (d > 0)
{
float radius = System.Math.Min(b.Radius, d);
if (!Intersect(a, new Vector2(
b.Position.X + radius * dx / d,
b.Position.Y + radius * dy / d),
out intersection))
{
return(false);
}
}
return(Intersect1DLines(a.Minimum.Z, a.Maximum.Z, b.Position.Z, b.Position.Z + b.Height));
}
// ----------------------------------------------------------------------------------------------
// -- Cylinder ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
public static bool Intersect(Bounding.Cylinder a, Bounding.Line b, out object intersection) // CylHeight implemented
{
intersection = null;
Vector3 v = b.P1 - b.P0;
RayIntersection rOut;
return(Intersect(a, new Ray(b.P0, Vector3.Normalize(v)), out rOut) && rOut.Distance <= v.Length());
}
public override EPlacement Placement(Cylinder bound)
{
EPlacement ap = a.Placement(bound);
EPlacement bp = b.Placement(bound);
if (ap == EPlacement.BInsideA || bp == EPlacement.BInsideA) return EPlacement.BInsideA; //this one isn't quite right, might be BInsideA also if there's an intersection
if (ap == EPlacement.Outside && bp == EPlacement.Outside) return EPlacement.Outside;
if (ap == EPlacement.AInsideB && bp == EPlacement.AInsideB) return EPlacement.AInsideB;
return EPlacement.Intersect;
}
// ----------------------------------------------------------------------------------------------
// -- Cylinder ----------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------
public static RSpatialRelation Relation(Bounding.Cylinder a, Vector3 b)
{
float z = b.Z - a.Position.Z;
if ((a.Position - b).Length() <= a.Radius && z >= 0 && z <= a.Height)
{
return(RSpatialRelation.BInsideA);
}
return(RSpatialRelation.Outside);
}
public static bool Intersect(Bounding.Cylinder a, Vector3 b, out object intersection) // CylHeight implemented
{
if (!Intersect(a, Math.ToVector2(b), out intersection))
{
return(false);
}
intersection = null;
float z1 = a.Position.Z; float z2 = b.Z;
return(z2 >= z1 && z2 <= (z1 + a.Height));
}
public static Bounding.Cylinder Transform(Bounding.Cylinder bounding, Matrix transformation)
{
var newPos = Vector3.TransformCoordinate(bounding.Position, transformation);
var height = Vector3.TransformNormal(Vector3.UnitZ * bounding.Height, transformation).Z;
var r = Vector3.TransformNormal(Vector3.UnitX * bounding.Radius, transformation);
r.Z = 0;
var radius = r.Length();
return(new Bounding.Cylinder(newPos,
height,
radius,
bounding.SolidRayIntersection));
}
public static bool Intersect(Bounding.Cylinder a, Bounding.Cylinder b, out object intersection) // CylHeight implemented
{
intersection = null;
// check horizontal position
float dx = b.Position.X - a.Position.X, dy = b.Position.Y - a.Position.Y;
float d = (float)System.Math.Sqrt(dx * dx + dy * dy);
if (d > a.Radius + b.Radius)
{
return(false);
}
float z1Bottom = a.Position.Z; float z1Top = z1Bottom + a.Height;
float z2Bottom = b.Position.Z; float z2Top = z2Bottom + b.Height;
return(Intersect1DLines(z1Bottom, z1Top, z2Bottom, z2Top));
}
public override ECull Cull(Cylinder b)
{
return b.Cull(this);
}
public override ECull Cull(Cylinder bound)
{
if (a.Cull(bound) == ECull.Intersect) return ECull.Intersect;
return b.Cull(bound);
}
public static bool Intersect(Bounding.Cylinder a, Ray b, out object intersection) // CylHeight implemented
{
intersection = null;
// check if ray is positioned inside cylinder
if (Intersect(a, b.Position, out intersection))
{
if (a.SolidRayIntersection)
{
intersection = new RayIntersection {
Distance = 0
};
return(true);
}
else
{
return(false);
}
}
if (b.Direction.X == 0 && b.Direction.Y == 0)
{
// check horizontal position
if (Common.Math.ToVector2(a.Position - b.Position).Length() > a.Radius)
{
return(false);
}
// we can't use Ray.Intersects(ray, plane) since it also intersects with backsides of planes which we don't want unless SolidRayIntersection is set
float zPos = b.Position.Z;
float floor = a.Position.Z;
float ceiling = floor + a.Height;
if (b.Direction.Z < 0 && zPos > ceiling)
{
intersection = new RayIntersection {
Distance = zPos - ceiling
};
return(true);
}
else if (b.Direction.Z > 0 && zPos < floor)
{
intersection = new RayIntersection {
Distance = floor - zPos
};
return(true);
}
else if (!a.SolidRayIntersection)
{
return(false);
}
}
// assume normalized ray direction vector
// Ray: P + t*D
// Cylinder: O (center), r (Radius)
// |P + t*D - O| = radius
// (P + t*D - O)(P + t*D - O) - r^2 = 0
// A*t^2 + B*t + C = 0
// with A: (D . D)
// B: 2 * ((P - O) . D),
// C: (P - O) . (P - O) - r^2 = = |P - O|^2 - r^2
// t = -B/(2A) +- sqrt(B^2/(4A^2) - C)
Vector3 cylinderAxis = Vector3.UnitZ;
Vector3 P = b.Position - Vector3.Dot(cylinderAxis, b.Position) * cylinderAxis;
Vector3 D = b.Direction - Vector3.Dot(cylinderAxis, b.Direction) * cylinderAxis;
Vector3 O = a.Position - Vector3.Dot(cylinderAxis, a.Position) * cylinderAxis;
float A = Vector3.Dot(D, D);
float B = 2 * Vector3.Dot(P - O, D);
float C = Vector3.Dot(P - O, P - O) - a.Radius2;
float x = -B / (2 * A);
float y = x * x - C / A;
if (y < 0)
{
return(false); // imaginary roots
}
float z = (float)System.Math.Sqrt(y);
float t1 = x + z;
float t2 = x - z;
float minT = float.MaxValue;
if (t1 < 0)
{
if (t2 < 0)
{
return(false);
}
minT = t2;
}
else
{
if (t2 < 0)
{
minT = t1;
}
else
{
minT = (float)System.Math.Min(t1, t2);
}
}
// Check that the point is within cylinder vertical limits
Vector3 Q = b.Position + minT * b.Direction;
Vector3 PO = Q - a.Position;
Vector3 POp = Vector3.Dot(PO, cylinderAxis) * Vector3.Normalize(cylinderAxis);
var debug1 = Vector3.Dot(POp, cylinderAxis);
var debug2 = POp.Length();
if (Vector3.Dot(POp, cylinderAxis) >= 0 && // not below
POp.Length() <= a.Height) // not above
{
intersection = new RayIntersection {
Distance = minT
};
return(true);
}
else
{
return(false);
}
}
public static RSpatialRelation Relation(BoundingBox a, Bounding.Cylinder b)
{
// NOTE: Not tested thoroughly!
/*
* This all is based on finding the intersections between the axises of the box
* */
float m, n;
float boxMinZ = a.Minimum.Z; float boxMaxZ = a.Maximum.Z;
float cylMinZ = b.Position.Z; float cylMaxZ = cylMinZ + b.Height;
RSpatialRelation verticalRelation = Relation1DLines(boxMinZ, boxMaxZ, cylMinZ, cylMaxZ);
if (verticalRelation == RSpatialRelation.Outside)
{
return(RSpatialRelation.Outside);
}
// from this point on we know that the cylinder at least intersects the box on the vertical axis
//X Axis, first find intersections
CylP x1 = CylP.NotIntersecting;
if (Math.CircleXAxisIntersection(b.Position, b.Radius, a.Minimum.Y, out m, out n))
{
if (m > a.Maximum.X || n < a.Minimum.X)
{
x1 = CylP.Outside;
}
else if (m < a.Minimum.X && n > a.Maximum.X)
{
x1 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
CylP x2 = CylP.NotIntersecting;
if (Math.CircleXAxisIntersection(b.Position, b.Radius, a.Maximum.Y, out m, out n))
{
if (m > a.Maximum.X || n < a.Minimum.X)
{
x2 = CylP.Outside;
}
else if (m < a.Minimum.X && n > a.Maximum.X)
{
x2 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
//Y Axis, first find intersections
CylP y1 = CylP.NotIntersecting;
if (Math.CircleYAxisIntersection(b.Position, b.Radius, a.Minimum.X, out m, out n))
{
if (m > a.Maximum.Y || n < a.Minimum.Y)
{
y1 = CylP.Outside;
}
else if (m < a.Minimum.Y && n > a.Maximum.Y)
{
y1 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
CylP y2 = CylP.NotIntersecting;
if (Math.CircleYAxisIntersection(b.Position, b.Radius, a.Maximum.X, out m, out n))
{
if (m > a.Maximum.Y || n < a.Minimum.Y)
{
y2 = CylP.Outside;
}
else if (m < a.Minimum.Y && n > a.Maximum.Y)
{
y2 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
if (x1 == CylP.AInsideB && x2 == CylP.AInsideB && y1 == CylP.AInsideB && y2 == CylP.AInsideB)
{
if (verticalRelation == RSpatialRelation.AInsideB)
{
return(RSpatialRelation.AInsideB);
}
return(RSpatialRelation.Intersect);
}
//var horizontalRelation = RectCircleRelation(new RectangleF(new PointF(a.Minimum.X, a.Minimum.Y), new SizeF(a.Maximum.X - a.Minimum.X, a.Maximum.Y - a.Minimum.Y)), b);
//if (horizontalRelation == RSpatialRelation.Outside)
// return RSpatialRelation.Outside;
//else if (horizontalRelation == verticalRelation)
// return horizontalRelation;
//else
// return RSpatialRelation.Intersect;
if (x1 == CylP.NotIntersecting && x2 == CylP.NotIntersecting &&
y1 == CylP.NotIntersecting && y2 == CylP.NotIntersecting &&
Intersection.Intersect(a, Math.ToVector2(b.Position)))
{
if (verticalRelation == RSpatialRelation.BInsideA)
{
return(RSpatialRelation.BInsideA);
}
return(RSpatialRelation.Intersect);
}
else
{
return(RSpatialRelation.Outside);
}
#if false
//We start by checking if the circle is so large it covers the whole rectangle
bool intersects = false, ainsideb = true;
if (p.CullPoint(position.X, position.Y) == ECull.Intersect)
{
intersects = true;
}
else
{
ainsideb = false;
}
if (p.CullPoint(position.X + size.X, position.Y) == ECull.Intersect)
{
intersects = true;
}
else
{
ainsideb = false;
}
if (p.CullPoint(position.X, position.Y + size.Y) == ECull.Intersect)
{
intersects = true;
}
else
{
ainsideb = false;
}
if (p.CullPoint(position.X + size.X, position.Y + size.Y) == ECull.Intersect)
{
intersects = true;
}
else
{
ainsideb = false;
}
if (ainsideb)
{
return(RSpatialRelation.AInsideB);
}
if (intersects)
{
return(RSpatialRelation.Intersect);
}
//The find the point on the circle which is closest to the center of the rectangle
Vector3 k = center - p.Position;
if (k.LengthSquared() == 0)
{
k.X = 1;
}
Vector3 d = Vector3.Normalize(k) * p.Radius;
Vector3 a = p.Position + d;
//And check if it is outside the rectangle
if (CullPoint(a.X, a.Y) == ECull.Outside)
{
return(RSpatialRelation.Outside);
}
//The we take the point furthest away from the center
Vector3 b = p.Position - d;
if (CullPoint(b.X, b.Y) == ECull.Outside)
{
return(RSpatialRelation.Intersect);
}
return(RSpatialRelation.BInsideA);
#endif
}
private static RSpatialRelation RectCircleRelation(RectangleF a, Bounding.Cylinder b)
{
float m, n;
//X Axis, first find intersections
CylP x1 = CylP.NotIntersecting;
if (Math.CircleXAxisIntersection(b.Position, b.Radius, a.Y, out m, out n))
{
if (m > a.X + a.Width || n < a.X)
{
x1 = CylP.Outside;
}
else if (m < a.X && n > a.X + a.Width)
{
x1 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
CylP x2 = CylP.NotIntersecting;
if (Math.CircleXAxisIntersection(b.Position, b.Radius, a.Y + a.Height, out m, out n))
{
if (m > a.X + a.Height || n < a.X)
{
x2 = CylP.Outside;
}
else if (m < a.X && n > a.X + a.Height)
{
x2 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
//Y Axis, first find intersections
CylP y1 = CylP.NotIntersecting;
if (Math.CircleYAxisIntersection(b.Position, b.Radius, a.X, out m, out n))
{
if (m > a.Y + a.Height || n < a.Y)
{
y1 = CylP.Outside;
}
else if (m < a.Y && n > a.Y + a.Height)
{
y1 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
CylP y2 = CylP.NotIntersecting;
if (Math.CircleYAxisIntersection(b.Position, b.Radius, a.X + a.Width, out m, out n))
{
if (m > a.Y + a.Height || n < a.Y)
{
y2 = CylP.Outside;
}
else if (m < a.Y && n > a.Y + a.Height)
{
y2 = CylP.AInsideB;
}
else
{
return(RSpatialRelation.Intersect);
}
}
if (x1 == CylP.AInsideB && x2 == CylP.AInsideB && y1 == CylP.AInsideB && y2 == CylP.AInsideB)
{
return(RSpatialRelation.AInsideB);
}
if (x1 == CylP.NotIntersecting && x2 == CylP.NotIntersecting &&
y1 == CylP.NotIntersecting && y2 == CylP.NotIntersecting &&
Intersection.Intersect(a, Math.ToVector2(b.Position)))
{
return(RSpatialRelation.BInsideA);
}
else
{
return(RSpatialRelation.Outside);
}
}
public void CylinderBInsideA()
{
Cylinder binsidea = new Cylinder(new Vector3(11, 11, 0), 3);
//Assert.AreEqual(EPlacement.BInsideA, b.Placement(binsidea));
Assert.AreEqual(ECull.Intersect, b.Cull(binsidea));
}
public void CylinderAInsideB()
{
Cylinder ainsideb = new Cylinder(new Vector3(15, 15, 0), 300);
//Assert.AreEqual(EPlacement.AInsideB, b.Placement(ainsideb));
Assert.AreEqual(ECull.Intersect, b.Cull(ainsideb));
}
public override ECull Cull(Cylinder c)
{
float l = Math.LinePointMinDistance(a, b, c.Position);
if (l >= 0 && l < c.Radius) return ECull.Intersect;
return ECull.Outside;
}
public static bool Intersect(RectangleF a, Bounding.Cylinder b, out object intersection)
{
intersection = null;
return(SpatialRelation.Relation(a, b) != RSpatialRelation.Outside);
}
public static RSpatialRelation Relation(RectangleF a, Bounding.Cylinder b)
{
return(RectCircleRelation(a, b));
}
public static BoundingBox BoundingToBox(Bounding.Cylinder a)
{
return(new BoundingBox(new Vector3(a.Position.X - a.Radius, a.Position.Y - a.Radius, a.Position.Z),
new Vector3(a.Position.X + a.Radius, a.Position.Y + a.Radius, a.Position.Z + a.Height)));
}
public static float Radius(Bounding.Cylinder bounding)
{
return(bounding.Radius);
}
public void CylinderOutside()
{
Cylinder outside = new Cylinder(new Vector3(-10, 0, 0), 10);
//Assert.AreEqual(EPlacement.Outside, b.Placement(outside));
Assert.AreEqual(ECull.Outside, b.Cull(outside));
}
public void CylinderIntersect()
{
Cylinder intersect = new Cylinder(new Vector3(9, 0, 0), 5);
//Assert.AreEqual(EPlacement.Intersect, b.Placement(intersect));
Assert.AreEqual(ECull.Intersect, b.Cull(intersect));
}