public InCone ( |
||
tip | Tip of the cone. | |
direction | double | Cone's direction as radian. |
radius | int | Cone's radius. |
angle | double | Cone's angle as radian. |
Résultat | bool |
public bool InCone(Position tip, double direction, int radius, double angle)
{
// Cap angle at +180°, to prevent possibly unexpected inverting
// behavior. (The cone would go into the opposite direction.)
angle = Math.Min(Math.PI, Math.Abs(angle));
var halfAngle = angle / 2;
var tx1 = tip.X + (Math.Cos(-halfAngle + direction) * radius);
var ty1 = tip.Y + (Math.Sin(-halfAngle + direction) * radius);
var tx2 = tip.X + (Math.Cos(halfAngle + direction) * radius);
var ty2 = tip.Y + (Math.Sin(halfAngle + direction) * radius);
var tx3 = tip.X;
var ty3 = tip.Y;
var px = X;
var py = Y;
// Check if position is inside the triangle part of the cone.
// http://stackoverflow.com/questions/2049582/how-to-determine-a-point-in-a-2d-triangle
var A = 1.0 / 2.0 * (-ty2 * tx3 + ty1 * (-tx2 + tx3) + tx1 * (ty2 - ty3) + tx2 * ty3);
var sign = (A < 0 ? -1 : 1);
var s = (ty1 * tx3 - tx1 * ty3 + (ty3 - ty1) * px + (tx1 - tx3) * py) * sign;
var t = (tx1 * ty2 - ty1 * tx2 + (ty1 - ty2) * px + (tx2 - tx1) * py) * sign;
var isInTriangle = (s > 0 && t > 0 && (s + t) < 2 * A * sign);
if (isInTriangle)
return true;
// Check if position is on the triangle part's side of the cone.
// If it is, we can stop, since we already checked for the
// triangle. The only way the position could now be in the cone
// is if it was on the circle's side.
// http://stackoverflow.com/a/3461533/1171898
var isOnTriangleSide = (((tx2 - tx1) * (py - ty1) - (ty2 - ty1) * (px - tx1)) > 0);
if (isOnTriangleSide)
return false;
// Check if position is inside the circle.
var tx4 = (int)((tx1 + tx2) / 2);
var ty4 = (int)((ty1 + ty2) / 2);
var circleRadius = Math.Sqrt(Math.Pow(tx2 - tx1, 2) + Math.Pow(ty2 - ty1, 2)) / 2;
var targetDistance = Math.Sqrt(Math.Pow(tx4 - px, 2) + Math.Pow(ty4 - py, 2));
var isInCircle = (targetDistance < circleRadius);
return isInCircle;
}
public void InCone() { var centerPos = new Position(0, 0); var topPos = new Position(0, 500); var bottomPos = new Position(0, -500); var leftPos = new Position(-500, 0); var rightPos = new Position(500, 0); var up = MabiMath.DegreeToRadian(90); var down = MabiMath.DegreeToRadian(270); var left = MabiMath.DegreeToRadian(180); var right = MabiMath.DegreeToRadian(0); Assert.Equal(true, topPos.InCone(centerPos, up, 600, MabiMath.DegreeToRadian(10))); Assert.Equal(true, bottomPos.InCone(centerPos, down, 600, MabiMath.DegreeToRadian(10))); Assert.Equal(true, leftPos.InCone(centerPos, left, 600, MabiMath.DegreeToRadian(10))); Assert.Equal(true, rightPos.InCone(centerPos, right, 600, MabiMath.DegreeToRadian(10))); Assert.Equal(true, topPos.InCone(centerPos, up, 500, MabiMath.DegreeToRadian(10))); Assert.Equal(false, topPos.InCone(centerPos, up, 500, MabiMath.DegreeToRadian(180))); Assert.Equal(true, topPos.InCone(centerPos, up, 600, MabiMath.DegreeToRadian(180))); Assert.Equal(true, topPos.InCone(centerPos, up, 1000, MabiMath.DegreeToRadian(180))); Assert.Equal(true, topPos.InCone(centerPos, up, 1000, MabiMath.DegreeToRadian(190))); Assert.Equal(true, topPos.InCone(centerPos, up, 1000, MabiMath.DegreeToRadian(-190))); Assert.Equal(true, topPos.InCone(centerPos, up, 1000, MabiMath.DegreeToRadian(9999))); Assert.Equal(true, topPos.InCone(centerPos, up, 1000, MabiMath.DegreeToRadian(-9999))); Assert.Equal(false, topPos.InCone(centerPos, MabiMath.DegreeToRadian(45), 2000, MabiMath.DegreeToRadian(45))); Assert.Equal(true, topPos.InCone(centerPos, MabiMath.DegreeToRadian(68), 2000, MabiMath.DegreeToRadian(45))); }