127 lines
3.6 KiB
C#
127 lines
3.6 KiB
C#
using PropertyTools.DataAnnotations;
|
|
using System;
|
|
|
|
namespace VisionCommon {
|
|
public class Angle : ICloneable {
|
|
private double mRadian;
|
|
[System.ComponentModel.Browsable(false)]
|
|
public double Radian {
|
|
get { return mRadian; }
|
|
set {
|
|
mRadian = value;
|
|
mDegree = MathUtil.ConvertRadianToDegree(value);
|
|
}
|
|
}
|
|
|
|
private double mDegree;
|
|
[DisplayName("Deg")]
|
|
public double Degree {
|
|
get { return mDegree; }
|
|
set {
|
|
mDegree = value;
|
|
mRadian = MathUtil.ConvertDegreeToRadian(value);
|
|
}
|
|
}
|
|
|
|
public Angle() {
|
|
this.Radian = default(double);
|
|
this.Degree = default(double);
|
|
}
|
|
|
|
public static Angle CreateWithRadian(double radian) {
|
|
var angle = new Angle();
|
|
angle.Radian = radian;
|
|
return angle;
|
|
}
|
|
|
|
public static Angle CreateWithDegree(double degree) {
|
|
var angle = new Angle();
|
|
angle.Degree = degree % 360;
|
|
return angle;
|
|
}
|
|
|
|
public static Angle operator +(Angle a) {
|
|
return a;
|
|
}
|
|
|
|
public static Angle operator +(Angle a1, Angle a2) {
|
|
return Angle.CreateWithDegree(a1.Degree + a2.Degree);
|
|
}
|
|
|
|
public static Angle operator -(Angle a) {
|
|
return Angle.CreateWithDegree(-a.Degree);
|
|
}
|
|
|
|
public static Angle operator -(Angle a1, Angle a2) {
|
|
return Angle.CreateWithDegree(a1.Degree - a2.Degree);
|
|
}
|
|
|
|
public static Angle operator *(Angle a1, double b) {
|
|
return Angle.CreateWithDegree(a1.Degree * b);
|
|
}
|
|
|
|
public static bool operator >(Angle a1, Angle a2) {
|
|
return (a1.Degree > a2.Degree);
|
|
}
|
|
|
|
public static bool operator <(Angle a1, Angle a2) {
|
|
return (a1.Degree < a2.Degree);
|
|
}
|
|
|
|
public static Angle NormalizeAngle(Angle angle) {
|
|
// Normalized Degree [-180, 180) -> -180 ~ 179
|
|
double normalizedDeg = angle.Degree % 360;
|
|
|
|
if (normalizedDeg < -180) {
|
|
normalizedDeg += 360;
|
|
}
|
|
else if (normalizedDeg >= 180) {
|
|
normalizedDeg -= 360;
|
|
}
|
|
|
|
return Angle.CreateWithDegree(normalizedDeg);
|
|
}
|
|
|
|
public static Angle DenormalizeAngle(Angle angle) {
|
|
// Denormalized Degree [0, 360) -> 0 ~ 359
|
|
double denormalizedDeg = angle.Degree;
|
|
|
|
if (denormalizedDeg < 0) {
|
|
denormalizedDeg += 360;
|
|
}
|
|
|
|
return Angle.CreateWithDegree(denormalizedDeg);
|
|
}
|
|
|
|
public static Angle GetSignedNormalizeAngle(double acos, double asin) {
|
|
// acos : Magnitude
|
|
// asin : Direction
|
|
double normalizedRadian = (asin > 0) ? acos : (acos * -1);
|
|
|
|
return Angle.CreateWithRadian(normalizedRadian);
|
|
}
|
|
|
|
public override bool Equals(object obj) {
|
|
var a = obj as Angle;
|
|
|
|
return MathUtil.Equals(this.Degree, a.Degree);
|
|
}
|
|
|
|
public override string ToString() {
|
|
return String.Format("Angle : {0} [deg], {1} [rad]", Degree.ToString("F4"), Radian.ToString("F4"));
|
|
}
|
|
|
|
public string ToStringOnlyDeg() {
|
|
return String.Format("{0} [deg]", Degree.ToString("F4"));
|
|
}
|
|
|
|
public object Clone() {
|
|
return Angle.CreateWithDegree(Degree);
|
|
}
|
|
|
|
public override int GetHashCode() {
|
|
return base.GetHashCode();
|
|
}
|
|
}
|
|
}
|