public float[,] Smooth(float[,] input, byte[,] orientation, BinaryMap mask, BlockMap blocks)
{
Point[][] lines = Lines.Construct();
float[,] output = new float[input.GetLength(0), input.GetLength(1)];
Parallel.ForEach(blocks.AllBlocks, delegate(Point block)
{
if (mask.GetBit(block))
{
Point[] line = lines[Angle.Quantize(Angle.Add(orientation[block.Y, block.X], AngleOffset), lines.Length)];
foreach (Point linePoint in line)
{
RectangleC target = blocks.BlockAreas[block];
RectangleC source = target.GetShifted(linePoint);
source.Clip(new RectangleC(blocks.PixelCount));
target = source.GetShifted(Calc.Negate(linePoint));
for (int y = target.Bottom; y < target.Top; ++y)
for (int x = target.Left; x < target.Right; ++x)
output[y, x] += input[y + linePoint.Y, x + linePoint.X];
}
RectangleC blockArea = blocks.BlockAreas[block];
for (int y = blockArea.Bottom; y < blockArea.Top; ++y)
for (int x = blockArea.Left; x < blockArea.Right; ++x)
output[y, x] *= 1f / line.Length;
}
});
Logger.Log(output);
return output;
}
static double[,] SmoothByOrientation(double[,] input, byte[,] orientation, bool[,] mask, BlockMap blocks, byte angle, Point[][] lines)
{
double[,] output = new double[input.GetLength(0), input.GetLength(1)];
foreach (var block in blocks.AllBlocks)
{
if (block.Get(mask))
{
Point[] line = lines[Angle.Quantize(Angle.Add(orientation[block.Y, block.X], angle), lines.Length)];
foreach (Point linePoint in line)
{
Rectangle target = blocks.BlockAreas[block];
Rectangle source = target.GetShifted(linePoint);
source.Clip(new Rectangle(blocks.PixelCount));
target = source.GetShifted(-linePoint);
for (int y = target.Bottom; y < target.Top; ++y)
{
for (int x = target.Left; x < target.Right; ++x)
{
output[y, x] += input[y + linePoint.Y, x + linePoint.X];
}
}
}
Rectangle blockArea = blocks.BlockAreas[block];
for (int y = blockArea.Bottom; y < blockArea.Top; ++y)
{
for (int x = blockArea.Left; x < blockArea.Right; ++x)
{
output[y, x] *= 1.0 / line.Length;
}
}
}
}
return(output);
}
List <List <NeighborInfo> > PrepareNeighbors()
{
Random random = new Random(0);
List <List <NeighborInfo> > allSplits = new List <List <NeighborInfo> >();
for (int i = 0; i < NeighborListSplit; ++i)
{
List <NeighborInfo> neighbors = new List <NeighborInfo>();
for (int j = 0; j < NeighborsChecked; ++j)
{
NeighborInfo neighbor = new NeighborInfo();
do
{
float angle = Angle.FromFraction((float)random.NextDouble() * 0.5f);
float distance = Calc.InterpolateExponential(MinHalfDistance, MaxHalfDistance, (float)random.NextDouble());
neighbor.Position = Calc.Round(Calc.Multiply(distance, Angle.ToVector(angle)));
} while (neighbor.Position == new Point() || neighbor.Position.Y < 0);
neighbor.Orientation = Angle.ToVector(Angle.Add(Angle.ToOrientation(Angle.Atan(neighbor.Position)), Angle.PI));
if (!neighbors.Any(info => info.Position == neighbor.Position))
{
neighbors.Add(neighbor);
}
}
neighbors.Sort((left, right) => Calc.CompareYX(left.Position, right.Position));
allSplits.Add(neighbors);
}
return(allSplits);
}
public static StandardChunkMetadata GetRangeFromKey(long key)
{
if (key == 0)
{
return(null);
}
int zoomLevel = (int)(key % (MaxZoomLevel + 1));
Angle frameSize = frameSizeForZoom[zoomLevel];
int latNumPossible = (int)(usDeltaLat.Abs / frameSize.Abs);
int lonNumPossible = (int)(usDeltaLon.Abs / frameSize.Abs);
int encodedLat = (int)(key / (MaxZoomLevel + 1) % 0x100000000);
int encodedLon = (int)(key / (MaxZoomLevel + 1) / 0x100000000);
int sizeMultiplierLat = encodedLat - 3 * latNumPossible;
int sizeMultiplierLon = encodedLon - 3 * lonNumPossible;
Angle latLo = Angle.Add(Angle.Multiply(frameSize, sizeMultiplierLat), usMinLat);
Angle lonLo = Angle.Add(Angle.Multiply(frameSize, sizeMultiplierLon), usMinLon);
Angle latHi = Angle.Add(Angle.Multiply(frameSize, sizeMultiplierLat + 1), usMinLat);
Angle lonHi = Angle.Add(Angle.Multiply(frameSize, sizeMultiplierLon + 1), usMinLon);
StandardChunkMetadata ret = new StandardChunkMetadata(
numPixelsForZoom + 1, numPixelsForZoom + 1,
latLo, lonLo,
latHi, lonHi,
zoomLevel, key);
return(ret);
}
public Particle(int Dim, double[] startPos = null, double startAngl = 0.0)
{
SpatialDim = Dim;
// Particle history
// =============================
for (int i = 0; i < m_HistoryLength; i++)
{
Position.Add(new double[Dim]);
Angle.Add(new double());
TranslationalVelocity.Add(new double[Dim]);
TranslationalAcceleration.Add(new double[Dim]);
RotationalVelocity.Add(new double());
RotationalAcceleration.Add(new double());
HydrodynamicForces.Add(new double[Dim]);
HydrodynamicTorque.Add(new double());
}
// =============================
if (startPos == null)
{
startPos = new double[Dim];
}
Position[0] = startPos;
Position[1] = startPos;
//From degree to radiant
Angle[0] = StartingAngle = startAngl * 2 * Math.PI / 360;
Angle[1] = startAngl * 2 * Math.PI / 360;
//UpdateLevelSetFunction();
}
internal StandardChunkMetadata GetParentChunk()
{
var latLoopCenter = Angle.Add(this.LatLo, Angle.Divide(this.LatDelta, 2));
var lonLoopCenter = Angle.Add(this.LonLo, Angle.Divide(this.LonDelta, 2));
return(GetRangeContaingPoint(latLoopCenter, lonLoopCenter, this.ZoomLevel - 1));
}
private static IEnumerable <int> GetShapeCoverage(EdgeShape edge)
{
var minLengthBin = (edge.Length - MaxDistanceError) / MaxDistanceError;
var maxLengthBin = (edge.Length + MaxDistanceError) / MaxDistanceError;
var angleBins = MathEx.DivRoundUp(256, MaxAngleError);
var minReferenceBin = Angle.Difference(edge.ReferenceAngle, MaxAngleError) / MaxAngleError;
var maxReferenceBin = Angle.Add(edge.ReferenceAngle, MaxAngleError) / MaxAngleError;
var endReferenceBin = (maxReferenceBin + 1) % angleBins;
var minNeighborBin = Angle.Difference(edge.NeighborAngle, MaxAngleError) / MaxAngleError;
var maxNeighborBin = Angle.Add(edge.NeighborAngle, MaxAngleError) / MaxAngleError;
var endNeighborBin = (maxNeighborBin + 1) % angleBins;
for (var lengthBin = minLengthBin; lengthBin <= maxLengthBin; ++lengthBin)
{
for (var referenceBin = minReferenceBin;
referenceBin != endReferenceBin;
referenceBin = (referenceBin + 1) % angleBins)
{
for (var neighborBin = minNeighborBin;
neighborBin != endNeighborBin;
neighborBin = (neighborBin + 1) % angleBins)
{
yield return((referenceBin << 24) + (neighborBin << 16) + lengthBin);
}
}
}
}
static List <List <ConsideredOrientation> > GetTestedOrientations()
{
const double minHalfDistance = 2;
const double maxHalfDistance = 6;
const int orientationListSplit = 50;
const int orientationsChecked = 20;
Random random = new Random(0);
List <List <ConsideredOrientation> > allSplits = new List <List <ConsideredOrientation> >();
for (int i = 0; i < orientationListSplit; ++i)
{
List <ConsideredOrientation> orientations = new List <ConsideredOrientation>();
for (int j = 0; j < orientationsChecked; ++j)
{
ConsideredOrientation orientation = new ConsideredOrientation();
do
{
double angle = Angle.FromFraction(random.NextDouble() * 0.5);
double distance = MathEx.InterpolateExponential(minHalfDistance, maxHalfDistance, random.NextDouble());
orientation.CheckLocation = (distance * Angle.ToVector(angle)).Round();
} while (orientation.CheckLocation == new Point() || orientation.CheckLocation.Y < 0);
orientation.OrientationVector = Angle.ToVector(Angle.Add(Angle.ToOrientation(Angle.Atan(orientation.CheckLocation)), Math.PI));
if (!orientations.Any(info => info.CheckLocation == orientation.CheckLocation))
{
orientations.Add(orientation);
}
}
orientations.Sort((left, right) => MathEx.CompareYX(left.CheckLocation, right.CheckLocation));
allSplits.Add(orientations);
}
return(allSplits);
}
/// <summary>
/// Applies rotation to the existing coordinate.
/// </summary>
/// <param name="angle">The amount of rotation to apply (above zero for clockwise).</param>
/// <returns>A <strong>PolarCoordinate</strong> adjusted by the specified rotation amount.</returns>
public PolarCoordinate Rotate(double angle)
{
if (angle == 0)
{
return(this);
}
return(new PolarCoordinate(_r, _theta.Add(angle), _origin, _orientation));
}
/// <summary>
/// Applies rotation to the existing coordinate.
/// </summary>
/// <param name="angle">The amount of rotation to apply (above zero for clockwise).</param>
/// <returns>A <strong>PolarCoordinate</strong> adjusted by the specified rotation amount.</returns>
public PolarCoordinate Rotate(double angle)
{
if (angle == 0)
{
return(this);
}
else
{
return(new PolarCoordinate(_R, _Theta.Add(angle), _Origin, _Orientation));
}
}
public static IEnumerable <Angle> Enumerate360Angles(Angle initialAngle, int increments = 20)
{
var opposite = initialAngle.Opposite();
for (var i = 1; i <= increments; i++)
{
if (i == 1)
{
yield return(initialAngle);
}
else if (i == 1)
{
yield return(opposite);
}
else
{
var increment = 180f * i / increments;
yield return(initialAngle.Add(increment));
yield return(initialAngle.Add(-increment));
}
}
}
public ChunkMetadata(int latSteps, int lonSteps, Angle latLo, Angle lonLo, Angle latHi, Angle lonHi)
{
this.LatSteps = latSteps;
this.LonSteps = lonSteps;
this.LatLo = latLo;
this.LonLo = lonLo;
this.LatHi = latHi;
this.LonHi = lonHi;
this.LatDelta = Angle.Subtract(LatHi, LatLo);
this.LonDelta = Angle.Subtract(LonHi, LonLo);
this.PixelSizeLatDeg = LatDelta.DecimalDegree / (LatSteps - 1);
this.PixelSizeLonDeg = LonDelta.DecimalDegree / (LonSteps - 1);
this.latMid = new Lazy <Angle>(() => Angle.Divide(Angle.Add(this.LatLo, this.LatHi), 2));
this.lonMid = new Lazy <Angle>(() => Angle.Divide(Angle.Add(this.LonLo, this.LonHi), 2));
}
/// <summary>
/// The standard description of motion including hydrodynamics.
/// </summary>
/// <param name="gravity">
/// The gravity (volume forces) acting on the particle.
/// </param>
/// <param name="density">
/// The density of the particle.
/// </param>
public Motion(Vector gravity, double density)
{
if (gravity.IsNullOrEmpty())
{
gravity = new Vector(0, 0);
}
Gravity = new Vector(gravity);
Density = density;
for (int i = 0; i < NumberOfHistoryEntries; i++)
{
Position.Add(new Vector(SpatialDim));
TranslationalVelocity.Add(new Vector(SpatialDim));
TranslationalAcceleration.Add(new Vector(SpatialDim));
HydrodynamicForces.Add(new Vector(SpatialDim));
Angle.Add(new double());
RotationalVelocity.Add(new double());
RotationalAcceleration.Add(new double());
HydrodynamicTorque.Add(new double());
}
}
public IEnumerable <StandardChunkMetadata> GetChildChunks()
{
var ratio = Angle.FloorDivide(frameSizeForZoom[this.ZoomLevel], frameSizeForZoom[this.ZoomLevel + 1]);
var latLoopLo = Angle.Add(this.LatLo, Angle.Divide(frameSizeForZoom[this.ZoomLevel + 1], 2));
var lonLoopLo = Angle.Add(this.LonLo, Angle.Divide(frameSizeForZoom[this.ZoomLevel + 1], 2));
List <StandardChunkMetadata> ret = new List <StandardChunkMetadata>();
for (int i = 0; i < ratio; i++)
{
var li = Angle.Add(latLoopLo, Angle.Multiply(frameSizeForZoom[this.ZoomLevel + 1], i));
for (int j = 0; j < ratio; j++)
{
var lj = Angle.Add(lonLoopLo, Angle.Multiply(frameSizeForZoom[this.ZoomLevel + 1], j));
ret.Add(GetRangeContaingPoint(li, lj, this.ZoomLevel + 1));
}
}
return(ret);
}
protected override async Task <ChunkHolder <MyColor> > GenerateData(StandardChunkMetadata template, TraceListener log)
{
var ret = new ChunkHolder <MyColor>(
template.LatSteps, template.LonSteps,
template.LatLo, template.LonLo,
template.LatHi, template.LonHi,
null,
toDouble,
fromDouble);
var targetChunks = (await UsgsRawImageChunks.GetChunkMetadata(log))
.Select(p => new
{
p,
Chunk = new ChunkMetadata(0, 0,
Angle.FromDecimalDegrees(p.Points.Min(q => q.Item1)),
Angle.FromDecimalDegrees(p.Points.Min(q => q.Item2)),
Angle.FromDecimalDegrees(p.Points.Max(q => q.Item1)),
Angle.FromDecimalDegrees(p.Points.Max(q => q.Item2)))
})
.Where(p => !ret.Disjoint(p.Chunk))
.ToArray();
var chunks = new List <ChunkHolder <MyColor> >();
foreach (var tmp in targetChunks)
{
log?.WriteLine(tmp.Chunk);
var col = await UsgsRawImageChunks.GetRawColors(
Angle.Add(tmp.Chunk.LatLo, Angle.Divide(tmp.Chunk.LatDelta, 2)),
Angle.Add(tmp.Chunk.LonLo, Angle.Divide(tmp.Chunk.LonDelta, 2)), log);
if (col != null)
{
chunks.Add(col);
}
}
ret.RenderChunksInto(chunks, aggregate, log);
return(ret);
}
public IEnumerable <int> HashCoverage(EdgeShape edge)
{
int minLengthBin = (edge.Length - MaxDistanceError) / MaxDistanceError;
int maxLengthBin = (edge.Length + MaxDistanceError) / MaxDistanceError;
int angleBins = 255 / MaxAngleError + 1;
int minReferenceBin = Angle.Difference(edge.ReferenceAngle, MaxAngleError) / MaxAngleError;
int maxReferenceBin = Angle.Add(edge.ReferenceAngle, MaxAngleError) / MaxAngleError;
int endReferenceBin = (maxReferenceBin + 1) % angleBins;
int minNeighborBin = Angle.Difference(edge.NeighborAngle, MaxAngleError) / MaxAngleError;
int maxNeighborBin = Angle.Add(edge.NeighborAngle, MaxAngleError) / MaxAngleError;
int endNeighborBin = (maxNeighborBin + 1) % angleBins;
for (int lengthBin = minLengthBin; lengthBin <= maxLengthBin; ++lengthBin)
{
for (int referenceBin = minReferenceBin; referenceBin != endReferenceBin; referenceBin = (referenceBin + 1) % angleBins)
{
for (int neighborBin = minNeighborBin; neighborBin != endNeighborBin; neighborBin = (neighborBin + 1) % angleBins)
{
yield return((referenceBin << 24) + (neighborBin << 16) + lengthBin);
}
}
}
}
public Angle GetLon(int j)
{
return(Angle.Add(LonLo, Angle.Divide(Angle.Multiply(LonDelta, LonSteps - 1 - j), LonSteps - 1)));
}
public Angle GetLat(int i)
{
return(Angle.Add(LatLo, Angle.Divide(Angle.Multiply(LatDelta, i), LatSteps - 1)));
}
public Angle Add()
{
return(Angle.Add(angle0, angle1));
}
private RobotCommand GetMoveInCircle(Vector middle, double sign1, Robot robot)
{
var angleLeftFromMiddle = new Angle(middle).Add(new Angle(sign1 * Math.PI / 2));
var angleLeft = robot.Direction.Add(new Angle(sign1 * Math.PI / 2));
double angleInTriangle = 0;
if (Math.Sign(angleLeft.Radians) == Math.Sign(angleLeftFromMiddle.Radians))
{
if (Math.Abs(angleLeft.Radians) >= Math.Abs(angleLeftFromMiddle.Radians))
{
angleInTriangle = Math.Abs(angleLeft.Radians) - Math.Abs(angleLeftFromMiddle.Radians);
}
else
{
angleInTriangle = Math.Abs(angleLeftFromMiddle.Radians) - Math.Abs(angleLeft.Radians);
}
}
else if (angleLeft.Radians > 0)
{
var locAng = angleLeftFromMiddle.Add(new Angle(Math.PI));
if (Math.Abs(angleLeft.Radians) >= Math.Abs(locAng.Radians))
{
angleInTriangle = Math.Abs(angleLeft.Radians) - Math.Abs(locAng.Radians);
}
else
{
angleInTriangle = Math.Abs(locAng.Radians) - Math.Abs(angleLeft.Radians);
}
}
else
{
var locAng = angleLeft.Add(new Angle(Math.PI));
if (Math.Abs(locAng.Radians) >= Math.Abs(angleLeftFromMiddle.Radians))
{
angleInTriangle = Math.Abs(locAng.Radians) - Math.Abs(angleLeftFromMiddle.Radians);
}
else
{
angleInTriangle = Math.Abs(angleLeftFromMiddle.Radians) - Math.Abs(locAng.Radians);
}
}
angleInTriangle = Math.PI / 2 - angleInTriangle;
//var angleInTriangle = Math.Abs(new Angle(middle).Add(angleLeft).Radians);
var radiusInNewCentre = Math.Abs(middle.Length / Math.Cos(angleInTriangle));
var newCentre = new Vector(angleLeft, radiusInNewCentre).Add(robot.Position);
var newSpeed = Math.Min(radiusInNewCentre * robot.AngularVelocity, robot.LinearVelocity);
var angleInCircuit = Math.Acos((
Math.Pow(Destination.Subtract(newCentre).Length, 2) +
Math.Pow(robot.Position.Subtract(newCentre).Length, 2) -
Math.Pow(Destination.Subtract(robot.Position).Length, 2)) /
(2 * Destination.Subtract(newCentre).Length *robot.Position.Subtract(newCentre).Length));
var sign = Math.Sign(
Math.PI / 2 - Math.Abs(robot.Direction.Substract(new Angle(Destination.Subtract(robot.Position))).Radians));
if (sign == 0)
{
sign = 1;
}
return(new RobotCommand(
angleInCircuit / robot.AngularVelocity, sign * newSpeed, sign1 * robot.AngularVelocity));
}
public void AddIsAssociative(Angle x, Angle y, Angle z)
{
Assert.Equal(
x.Add(y).Add(z),
x.Add(y.Add(z)));
}
public void AddHasIdentity(Angle x)
{
Assert.Equal(x, Angle.Identity.Add(x));
Assert.Equal(x, x.Add(Angle.Identity));
}
private RobotCommand GetMoveOutCircle(Vector middle, double sign1, Robot robot)
{
var angleLeftFromMiddle = new Angle(middle).Add(new Angle(sign1 * Math.PI / 2));
var angleLeft = robot.Direction.Add(new Angle(sign1 * Math.PI / 2));
double angleInTriangle = 0;
if (Math.Sign(angleLeft.Radians) == Math.Sign(angleLeftFromMiddle.Radians))
{
if (Math.Abs(angleLeft.Radians) >= Math.Abs(angleLeftFromMiddle.Radians))
{
angleInTriangle = Math.Abs(angleLeft.Radians) - Math.Abs(angleLeftFromMiddle.Radians);
}
else
{
angleInTriangle = Math.Abs(angleLeftFromMiddle.Radians) - Math.Abs(angleLeft.Radians);
}
}
else if (angleLeft.Radians > 0)
{
var locAng = angleLeftFromMiddle.Add(new Angle(Math.PI));
if (Math.Abs(angleLeft.Radians) >= Math.Abs(locAng.Radians))
{
angleInTriangle = Math.Abs(angleLeft.Radians) - Math.Abs(locAng.Radians);
}
else
{
angleInTriangle = Math.Abs(locAng.Radians) - Math.Abs(angleLeft.Radians);
}
}
else
{
var locAng = angleLeft.Add(new Angle(Math.PI));
if (Math.Abs(locAng.Radians) >= Math.Abs(angleLeftFromMiddle.Radians))
{
angleInTriangle = Math.Abs(locAng.Radians) - Math.Abs(angleLeftFromMiddle.Radians);
}
else
{
angleInTriangle = Math.Abs(angleLeftFromMiddle.Radians) - Math.Abs(locAng.Radians);
}
}
angleInTriangle = Math.PI / 2 - angleInTriangle;
//angleInTriangle = Math.Abs(new Angle(middle).Add(angleLeft).Radians);
var radiusInNewCentre = Math.Abs(middle.Length / Math.Cos(angleInTriangle));
var newCentre = new Vector(angleLeft, radiusInNewCentre).Add(robot.Position);
var newSpeed = robot.LinearVelocity / radiusInNewCentre;
var angleInCircuit = Math.Pow(Destination.Subtract(newCentre).Length, 2);//теорема коминусов; решение треугольника зня 3 его стороны;
angleInCircuit += Math.Pow(robot.Position.Subtract(newCentre).Length, 2);
angleInCircuit -= Math.Pow(Destination.Subtract(robot.Position).Length, 2);
angleInCircuit /= (2 * Destination.Subtract(newCentre).Length *robot.Position.Subtract(newCentre).Length);
angleInCircuit = Math.Acos(angleInCircuit);
var sign = Math.Sign(
Math.PI / 2 -
Math.Abs(robot.Direction.Substract(new Angle(Destination.Subtract(robot.Position))).Radians));
if (sign == 0)
{
sign = 1;
}
double duration = angleInCircuit / newSpeed;
if (angleInCircuit < 1e-6 || newSpeed < 1e-6)
{
duration = Destination.Subtract(robot.Position).Length / robot.LinearVelocity;
}
return(new RobotCommand(duration, sign * robot.LinearVelocity, sign1 * newSpeed));
}
private IEnumerable <RobotCommand> GetCommandsNew(Robot robot)
{
var radius = robot.LinearVelocity / robot.AngularVelocity;
var middle = Destination.Subtract(robot.Position).Multiply(1.0 / 2.0);
var triangleSideLength = Math.Sqrt(Math.Pow(radius, 2) - Math.Pow(middle.Length, 2));
var leftAngleSize = new Angle(middle).Substract(robot.Direction);
var sign = 0;
if (leftAngleSize.Radians > 0)
{
sign = 1;
}
else
{
sign = -1;
}
//var sign = Math.Sign(robot.Direction.Substract(new Angle(middle)).Radians);
var angleForVectorInNewCentre = new Angle(middle).Add(new Angle(sign * Math.PI / 2));
var newCenter = middle.Add(new Vector(angleForVectorInNewCentre, triangleSideLength));
var angleFromCentre = new Angle(newCenter.Subtract(robot.Position));
var circleTangentAngle = angleFromCentre.Add(new Angle(sign * Math.PI / 2));//не хочет ехать задом и поворачивать на малый угол :(
if (Math.Abs(circleTangentAngle.Radians) > Math.PI / 2)
{
circleTangentAngle = circleTangentAngle.Add(new Angle(-1 * sign * Math.PI / 2));
}
var rotateAngle = robot.Direction.Substract(circleTangentAngle);
var normalizeRotateAngle = new Angle(-1 * rotateAngle.Radians);
if (normalizeRotateAngle.Radians < -Math.PI / 2)
{
normalizeRotateAngle = new Angle(new Angle(Math.PI).Add(normalizeRotateAngle).Radians);
}
if (normalizeRotateAngle.Radians > Math.PI / 2)
{
normalizeRotateAngle = new Angle(normalizeRotateAngle.Substract(new Angle(Math.PI)).Radians);
}
var rotateDuration = Math.Abs(normalizeRotateAngle.Radians / robot.AngularVelocity);
if (!Double.IsNaN(rotateDuration))
{
yield return(new RobotCommand(
rotateDuration, 0, Math.Sign(normalizeRotateAngle.Radians) * robot.AngularVelocity));
}
else
{
yield return(new RobotCommand(Double.PositiveInfinity, 0, 0));
yield break;
}
var angleInCircle = 2 * Math.Asin(middle.Length / radius);
var duration = Math.Abs(angleInCircle / robot.AngularVelocity);
if (duration < 1e-6)
{
duration = (middle.Length * 2) / robot.LinearVelocity;
}
if (Double.IsNaN(duration))
{
duration = Double.PositiveInfinity;
}
yield return(new RobotCommand(
duration, robot.LinearVelocity, Math.Sign(normalizeRotateAngle.Radians) * robot.AngularVelocity));
}
public void Update(double dt)
{
deltaT = dt;
double roll_t = roll.GetValue();
double pitch_t = pitch.GetValue();
double yaw_t = yaw.GetValue();
double cr = System.Math.Cos(roll_t);
double sr = System.Math.Sin(roll_t);
double cp = System.Math.Cos(pitch_t);
double sp = System.Math.Sin(pitch_t);
double ch = System.Math.Cos(yaw_t);
double sh = System.Math.Sin(yaw_t);
wv_b.SetVec(wv);
wv.x = (ov.x * (cr * ch + sr * sp * sh) - ov.y
* (sr * ch - cr * sp * sh) + ov.z * (cp * sh));
wv.y = (-(ov.x * (-sr * cp) - ov.y * (cr * cp) + ov.z
* sp));
wv.z = (ov.x * (-cr * sh + sr * sp * ch) - ov.y
* (-sr * sh - cr * sp * ch) + ov.z * (cp * ch));
dpitch_b = dpitch;
droll_b = droll;
dyaw_b = dheading;
if (pitch_t == 1.570796326794897D)
{
droll = 0.0D;
dpitch = (ow.x + ow.y);
dheading = (-ow.z);
}
else if (pitch_t == -1.570796326794897D)
{
droll = 0.0D;
dpitch = (ow.x + ow.y);
dheading = ow.z;
}
else
{
droll = (-(-ow.z - ow.x * sr * sp / cp - ow.y
* cr * sp / cp));
dpitch = (-(-ow.x * cr + ow.y * sr));
dheading = (-(-ow.x * sr / cp - ow.y * cr / cp));
}
wpos_b.SetVec(wpos);
if (pos_integral_mode == 0)
{
wpos.x += 0.5D * (3.0D * wv.x - wv_b.x) * dt;
wpos.y += 0.5D * (3.0D * wv.y - wv_b.y) * dt;
wpos.z += 0.5D * (3.0D * wv.z - wv_b.z) * dt;
}
else
{
wpos.x += 0.5D * (wv.x + wv_b.x) * dt;
wpos.y += 0.5D * (wv.y + wv_b.y) * dt;
wpos.z += 0.5D * (wv.z + wv_b.z) * dt;
}
if (pos_integral_mode == 0)
{
roll.Add(droll * dt);
pitch.Add(dpitch * dt);
yaw.Add(dheading * dt);
}
else
{
roll.Add(droll_b * dt);
pitch.Add(dpitch_b * dt);
yaw.Add(dyaw_b * dt);
}
if (pitch.GetValue() == 1.570796326794897D)
{
yaw.Sub(roll.GetValue(0));
roll.SetValue(0.0D);
}
if (pitch.GetValue() == -1.570796326794897D)
{
yaw.Add(roll.GetValue(0));
roll.SetValue(0.0D);
}
if (pitch.GetValue() > 1.570796326794897D)
{
pitch.SetValue(Math.PI - pitch.GetValue());
yaw.Reverse();
roll.Reverse();
}
if (pitch.GetValue() < -1.570796326794897D)
{
pitch.SetValue(-Math.PI - pitch.GetValue());
yaw.Reverse();
roll.Reverse();
}
SetCoodinateConvertMatrix();
}
