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