/// <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);
}
}
/// <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);
}
/// <summary>
/// Converts a point on the circle to an angle on the circle.
/// </summary>
/// <param name="up">Up vector determining the orientation of the circle.</param>
/// <param name="point">Point along a unit circle centered around the origin.</param>
/// <returns>Angle at which the provided point is located on the circle.</returns>
private Degree PointOnCircleToAngle(Vector3 up, Vector3 point)
{
Quaternion rot = Quaternion.FromToRotation(up, Vector3.YAxis);
Matrix4 worldToPlane = Matrix4.TRS(Vector3.Zero, rot, Vector3.One);
point = worldToPlane.MultiplyDirection(point);
return(MathEx.Atan2(-point.z, -point.x) + MathEx.Pi);
}
public float Distance = 0; // meter
public void Calculate(float dt)
{
double Lat1;
double Lon1;
double Lat2;
double Lon2;
float bearing;
float rdRatio;
double dLon;
double CosLat2;
double y;
double x;
//Calculate position
if (GpsHasFix)
{
Latitude = GpsLatitude;
Longitude = GpsLongitude;
}
else
{
Latitude += AirSpeed * dt * MathEx.Cos(Heading * pi180) / Rpi180;
Longitude += AirSpeed * dt * MathEx.Sin(Heading * pi180) * MathEx.Cos(Latitude * pi180) / Rpi180;
}
//Calculate flight path
Lat1 = Latitude * pi180;
Lon1 = Longitude * pi180;
Lat2 = DestLatitude * pi180;
Lon2 = DestLongitude * pi180;
dLon = Lon2 - Lon1;
CosLat2 = MathEx.Cos(Lat2);
y = MathEx.Sin(dLon) * CosLat2;
x = MathEx.Cos(Lat1) * MathEx.Sin(Lat2) -
MathEx.Sin(Lat1) * CosLat2 * MathEx.Cos(dLon);
bearing = (float)(MathEx.Atan2(y, x) / pi180);
x = (Lon2 - Lon1) * MathEx.Cos((Lat1 + Lat2) / 2);
y = (Lat2 - Lat1);
Distance = (float)MathEx.Sqrt(x * x + y * y) * R;
if (Distance > 1.0)
{
rdRatio = (Radius / Distance);
TargetHeading = (bearing + 90 * (rdRatio * rdRatio) + 540) % 360 - 180;
}
}
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);
}
////////////////////////////////////////////////////////////////////////////
//--------------------------------- REVISIONS ------------------------------
// Date Name Tracking # Description
// --------- ------------------- ------------- ----------------------
// 15JUN2009 James Shen Initial Creation
////////////////////////////////////////////////////////////////////////////
/**
* Constructs a {@code LinearGradientBrush}.
*
* @param start the gradient axis start {@code Point} in user space
* @param end the gradient axis end {@code Point} in user space
* @param fractions numbers ranging from 0 to 255 specifying the
* distribution of colors along the gradient
* @param colors array of colors corresponding to each fractional Value
* @param fillType either {@code NO_CYCLE}, {@code REFLECT},
* or {@code REPEAT}
* @param gradientTransform transform to apply to the gradient
*
* @throws NullPointerException
* if one of the points is null,
* or {@code fractions} array is null,
* or {@code colors} array is null,
* or {@code cycleMethod} is null,
* or {@code colorSpace} is null,
* or {@code gradientTransform} is null
* @throws IllegalArgumentException
* if start and end points are the same points,
* or {@code fractions.length != colors.length},
* or {@code colors} is less than 2 in size,
* or a {@code fractions} Value is less than 0.0 or greater than 1.0,
* or the {@code fractions} are not provided in strictly increasing order
*/
public LinearGradientBrush(Point start, Point end,
int[] fractions, Color[] colors,
AffineTransform gradientTransform, int fillType)
{
if (fractions == null)
{
throw new NullReferenceException("Fractions array cannot be null");
}
if (colors == null)
{
throw new NullReferenceException("Colors array cannot be null");
}
if (gradientTransform == null)
{
throw new NullReferenceException("Gradient transform cannot be " +
"null");
}
if (fractions.Length != colors.Length)
{
throw new ArgumentException("Colors and fractions must " +
"have equal size");
}
if (colors.Length < 2)
{
throw new ArgumentException("User must specify at least " +
"2 colors");
}
// check that values are in the proper range and progress
// in increasing order from 0 to 1
int previousFraction = -255;
for (int i = 0; i < fractions.Length; i++)
{
int currentFraction = fractions[i];
if (currentFraction < 0 || currentFraction > 255)
{
throw new ArgumentException("Fraction values must " +
"be in the range 0 to 255: " +
currentFraction);
}
if (currentFraction <= previousFraction)
{
throw new ArgumentException("Keyframe fractions " +
"must be increasing: " +
currentFraction);
}
previousFraction = currentFraction;
}
// We have to deal with the cases where the first gradient stop is not
// equal to 0 and/or the last gradient stop is not equal to 1.
// In both cases, create a new point and replicate the previous
// extreme point's color.
bool fixFirst = false;
bool fixLast = false;
int len = fractions.Length;
int off = 0;
if (fractions[0] != 0)
{
// first stop is not equal to zero, fix this condition
fixFirst = true;
len++;
off++;
}
if (fractions[fractions.Length - 1] != 255)
{
// last stop is not equal to one, fix this condition
fixLast = true;
len++;
}
this._fractions = new int[len];
Array.Copy(fractions, 0, this._fractions, off, fractions.Length);
this._colors = new Color[len];
Array.Copy(colors, 0, this._colors, off, colors.Length);
if (fixFirst)
{
this._fractions[0] = 0;
this._colors[0] = colors[0];
}
if (fixLast)
{
this._fractions[len - 1] = 255;
this._colors[len - 1] = colors[colors.Length - 1];
}
// copy the gradient transform
this._gradientTransform = new AffineTransform(gradientTransform);
// determine transparency
bool opaque = true;
for (int i = 0; i < colors.Length; i++)
{
opaque = opaque && (colors[i].GetAlpha() == 0xff);
}
_transparency = opaque ? Color.OPAQUE : Color.TRANSLUCENT;
// check input parameters
if (start == null || end == null)
{
throw new NullReferenceException("Start and end points must be" +
"non-null");
}
if (start.Equals(end))
{
throw new ArgumentException("Start point cannot equal" +
"endpoint");
}
// copy the points...
this._start = new Point(start.GetX(), start.GetY());
this._end = new Point(end.GetX(), end.GetY());
Rectangle rectangle = new Rectangle(start, end);
float dx = start.X - end.X;
float dy = start.Y - end.Y;
double angle = MathEx.Atan2(dy, dx);
int intAngle = SingleFP.FromDouble(angle);
RectangleFP r = Utils.ToRectangleFP(rectangle);
_wrappedBrushFP = new LinearGradientBrushFP(r.GetLeft(), r.GetTop(),
r.GetRight(), r.GetBottom(),
intAngle);
for (int i = 0; i < colors.Length; i++)
{
((LinearGradientBrushFP)_wrappedBrushFP).SetGradientColor
(SingleFP.FromFloat(fractions[i] / 100.0f),
colors[i]._value);
}
((LinearGradientBrushFP)_wrappedBrushFP).UpdateGradientTable();
_wrappedBrushFP.SetMatrix(Utils.ToMatrixFP(gradientTransform));
_wrappedBrushFP.FillMode = fillType;
}
void timer_Tick(GT.Timer timer)
{
compass.RequestMeasurement();
//accelerometer.RequestMeasurement();
//gps.PositionReceived += new GPS.PositionReceivedHandler(gps_PositionReceived);
// if current location was found
if (currentLatitude != 0 && currentLongitude != 0)
{
// CALCULATE BEARING
double y = MathEx.Sin(destinationLongitude - currentLongitude) * MathEx.Cos(destinationLatitude);
double x = MathEx.Cos(currentLatitude) * MathEx.Sin(destinationLatitude) - MathEx.Sin(currentLatitude) * MathEx.Cos(destinationLatitude) * MathEx.Cos(destinationLongitude - currentLongitude);
bearing = MathEx.Atan2(y, x);
// convert radian value to degrees (-180 to 180 deg)
bearing = bearing * (180.0 / MathEx.PI);
// normalise the angle (0 to 360 deg)
bearing = (bearing + 360) % 360;
/*
* // CALCULATE HEADING
*
* // nearby ferrous effect compensation calculations
* double Xsf = (compassYmax - compassYmin) / (compassXmax - compassXmin);
* if (Xsf < 1)
* Xsf = 1;
* double Ysf = (compassXmax - compassXmin) / (compassYmax - compassYmin);
* if (Ysf < 1)
* Ysf = 1;
* double Xoff = (((compassXmax - compassXmin) / 2) - compassXmax) * Xsf;
* double Yoff = (((compassYmax - compassYmin) / 2) - compassYmax) * Ysf;
*
* // tilt compensation calculations
* double pitchRadians = MathEx.Asin(accelerometerX);
* double rollRadians = MathEx.Asin(accelerometerY);
*
* // we cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0.
* if (rollRadians > 0.78 || rollRadians < -0.78 || pitchRadians > 0.78 || pitchRadians < -0.78)
* {
* heading = 0;
* }
*
* else
* {
*
* // tilt compensation algorithm
* double Xh = compassX * MathEx.Cos(pitchRadians) + compassY * MathEx.Sin(rollRadians) * MathEx.Sin(pitchRadians) - compassZ * MathEx.Cos(rollRadians) * MathEx.Sin(pitchRadians);
* double Yh = compassY * MathEx.Cos(rollRadians) + compassZ * MathEx.Sin(rollRadians);
*
* // calculate final compass X and Y coordinates
* double Xvalue = Xsf * Xh + Xoff;
* double Yvalue = Ysf * Yh + Yoff;
*
* // calculate heading
* if (Xvalue == 0 && Yvalue < 0)
* heading = 90;
* else if (Xvalue == 0 && Yvalue > 0)
* heading = 270;
* else if (Xvalue < 0)
* heading = 180 - ( MathEx.Atan( Yvalue / Xvalue ) * 180 / MathEx.PI );
* else if (Xvalue > 0 && Yvalue < 0)
* heading = - ( MathEx.Atan( Yvalue / Xvalue ) * 180 / MathEx.PI );
* else if (Xvalue > 0 && Yvalue > 0)
* heading = 360 - ( MathEx.Atan(Yvalue / Xvalue) * 180 / MathEx.PI );
*
* /*if (compassX == 0 && compassY < 0)
* heading = 90;
* else if (compassX == 0 && compassY > 0)
* heading = 270;
* else if (compassX < 0)
* heading = 180 - (MathEx.Atan(compassY / compassX) * 180 / MathEx.PI);
* else if (compassX > 0 && compassY < 0)
* heading = -(MathEx.Atan(compassY / compassX) * 180 / MathEx.PI);
* else if (compassX > 0 && compassY > 0)
* heading = 360 - (MathEx.Atan(compassY / compassX) * 180 / MathEx.PI);*/
/*
* }
*/
// CALCULATE RELATIVE BEARING
if (heading <= bearing)
{
relativeBearing = bearing - heading;
}
else if (heading > bearing)
{
relativeBearing = 360 - (heading - bearing);
}
// MOVE ARROW
/*
* if (relativeBearing <= 180)
* servoValue = map(relativeBearing, 0, 180, 150, 0);
* else if (relativeBearing > 180)
* servoValue = map(relativeBearing, 180, 360, 300, 150);
*
* servo.SetPulse(20 * 1000 * 1000, (uint)servoValue * 10 * 1000);
*/
}
// if no gps coordinates found
else
{
/*
* // set direction
* if (servoValue == 180)
* direction = false;
* else if (servoValue == 120)
* direction = true;
*
* // calculate servo position
* if (direction == false)
* servoValue -= 10;
* else if (direction == true)
* servoValue += 10;
*
* servo.SetPulse(20 * 1000 * 1000, (uint)servoValue * 10 * 1000);
*/
Debug.Print("No GPS position");
}
/*z
* // print measured data
* //Debug.Print("lat: " + currentLatitude.ToString());
* //Debug.Print("lon: " + currentLongitude.ToString());
* Debug.Print("---".ToString());
* Debug.Print("heading: " + heading.ToString());
* Debug.Print("bearing: " + bearing.ToString());
* Debug.Print("relative bearing: " + relativeBearing.ToString());
* Debug.Print("---".ToString());
*/
}
