// ----------------- Collision Avoidance method -----------------
/// <summary> Method used to determine if robot is blocked </summary>
public bool isBlocked()
{
bool block = false;
double angle = Math.Atan2(heading.Y, heading.X);
double angleLeft = angle - Math.PI / 4;
double angleRight = angle + Math.PI / 4;
if (Math.Abs(angleLeft) > Math.PI)
{
angleLeft = angleLeft - Math.Sign(angleLeft) * 2 * Math.PI;
}
if (Math.Abs(angleRight) > Math.PI)
{
angleRight = angleRight - Math.Sign(angleRight) * 2 * Math.PI;
}
DoublePoint HeadingCornerLeft = new DoublePoint(Math.Cos(angleLeft), Math.Sin(angleLeft));
DoublePoint HeadingCornerRight = new DoublePoint(Math.Cos(angleRight), Math.Sin(angleRight));
DoublePoint EdgePointLeft = position + HeadingCornerLeft * Program.robotRadius;
DoublePoint EdgePointRight = position + HeadingCornerRight * Program.robotRadius;
DoublePoint EdgePointMiddle = position + heading * Program.robotRadius;
foreach (Robot neighbor in neighbors)
{
if (EdgePointLeft.DistanceTo(neighbor.getPosition()) < Program.robotRadius || EdgePointRight.DistanceTo(neighbor.getPosition()) < Program.robotRadius || EdgePointMiddle.DistanceTo(neighbor.getPosition()) < Program.robotRadius)
{
block = true;
}
}
return(block);
}
public AForge.DoublePoint getNearestPoint(AForge.DoublePoint point)
{
DoublePoint closestPoint = points[0];
double distance2P, minDistance = closestPoint.DistanceTo(point);
foreach (DoublePoint p in points)
{
distance2P = point.DistanceTo(p);
if (distance2P < minDistance)
{
closestPoint = p;
minDistance = closestPoint.DistanceTo(point);
}
}
return(closestPoint);
}
// Used by camera calibration to calculate new position of camera
public static DoublePoint translateNewPosition(DoublePoint p, DoublePoint refCoord, DoublePoint transCoord, double angleToRef, double sizeScaling)
{
double distToPoint = p.DistanceTo(refCoord);
double angleToPoint = angleX(refCoord, p);
double newX = transCoord.X - distToPoint * sizeScaling * Math.Sin(angleToPoint - angleToRef);
double newY = transCoord.Y - distToPoint * sizeScaling * Math.Cos(angleToPoint - angleToRef);
return(new DoublePoint(newX, newY));
}
public DoublePoint[] intersect(Circle c)
{
double distanceSquared = Math.Pow(center.DistanceTo(c.center), 2);
double area = Math.Sqrt((Math.Pow(radius + c.radius, 2) - distanceSquared) * (distanceSquared - Math.Pow(radius - c.radius, 2))) / 4;
double xa = (c.center.X + center.X)/2 + (c.center.X - center.X)*(Math.Pow(radius, 2) - Math.Pow(c.radius, 2)) / (2 * distanceSquared);
double xb = 2 * (c.center.Y - center.Y) * area / distanceSquared;
double ya = (c.center.Y + center.Y) / 2 + (c.center.Y - center.Y) * (Math.Pow(radius, 2) - Math.Pow(c.radius, 2)) / (2 * distanceSquared);
double yb = - 2 * (c.center.X - center.X) * area / distanceSquared;
return new DoublePoint[] { new DoublePoint(xa + xb, ya + yb), new DoublePoint(xa - xb, ya - yb)};
}
/// <summary>
/// Calculate Euclidean distance between a point and a finite line segment.
/// </summary>
///
/// <param name="point">The point to calculate the distance to.</param>
///
/// <returns>Returns the Euclidean distance between this line segment and the specified point. Unlike
/// <see cref="Line.DistanceToPoint"/>, this returns the distance from the finite segment. (0,0) is 5 units
/// from the segment (0,5)-(0,8), but is 0 units from the line through those points.</returns>
///
public double DistanceToPoint(DoublePoint point)
{
double segmentDistance;
switch (LocateProjection(point))
{
case ProjectionLocation.RayA:
segmentDistance = point.DistanceTo(start);
break;
case ProjectionLocation.RayB:
segmentDistance = point.DistanceTo(end);
break;
default:
segmentDistance = line.DistanceToPoint(point);
break;
}
;
return(segmentDistance);
}
// globalCoords refer to translation from local to global coordinate system
public static DoublePoint translateCoordToGlobal(DoublePoint p, Camera cam)
{
if (cam != null)
{
DoublePoint zero = new DoublePoint(0, 0);
double distToPoint = p.DistanceTo(zero);
double angleToPoint = angleX(zero, p);
double newX = cam.camPosition.X * Program.calibrationScaling.X - distToPoint * cam.sizeScaling * Math.Sin(angleToPoint - cam.angleToRef);
double newY = cam.camPosition.Y * Program.calibrationScaling.Y - distToPoint * cam.sizeScaling * Math.Cos(angleToPoint - cam.angleToRef);
return(new DoublePoint(newX, newY));
}
Console.WriteLine("ERROR: A failure has accured in translateCoordToGlobal(), the camera parameter had a null value.");
return(p);
}
public void calculateNextMove(DoublePoint referencePoint, int offset, DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
this.referencePoint = referencePoint;
if (robotPosition != referencePoint)
{
referenceHeading = ControlStrategy.vector2Point(robotPosition, referencePoint);
double distance = robotPosition.DistanceTo(referencePoint);
referenceSpeed = speedRegulator(distance - offset);
}
else
{
referenceSpeed = Program.neutralSpeed;
referenceHeading = heading;
}
}
public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
DoublePoint vectorBetweenRobots;
DoublePoint summaryVector = new DoublePoint(0, 0);
double distanceBetweenRobots;
double dispersionSpeed;
List <Robot> neighborsTemp = new List <Robot>();
neighborsTemp.AddRange(neighbors);
foreach (Robot neighbor in neighborsTemp)
{
if (neighbor.getDetected())
{
distanceBetweenRobots = robotPosition.DistanceTo(neighbor.getPosition());
vectorBetweenRobots = robotPosition - neighbor.getPosition();
if (distanceBetweenRobots - Program.dispersionRange < 0)
{
dispersionSpeed = speedRegulator(distanceBetweenRobots - Program.dispersionRange);
}
else
{
dispersionSpeed = 0;
}
summaryVector += (vectorBetweenRobots / distanceBetweenRobots) * dispersionSpeed;
}
}
referenceSpeed = summaryVector.EuclideanNorm();
// If the length of the heading vector is non-zero it's also safe to normalize the heading.
if (summaryVector.EuclideanNorm() != 0)
{
referenceHeading = summaryVector / summaryVector.EuclideanNorm();
}
else
{
referenceHeading = heading;
}
}
public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
if (!pointCalculated)
{
pointAcrossField = calculatePointAcrossField(robotPosition, neighbors);
nextPoint = pointAcrossField;
startPoint = robotPosition;
pointCalculated = true;
pointReached = false;
}
if (robotPosition.DistanceTo(nextPoint) < 20)
{
pointReached = true;
referenceHeading = heading;
referenceSpeed = 0;
}
else
{
goToPoint.calculateNextMove(nextPoint, robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
collisionStrategy.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
}
}
/// <summary>
/// Calculate Euclidean distance between a point and a finite line segment.
/// </summary>
///
/// <param name="point">The point to calculate the distance to.</param>
///
/// <returns>Returns the Euclidean distance between this line segment and the specified point. Unlike
/// <see cref="Line.DistanceToPoint"/>, this returns the distance from the finite segment. (0,0) is 5 units
/// from the segment (0,5)-(0,8), but is 0 units from the line through those points.</returns>
///
public double DistanceToPoint( DoublePoint point )
{
double segmentDistance;
switch ( LocateProjection( point ) )
{
case ProjectionLocation.RayA:
segmentDistance = point.DistanceTo( start );
break;
case ProjectionLocation.RayB:
segmentDistance = point.DistanceTo( end );
break;
default:
segmentDistance = line.DistanceToPoint( point );
break;
};
return segmentDistance;
}
public int distance2Middle(AForge.DoublePoint point)
{
return((int)middlePoint.DistanceTo(point));
}