/// <summary>
/// Converts an orthonormal matrix to euler angle (pitch/yaw/roll) representation.
/// </summary>
/// <returns>Euler angles in degrees representing the rotation in this matrix.</returns>
public Vector3 ToEulerAngles()
{
Radian xAngle = -MathEx.Asin(this[1, 2]);
if (xAngle < MathEx.HalfPi)
{
if (xAngle > -MathEx.HalfPi)
{
Radian yAngle = MathEx.Atan2(this[0, 2], this[2, 2]);
Radian zAngle = MathEx.Atan2(this[1, 0], this[1, 1]);
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
else
{
// Note: Not an unique solution.
xAngle = -MathEx.HalfPi;
Radian yAngle = MathEx.Atan2(-this[0, 1], this[0, 0]);
Radian zAngle = (Radian)0.0f;
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
}
else
{
// Note: Not an unique solution.
xAngle = MathEx.HalfPi;
Radian yAngle = MathEx.Atan2(this[0, 1], this[0, 0]);
Radian zAngle = (Radian)0.0f;
return(new Vector3(xAngle.Degrees, yAngle.Degrees, zAngle.Degrees));
}
}
public void Calculate(float dt)
{
//Correcting centrifugal acceleration
//CorrAccZ = InpAcc.Z - InpAirSpeed * InpGyr.X / degPerPi; //Check sign!
if (dt < 0.5 && dt > 0)
{
SamplePeriod = dt;
Update(InpGyr.X * piPerDeg, InpGyr.Y * piPerDeg, InpGyr.Z * piPerDeg, InpAcc.X, InpAcc.Y, InpAcc.Z, InpMag.X, InpMag.Y, InpMag.Z);
float a01 = 2 * Quat[0] * Quat[1];
float a02 = 2 * Quat[0] * Quat[2];
float a03 = 2 * Quat[0] * Quat[3];
float a11 = 2 * Quat[1] * Quat[1];
float a12 = 2 * Quat[1] * Quat[2];
float a13 = 2 * Quat[1] * Quat[3];
float a22 = 2 * Quat[2] * Quat[2];
float a23 = 2 * Quat[2] * Quat[3];
float a33 = 2 * Quat[3] * Quat[3];
Pitch = (float)MathEx.Asin(a01 + a23) * degPerPi;
Roll = -(float)MathEx.Atan2(a02 - a13, 1 - (a22 + a11)) * degPerPi;
Yaw = (float)MathEx.Atan2((a11 + a33) - 1, a12 - a03) * degPerPi;
}
}
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 Vector3D GetEulerAnglesFromQuaternion(Quaternion q)
{
Vector3D result = new Vector3D();
float a01 = 2 * q.W * q.X;
float a02 = 2 * q.W * q.Y;
float a03 = 2 * q.W * q.Z;
float a11 = 2 * q.X * q.X;
float a12 = 2 * q.X * q.Y;
float a13 = 2 * q.X * q.Z;
float a22 = 2 * q.Y * q.Y;
float a23 = 2 * q.Y * q.Z;
float a33 = 2 * q.Z * q.Z;
//result.Y = (float)MathEx.Atan2(a01 + a23, 1 - (a11 + a22)) * degPerPi;
//result.X = (float)MathEx.Asin(a02 - a13) * degPerPi;
//result.Z = (float)MathEx.Atan2(a03 + a12, 1 - (a22 + a33)) * degPerPi;
result.Y = (float)MathEx.Atan2(a02 - a13, 1 - (a22 + a11)) * degPerPi;
result.X = (float)MathEx.Asin(a01 + a23) * degPerPi;
result.Z = (float)MathEx.Atan2(a03 - a12, 1 - (a11 + a33)) * degPerPi;
return(result);
}
private int Calculate(Direction direction)
{
// doy (N)
int N = date.DayOfYear;
// appr. time (t)
double lngHour = longitude / 15.0;
double t;
if (direction == Direction.Sunrise)
{
t = N + ((6.0 - lngHour) / 24.0);
}
else
{
t = N + ((18.0 - lngHour) / 24.0);
}
// mean anomaly (M)
double M = (0.9856 * t) - 3.289;
// true longitude (L)
double L = M + (1.916 * MathEx.Sin(Deg2Rad(M))) + (0.020 * MathEx.Sin(Deg2Rad(2 * M))) + 282.634;
L = FixValue(L, 0, 360);
// right asc (RA)
double RA = Rad2Deg(MathEx.Atan(0.91764 * MathEx.Tan(Deg2Rad(L))));
RA = FixValue(RA, 0, 360);
// adjust quadrant of RA
double Lquadrant = (Math.Floor(L / 90.0)) * 90.0;
double RAquadrant = (Math.Floor(RA / 90.0)) * 90.0;
RA = RA + (Lquadrant - RAquadrant);
RA = RA / 15.0;
// sin cos DEC (sinDec / cosDec)
double sinDec = 0.39782 * MathEx.Sin(Deg2Rad(L));
double cosDec = MathEx.Cos(MathEx.Asin(sinDec));
// local hour angle (cosH)
double cosH = (MathEx.Cos(Deg2Rad((double)zenith / 1000.0f)) - (sinDec * MathEx.Sin(Deg2Rad(latitude)))) / (cosDec * MathEx.Cos(Deg2Rad(latitude)));
// local hour (H)
double H;
if (direction == Direction.Sunrise)
{
H = 360.0 - Rad2Deg(MathEx.Acos(cosH));
}
else
{
H = Rad2Deg(MathEx.Acos(cosH));
}
H = H / 15.0;
// time (T)
double T = H + RA - (0.06571 * t) - 6.622;
// universal time (T)
double UT = T - lngHour;
UT += utcOffset; // local UTC offset
if (daylightChanges != null)
{
if ((date > daylightChanges.Start) && (date < daylightChanges.End))
{
UT += (double)daylightChanges.Delta.Ticks / 36000000000;
}
}
UT = FixValue(UT, 0, 24);
return((int)Math.Round(UT * 3600)); // Convert to seconds
}
