/// <summary>
/// Calculates an inverse of the matrix if it exists.
/// </summary>
public void Invert()
{
float[,] invVals = new float[3, 3];
invVals[0, 0] = m11 * m22 - m12 * m21;
invVals[1, 0] = m12 * m20 - m10 * m22;
invVals[2, 0] = m10 * m21 - m11 * m20;
float det = m00 * invVals[0, 0] + m01 * invVals[1, 0] + m02 * invVals[2, 0];
if (MathEx.Abs(det) <= 1e-06f)
{
throw new DivideByZeroException("Matrix determinant is zero. Cannot invert.");
}
invVals[0, 1] = m02 * m21 - m01 * m22;
invVals[0, 2] = m01 * m12 - m02 * m11;
invVals[1, 1] = m00 * m22 - m02 * m20;
invVals[1, 2] = m02 * m10 - m00 * m12;
invVals[2, 1] = m01 * m20 - m00 * m21;
invVals[2, 2] = m00 * m11 - m01 * m10;
float invDet = 1.0f / det;
for (int row = 0; row < 3; row++)
{
for (int col = 0; col < 3; col++)
{
invVals[row, col] *= invDet;
}
}
}
private void UpdateScrollBarPosition()
{
Vector2 visibleRange = guiCurveEditor.Range;
Vector2 totalRange = GetTotalRange();
Vector2 scrollableRange = totalRange - visibleRange;
Vector2 offset = guiCurveEditor.Offset;
if (scrollableRange.x > 0.0f)
{
horzScrollBar.Position = offset.x / scrollableRange.x;
}
else
{
horzScrollBar.Position = 0.0f;
}
if (scrollableRange.y > 0.0f)
{
float pos = offset.y / scrollableRange.y;
float sign = MathEx.Sign(pos);
pos = sign * MathEx.Clamp01(MathEx.Abs(pos));
pos = (1.0f - pos) / 2.0f;
vertScrollBar.Position = pos;
}
else
{
vertScrollBar.Position = 0.0f;
}
}
/// <summary>
/// Draws text displaying the value at the provided position.
/// </summary>
/// <param name="yPos">Position to draw the text at.</param>
/// <param name="value">Value to display.</param>
/// <param name="above">If true the text will be displayed above the provided position, otherwise below.</param>
private void DrawValue(int yPos, float value, bool above)
{
int exponent = MathEx.FloorToInt(MathEx.Log10(MathEx.Abs(value)));
int maxDecimalPoints = MathEx.Max(0, 1 - exponent);
string valueString = value.ToString("F" + maxDecimalPoints);
Vector2I textBounds = GUIUtility.CalculateTextBounds(valueString, EditorBuiltin.DefaultFont,
EditorStyles.DefaultFontSize);
Vector2I textPosition = new Vector2I();
textPosition.x = width - textBounds.x;
if (above)
{
textPosition.y = yPos - textBounds.y;
}
else // Below
{
const int PADDING = 3; // So the text doesn't touch the tick
textPosition.y = yPos + PADDING;
}
canvas.DrawText(valueString, textPosition, EditorBuiltin.DefaultFont, COLOR_TRANSPARENT_LIGHT_GRAY,
EditorStyles.DefaultFontSize);
}
/// <summary>
/// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
/// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
/// </summary>
/// <param name="from">Start quaternion.</param>
/// <param name="to">End quaternion.</param>
/// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
/// <paramref name="from"/> and <paramref name="to"/>.</param>
/// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
/// the two quaternions.</param>
/// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
/// <paramref name="to"/>.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
{
float dot = Dot(from, to);
Quaternion quat;
if (dot < 0.0f && shortestPath)
{
dot = -dot;
quat = -to;
}
else
{
quat = to;
}
if (MathEx.Abs(dot) < (1 - epsilon))
{
float sin = MathEx.Sqrt(1 - (dot * dot));
Radian angle = MathEx.Atan2(sin, dot);
float invSin = 1.0f / sin;
float a = MathEx.Sin((1.0f - t) * angle) * invSin;
float b = MathEx.Sin(t * angle) * invSin;
return(a * from + b * quat);
}
else
{
Quaternion ret = (1.0f - t) * from + t * quat;
ret.Normalize();
return(ret);
}
}
private static DirectResult Direct(double lat1, double lon1, double crs12, double d12)
{
const double eps = 0.00000000005;
double lon;
if ((MathEx.Abs(MathEx.Cos(lat1)) < eps) && !(MathEx.Abs(MathEx.Sin(crs12)) < eps))
{
throw new ArgumentException("Only N-S courses are meaningful, starting at a pole!");
}
double lat = MathEx.Asin(MathEx.Sin(lat1) * MathEx.Cos(d12) +
MathEx.Cos(lat1) * MathEx.Sin(d12) * MathEx.Cos(crs12));
if (MathEx.Abs(MathEx.Cos(lat)) < eps)
{
lon = 0.0; //endpoint a pole
}
else
{
double dlon = MathEx.Atan2(MathEx.Sin(crs12) * MathEx.Sin(d12) * MathEx.Cos(lat1),
MathEx.Cos(d12) - MathEx.Sin(lat1) * MathEx.Sin(lat));
lon = Mod(lon1 - dlon + MathEx.PI, 2 * MathEx.PI) - MathEx.PI;
}
DirectResult outValue = new DirectResult {
_lat = lat, _lon = lon
};
return(outValue);
}
private static CrsdistResult Crsdist(double lat1, double lon1, double lat2, double lon2)
{ // radian args
/* compute course and Distance (spherical) */
double crs12, crs21;
double argacos;
if ((lat1 + lat2 == 0.0) && (MathEx.Abs(lon1 - lon2) == MathEx.PI) && (MathEx.Abs(lat1) != (MathEx.PI / 180) * 90.0))
{
throw new ArgumentException("Course between antipodal points is undefined");
}
double d = MathEx.Acos(MathEx.Sin(lat1) * MathEx.Sin(lat2) + MathEx.Cos(lat1) * MathEx.Cos(lat2) * MathEx.Cos(lon1 - lon2));
if ((d == 0.0) || (lat1 == -(MathEx.PI / 180) * 90.0))
{
crs12 = 2 * MathEx.PI;
}
else if (lat1 == (MathEx.PI / 180) * 90.0)
{
crs12 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat2) - MathEx.Sin(lat1) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat1));
if (MathEx.Sin(lon2 - lon1) < 0)
{
crs12 = Acosf(argacos);
}
else
{
crs12 = 2 * MathEx.PI - Acosf(argacos);
}
}
if ((d == 0.0) || (lat2 == -(MathEx.PI / 180) * 90.0))
{
crs21 = 0.0;
}
else if (lat2 == (MathEx.PI / 180) * 90.0)
{
crs21 = MathEx.PI;
}
else
{
argacos = (MathEx.Sin(lat1) - MathEx.Sin(lat2) * MathEx.Cos(d)) / (MathEx.Sin(d) * MathEx.Cos(lat2));
if (MathEx.Sin(lon1 - lon2) < 0)
{
crs21 = Acosf(argacos);
}
else
{
crs21 = 2 * MathEx.PI - Acosf(argacos);
}
}
CrsdistResult outValue = new CrsdistResult {
_d = d, _crs12 = crs12, _crs21 = crs21
};
return(outValue);
}
private static double Acosf(double x)
{ /* protect against rounding error on input argument */
if (MathEx.Abs(x) > 1)
{
x /= MathEx.Abs(x);
}
return(MathEx.Acos(x));
}
////////////////////////////////////////////////////////////////////////////
//--------------------------------- REVISIONS ------------------------------
// Date Name Tracking # Description
// --------- ------------------- ------------- ----------------------
// 13JUN2009 James Shen Initial Creation
////////////////////////////////////////////////////////////////////////////
internal ArcIterator(Arc a, AffineTransform at)
{
_w = a.GetWidth() / 2.0;
_h = a.GetHeight() / 2.0;
_x = a.GetX() + _w;
_y = a.GetY() + _h;
_angStRad = -MathEx.ToRadians(a.GetAngleStart());
_affine = at;
double ext = -a.GetAngleExtent();
if (ext >= 360.0 || ext <= -360)
{
_arcSegs = 4;
_increment = Math.PI / 2;
// btan(Math.PI / 2);
_cv = 0.5522847498307933;
if (ext < 0)
{
_increment = -_increment;
_cv = -_cv;
}
}
else
{
_arcSegs = (int)MathEx.Ceil(MathEx.Abs(ext) / 90.0);
_increment = MathEx.ToRadians(ext / _arcSegs);
_cv = Btan(_increment);
if (_cv == 0)
{
_arcSegs = 0;
}
}
switch (a.GetArcType())
{
case Arc.OPEN:
_lineSegs = 0;
break;
case Arc.CHORD:
_lineSegs = 1;
break;
case Arc.PIE:
_lineSegs = 2;
break;
}
if (_w < 0 || _h < 0)
{
_arcSegs = _lineSegs = -1;
}
}
/// <summary>
/// Transforms the bounding box by the given matrix.
///
/// As the resulting box will no longer be axis aligned, an axis align box is instead created by encompassing the
/// transformed oriented bounding box. Retrieving the value as an actual OBB would provide a tighter fit.
/// </summary>
/// <param name="tfrm">Affine matrix to transform the box with.</param>
public void TransformAffine(Matrix4 tfrm)
{
Vector3 center = Center;
Vector3 halfSize = Size * 0.5f;
Vector3 newCenter = tfrm.MultiplyAffine(center);
Vector3 newHalfSize = new Vector3(
MathEx.Abs(tfrm.m00) * halfSize.x + MathEx.Abs(tfrm.m01) * halfSize.y + MathEx.Abs(tfrm.m02) * halfSize.z,
MathEx.Abs(tfrm.m10) * halfSize.x + MathEx.Abs(tfrm.m11) * halfSize.y + MathEx.Abs(tfrm.m12) * halfSize.z,
MathEx.Abs(tfrm.m20) * halfSize.x + MathEx.Abs(tfrm.m21) * halfSize.y + MathEx.Abs(tfrm.m22) * halfSize.z);
minimum = newCenter - newHalfSize;
maximum = newCenter + newHalfSize;
}
/// <inheritdoc/>
protected internal override void PreInput()
{
Camera cam = EditorApplication.SceneViewCamera;
if (cam == null)
{
return;
}
position = new Vector3(0, 0, -5.0f);
rotation = cam.SceneObject.Rotation.Inverse;
Vector3 xOffset = rotation.Rotate(new Vector3(BOX_EXTENT, 0.0f, 0.0f));
Vector3 yOffset = rotation.Rotate(new Vector3(0.0f, BOX_EXTENT, 0.0f));
Vector3 zOffset = rotation.Rotate(new Vector3(0.0f, 0.0f, BOX_EXTENT));
xAxis.Position = position + xOffset;
yAxis.Position = position + yOffset;
zAxis.Position = position + zOffset;
xAxis.Rotation = rotation;
yAxis.Rotation = rotation;
zAxis.Rotation = rotation;
xNegAxis.Position = position - xOffset;
yNegAxis.Position = position - yOffset;
zNegAxis.Position = position - zOffset;
xNegAxis.Rotation = rotation;
yNegAxis.Rotation = rotation;
zNegAxis.Rotation = rotation;
Vector3 cameraForward = cam.SceneObject.Forward;
xAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.XAxis)) < DISABLE_THRESHOLD;
yAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.YAxis)) < DISABLE_THRESHOLD;
zAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.ZAxis)) < DISABLE_THRESHOLD;
xNegAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.XAxis)) < DISABLE_THRESHOLD;
yNegAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.YAxis)) < DISABLE_THRESHOLD;
zNegAxis.Enabled = MathEx.Abs(Vector3.Dot(cameraForward, Vector3.ZAxis)) < DISABLE_THRESHOLD;
Vector3 freeAxisOffset = new Vector3(-BOX_EXTENT, -BOX_EXTENT, 0.2f);
projTypePlane.Rotation = Quaternion.Identity;
projTypePlane.Position = position + freeAxisOffset;
}
/// <summary>
/// Orients the camera so it looks along the provided axis.
/// </summary>
public void LookAlong(Vector3 axis)
{
Vector3 up = Vector3.YAxis;
if (MathEx.Abs(Vector3.Dot(axis, up)) > 0.9f)
{
up = Vector3.ZAxis;
}
CameraState state = new CameraState();
state.Position = camera.SceneObject.Position;
state.Rotation = Quaternion.LookRotation(axis, up);
state.Ortographic = camera.ProjectionType == ProjectionType.Orthographic;
state.FrustumWidth = frustumWidth;
SetState(state);
}
private FrameworkElement FindFocusableControl(IntEx factor, FrameworkElement from, SearchDirection direction)
{
var fromLocation = GetLocation(from);
return(this.tabList
.Select(c => new { Location = GetLocation(c), Control = c })
.OrderBy(x =>
{
switch (direction)
{
case SearchDirection.Left:
return -x.Location.Right;
case SearchDirection.Top:
return -x.Location.Bottom;
case SearchDirection.Bottom:
return x.Location.Top;
case SearchDirection.Right:
return x.Location.Left;
default:
throw new ArgumentException();
}
})
.Where(c =>
{
switch (direction)
{
case SearchDirection.Left:
{
var bottomGreaterControlTop = fromLocation.Top - MathEx.Abs(fromLocation.Left - c.Location.Left) / factor < c.Location.Bottom;
var topGreaterControlTop = fromLocation.Top - MathEx.Abs(fromLocation.Left - c.Location.Left) / factor < c.Location.Top;
var topLessthenControlBotom = fromLocation.Bottom + MathEx.Abs(fromLocation.Left - c.Location.Left) / factor > c.Location.Top;
var bottomLessthenControlBotom = fromLocation.Bottom + MathEx.Abs(fromLocation.Left - c.Location.Left) / factor > c.Location.Bottom;
return fromLocation.Left > c.Location.Left &&
(
bottomGreaterControlTop && !topGreaterControlTop ||
topLessthenControlBotom && !bottomLessthenControlBotom ||
topGreaterControlTop && bottomLessthenControlBotom ||
!topGreaterControlTop && !bottomLessthenControlBotom
);
}
case SearchDirection.Top:
{
var rightGreaterControlLeft = fromLocation.Left - MathEx.Abs(fromLocation.Top - c.Location.Top) / factor < c.Location.Right;
var leftGreaterControlLeft = fromLocation.Left - MathEx.Abs(fromLocation.Top - c.Location.Top) / factor < c.Location.Left;
var leftLessthenControlRight = fromLocation.Right + MathEx.Abs(fromLocation.Top - c.Location.Top) / factor > c.Location.Right;
var rightLessthenControlRight = fromLocation.Right + MathEx.Abs(fromLocation.Top - c.Location.Top) / factor > c.Location.Left;
return fromLocation.Top > c.Location.Top &&
(
rightGreaterControlLeft && !leftGreaterControlLeft ||
leftLessthenControlRight && !rightLessthenControlRight ||
leftGreaterControlLeft && rightLessthenControlRight ||
!leftGreaterControlLeft && !rightLessthenControlRight
);
}
case SearchDirection.Bottom:
{
var rightGreaterControlLeft = fromLocation.Left - MathEx.Abs(fromLocation.Bottom - c.Location.Bottom) / factor < c.Location.Right;
var leftGreaterControlLeft = fromLocation.Left - MathEx.Abs(fromLocation.Bottom - c.Location.Bottom) / factor < c.Location.Left;
var leftLessthenControlRight = fromLocation.Right + MathEx.Abs(fromLocation.Bottom - c.Location.Bottom) / factor > c.Location.Right;
var rightLessthenControlRight = fromLocation.Right + MathEx.Abs(fromLocation.Bottom - c.Location.Bottom) / factor > c.Location.Left;
return fromLocation.Bottom < c.Location.Bottom &&
(
rightGreaterControlLeft && !leftGreaterControlLeft ||
leftLessthenControlRight && !rightLessthenControlRight ||
leftGreaterControlLeft && rightLessthenControlRight ||
!leftGreaterControlLeft && !rightLessthenControlRight
);
}
case SearchDirection.Right:
{
var bottomGreaterControlTop = fromLocation.Top - MathEx.Abs(fromLocation.Right - c.Location.Right) / factor < c.Location.Bottom;
var topGreaterControlTop = fromLocation.Top - MathEx.Abs(fromLocation.Right - c.Location.Right) / factor < c.Location.Top;
var topLessthenControlBotom = fromLocation.Bottom + MathEx.Abs(fromLocation.Right - c.Location.Right) / factor > c.Location.Top;
var bottomLessthenControlBotom = fromLocation.Bottom + MathEx.Abs(fromLocation.Right - c.Location.Right) / factor > c.Location.Bottom;
return fromLocation.Right < c.Location.Right &&
(
bottomGreaterControlTop && !topGreaterControlTop ||
topLessthenControlBotom && !bottomLessthenControlBotom ||
topGreaterControlTop && bottomLessthenControlBotom ||
!topGreaterControlTop && !bottomLessthenControlBotom
);
}
default:
throw new ArgumentException();
}
})
.FirstOrDefault()?.Control);
}
/// <inheritdoc/>
protected internal override void Draw()
{
HandleDrawing.Layer = LAYER;
HandleDrawing.Transform = Matrix4.TRS(position, rotation, Vector3.One);
Vector3 cameraForward = EditorApplication.SceneViewCamera.SceneObject.Forward;
// Draw 1D arrows
Color xColor = Color.Red;
if (xAxis.State == HandleSlider.StateType.Active)
{
xColor = Color.White;
}
else if (xAxis.State == HandleSlider.StateType.Hover)
{
xColor = Color.BansheeOrange;
}
xColor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.XAxis)), 0.8f, 1.0f);
HandleDrawing.Color = xColor;
Vector3 xLineStart = Vector3.XAxis * BOX_EXTENT;
Vector3 xConeStart = Vector3.XAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(xLineStart, xConeStart);
HandleDrawing.DrawCone(xConeStart, Vector3.XAxis, CONE_HEIGHT, CONE_RADIUS);
Color yColor = Color.Green;
if (yAxis.State == HandleSlider.StateType.Active)
{
yColor = Color.White;
}
else if (yAxis.State == HandleSlider.StateType.Hover)
{
yColor = Color.BansheeOrange;
}
yColor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.YAxis)), 0.8f, 1.0f);
HandleDrawing.Color = yColor;
Vector3 yLineStart = Vector3.YAxis * BOX_EXTENT;
Vector3 yConeStart = Vector3.YAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(yLineStart, yConeStart);
HandleDrawing.DrawCone(yConeStart, Vector3.YAxis, CONE_HEIGHT, CONE_RADIUS);
Color zColor = Color.Blue;
if (zAxis.State == HandleSlider.StateType.Active)
{
zColor = Color.White;
}
else if (zAxis.State == HandleSlider.StateType.Hover)
{
zColor = Color.BansheeOrange;
}
zColor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.ZAxis)), 0.8f, 1.0f);
HandleDrawing.Color = zColor;
Vector3 zLineStart = Vector3.ZAxis * BOX_EXTENT;
Vector3 zConeStart = Vector3.ZAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(zLineStart, zConeStart);
HandleDrawing.DrawCone(zConeStart, Vector3.ZAxis, CONE_HEIGHT, CONE_RADIUS);
// Draw negative 1D arrows
Color xNegColor = Color.LightGray;
if (xNegAxis.State == HandleSlider.StateType.Active)
{
xNegColor = Color.White;
}
else if (xNegAxis.State == HandleSlider.StateType.Hover)
{
xNegColor = Color.BansheeOrange;
}
xNegColor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.XAxis)), 0.8f, 1.0f);
HandleDrawing.Color = xNegColor;
Vector3 xNegLineStart = -Vector3.XAxis * BOX_EXTENT;
Vector3 xNegConeStart = -Vector3.XAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(xNegLineStart, xNegConeStart);
HandleDrawing.DrawCone(xNegConeStart, -Vector3.XAxis, CONE_HEIGHT, CONE_RADIUS);
Color yNegColor = Color.LightGray;
if (yNegAxis.State == HandleSlider.StateType.Active)
{
yNegColor = Color.White;
}
else if (yNegAxis.State == HandleSlider.StateType.Hover)
{
yNegColor = Color.BansheeOrange;
}
yNegColor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.YAxis)), 0.8f, 1.0f);
HandleDrawing.Color = yNegColor;
Vector3 yNegLineStart = -Vector3.YAxis * BOX_EXTENT;
Vector3 yNegConeStart = -Vector3.YAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(yNegLineStart, yNegConeStart);
HandleDrawing.DrawCone(yNegConeStart, -Vector3.YAxis, CONE_HEIGHT, CONE_RADIUS);
Color zNegcolor = Color.LightGray;
if (zNegAxis.State == HandleSlider.StateType.Active)
{
zNegcolor = Color.White;
}
else if (zNegAxis.State == HandleSlider.StateType.Hover)
{
zNegcolor = Color.BansheeOrange;
}
zNegcolor.a = MathEx.Lerp(1.0f, 0.0f, MathEx.Abs(Vector3.Dot(cameraForward, Vector3.ZAxis)), 0.8f, 1.0f);
HandleDrawing.Color = zNegcolor;
Vector3 zNegLineStart = -Vector3.ZAxis * BOX_EXTENT;
Vector3 zNegConeStart = -Vector3.ZAxis * (1.0f - CONE_HEIGHT);
HandleDrawing.DrawLine(zNegLineStart, zNegConeStart);
HandleDrawing.DrawCone(zNegConeStart, -Vector3.ZAxis, CONE_HEIGHT, CONE_RADIUS);
// Draw projection type handle
if (projTypePlane.State == HandleSlider.StateType.Active)
{
HandleDrawing.Color = Color.White;
}
else if (projTypePlane.State == HandleSlider.StateType.Hover)
{
HandleDrawing.Color = Color.BansheeOrange;
}
else
{
HandleDrawing.Color = Color.White;
}
HandleDrawing.DrawCube(Vector3.Zero, new Vector3(BOX_EXTENT, BOX_EXTENT, BOX_EXTENT));
}
/// <summary>
/// Calculates an angle between two rotations.
/// </summary>
/// <param name="a">First rotation.</param>
/// <param name="b">Second rotation.</param>
/// <returns>Angle between the rotations, in degrees.</returns>
public static Degree Angle(Quaternion a, Quaternion b)
{
return(MathEx.Acos(MathEx.Min(MathEx.Abs(Dot(a, b)), 1.0f)) * 2.0f);
}
public static Fix64 ManhattanDist(Vec2 v1, Vec2 v2)
{
return(MathEx.Abs(v1.x - v2.x) + MathEx.Abs(v1.y - v2.y));
}
private static DirectResult DirectEll(double glat1, double glon1, double faz, double s, int ellipse)
{
// glat1 initial geodetic latitude in radians N positive
// glon1 initial geodetic longitude in radians E positive
// faz forward azimuth in radians
// s Distance in units of a (=nm)
const double eps = 0.00000000005;
double b;
double sy = 0, cy = 0, cz = 0, e = 0;
if ((MathEx.Abs(MathEx.Cos(glat1)) < eps) && !(MathEx.Abs(MathEx.Sin(faz)) < eps))
{
throw new ArgumentException("Only N-S courses are meaningful, starting at a pole!");
}
double a = EarthModel[ellipse][0];
double f = 1 / EarthModel[ellipse][1];
double r = 1 - f;
double tu = r * MathEx.Tan(glat1);
double sf = MathEx.Sin(faz);
double cf = MathEx.Cos(faz);
if (cf == 0)
{
b = 0.0;
}
else
{
b = 2.0 * Atan2(tu, cf);
}
double cu = 1.0 / MathEx.Sqrt(1 + tu * tu);
double su = tu * cu;
double sa = cu * sf;
double c2A = 1 - sa * sa;
double x = 1.0 + MathEx.Sqrt(1.0 + c2A * (1.0 / (r * r) - 1.0));
x = (x - 2.0) / x;
double c = 1.0 - x;
c = (x * x / 4.0 + 1.0) / c;
double d = (0.375 * x * x - 1.0) * x;
tu = s / (r * a * c);
double y = tu;
c = y + 1;
while (MathEx.Abs(y - c) > eps)
{
sy = MathEx.Sin(y);
cy = MathEx.Cos(y);
cz = MathEx.Cos(b + y);
e = 2.0 * cz * cz - 1.0;
c = y;
x = e * cy;
y = e + e - 1.0;
y = (((sy * sy * 4.0 - 3.0) * y * cz * d / 6.0 + x) *
d / 4.0 - cz) * sy * d + tu;
}
b = cu * cy * cf - su * sy;
c = r * MathEx.Sqrt(sa * sa + b * b);
d = su * cy + cu * sy * cf;
double glat2 = Modlat(Atan2(d, c));
c = cu * cy - su * sy * cf;
x = Atan2(sy * sf, c);
c = ((-3.0 * c2A + 4.0) * f + 4.0) * c2A * f / 16.0;
d = ((e * cy * c + cz) * sy * c + y) * sa;
double glon2 = Modlon(glon1 + x - (1.0 - c) * d * f);
double baz = Modcrs(Atan2(sa, b) + MathEx.PI);
DirectResult outValue = new DirectResult {
_lat = glat2, _lon = glon2, _crs21 = baz
};
return(outValue);
}
private static CrsdistResult CrsdistEll(double glat1, double glon1, double glat2, double glon2, int ellipse)
{
// glat1 initial geodetic latitude in radians N positive
// glon1 initial geodetic longitude in radians E positive
// glat2 final geodetic latitude in radians N positive
// glon2 final geodetic longitude in radians E positive
double a = EarthModel[ellipse][0];
double f = 1 / EarthModel[ellipse][1];
//alert("a="+a+" f="+f)
double sx = 0, cx = 0, sy = 0, cy = 0, y = 0;
double c2A = 0, cz = 0, e = 0, c;
const double eps = 0.00000000005;
double iter = 1;
const double maxiter = 100;
CrsdistResult outValue = new CrsdistResult();
if ((glat1 + glat2 == 0.0) && (MathEx.Abs(glon1 - glon2) == MathEx.PI))
{
glat1 = glat1 + 0.00001; // allow algorithm to complete
}
if (glat1 == glat2 && (glon1 == glon2 || MathEx.Abs(MathEx.Abs(glon1 - glon2) - 2 * MathEx.PI) < eps))
{
outValue._d = 0;
outValue._crs12 = 0;
outValue._crs21 = MathEx.PI;
return(outValue);
}
double r = 1 - f;
double tu1 = r * MathEx.Tan(glat1);
double tu2 = r * MathEx.Tan(glat2);
double cu1 = 1.0 / MathEx.Sqrt(1.0 + tu1 * tu1);
double su1 = cu1 * tu1;
double cu2 = 1.0 / MathEx.Sqrt(1.0 + tu2 * tu2);
double s1 = cu1 * cu2;
double b1 = s1 * tu2;
double f1 = b1 * tu1;
double x = glon2 - glon1;
double d = x + 1;
while ((MathEx.Abs(d - x) > eps) && (iter < maxiter))
{
iter = iter + 1;
sx = MathEx.Sin(x);
cx = MathEx.Cos(x);
tu1 = cu2 * sx;
tu2 = b1 - su1 * cu2 * cx;
sy = MathEx.Sqrt(tu1 * tu1 + tu2 * tu2);
cy = s1 * cx + f1;
y = Atan2(sy, cy);
double sa = s1 * sx / sy;
c2A = 1 - sa * sa;
cz = f1 + f1;
if (c2A > 0.0)
{
cz = cy - cz / c2A;
}
e = cz * cz * 2.0 - 1.0;
c = ((-3.0 * c2A + 4.0) * f + 4.0) * c2A * f / 16.0;
d = x;
x = ((e * cy * c + cz) * sy * c + y) * sa;
x = (1.0 - c) * x * f + glon2 - glon1;
}
double faz = Modcrs(Atan2(tu1, tu2));
double baz = Modcrs(Atan2(cu1 * sx, b1 * cx - su1 * cu2) + MathEx.PI);
x = MathEx.Sqrt((1 / (r * r) - 1) * c2A + 1);
x += 1;
x = (x - 2.0) / x;
c = 1.0 - x;
c = (x * x / 4.0 + 1.0) / c;
d = (0.375 * x * x - 1.0) * x;
x = e * cy;
double s = ((((sy * sy * 4.0 - 3.0) * (1.0 - e - e) * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * a * r;
outValue._d = s;
outValue._crs12 = faz;
outValue._crs21 = baz;
if (MathEx.Abs(iter - maxiter) < eps)
{
throw new ArgumentException("Algorithm did not converge");
}
return(outValue);
}