private bool CheckTargetInBeam(out Vector3 position, out Vector3 velocity)
{
position = default;
velocity = default;
var index = -1;
float length;
var minLength = float.MaxValue;
foreach (var p in targetPositions)
{
if (AngleUtils.GetRangeBetweenAnglesAbs(p.y, beamOrientation.x) > beamSize.x / 2 ||
AngleUtils.GetRangeBetweenAnglesAbs(p.z, beamOrientation.y) > beamSize.y / 2)
{
continue;
}
length = Mathf.Pow(AngleUtils.GetRangeBetweenAnglesAbs(p.y, beamOrientation.x), 2) +
Mathf.Pow(AngleUtils.GetRangeBetweenAnglesAbs(p.z, beamOrientation.y), 2);
if (length < minLength)
{
minLength = length;
index = targetPositions.IndexOf(p);
}
}
if (index == -1)
{
return(false);
}
position = targetPositions[index];
velocity = targetVelocities[index];
return(true);
}
private void UpdateLocation()
{
double latitude, longitude;
this.m_errorMessage = null;
if (!double.TryParse(this.m_latitudeString, out latitude))
{
if (!AngleUtils.TryParseDMS(this.m_latitudeString, out latitude))
{
this.m_errorMessage = "Invalid latitude";
return;
}
}
if (!double.TryParse(this.m_longitudeString, out longitude))
{
if (!AngleUtils.TryParseDMS(this.m_longitudeString, out longitude))
{
this.m_errorMessage = "Invalid longitude";
return;
}
}
var location = new GlobalLocation(this.TargetLocation.Body,
new Coordinates(latitude: latitude, longitude: longitude));
this.m_module.SetTargetLocation(location);
Reset();
}
public ACommand scanProcess(ScanCommand command, Battlefield.RobotAndBattlefield robotAndBattlefield)
{
double minDistance = Battlefield.ARENA_MAX_SIZE * 10;
BattlefieldRobot robot = robotAndBattlefield.ROBOT;
Battlefield battlefield = robotAndBattlefield.BATTLEFIELD;
BattlefieldRobot minTarget = robot;
if (robot.HitPoints > 0)
{
foreach (BattlefieldRobot target in battlefield.robots)
{
if (robot.ID != target.ID && target.HitPoints > 0 && battlefield.obstacleManager.CanScan(battlefield.turn, robot.X, robot.Y, target.X, target.Y))
{
double distance = EuclideanSpaceUtils.Distance(robot.X, robot.Y, target.X, target.Y);
if (distance < minDistance)
{
double degree = AngleUtils.NormalizeDegree(AngleUtils.AngleDegree(robot.X, robot.Y, target.X, target.Y));
if (Math.Abs(degree - command.ANGLE) <= command.PRECISION)
{
minDistance = distance;
minTarget = target;
}
}
}
}
battlefield.battlefieldTurn.AddScan(new Scan(command.ANGLE, command.PRECISION, minDistance, robot.X, robot.Y));
}
return(new ScanAnswerCommand(minDistance, minTarget.ID));
}
private void drawRobotBody(Robot robot, Pen pen, Graphics g)
{
PointF[] spike = DrawerUtils.Rotate(AngleUtils.ToRads(robot.ANGLE), PointF.Empty, DefaultDrawer.spike);
PointF[] bottom = DrawerUtils.Rotate(AngleUtils.ToRads(robot.ANGLE), PointF.Empty, DefaultDrawer.bottom);
PointF robotPosition = robot.GetPosition();
Pen teamPen = DefaultDrawer.GetTeamPen(robot.TEAM_ID);
lock (g) {
lock (teamPen) {
float oldTeamPenWidth = teamPen.Width;
teamPen.Width = 5;
g.DrawLines(teamPen, DrawerUtils.Translate(robotPosition, spike));
teamPen.Width = oldTeamPenWidth;
}
lock (pen) {
pen.Width = 5;
g.DrawLines(pen, DrawerUtils.Translate(robotPosition, bottom));
}
lock (Brushes.Red) {
g.FillEllipse(Brushes.Red, robotPosition.X - 1, robotPosition.Y - 1, 2, 2);
}
}
}
public override MoveInfo MakeMove(State state)
{
var nearest = state.Enemies[0];
var me = state.Fighter.Coordinates;
foreach (var enem in state.Enemies)
{
if (length(me, enem.Coordinates) < length(me, nearest.Coordinates))
{
nearest = enem;
}
}
var toCoordinates = state.Fighter.Coordinates;
var toAngle = state.Fighter.Angle;
if (length(nearest.Coordinates, me) > state.Fighter.Properties.Speed)
{
toCoordinates.X = me.X + ((me.X - nearest.Coordinates.X) * state.Fighter.Properties.Speed) / length(nearest.Coordinates, me);
toCoordinates.Y = me.Y + ((me.Y - nearest.Coordinates.Y) * state.Fighter.Properties.Speed) / length(nearest.Coordinates, me);
}
toAngle = AngleUtils.GetDirection(me, nearest.Coordinates);
return(new MoveInfo(
toCoordinates,
toAngle)
{
FireTarget = nearest.Coordinates,
});
}
private Vector3 GetCircularPos(MoveType type)
{
angle -= angleSpeed * Time.deltaTime;
x = Mathf.Cos(AngleUtils.Deg2Rad(angle)) * radius;
y = Mathf.Sin(AngleUtils.Deg2Rad(angle)) * radius;
return(new Vector3(x, type == MoveType.CircularSin ? GetHeight(angle, height) : height, y));
}
/// <summary>
/// Crea un arco de circunferencia indicando puntoInicial, puntoFinal, centro, radio y sentido de giro (cw).
/// </summary>
public static CircleArc2 NewArc(Point2d pt1, Point2d pt2, Point2d center, double radius, ArcDirection dir)
{
double angle1 = AngleUtils.Ensure0To2Pi(pt1.Sub(center).Angle, false);
double angle2 = AngleUtils.Ensure0To2Pi(pt2.Sub(center).Angle, false);
if (dir == ArcDirection.Clockwise)
{
if (angle2 > angle1)
{
angle2 -= 2.0 * System.Math.PI;
}
}
else if (angle2 < angle1)
{
angle2 += 2.0 * System.Math.PI;
}
if (angle2 > 2.0 * System.Math.PI)
{
angle1 -= 2.0 * System.Math.PI;
angle2 -= 2.0 * System.Math.PI;
}
else if (angle2 < -2.0 * System.Math.PI)
{
angle1 += 2.0 * System.Math.PI;
angle2 += 2.0 * System.Math.PI;
}
return(new CircleArc2(center, radius, angle1, angle2));
}
public override double GetLength(double t0, double t1)
{
double a0 = this.GetAngle(t0);
double a1 = this.GetAngle(t1);
return(AngleUtils.Diff(a0, a1) * this.Radius);
}
private static void driveTo(FlagPlace place)
{
double driveAngle = AngleUtils.AngleDegree(tank.X, tank.Y, place.X, place.Y);
if (tank.HitPoints == 0)
{
tank.Wait(); // wait to respawn
}
if (Math.Abs(driveAngle - tank.AngleDrive) > 5)
{
if (tank.Power > tank.Motor.ROTATE_IN && tank.WantedPower > tank.Motor.ROTATE_IN)
{
tank.Drive(driveAngle, tank.Motor.ROTATE_IN);
tank.WantedPower = tank.Motor.ROTATE_IN;
}
else if (tank.Power <= tank.Motor.ROTATE_IN)
{
tank.Drive(driveAngle, 100);
tank.WantedPower = 100;
}
}
for (int angle = 0; angle < 360; angle += 30)
{
ScanAnswerCommand scanAnswer = tank.Scan(angle, 10);
if (scanAnswer.ENEMY_ID != tank.ID)
{
tank.Shoot(angle, scanAnswer.RANGE);
}
}
}
public void Test1()
{
double angle = Point2d.EvAngle(new Point2d(10, 10), new Point2d(0, 0), new Point2d(20, 0));
angle = AngleUtils.Ensure0To2Pi(angle);
Assert.IsTrue(angle.EpsilonEquals(Math.PI / 2));
}
protected void OnCollisionEnter2D(Collision2D collision)
{
var node = collision.collider.gameObject.GetComponent <Node2D>();
if (null == node)
{
return;
}
if (!node.HasTypeTag("ball"))
{
return;
}
//Debug.Log("Ball hit paddle");
ContactPoint2D contactPoint = collision.contacts[0];
var myCollider = GetComponent <CapsuleCollider2D>();
var factor = (contactPoint.point.x - myCollider.bounds.center.x) / (myCollider.bounds.size.x / 2.0f);
var reflectDegreeAngle = factor * 45.0f;
//Debug.Log("Paddle factor: " + factor.ToString() + "Reflect angle: " + reflectDegreeAngle);
node.Velocity = AngleUtils.DegreeAngleToVector2(reflectDegreeAngle, 10);
GetComponent <AudioSource>().PlayOneShot(hitBallClip);
}
// Returns the grid points in XY occupied by the given obstacle
public List <mPoint> LocalizeObstacle(Obstacle obstacle)
{
double currentDirection = AngleUtils.DegreesToRadians(CurrentPose.CurrentDirection);
// Convert the left and right points from mm to cm
Vector leftObstaclePoint = obstacle.leftPoint / 10;
Vector rightObstaclePoint = obstacle.rightPoint / 10;
// Find the offset of the left and right of the obstacle from the current position
Vector leftOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(leftObstaclePoint, currentDirection);
Vector rightOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(rightObstaclePoint, currentDirection);
// Estimate the locations of the actual grid points
mPoint leftMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom);
mPoint rightMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom);
// Find the actual grid points from the estimated grid points
leftMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(leftMapPoint.GetUVFromXY(CurrentPose.CurrentRoom));
rightMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(rightMapPoint.GetUVFromXY(CurrentPose.CurrentRoom));
List <mPoint> points = new List <mPoint>();
points.Add(leftMapPoint);
points.Add(rightMapPoint);
return(points);
}
public Vector3 Evaluate(
Vector3 initialValue,
Vector3 finalValue,
float time,
EaseDelegate easeFunction
)
{
if (easeFunction == null)
{
throw new ArgumentNullException($"Tried to Evaluate with a " +
$"null {nameof(EaseDelegate)} on {nameof(Vector3Interpolator)}");
}
if (rotationMode == RotationMode.Fast)
{
Vector3 deltaAngle = AngleUtils.DeltaAngle(initialValue, finalValue);
finalValue = initialValue + deltaAngle;
}
return(new Vector3(
easeFunction(initialValue.x, finalValue.x, time),
easeFunction(initialValue.y, finalValue.y, time),
easeFunction(initialValue.z, finalValue.z, time)
));
}
public static double AngleBetween(double aX, double aY, double bX, double bY, double cX, double cY)
{
double a1 = AngleBetween(bX, bY, aX, aY);
double a2 = AngleBetween(bX, bY, cX, cY);
return(AngleUtils.Diff(a1, a2));
}
/// <summary>
/// Compute the Cartesian distance between two coordinatesusing the Bowring method (moderately fast, moderately accurate)
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="toX">destination X ordinate</param>
/// <param name="toY">destination Y ordinate</param>
/// <returns>The great circle distance from a to b</returns>
public double DistanceBowring(double fromX, double fromY, double toX, double toY)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
toX = AngleUtils.ToRadians(toX);
toY = AngleUtils.ToRadians(toY);
double a = Math.Sqrt(1 + er * Math.Pow(Math.Cos(fromY), 4.0));
double b = Math.Sqrt(1 + er * Math.Pow(Math.Cos(fromY), 2.0));
double c = Math.Sqrt(1 + er);
double dLat = toY - fromY;
double dLon = toX - fromX;
double w = (a * dLon) / 2.0;
double d = dLat * Math.Sin(2 * fromY + Constants.TwoThirds * dLat);
d = d * (1 + (er3 / (4 * Math.Pow(b, 2.0))));
d = d * (dLat / (2.0 * b));
double ia = 1 / a;
double e = Math.Sin(d) * Math.Cos(w);
double f = ia * Math.Sin(w) * (b * Math.Cos(fromY) * Math.Cos(d) - Math.Sin(fromY) * Math.Sin(d));
//double g = Math.Atan(f / e);
double s = Math.Asin(Math.Sqrt(Math.Pow(e, 2.0) + Math.Pow(f, 2.0)) * 2.0);
//double h = ia * (Math.Sin(fromY) + b * Math.Cos(fromY) * Math.Tan(d)) * Math.Tan(w);
return((a * c * s) / Math.Pow(b, 2.0));
}
public void Test1()
{
const double error = 1e-8;
// desarrollo, r, parametro, x, y, c, normal ¿o direccion?,
for (int i = 0; i < this.testData1.Length;)
{
double l = this.testData1[i++];
double r = this.testData1[i++];
double a = this.testData1[i++];
double x = this.testData1[i++];
double y = this.testData1[i++];
double radio = this.testData1[i++];
double tg = this.testData1[i++];
bool invertY = ((l < 0 && r > 0) || (l > 0 && r < 0));
double x2, y2;
ClothoUtils.Clotho(l, invertY, a, out x2, out y2);
double radio2 = ClothoUtils.ClothoRadius(l, invertY, a);
double tg2 = ClothoUtils.ClothoTangent(l, invertY, a);
Assert.IsTrue(x.EpsilonEquals(x2, error));
Assert.IsTrue(y.EpsilonEquals(y2, error));
Assert.IsTrue((double.IsInfinity(radio) && double.IsInfinity(radio2)) || radio.EpsilonEquals(radio2, error));
Assert.IsTrue(AngleUtils.Ensure0To2Pi(tg).EpsilonEquals(AngleUtils.Ensure0To2Pi(tg2), error));
}
}
// angle ABC
public static double AngleBetween(Coordinate2 <double> a, Coordinate2 <double> b, Coordinate2 <double> c)
{
double a1 = AngleBetween(b, a);
double a2 = AngleBetween(b, c);
return(AngleUtils.Diff(a1, a2));
}
/// <summary>
/// Crea un arco de circunferencia indicando puntoInicial, puntoFinal, centro, radio y sentido de giro (cw).
/// </summary>
public static CircleArc2 NewArc(Vec2d pt1, Vec2d pt2, Vec2d center, double radius, bool cw)
{
double a1 = vecMath.Angle(pt1.Sub(center));
double a2 = vecMath.Angle(pt2.Sub(center));
a1 = AngleUtils.Ensure0To2Pi(a1);
a2 = AngleUtils.Ensure0To2Pi(a2);
if (cw)
{
if (a2 > a1)
{
a2 -= 2 * SysMath.PI;
}
}
else
{
if (a2 < a1)
{
a2 += 2 * SysMath.PI;
}
}
return(new CircleArc2(center, radius, a1, a2));
}
protected static IEnumerable <WayPoint> ImportWayPoints(string filePath)
{
var list = new List <WayPoint>();
foreach (var s in new StringReader(hc[filePath]).ReadToEnd().Split(new [] { Environment.NewLine }, StringSplitOptions.RemoveEmptyEntries))
{
var xs = s.Split();
var wp = new WayPoint {
Coordinates = new Point
{
X = Convert.ToInt32(xs[0]),
Y = Convert.ToInt32(xs[1])
}
};
if (xs.Length == 3)
{
wp.Angle = Math.PI * 2 - Convert.ToDouble(xs[2], CultureInfo.InvariantCulture) + 2 * Math.PI;
}
list.Add(wp);
}
for (int i = 0; i < list.Count; i++)
{
if (list[i].Angle < 2 * Math.PI)
{
list[i].Angle = AngleUtils.GetDirection(list[i].Coordinates, list[(i + 1) % list.Count].Coordinates);
}
list[i].Angle = AngleUtils.Normalize(list[i].Angle);
}
return(list);
}
public override void Animate(float angle)
{
var length = transform.localScale.x;
var size = Mathf.Tan(AngleUtils.Deg2Rad(angle / 2)) * length;
var curScale = transform.localScale;
curScale[(int)(type + 1)] = size;
transform.localScale = curScale;
}
/// <summary>
/// Compute the Cartesian distance between two coordinates using the law of cosines method (fast, low accuracy)
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="toX">destination X ordinate</param>
/// <param name="toY">destination Y ordinate</param>
/// <returns>The great circle distance from a to b</returns>
public double DistanceCosines(double fromX, double fromY, double toX, double toY)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
toX = AngleUtils.ToRadians(toX);
toY = AngleUtils.ToRadians(toY);
double a = Math.Acos(Math.Sin(fromY) * Math.Sin(toY) + Math.Cos(fromY) * Math.Cos(toY) * Math.Cos(toX - fromX));
return(a * meanRadius);
}
/// <summary>
/// Compute the Cartesian distance between two coordinatesusing the Vincente method (slower - iterative, highly accurate)
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="toX">destination X ordinate</param>
/// <param name="toY">destination Y ordinate</param>
/// <returns>The great circle distance from a to b</returns>
public double DistanceVincente(double fromX, double fromY, double toX, double toY)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
toX = AngleUtils.ToRadians(toX);
toY = AngleUtils.ToRadians(toY);
double L = toX - fromX;
double U1 = Math.Atan((1 - f) * Math.Tan(fromY));
double U2 = Math.Atan((1 - f) * Math.Tan(toY));
double sinU1 = Math.Sin(U1);
double cosU1 = Math.Cos(U1);
double sinU2 = Math.Sin(U2);
double cosU2 = Math.Cos(U2);
double lambda = L;
double lambdaP;
double iterLimit = 100;
double cosSqAlpha = 0.0;
double sinSigma = 0.0;
double cos2SigmaM = 0.0;
double cosSigma = 0.0;
double sigma = 0.0;
do
{
double sinLambda = Math.Sin(lambda);
double cosLambda = Math.Cos(lambda);
sinSigma = Math.Sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
if (sinSigma == 0)
{
return(0); //coincident points
}
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
sigma = Math.Atan2(sinSigma, cosSigma);
double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
cosSqAlpha = 1 - sinAlpha * sinAlpha;
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
if (double.IsNaN(cos2SigmaM))
{
cos2SigmaM = 0; //equatorial line: cosSqAlpha=0
}
double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
lambdaP = lambda;
lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
} while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);
if (iterLimit == 0)
{
return(this.DistanceHaversine(fromX, fromY, toX, toY)); // formula failed to converge, use next method
}
double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
return(b * A * (sigma - deltaSigma));
}
/// <summary>
/// Конвертирует декартовые координаты в полярные. Используется координатная
/// система unity, где ось Y направленна вверх.
/// </summary>
/// <param name="decart">Вектор в декартовых координатах.
/// (Ось Y направленна вверх)</param>
/// <returns>Возвращает вектор в полярных координатах
/// [дальность; азимутальный угол; угол места]</returns>
public static Vector3 DecToPolar(Vector3 decart) =>
decart == Vector3.zero
? Vector3.zero
: new Vector3(decart.magnitude,
AngleUtils.NormalizeAngle(90 -
AngleUtils.Rad2Deg(decart.x > 0 && decart.z >= 0 ? Mathf.Atan(decart.z / decart.x) :
decart.x > 0 && decart.z < 0 ? Mathf.Atan(decart.z / decart.x) + 2 * Mathf.PI :
decart.x < 0 ? Mathf.Atan(decart.z / decart.x) + Mathf.PI :
Mathf.Abs(decart.x) < ZERO_ACCURACY && decart.z > 0 ? Mathf.PI / 2 :
Mathf.Abs(decart.x) < ZERO_ACCURACY && decart.z < 0 ? 3 * Mathf.PI / 2 : 0)),
AngleUtils.NormalizeAngle(90 - AngleUtils.Rad2Deg(Mathf.Acos(decart.y / decart.magnitude))));
/// <summary>
/// Compute the "destination" point by travelling along a great circle path starting at the provided point.
/// The path is based upon an initial direction and a fixed distance to travel along the great circle (may circumnavigate the globe)
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="distance">the distance to travel in meters</param>
/// <param name="direction">the direction to travel in radians of compass AngleUtils (0 is north)</param>
/// <returns>A coordinate of the final location</returns>
public Coordinate2 <double> Destination(double fromX, double fromY, double distance, double direction)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
double dR = distance / meanRadius;
double brng = AngleUtils.ToRadians(direction);
double lat2 = Math.Asin(Math.Sin(fromY) * Math.Cos(dR) + Math.Cos(fromY) * Math.Sin(dR) * Math.Cos(brng));
double lon2 = fromX + Math.Atan2(Math.Sin(brng) * Math.Sin(dR) * Math.Cos(fromY), Math.Cos(dR) - Math.Sin(fromY) * Math.Sin(lat2));
return(new Coordinate2 <double>(lon2, lat2));
}
/// <summary>
/// Compute the compass direction between two points using the starting bearing as the direction
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="toX">destination X ordinate</param>
/// <param name="toY">destination Y ordinate</param>
/// <returns>The compass direction from a to b</returns>
public double DirectionStartBearing(double fromX, double fromY, double toX, double toY)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
toX = AngleUtils.ToRadians(toX);
toY = AngleUtils.ToRadians(toY);
double dLon = toX - fromX;
double y = Math.Sin(dLon) * Math.Cos(toY);
double x = Math.Cos(fromY) * Math.Sin(toY) - Math.Sin(fromY) * Math.Cos(toY) * Math.Cos(dLon);
return(AngleUtils.ToDegrees(Math.Atan2(y, x)));
}
public static int OrientationIndex(double p1X, double p1Y, double p2X, double p2Y, double qX, double qY)
{
// travelling along p1->p2, turn counter clockwise to get to q return 1,
// travelling along p1->p2, turn clockwise to get to q return -1,
// p1, p2 and q are colinear return 0.
double dx1 = p2X - p1X;
double dy1 = p2Y - p1Y;
double dx2 = qX - p2X;
double dy2 = qY - p2Y;
return(AngleUtils.DeterminantSign(dx1, dy1, dx2, dy2));
}
/// <summary>
/// Set percentage power of motor and direction. It send this action to server asynchronously. At the end set <code>AngleDrive</code> if rotation success and fill answer data to <code>destination</code>.
/// </summary>
/// <param name="destination">Where to fill answer data.</param>
/// <param name="angle">in degree. 0 = 3 hour. 90 = 6 hour and so on. 12 hour in up.</param>
/// <param name="power">percentage from 0 to 100.</param>
/// <seealso cref="Robot.AngleDrive"/>
/// <returns></returns>
private async Task DriveAsync(DriveAnswerCommand destination, double angle, double power)
{
await sendCommandAsync(new DriveCommand(power, angle));
var answerCommand = await receiveCommandAsync <DriveAnswerCommand>();
if (answerCommand.SUCCESS)
{
AngleDrive = AngleUtils.NormalizeDegree(angle);
}
destination.FillData(answerCommand);
}
public static int OrientationIndex(Coordinate2 <double> p1, Coordinate2 <double> p2, Coordinate2 <double> q)
{
// travelling along p1->p2, turn counter clockwise to get to q return 1,
// travelling along p1->p2, turn clockwise to get to q return -1,
// p1, p2 and q are colinear return 0.
//double dx1 = p2.X - p1.X;
//double dy1 = p2.Y - p1.Y;
//double dx2 = q.X - p2.X;
//double dy2 = q.Y - p2.Y;
//return AngleUtils.DeterminantSign(dx1, dy1, dx2, dy2);
return(AngleUtils.DeterminantSign(p2.X - p1.X, p2.Y - p1.Y, q.X - p2.X, q.Y - p2.Y));
}
private static DriveAnswerCommand robotDriveToBase(ClientRobot robot)
{
double angle = AngleUtils.AngleDegree(robot.X, robot.Y, capturedBase.X, capturedBase.Y);
if (robot.Power > robot.Motor.ROTATE_IN && Math.Abs(angle - robot.AngleDrive) > 1)
{
return(robot.Drive(robot.AngleDrive, robot.Motor.ROTATE_IN));
}
else
{
return(robot.Drive(angle, 100));
}
}
/// <summary>
/// Compute the Cartesian distance between two coordinates using the Haversine method (moderately fast, moderately accurate)
/// </summary>
/// <param name="fromX">source X ordinate</param>
/// <param name="fromY">source Y ordinate</param>
/// <param name="toX">destination X ordinate</param>
/// <param name="toY">destination Y ordinate</param>
/// <returns>The great circle distance from a to b</returns>
public double DistanceHaversine(double fromX, double fromY, double toX, double toY)
{
fromX = AngleUtils.ToRadians(fromX);
fromY = AngleUtils.ToRadians(fromY);
toX = AngleUtils.ToRadians(toX);
toY = AngleUtils.ToRadians(toY);
double dY = toY - fromY;
double dX = toX - fromX;
double a = Math.Pow(Math.Sin(dY / 2), 2.0) + (Math.Cos(toY) * Math.Cos(fromY) * Math.Pow(Math.Sin(dX / 2), 2.0));
double c = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
return(meanRadius * c);
}
