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(); }