public void PtsToMm_Coordinates_Null()
{
BallCoordinates expected = null;
BallCoordinates actual = _testAsset.PtsToMm(expected);
Assert.AreEqual(actual, expected);
}
/// <summary>
/// Stabilization method used to remove shaking
/// </summary>
/// <param name="newCoordinates">New coordinates</param>
/// <param name="storedCoordinates">Last known stored coordinates</param>
/// <returns>Approximate Coordinates without shaking</returns>
public BallCoordinates Stabilize(BallCoordinates newCoordinates, BallCoordinates storedCoordinates)
{
if (newCoordinates.IsDefined)
{
_undefinedCoordinatesCounter = 0;
_lastGoodCoordinates = newCoordinates;
BallCoordinates coordinates = RemoveShaking(newCoordinates, storedCoordinates);
return(coordinates);
}
else //new coordinates are undefined
{
if (_undefinedCoordinatesCounter < MAX_UNDEFINED_THRESHOLD)
{
_undefinedCoordinatesCounter++;
if (storedCoordinates.IsDefined)
{
_lastGoodCoordinates = storedCoordinates;
}
}
else
{
_lastGoodCoordinates = newCoordinates;
}
return(_lastGoodCoordinates);
}
}
/// <summary>
/// Calculate actual linear movement (Y Axe) for current rod to perform
/// Is based on desired linear move type
/// </summary>
/// <param name="rod">Current rod</param>
/// <param name="respondingPlayer">Responding player in current rod (1 based index)</param>
/// <param name="bfc">Ball Future coordinates</param>
/// <param name="desiredLinearMove">Desired Linear Move Type</param>
/// <returns>New rod coordinate to move to (Axe Y)</returns>
protected int CalculateNewRodCoordinate(IRod rod, int respondingPlayer, BallCoordinates bfc, eLinearMove desiredLinearMove)
{
//Define actual desired rod coordinate to move to
//NOTE: responding player might be undefined will be -1
switch (desiredLinearMove)
{
case eLinearMove.BALL_Y:
return(_helper.LocateRespondingPlayer(rod, bfc.Y, respondingPlayer));
// return bfc.Y - _helper.CalculateCurrentPlayerYCoordinate(rod, _currentRodYCoordinate[rod.RodType], respondingPlayer);
//case eLinearMove.LEFT_BALL_DIAMETER:
// return (-1) * 2 * BALL_RADIUS;
//case eLinearMove.RIGHT_BALL_DIAMETER:
// return 2 * BALL_RADIUS;
case eLinearMove.VECTOR_BASED:
if (rod.Intersection.IsDefined)
{
int stopperPosition = _helper.LocateRespondingPlayer(rod, rod.Intersection.Y, respondingPlayer);
return((IsBallNearTheRod(bfc, rod, stopperPosition)) ? stopperPosition : rod.State.DcPosition);
}
//return rod.IntersectionY - _helper.CalculateCurrentPlayerYCoordinate(rod, _currentRodYCoordinate[rod.RodType], respondingPlayer);
return(rod.State.DcPosition);
//case eLinearMove.BEST_EFFORT:
// return rod.BestEffort;
default:
return(rod.State.DcPosition);
}
}
/// <summary>
/// Only if ball is near the rod new location will be sent to communication layer.
/// Method is based on distance, ball speed and BALL_DISTANCE_FACTOR, BALL_MAX_SPEED
/// This method added to support Amit's requirement, to be able to set new position of rod based on distance to it.
/// </summary>
/// <param name="ballCoords">Ball Future Coordinates to respond to</param>
/// <param name="currentRod">Current responding rod</param>
/// <returns>[True] if we are near the rod or high speed, [False] otherwise</returns>
protected bool IsBallNearTheRod(BallCoordinates ballCoords, IRod currentRod, int stopperPosition)
{
//By changing this parameter we set system sensitivity
const double BALL_DISTANCE_FACTOR = 3000;
const double BALL_MAX_SPEED = 100;
if (!ballCoords.IsDefined)
{
return(false);
}
double distanceFactor = Convert.ToDouble(TABLE_WIDTH) / Math.Abs(ballCoords.X - currentRod.RodXCoordinate);
double speedFactor = 1;
if (ballCoords.Vector != null && ballCoords.Vector.IsDefined)
{
speedFactor += Math.Abs(ballCoords.Vector.X / BALL_MAX_SPEED);
}
double diff = (double)Math.Abs(currentRod.State.DcPosition - stopperPosition) / (double)(currentRod.PlayerCount);
bool result = (distanceFactor * diff * speedFactor > BALL_DISTANCE_FACTOR);
return(result);
}
public void MmToPts_Coordinates_Not_Defined()
{
BallCoordinates expected = new BallCoordinates(DateTime.Now);
BallCoordinates actual = _testAsset.MmToPts(expected);
Assert.AreEqual(actual, expected);
}
/// <summary>
/// Ricochet coordinate calculation
/// </summary>
/// <param name="ballCoordinates">Ball Coordinates</param>
/// <returns>Ricochet Ball Coordinates</returns>
public BallCoordinates Ricochet(BallCoordinates ballCoordinates)
{
try
{
Coordinates2D intersectionPoint = FindNearestIntersectionPoint(ballCoordinates);
//Marks.DrawRicochetMark(Convert.ToInt32(intersectionPoint.X), Convert.ToInt32(intersectionPoint.Y), true);
DateTime ricocheTime = FindRicochetTime(ballCoordinates, intersectionPoint);
Vector2D vector = FindIntersectionVector(ballCoordinates.Vector, intersectionPoint);
BallCoordinates coordinates = new BallCoordinates(
Convert.ToInt32(intersectionPoint.X),
Convert.ToInt32(intersectionPoint.Y), ricocheTime);
coordinates.Vector = vector;
return(coordinates);
}
catch (Exception e)
{
throw new NotSupportedException(String.Format(
"[{0}] Failed to find ricochet coordinates. Reason: {1}",
MethodBase.GetCurrentMethod().Name, e.Message));
}
}
/// <summary>
/// Main working method - runs in loop on Start
/// </summary>
public override void Job()
{
Initialize();
//Detach from streamer
_publisher.Detach(this);
//get current ball coordinates
BallCoordinates coordinates = SampleCoordinates();
//show current ball coordinates on screen and GUI
System.Drawing.PointF p = TransformAgent.Data.InvertTransform(new System.Drawing.PointF(_x, _y));
Marks.DrawBall(new Point(p.X, p.Y), _ballRadius);
Statistics.TryUpdateBasicImageProcessingInfo(String.Format("Generated coordinates: {0}x{1}", _x, _y));
//set current coordinates to update
ImagingData.BallCoords = coordinates;
//set current coordinates and publish new ball coordinates
BallLocationUpdater.UpdateAndNotify();
//attach back to streamer
_publisher.Attach(this);
}
public void FindNearestIntersectionPoint_NoMove_from_50_50()
{
_initialCoordinates = new BallCoordinates(50, 50, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(0, 0);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
}
/// <summary>
/// Convert BallCoordinates in points to BallCoordinates in mm
/// </summary>
/// <param name="pts">BallCoordinates in Points</param>
/// <returns>BallCoordinates in Milimeters</returns>
public BallCoordinates PtsToMm(BallCoordinates pts)
{
BallCoordinates mmCoords = null;
if (pts != null && pts.IsDefined)
{
int xMm = pts.X * XMaxMm / XMaxPts;
int yMm = pts.Y * YMaxMm / YMaxPts;
mmCoords = new BallCoordinates(xMm, yMm, pts.Timestamp);
}
else
{
return(pts);
}
if (pts.Vector != null && pts.Vector.IsDefined)
{
double xMm = pts.Vector.X * (double)XMaxMm / (double)XMaxPts;
double yMm = pts.Vector.Y * (double)YMaxMm / (double)YMaxPts;
mmCoords.Vector = new Vector2D(xMm, yMm);
}
else
{
mmCoords.Vector = pts.Vector;
}
return(mmCoords);
}
/// <summary>
/// Convert BallCoordinates in mm to BallCoordinates in points
/// </summary>
/// <param name="mm">BallCoordinates in Milimeters</param>
/// <returns>BallCoordinates in Points</returns>
public BallCoordinates MmToPts(BallCoordinates mm)
{
BallCoordinates pointsCoords = null;
if (mm != null && mm.IsDefined)
{
int xMm = mm.X * XMaxPts / XMaxMm;
int yMm = mm.Y * YMaxPts / YMaxMm;
pointsCoords = new BallCoordinates(xMm, yMm, mm.Timestamp);
}
else
{
return(mm);
}
if (mm.Vector != null && mm.Vector.IsDefined)
{
double xMm = mm.Vector.X * (double)XMaxPts / (double)XMaxMm;
double yMm = mm.Vector.Y * (double)YMaxPts / (double)YMaxMm;
pointsCoords.Vector = new Vector2D(xMm, yMm);
}
else
{
pointsCoords.Vector = mm.Vector;
}
return(pointsCoords);
}
/// <summary>
/// Main Decision Flow
/// </summary>
/// <param name="currentCoordinates"></param>
public void Flow(BallCoordinates currentCoordinates)
{
//Calculate Actual Possible Action Time
DateTime timeOfAction = DateTime.Now + DELAYS;
//Calculate ball future coordinates
FindBallFutureCoordinates(currentCoordinates, timeOfAction);
//Calculate dynamic sectors
if (currentCoordinates.IsDefined && currentCoordinates.Vector.IsDefined)
{
CalculateDynamicSectors(currentCoordinates.X, currentCoordinates.Vector.X);
}
else
{
throw new NotSupportedException("Currently not defined coordinates are not supported by Pre-Decision Flow");
}
//Calculate Rod Intersection with a ball for all rods
CalculateSectorIntersection(currentCoordinates);
//Take decision for each rod
foreach (Rod rod in _rods.Values)
{
_decisionTree.Decide(rod, _bfc);
}
}
/// <summary>
/// Callculate Rod Intersection with current rod
/// </summary>
/// <param name="rod">Current rod to calculate intersection with</param>
/// <param name="currentCoordinates">Current ball coordinates to calculate intersection</param>
private void CalculateSectorIntersection(Rod rod, BallCoordinates currentCoordinates)
{
int xintersection = (rod.RodXCoordinate > currentCoordinates.X) ?
Convert.ToInt32(rod.RodXCoordinate - rod.DynamicSector / 2.0) :
Convert.ToInt32(rod.RodXCoordinate + rod.DynamicSector / 2.0);
double inms = (xintersection - currentCoordinates.X) / currentCoordinates.Vector.X;
if (inms >= 0)
{
TimeSpan intersectionTime = TimeSpan.FromMilliseconds(inms);
int yintersection = Convert.ToInt32(currentCoordinates.Vector.Y * intersectionTime.TotalMilliseconds + currentCoordinates.Y);
if (IsCoordinatesYInRange(yintersection))
{
rod.SetBallIntersection(xintersection, yintersection, currentCoordinates.Timestamp + intersectionTime);
}
else
{
//ToDo: Call ricoshet algorithm with:
BallCoordinates ricoshetCoordiantes = null;
//ricoshetCoordiantes = RicoshetAlgorithm(currentCoordinates);
CalculateSectorIntersection(rod, ricoshetCoordiantes);
}
}
else
{
rod.SetBallIntersection();
}
}
/// <summary>
/// Callculate Rod Intersection for all rods
/// </summary>
/// <param name="currentCoordinates">Current ball coordinates to calculate intersection</param>
private void CalculateSectorIntersection(BallCoordinates currentCoordinates)
{
foreach (Rod rod in _rods.Values)
{
CalculateSectorIntersection(rod, currentCoordinates);
}
}
/// <summary>
/// Calculate Ball Future Coordinates in actual time system can responce
/// </summary>
/// <param name="currentCoordinates"><Current ball coordinates/param>
/// <param name="actionTime">Actual system responce time</param>
private void FindBallFutureCoordinates(BallCoordinates currentCoordinates, DateTime actionTime)
{
TimeSpan deltaT = actionTime - currentCoordinates.Timestamp;
if (currentCoordinates.IsDefined && currentCoordinates.Vector.IsDefined)
{
int xfc = Convert.ToInt32(currentCoordinates.Vector.X * deltaT.TotalMilliseconds + currentCoordinates.X);
int yfc = Convert.ToInt32(currentCoordinates.Vector.Y * deltaT.TotalMilliseconds + currentCoordinates.Y);
if (IsCoordinatesInRange(xfc, yfc))
{
_bfc = new BallCoordinates(xfc, yfc, actionTime);
}
else
{
//ToDo: Call ricoshet algorithm with:
BallCoordinates ricoshetCoordiantes = null;
//ricoshetCoordiantes = RicoshetAlgorithm(currentCoordinates);
FindBallFutureCoordinates(ricoshetCoordiantes, actionTime);
}
}
else
{
throw new NotSupportedException("Currently not defined coordinates are not supported by Pre-Decision Flow");
}
}
public void MmToPts_Coordinates_Null()
{
BallCoordinates expected = null;
BallCoordinates actual = _testAsset.MmToPts(expected);
Assert.AreEqual(actual, expected);
}
/// <summary>
/// Verify coordinates and vector are defined, not null and not both vector x and y are 0
/// </summary>
/// <param name="ballCoordinates">Ball Coordianates with vector</param>
/// <exception cref="NotSupportedException">Thrown in case vector or coordinates are not defined or are NULL, or both vector x and y are 0</exception>
private void VerifyBallCoordinatesAndVectorInput(BallCoordinates ballCoordinates)
{
if (ballCoordinates == null)
{
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because ball coordinates are NULL", MethodBase.GetCurrentMethod().Name));
}
if (!ballCoordinates.IsDefined)
{
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because ball coordinates are not defined", MethodBase.GetCurrentMethod().Name));
}
if (ballCoordinates.Vector == null)
{
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is NULL", MethodBase.GetCurrentMethod().Name));
}
if (!ballCoordinates.Vector.IsDefined)
{
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is undefined", MethodBase.GetCurrentMethod().Name));
}
if (ballCoordinates.Vector.X == 0 && ballCoordinates.Vector.Y == 0)
{
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is 0x0", MethodBase.GetCurrentMethod().Name));
}
}
/// <summary>
/// Convert BallCoordinates in points to BallCoordinates in mm
/// </summary>
/// <param name="pts">BallCoordinates in Points</param>
/// <returns>BallCoordinates in Millimeters</returns>
public BallCoordinates PtsToMm(BallCoordinates pts)
{
BallCoordinates mmCoords = null;
if (pts != null && pts.IsDefined)
{
int xMm = pts.X * XMaxMm / XMaxPts;
int yMm = pts.Y * YMaxMm / YMaxPts;
mmCoords = new BallCoordinates(xMm, yMm, pts.Timestamp);
}
else
{
return pts;
}
if (Vector2D.NotNullAndDefined(pts.Vector))
{
double xMm = pts.Vector.X * (double)XMaxMm / (double)XMaxPts;
double yMm = pts.Vector.Y * (double)YMaxMm / (double)YMaxPts;
mmCoords.Vector = new Vector2D(xMm, yMm);
}
else
{
mmCoords.Vector = pts.Vector;
}
return mmCoords;
}
public void FindBallFutureCoordinates_RicochetTestThree()
{
//arrange
DateTime timeStamp = DateTime.Now;
BallCoordinates currentCoordinates = new BallCoordinates(300, 200, timeStamp);
currentCoordinates.Vector = new Vector2D(100, -100);
DateTime actionTime = timeStamp + TimeSpan.FromSeconds(10);
BallCoordinates ricocheteCoordinates = new BallCoordinates(500, 0, timeStamp + TimeSpan.FromSeconds(2));
ricocheteCoordinates.Vector = new Vector2D(100, -100);
_surveyorMock.IsCoordinatesInRange(Arg.Any <int>(), Arg.Any <int>()).Returns(false, true);
_ricochetMock.Ricochet(Arg.Any <BallCoordinates>()).Returns(ricocheteCoordinates);
double deltaT = (actionTime - ricocheteCoordinates.Timestamp).TotalSeconds;
int expectedX = Convert.ToInt32(ricocheteCoordinates.X + ricocheteCoordinates.Vector.X * deltaT);
int expectedY = Convert.ToInt32(ricocheteCoordinates.Y + ricocheteCoordinates.Vector.Y * deltaT);
double expectedVectorX = ricocheteCoordinates.Vector.X;
double expectedVectorY = ricocheteCoordinates.Vector.Y;
//act
BallCoordinates actualResult = _testAsset.FindBallFutureCoordinates(currentCoordinates, actionTime);
//assert
Assert.AreEqual(actualResult.X, expectedX);
Assert.AreEqual(actualResult.Y, expectedY);
Assert.AreEqual(actualResult.Vector.X, expectedVectorX);
Assert.AreEqual(actualResult.Vector.Y, expectedVectorY);
Assert.AreEqual(actualResult.Timestamp, actionTime);
}
/// <summary>
/// Stabilization method used to remove shaking
/// </summary>
/// <param name="newCoordinates">New coordinates</param>
/// <param name="storedCoordinates">Last known stored coordinates</param>
/// <returns>Approximate Coordinates without shaking</returns>
public BallCoordinates Stabilize(BallCoordinates newCoordinates, BallCoordinates storedCoordinates)
{
if (newCoordinates.IsDefined)
{
_undefinedCoordinatesCounter = 0;
_lastGoodCoordinates = newCoordinates;
BallCoordinates coordinates = RemoveShaking(newCoordinates, storedCoordinates);
return coordinates;
}
else //new coordinates are undefined
{
if (_undefinedCoordinatesCounter < MAX_UNDEFINED_THRESHOLD)
{
_undefinedCoordinatesCounter++;
if (storedCoordinates.IsDefined)
_lastGoodCoordinates = storedCoordinates;
}
else
{
_lastGoodCoordinates = newCoordinates;
}
return _lastGoodCoordinates;
}
}
/// <summary>
/// Calculate Rod Intersection with current rod.
///
/// The Main Idea of this method is to set Intersection Property of
/// ControlRod Class as Intersection Coordinate with Timestamp.
/// In current case both time and coordinates should be defined
/// if intersection exists or both undefined because if there is no
/// intersection found the timestamp is also not relevant.
/// </summary>
/// <param name="currentCoordinates">Current ball coordinates to calculate intersection</param>
public void CalculateSectorIntersection(BallCoordinates currentCoordinates)
{
try
{
//If unable to calculate OR no intersection - set Intersection as undefined and exit
if (currentCoordinates == null || !currentCoordinates.IsDefined ||
currentCoordinates.Vector == null || !currentCoordinates.Vector.IsDefined ||
currentCoordinates.Vector.X == 0)
{
Intersection = new TimedPoint();
return;
}
/*
* After practical simulations it seems we don't want to define
* intersection with X using sector definition. Anyway if this need
* to be changed, following code can be used:
*
* int xintersection = (RodXCoordinate > currentCoordinates.X) ?
* Convert.ToInt32(RodXCoordinate - DynamicSector / 2.0) :
* Convert.ToInt32(RodXCoordinate + DynamicSector / 2.0);
*
*/
int xintersection = RodXCoordinate;
//find intersection time using: T = dX/V in seconds
double intersectionTimestamp = (xintersection - currentCoordinates.X) / currentCoordinates.Vector.X;
//If intersecion will happen in the future but before maximum future time limit
if (intersectionTimestamp >= 0 && intersectionTimestamp < PredictIntersectionMaxTimespan)
{
TimeSpan intersectionTime = TimeSpan.FromSeconds(intersectionTimestamp);
int yintersection = Convert.ToInt32(currentCoordinates.Vector.Y * intersectionTime.TotalSeconds + currentCoordinates.Y);
if (_surveyor.IsCoordinatesYInRange(yintersection))
{
Intersection = new TimedPoint(xintersection, yintersection, currentCoordinates.Timestamp + intersectionTime);
// Log.Common.Debug(String.Format("[{0}] Found intersection point with rod [{1}]: [{2}x{3}]",
// MethodBase.GetCurrentMethod().Name, _rodType, xintersection, yintersection));
}
else
{
BallCoordinates ricoshetCoordiantes = _vectorUtils.Ricochet(currentCoordinates);
CalculateSectorIntersection(ricoshetCoordiantes);
}
}
else
{
//Intersection found is from the past or not in the near future and is irrelevant
Intersection = new TimedPoint();
}
}
catch (Exception e)
{
Intersection = new TimedPoint();
Log.Print(String.Format("Unable to calculate rod intersection. Reason: {0}",
e.Message), eCategory.Error, LogTag.DECISION);
}
}
/// <summary>
/// Choose player to respond on current rod and action to perform
/// </summary>
/// <param name="rod">Current rod</param>
/// <param name="bfc">Ball Future Coordinates</param>
/// <param name="respondingPlayer">Responding Player index (1 based) on current rod [out]</param>
/// <returns>Rod Action to be performed</returns>
protected RodAction DefineActionAndRespondingPlayer(IRod rod, BallCoordinates bfc, out int respondingPlayer)
{
if (rod == null)
{
throw new ArgumentException(String.Format(
"[{0}] Unable to define action and responding player while rod argument is NULL!",
MethodBase.GetCurrentMethod().Name));
}
if (bfc == null || !bfc.IsDefined)
{
throw new ArgumentException(String.Format(
"[{0}] Unable to define action and responding player while ball coordinates are NULL or UNDEFINED!",
MethodBase.GetCurrentMethod().Name));
}
RodAction action = null;
respondingPlayer = -1;
switch (_helper.IsBallInSector(bfc.X, rod.RodXCoordinate, rod.DynamicSector))
{
//Ball is in Current Rod Sector
case eXPositionSectorRelative.IN_SECTOR:
action = SubTree.Decide(rod, bfc);
respondingPlayer = SubTree.RespondingPlayer;
break;
/* OLD :
* //The Big Sub Tree
* action = EnterDecisionTreeBallInSector(rod, bfc, out respondingPlayer);
*/
//Ball is ahead of Current Rod Sector
case eXPositionSectorRelative.AHEAD_SECTOR:
//Ball Vector Direction is TO Current Rod and we have intersection point
if (_helper.IsBallVectorToRod(bfc.Vector) &&
rod.Intersection.IsDefined)
{
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.VECTOR_BASED);
//Define responding player index
BallYPositionToPlayerYCoordinate(bfc.Y, rod);
respondingPlayer = this.RespondingPlayer;
}
else
{
//Ball Vector Direction is FROM Current Rod
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.BEST_EFFORT);
}
break;
//Ball is behind Current Rod Sector
case eXPositionSectorRelative.BEHIND_SECTOR:
action = new RodAction(rod.RodType, eRotationalMove.RISE, eLinearMove.BEST_EFFORT);
break;
}
return(action);
}
/// <summary>
/// Find ball location in image in defined area
/// </summary>
/// <param name="image">Image to detect ball on</param>
/// <param name="imageTimestamp">Image timestamp</param>
/// <param name="isFullAreaSearch">[True] search will be performed in all image = default,
/// otherwise area will be defined based on last stored location adn maximum possible speed
/// </param>
/// <returns>[True] if ball location found, [False] otherwise</returns>
private bool FindBallLocation(Image <Gray, byte> image, DateTime imageTimestamp, bool isFullAreaSearch = true)
{
image = image.Clone();
int additionalOffsetX = 0;
int additionalOffsetY = 0;
string area = (isFullAreaSearch) ? "FULL" : "SELECTED";
if (!isFullAreaSearch)
{
TimeSpan deltaT = imageTimestamp - _storedLocation.Timestamp;
double searchRadius = MAX_BALL_SPEED * deltaT.TotalSeconds;
int maxX = (Convert.ToInt32(_storedLocation.X + searchRadius) > image.Width) ? image.Width : Convert.ToInt32(_storedLocation.X + searchRadius);
int maxY = (Convert.ToInt32(_storedLocation.Y + searchRadius) > image.Height) ? image.Height : Convert.ToInt32(_storedLocation.Y + searchRadius);
additionalOffsetX = (Convert.ToInt32(_storedLocation.X - searchRadius) < 0) ? 0 : Convert.ToInt32(_storedLocation.X - searchRadius);
additionalOffsetY = (Convert.ToInt32(_storedLocation.Y - searchRadius) < 0) ? 0 : Convert.ToInt32(_storedLocation.Y - searchRadius);
List <System.Drawing.PointF> croppingPoints = new List <System.Drawing.PointF>()
{
new System.Drawing.PointF(maxX, maxY),
new System.Drawing.PointF(maxX, additionalOffsetY),
new System.Drawing.PointF(additionalOffsetX, maxY),
new System.Drawing.PointF(additionalOffsetX, additionalOffsetY)
};
image = base.Crop(image, croppingPoints);
//croppedImage.Save("test\\" + imageTimestamp.ToString("mm_ss_fff") + ".png");
}
CircleF[] pos = base.DetectCircles(image, _calibrator.Radius, _calibrator.ErrorRate * 2, _calibrator.Radius * 5, 250, 37, 1.5);//300, 40, 1.3);
if (pos.Length > 0)
{
_storedLocation = new Location(pos[0].Center.X + additionalOffsetX, pos[0].Center.Y + additionalOffsetY, imageTimestamp);
int x = _storedLocation.X + OffsetX;
int y = _storedLocation.Y + OffsetY;
Log.Image.Info(String.Format("[{0}] Possible ball location in {1} area: {2}x{3}",
MethodBase.GetCurrentMethod().Name, area, x, y));
UpdateMarkup(Helpers.eMarkupKey.BALL_CIRCLE_MARK, new System.Windows.Point(x, y), Convert.ToInt32(pos[0].Radius));
System.Drawing.PointF coordinates = Transformation.Transform(new System.Drawing.PointF(x, y));
this.LastBallCoordinates = new BallCoordinates(Convert.ToInt32(coordinates.X), Convert.ToInt32(coordinates.Y), imageTimestamp);
IsBallLocationFound = true;
UpdateStatistics(Helpers.eStatisticsKey.BallCoordinates,
String.Format("Ball coordinates: {0}x{1}", LastBallCoordinates.X, LastBallCoordinates.Y));
return(true);
}
Log.Image.Debug(String.Format("[{0}] Ball not found in {1} area", MethodBase.GetCurrentMethod().Name, area));
IsBallLocationFound = false;
return(false);
}
public void FindNearestIntersectionPoint_MoveLeftUp_LeftFirst_from_50_50()
{
_initialCoordinates = new BallCoordinates(50, 50, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(-50, 100);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(XMIN, 150);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
/// <summary>
/// Remove Shaking method
/// </summary>
/// <param name="newCoordinates">New coordinates</param>
/// <param name="lastKnownCoordinates">Last known coordinates</param>
/// <returns>Approximate Coordinates without shaking</returns>
private BallCoordinates RemoveShaking(BallCoordinates newCoordinates, BallCoordinates lastKnownCoordinates)
{
if (lastKnownCoordinates.IsDefined)
{
if (IsInRadiusRange(newCoordinates, lastKnownCoordinates))
{
return(new BallCoordinates(lastKnownCoordinates.X, lastKnownCoordinates.Y, newCoordinates.Timestamp));
}
}
return(newCoordinates);
}
public void FindNearestIntersectionPoint_MoveDown_from_100_50()
{
_initialCoordinates = new BallCoordinates(100, 50, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(0, -100);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(100, YMIN);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
public void FindNearestIntersectionPoint_MoveRightDown_RightFirst_from_700_100()
{
_initialCoordinates = new BallCoordinates(700, 100, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(100, -50);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(XMAX, 50);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
public void FindRicochetTime_Vector_100_50()
{
DateTime now = DateTime.Now;
Coordinates2D intersection = new Coordinates2D(XMAX, 350);
BallCoordinates ballCoordinates = new BallCoordinates(700, 300, now);
ballCoordinates.Vector = new Vector2D(100, 50);
DateTime actualTime = _testAsset.FindRicochetTime(ballCoordinates, intersection);
DateTime expected = now + TimeSpan.FromSeconds(1.0);
Assert.AreEqual(expected, actualTime);
}
public void FindRicochetTime_Vector_minus140_minus20()
{
DateTime now = DateTime.Now;
Coordinates2D intersection = new Coordinates2D(XMIN, 200);
BallCoordinates ballCoordinates = new BallCoordinates(700, 300, now);
ballCoordinates.Vector = new Vector2D(-140, -20);
DateTime actualTime = _testAsset.FindRicochetTime(ballCoordinates, intersection);
DateTime expected = now + TimeSpan.FromSeconds(5.0);
Assert.AreEqual(expected, actualTime);
}
public void FindBallFutureCoordinates_ActionTimeIsEarlierThanTimeStamp()
{
//arrange
DateTime timeStamp = DateTime.Now;
BallCoordinates currentCoordinates = new BallCoordinates(100, 100, timeStamp);
//act
DateTime actionTime = timeStamp - TimeSpan.FromSeconds(5);
//assert
_testAsset.FindBallFutureCoordinates(currentCoordinates, actionTime);
}
public void VectorCalculationAlgorithm_Stored_BallCoordinates_Not_Defined_Test()
{
BallCoordinates ballCoordinates = new BallCoordinates(DateTime.Now);
PrivateObject po = new PrivateObject(typeof(VectorCalculationUnit), _mockPublisher,null);
Vector2D expected = new Vector2D();
Vector2D actual = (Vector2D)po.Invoke("VectorCalculationAlgorithm",
new Type[] { typeof(BallCoordinates) },
new Object[] { ballCoordinates },
new Type[] { typeof(BallCoordinates) });
Assert.AreEqual(actual, actual);
}
/// <summary>
/// Choose player to respond on current rod and action to perform
/// </summary>
/// <param name="rod">Current rod</param>
/// <param name="bfc">Ball Future Coordinates</param>
/// <param name="respondingPlayer">Responding Player index (1 based) on current rod [out]</param>
/// <returns>Rod Action to be performed</returns>
protected RodAction DefineActionAndRespondingPlayer(IRod rod, BallCoordinates bfc, out int respondingPlayer)
{
if (rod == null)
throw new ArgumentException(String.Format(
"[{0}] Unable to define action and responding player while rod argument is NULL!",
MethodBase.GetCurrentMethod().Name));
if (bfc == null || !bfc.IsDefined)
throw new ArgumentException(String.Format(
"[{0}] Unable to define action and responding player while ball coordinates are NULL or UNDEFINED!",
MethodBase.GetCurrentMethod().Name));
RodAction action = null;
respondingPlayer = -1;
switch (_helper.IsBallInSector(bfc.X, rod.RodXCoordinate, rod.DynamicSector))
{
//Ball is in Current Rod Sector
case eXPositionSectorRelative.IN_SECTOR:
action = SubTree.Decide(rod, bfc);
respondingPlayer = SubTree.RespondingPlayer;
break;
/* OLD :
* //The Big Sub Tree
* action = EnterDecisionTreeBallInSector(rod, bfc, out respondingPlayer);
*/
//Ball is ahead of Current Rod Sector
case eXPositionSectorRelative.AHEAD_SECTOR:
//Ball Vector Direction is TO Current Rod and we have intersection point
if (_helper.IsBallVectorToRod(bfc.Vector) &&
rod.Intersection.IsDefined)
{
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.VECTOR_BASED);
//Define responding player index
BallYPositionToPlayerYCoordinate(bfc.Y, rod);
respondingPlayer = this.RespondingPlayer;
}
else
{
//Ball Vector Direction is FROM Current Rod
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.BEST_EFFORT);
}
break;
//Ball is behind Current Rod Sector
case eXPositionSectorRelative.BEHIND_SECTOR:
action = new RodAction(rod.RodType, eRotationalMove.RISE, eLinearMove.BEST_EFFORT);
break;
}
return action;
}
public void MmToPts_Coordinates_Only()
{
BallCoordinates ballCoordinates = new BallCoordinates(50, 50, DateTime.Now);
int xMm = ballCoordinates.X * X_MAX_PTS / X_MAX_MM;
int yMm = ballCoordinates.Y * Y_MAX_PTS / Y_MAX_MM;
BallCoordinates expected = new BallCoordinates(xMm, yMm, ballCoordinates.Timestamp);
BallCoordinates actual = _testAsset.MmToPts(ballCoordinates);
Assert.AreEqual(actual.X, expected.X);
Assert.AreEqual(actual.Y, expected.Y);
}
public static byte[] SerializeBallCoordinates(Vector3 position, Vector3 direction, float speed)
{
var builder = new FlatBufferBuilder(1);
BallCoordinates.StartBallCoordinates(builder);
BallCoordinates.AddPosition(builder, Vec3.CreateVec3(builder, position.x, position.y, position.z));
BallCoordinates.AddDirection(builder, Vec3.CreateVec3(builder, direction.x, direction.y, direction.z));
BallCoordinates.AddSpeed(builder, speed);
var ball = BallCoordinates.EndBallCoordinates(builder);
BallCoordinates.FinishBallCoordinatesBuffer(builder, ball);
return(builder.SizedByteArray());
}
public void VectorCalculationAlgorithm_BallCoordinates_Not_Defined_Test()
{
BallCoordinates ballCoordinates = new BallCoordinates(DateTime.Now);
PrivateObject po = new PrivateObject(typeof(VectorCalculationUnit), _mockPublisher, null);
Vector2D expected = new Vector2D();
Vector2D actual = (Vector2D)po.Invoke("VectorCalculationAlgorithm",
new Type[] { typeof(BallCoordinates) },
new Object[] { ballCoordinates },
new Type[] { typeof(BallCoordinates) });
Assert.AreEqual(actual, actual);
}
public void FindBallFutureCoordinates_BallCoordinatesVectorNull()
{
//arrange
BallCoordinates currentCoordinates = new BallCoordinates(100, 100, DateTime.Now);
DateTime actionTime = DateTime.Now + TimeSpan.FromSeconds(5);
//act
BallCoordinates actualCoordinates = _testAsset.FindBallFutureCoordinates(currentCoordinates, actionTime);
//assert
Assert.AreEqual(currentCoordinates.X, actualCoordinates.X);
Assert.AreEqual(currentCoordinates.Y, actualCoordinates.Y);
Assert.AreEqual(actionTime, actualCoordinates.Timestamp);
Assert.IsNull(actualCoordinates.Vector);
}
/// <summary>
/// Calculate Ball Future Coordinates in actual time system can response
/// </summary>
/// <param name="currentCoordinates"><Current ball coordinates/param>
/// <param name="actionTime">Actual system response time</param>
/// <returns>Ball Future coordinates</returns>
public BallCoordinates FindBallFutureCoordinates(BallCoordinates currentCoordinates, DateTime actionTime)
{
if (currentCoordinates == null || !currentCoordinates.IsDefined)
throw new ArgumentException(String.Format(
"[{0}] Unable to calculate ball future coordinates while current coordinates are null or undefined",
MethodBase.GetCurrentMethod().Name));
if (actionTime < currentCoordinates.Timestamp)
throw new ArgumentException(String.Format(
"[{0}] Unable to calculate ball future coordinates while action time is earlier than time stamp",
MethodBase.GetCurrentMethod().Name));
BallCoordinates bfc;
if (!Vector2D.NotNullAndDefined(currentCoordinates.Vector))
{
bfc = new BallCoordinates(currentCoordinates.X, currentCoordinates.Y, actionTime);
bfc.Vector = currentCoordinates.Vector;
return bfc;
}
bfc = currentCoordinates;
try
{
TimeSpan deltaT = actionTime - currentCoordinates.Timestamp;
int xfc = Convert.ToInt32(currentCoordinates.Vector.X * deltaT.TotalSeconds + currentCoordinates.X);
int yfc = Convert.ToInt32(currentCoordinates.Vector.Y * deltaT.TotalSeconds + currentCoordinates.Y);
if (_surveyor.IsCoordinatesInRange(xfc, yfc))
{
bfc = new BallCoordinates(xfc, yfc, actionTime);
bfc.Vector = currentCoordinates.Vector;
}
else
{
BallCoordinates ricoshetCoordiantes = _ricochetCalc.Ricochet(currentCoordinates);
return FindBallFutureCoordinates(ricoshetCoordiantes, actionTime);
}
}
catch (Exception e)
{
Log.Print("Error: " + e.Message, eCategory.Error, LogTag.DECISION);
}
return bfc;
}
/// <summary>
/// Main Decision Flow Method
/// </summary>
/// <param name="rod">Rod to use for decision</param>
/// <param name="bfc">Ball Future coordinates</param>
/// <returns>Rod Action to perform</returns>
public override RodAction Decide(IRod rod, BallCoordinates bfc)
{
//Player to respond (index base is 0)
int respondingPlayer = -1;
//Chose responding player on rod and define action to perform
RodAction action = DefineActionAndRespondingPlayer(rod, bfc, out respondingPlayer);
//Define actual desired rod coordinate to move to
int startStopperDesiredY = CalculateNewRodCoordinate(rod, respondingPlayer, bfc, action.Linear);
action.DcCoordinate = rod.NearestPossibleDcPosition(startStopperDesiredY);
//Set last decided rod and player coordinates
rod.State.DcPosition = action.DcCoordinate;
if (_helper.ShouldSetServoStateFromTree(rod.RodType))
rod.State.ServoPosition = action.Rotation;
return action;
}
public void Ricochet_UndefinedVector()
{
_initialCoordinates = new BallCoordinates(10, 10, DateTime.Now);
_initialCoordinates.Vector = new Vector2D();
_testAsset.Ricochet(_initialCoordinates);
}
public void Ricochet_UndefinedCoordinates()
{
_initialCoordinates = new BallCoordinates(DateTime.Now);
_testAsset.Ricochet(_initialCoordinates);
}
public void Ricochet_NullVector()
{
_initialCoordinates = new BallCoordinates(10, 10, DateTime.Now);
_testAsset.Ricochet(_initialCoordinates);
}
public void Ricochet_NullCoordinates()
{
_initialCoordinates = null;
_testAsset.Ricochet(_initialCoordinates);
}
public void MmToPts_Vector_Not_Defined()
{
BallCoordinates ballCoordinates = new BallCoordinates(50, 50, DateTime.Now);
ballCoordinates.Vector = new Vector2D();
int xMm = ballCoordinates.X * X_MAX_PTS / X_MAX_MM;
int yMm = ballCoordinates.Y * Y_MAX_PTS / Y_MAX_MM;
BallCoordinates expected = new BallCoordinates(xMm, yMm, ballCoordinates.Timestamp);
expected.Vector = new Vector2D();
BallCoordinates actual = _testAsset.MmToPts(ballCoordinates);
Assert.AreEqual(actual.X, expected.X);
Assert.AreEqual(actual.Y, expected.Y);
Assert.AreEqual(actual.Vector.IsDefined, expected.Vector.IsDefined);
}
/// <summary>
/// Ricochet coordinate calculation
/// </summary>
/// <param name="ballCoordinates">Ball Coordinates</param>
/// <returns>Ricochet Ball Coordinates</returns>
public BallCoordinates Ricochet(BallCoordinates ballCoordinates)
{
try
{
Coordinates2D intersectionPoint = FindNearestIntersectionPoint(ballCoordinates);
DateTime ricocheTime = FindRicochetTime(ballCoordinates, intersectionPoint);
Vector2D vector = FindIntersectionVector(ballCoordinates.Vector, intersectionPoint);
BallCoordinates coordinates = new BallCoordinates(
Convert.ToInt32(intersectionPoint.X),
Convert.ToInt32(intersectionPoint.Y), ricocheTime);
coordinates.Vector = vector;
return coordinates;
}
catch (Exception e)
{
throw new NotSupportedException(String.Format(
"[{0}] Failed to find ricochet coordinates. Reason: {1}",
MethodBase.GetCurrentMethod().Name, e.Message));
}
}
/// <summary>
/// Find nearest intersection point with table borders based on
/// - given ball coordinates and vector
/// </summary>
/// <param name="ballCoordinates">Defined coordinates with defined vector</param>
/// <returns>Coordinates of intersection with border</returns>
public Coordinates2D FindNearestIntersectionPoint(BallCoordinates ballCoordinates)
{
//verify we can proceed to calculate intersection
VerifyBallCoordinatesAndVectorInput(ballCoordinates);
Dictionary<Vector2D, Coordinates2D> borderIntersection = new Dictionary<Vector2D, Coordinates2D>();
List<Vector2D> vectors = new List<Vector2D>();
//get line slope
double m = CalculateLineSlope(ballCoordinates.Vector);
if (ballCoordinates.Vector.X != 0)
{
double yb = CalculateY2OnLine(m, ballCoordinates.X, ballCoordinates.Y, MinBorderX);
double yc = CalculateY2OnLine(m, ballCoordinates.X, ballCoordinates.Y, MaxBorderX);
Coordinates2D B = new Coordinates2D(MinBorderX, yb);
Vector2D vB = new Vector2D(B.X - ballCoordinates.X, B.Y - ballCoordinates.Y);
vectors.Add(vB);
borderIntersection.Add(vB, B); //B
Coordinates2D C = new Coordinates2D(MaxBorderX, yc);
Vector2D vC = new Vector2D(C.X - ballCoordinates.X, C.Y - ballCoordinates.Y);
vectors.Add(vC);
borderIntersection.Add(vC, C); //C
}
if (ballCoordinates.Vector.Y != 0)
{
double xa = CalculateX2OnLine(m, ballCoordinates.X, ballCoordinates.Y, MinBorderY);
double xd = CalculateX2OnLine(m, ballCoordinates.X, ballCoordinates.Y, MaxBorderY);
Coordinates2D A = new Coordinates2D(xa, MinBorderY);
Vector2D vA = new Vector2D(A.X - ballCoordinates.X, A.Y - ballCoordinates.Y);
vectors.Add(vA);
borderIntersection.Add(vA, A); //A
Coordinates2D D = new Coordinates2D(xd, MaxBorderY);
Vector2D vD = new Vector2D(D.X - ballCoordinates.X, D.Y - ballCoordinates.Y);
vectors.Add(vD);
borderIntersection.Add(vD, D); //D
}
//remove points from wrong direction
foreach (Vector2D vector in vectors)
{
double direction = vector.ScalarProduct(ballCoordinates.Vector)
/ (vector.Velocity() * ballCoordinates.Vector.Velocity());
if (Math.Round(direction, 0) != 1)
borderIntersection.Remove(vector);
}
//if no points found => Error, consider adding error rate threshold
if (borderIntersection.Count < 1)
Log.Print("No vectors found!", eCategory.Error, LogTag.VECTOR);
//get nearest point
Coordinates2D intersectionPoint = null;
double minDistance = MaxBorderX * MaxBorderY;
foreach (Coordinates2D intersection in borderIntersection.Values)
{
double dist = intersection.Distance(ballCoordinates);
if (dist < minDistance)
{
minDistance = dist;
intersectionPoint = intersection;
}
}
return intersectionPoint;
}
/// <summary>
/// Main Decision Flow Method decides on action and sets property of responding player
/// </summary>
/// <param name="rod">Rod to use for decision</param>
/// <param name="bfc">Ball Future coordinates</param>
/// <returns>Rod Action to perform</returns>
public override RodAction Decide(IRod rod, BallCoordinates bfc)
{
RodAction action = new RodAction(rod.RodType);
//Action will be ignored if not enough time passed since last request was made inside sector
if (!IgnoreDecision(rod.RodType))
{
//Get relative Y position and set Responding Player
eYPositionPlayerRelative relativeY = BallYPositionToPlayerYCoordinate(bfc.Y, rod);
//Get relative X position
eXPositionRodRelative relativeX = BallXPositionToRodXPosition(bfc.X, rod);
/*
* Beta Version of inner DECISION TREE
*/
switch (relativeX)
{
case eXPositionRodRelative.FRONT:
switch (relativeY)
{
case eYPositionPlayerRelative.RIGHT:
case eYPositionPlayerRelative.LEFT:
switch (rod.State.ServoPosition)
{
case eRotationalMove.RISE:
case eRotationalMove.DEFENCE:
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.BALL_Y);
break;
case eRotationalMove.KICK:
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.NA);
break;
}
break;
case eYPositionPlayerRelative.CENTER:
switch (rod.State.ServoPosition)
{
case eRotationalMove.RISE:
case eRotationalMove.DEFENCE:
action = new RodAction(rod.RodType, eRotationalMove.KICK, eLinearMove.NA);
break;
case eRotationalMove.KICK:
if (_helper.IsEnoughSpaceToMove(rod, rod.State.DcPosition, BALL_RADIUS))
{
action = new RodAction(rod.RodType, eRotationalMove.NA, eLinearMove.RIGHT_BALL_DIAMETER);
}
else
{
action = new RodAction(rod.RodType, eRotationalMove.NA, eLinearMove.LEFT_BALL_DIAMETER);
}
break;
}
break;
}
break;
case eXPositionRodRelative.CENTER:
case eXPositionRodRelative.BACK:
switch (relativeY)
{
case eYPositionPlayerRelative.RIGHT:
case eYPositionPlayerRelative.LEFT:
switch (rod.State.ServoPosition)
{
case eRotationalMove.RISE:
action = new RodAction(rod.RodType, eRotationalMove.RISE, eLinearMove.BALL_Y);
break;
case eRotationalMove.DEFENCE:
action = new RodAction(rod.RodType, eRotationalMove.RISE, eLinearMove.NA);
break;
case eRotationalMove.KICK:
action = new RodAction(rod.RodType, eRotationalMove.DEFENCE, eLinearMove.NA);
break;
}
break;
case eYPositionPlayerRelative.CENTER:
switch (rod.State.ServoPosition)
{
case eRotationalMove.RISE:
action = new RodAction(rod.RodType, eRotationalMove.KICK, eLinearMove.NA);
break;
case eRotationalMove.DEFENCE:
case eRotationalMove.KICK:
if (_helper.IsEnoughSpaceToMove(rod, rod.State.DcPosition, BALL_RADIUS))
{
action = new RodAction(rod.RodType, eRotationalMove.NA, eLinearMove.RIGHT_BALL_DIAMETER);
}
else
{
action = new RodAction(rod.RodType, eRotationalMove.NA, eLinearMove.LEFT_BALL_DIAMETER);
}
break;
}
break;
}
break;
}
Log.Print(String.Format("Defined action for {0}: [{1}] [{2}]", rod.RodType, action.Rotation, action.Linear),
eCategory.Info, LogTag.DECISION);
ActivateDelay(action);
//Define actual desired rod coordinate to move to
int startStopperDesiredY = CalculateNewRodCoordinate(rod, RespondingPlayer, bfc, action.Linear);
action.DcCoordinate = rod.NearestPossibleDcPosition(startStopperDesiredY);
}
else
{
Log.Print(String.Format("Ignoring inner tree of {0} for {1} milliseconds", rod.RodType,
(ACTION_DELAY - _sectorWatch[rod.RodType].Elapsed).TotalMilliseconds), eCategory.Debug, LogTag.DECISION);
}
//Set last decided rod and player coordinates if it was defined
if (action.Linear!=eLinearMove.NA) rod.State.DcPosition = action.DcCoordinate;
if (_helper.ShouldSetServoStateFromTree(rod.RodType))
if (action.Rotation != eRotationalMove.NA) rod.State.ServoPosition = action.Rotation;
return action;
}
public void MmToPts_Coordinates_Not_Defined()
{
BallCoordinates expected = new BallCoordinates(DateTime.Now);
BallCoordinates actual = _testAsset.MmToPts(expected);
Assert.AreEqual(actual, expected);
}
public void MmToPts__Vector_And_Coordinates()
{
BallCoordinates ballCoordinates = new BallCoordinates(50, 50, DateTime.Now);
ballCoordinates.Vector = new Vector2D(100, 100);
int xMm = ballCoordinates.X * X_MAX_PTS / X_MAX_MM;
int yMm = ballCoordinates.Y * Y_MAX_PTS / Y_MAX_MM;
double xMmVector = ballCoordinates.Vector.X * (double)X_MAX_PTS / (double)X_MAX_MM;
double yMmVector = ballCoordinates.Vector.Y * (double)Y_MAX_PTS / (double)Y_MAX_MM;
BallCoordinates expected = new BallCoordinates(xMm, yMm, ballCoordinates.Timestamp);
expected.Vector = new Vector2D(xMmVector, yMmVector);
BallCoordinates actual = _testAsset.MmToPts(ballCoordinates);
Assert.AreEqual(actual.X, expected.X);
Assert.AreEqual(actual.Y, expected.Y);
Assert.AreEqual(actual.Vector.X, expected.Vector.X);
Assert.AreEqual(actual.Vector.Y, expected.Vector.Y);
}
public void Ricochet_ZeroVelocity()
{
_initialCoordinates = new BallCoordinates(10, 10, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(0, 0);
_testAsset.Ricochet(_initialCoordinates);
}
public void FindNearestIntersectionPoint_MoveDown_from_100_50()
{
_initialCoordinates = new BallCoordinates(100, 50, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(0, -100);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(100, YMIN);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
public void FindRicochetTime_Vector_minus140_minus20()
{
DateTime now = DateTime.Now;
Coordinates2D intersection = new Coordinates2D(XMIN, 200);
BallCoordinates ballCoordinates = new BallCoordinates(700, 300, now);
ballCoordinates.Vector = new Vector2D(-140, -20);
DateTime actualTime = _testAsset.FindRicochetTime(ballCoordinates, intersection);
DateTime expected = now + TimeSpan.FromSeconds(5.0);
Assert.AreEqual(expected, actualTime);
}
/// <summary>
/// Find intersection time based on ball coordinates, timestamp, vector and intersection point.
/// </summary>
/// <param name="ballCoordinates">Ball coordinates before intersection with border</param>
/// <param name="intersection">Intersection with border point</param>
/// <exception cref="ArgumentOutOfRangeException">Thrown in case calculated intersection time is too big</exception>
/// <exception cref="NotSupportedException">Thrown in case intersection coordinates undefined.</exception>
/// <returns>Intersection timestamp</returns>
public DateTime FindRicochetTime(BallCoordinates ballCoordinates, Coordinates2D intersection)
{
VerifyBallCoordinatesAndVectorInput(ballCoordinates);
if (intersection == null || !intersection.IsDefined)
throw new NotSupportedException(String.Format("[{0}] Intersection coordinates undefined!", MethodBase.GetCurrentMethod().Name));
double distance = ballCoordinates.Distance(intersection);
double velocity = ballCoordinates.Vector.Velocity();
double deltaT = distance / velocity;
return ballCoordinates.Timestamp + TimeSpan.FromSeconds(deltaT);
}
public void Ricochet_FromCoord100_50_SpeedMinus100_0()
{
DateTime initTime = DateTime.Now;
_initialCoordinates = new BallCoordinates(100, 50, initTime);
_initialCoordinates.Vector = new Vector2D(-100, 0);
DateTime expectedTime = initTime + TimeSpan.FromSeconds(1);
BallCoordinates expectedResult = new BallCoordinates(XMIN, 50, expectedTime);
expectedResult.Vector = new Vector2D(100 * RICOCHE, 0);
BallCoordinates actualResult = _testAsset.Ricochet(_initialCoordinates);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
Assert.AreEqual(expectedResult.Timestamp, actualResult.Timestamp);
Assert.AreEqual(expectedResult.Vector.X, actualResult.Vector.X);
Assert.AreEqual(expectedResult.Vector.Y, actualResult.Vector.Y);
}
public void FindNearestIntersectionPoint_MoveLeftUp_UpFirst_from_700_100()
{
_initialCoordinates = new BallCoordinates(700, 100, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(-100, 50);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(100, YMAX);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
public void MmToPts_Coordinates_Only()
{
BallCoordinates ballCoordinates = new BallCoordinates(50, 50, DateTime.Now);
int xMm = ballCoordinates.X * X_MAX_PTS / X_MAX_MM;
int yMm = ballCoordinates.Y * Y_MAX_PTS / Y_MAX_MM;
BallCoordinates expected = new BallCoordinates(xMm, yMm, ballCoordinates.Timestamp);
BallCoordinates actual = _testAsset.MmToPts(ballCoordinates);
Assert.AreEqual(actual.X, expected.X);
Assert.AreEqual(actual.Y, expected.Y);
}
/// <summary>
/// Decide on action to be taken for each rod
/// </summary>
/// <param name="currentCoordinates">Current ball coordinates and vector</param>
/// <returns>List of actions per each rod</returns>
public List<RodAction> Decide(BallCoordinates currentCoordinates)
{
if (!IsInitialized) Initialize();
if (currentCoordinates == null)
throw new ArgumentException(String.Format("[{0}] Coordinates received from vector calculation unit are null",
MethodBase.GetCurrentMethod().Name));
//Convert pts and pts/sec to mm and mm/sec
currentCoordinates = _surveyor.PtsToMm(currentCoordinates);
//Calculate Actual Possible Action Time
DateTime timeOfAction = DateTime.Now + SystemDelays;
//Calculate ball future coordinates
BallCoordinates bfc = _predictor.FindBallFutureCoordinates(currentCoordinates, timeOfAction);
List<RodAction> actions = new List<RodAction>();
foreach(IRod rod in _controlledRods)
{
//Calculate dynamic sectors
rod.CalculateDynamicSector(currentCoordinates);
//Draw dynamic sector, better to use on one rod at a time because together is chaos on the screen
Marks.DrawSector(rod.RodType, rod.DynamicSector);
//Calculate intersection point
rod.CalculateSectorIntersection(bfc);
//Decide on action
RodAction action = _decisionTree.Decide(rod, bfc);
actions.Add(action);
Marks.DrawRodPlayers(rod.RodType, rod.State.DcPosition,
rod.State.ServoPosition);
}
return actions;
}
public void FindNearestIntersectionPoint_MoveRight_from_50_100()
{
_initialCoordinates = new BallCoordinates(50, 100, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(100, 0);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
Coordinates2D expectedResult = new Coordinates2D(XMAX, 100);
Assert.AreEqual(expectedResult.X, actualResult.X);
Assert.AreEqual(expectedResult.Y, actualResult.Y);
}
public void FindNearestIntersectionPoint_NoMove_from_50_50()
{
_initialCoordinates = new BallCoordinates(50, 50, DateTime.Now);
_initialCoordinates.Vector = new Vector2D(0, 0);
Coordinates2D actualResult = _testAsset.FindNearestIntersectionPoint(_initialCoordinates);
}
public void FindRicochetTime_Vector_100_50()
{
DateTime now = DateTime.Now;
Coordinates2D intersection = new Coordinates2D(XMAX, 350);
BallCoordinates ballCoordinates = new BallCoordinates(700, 300, now);
ballCoordinates.Vector = new Vector2D(100, 50);
DateTime actualTime = _testAsset.FindRicochetTime(ballCoordinates, intersection);
DateTime expected = now + TimeSpan.FromSeconds(1.0);
Assert.AreEqual(expected, actualTime);
}
public void PtsToMm_Vector_Null()
{
BallCoordinates ballCoordinates = new BallCoordinates(50, 50, DateTime.Now);
int xMm = ballCoordinates.X * X_MAX_MM / X_MAX_PTS;
int yMm = ballCoordinates.Y * Y_MAX_MM / Y_MAX_PTS;
BallCoordinates expected = new BallCoordinates(xMm, yMm, ballCoordinates.Timestamp);
expected.Vector = null;
BallCoordinates actual = _testAsset.PtsToMm(ballCoordinates);
Assert.AreEqual(actual.X, expected.X);
Assert.AreEqual(actual.Y, expected.Y);
Assert.AreEqual(actual.Vector, expected.Vector);
}
/// <summary>
/// Verify coordinates and vector are defined, not null and not both vector x and y are 0
/// </summary>
/// <param name="ballCoordinates">Ball Coordianates with vector</param>
/// <exception cref="NotSupportedException">Thrown in case vector or coordinates are not defined or are NULL, or both vector x and y are 0</exception>
private void VerifyBallCoordinatesAndVectorInput(BallCoordinates ballCoordinates)
{
if (ballCoordinates == null)
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because ball coordinates are NULL", MethodBase.GetCurrentMethod().Name));
if (!ballCoordinates.IsDefined)
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because ball coordinates are not defined", MethodBase.GetCurrentMethod().Name));
if (ballCoordinates.Vector == null)
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is NULL", MethodBase.GetCurrentMethod().Name));
if (!ballCoordinates.Vector.IsDefined)
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is undefined", MethodBase.GetCurrentMethod().Name));
if (ballCoordinates.Vector.X == 0 && ballCoordinates.Vector.Y == 0)
throw new NotSupportedException(String.Format("[{0}] Intersection point can not be found because vector is 0x0", MethodBase.GetCurrentMethod().Name));
}
