use super::*;
use std::f64::consts::{PI, TAU};
use std::ops::{Add, Div, Mul, Neg, Sub};
impl Angular {
pub fn from_rad(rad: f64) -> Self {
Self {
default_type: AngularType::Rad,
v: rad,
}
}
pub fn from_deg(deg: f64) -> Self {
Self {
default_type: AngularType::Deg,
v: deg,
}
}
pub fn as_rad(&self) -> f64 {
return match self.default_type {
AngularType::Rad => self.v,
AngularType::Deg => self.v * PI / 180.0,
};
}
pub fn as_deg(&self) -> f64 {
return match self.default_type {
AngularType::Rad => self.v * 180.0 / PI,
AngularType::Deg => self.v,
};
}
pub fn sin(&self) -> f64 {
self.as_rad().sin()
}
pub fn tan(&self) -> f64 {
self.as_rad().tan()
}
pub fn cos(&self) -> f64 {
self.as_rad().cos()
}
pub fn mod_to_round_half(&self) -> Self {
let x = self.as_rad();
let period = 2.0 * PI;
Angular::from_rad(x - ((x + PI) / period).floor() * period)
}
pub fn mod_to_round(&self) -> Self {
let v = self.as_rad() - TAU * f64::floor(self.as_rad() / TAU);
Self::from_rad(v)
}
pub fn atan2(a: f64, b: f64) -> Self {
let v = f64::atan2(a, b);
Self::from_rad(v)
}
pub fn atan(a: f64) -> Self {
let v = f64::atan(a);
Self::from_rad(v)
}
pub fn asin(v: f64) -> Self {
let rad = f64::asin(v);
Self::from_rad(rad)
}
pub fn acos(v: f64) -> Self {
let rad = f64::acos(v);
Self::from_rad(rad)
}
}
impl From<Coef> for Angular {
fn from(value: Coef) -> Self {
Angular::from_rad(value.get_value())
}
}
impl Add for Angular {
type Output = Self;
fn add(self, rhs: Self) -> Self::Output {
let v = self.as_deg() + rhs.as_deg();
Angular::from_deg(v)
}
}
impl<'a, 'b> Add<&'b Angular> for &'a Angular {
type Output = Angular;
fn add(self, rhs: &'b Angular) -> Self::Output {
Angular::from_deg(self.as_deg() + rhs.as_deg())
}
}
impl<'a> Add<&'a Angular> for Angular {
type Output = Angular;
fn add(self, rhs: &'a Angular) -> Self::Output {
Angular::from_deg(self.as_deg() + rhs.as_deg())
}
}
impl<'a> Add<Angular> for &'a Angular {
type Output = Angular;
fn add(self, rhs: Angular) -> Self::Output {
Angular::from_deg(self.as_deg() + rhs.as_deg())
}
}
impl Add<f64> for Angular {
type Output = Self;
fn add(self, rhs: f64) -> Self::Output {
let v = self.v + rhs;
Angular {
v,
default_type: self.default_type,
}
}
}
impl Sub for Angular {
type Output = Self;
fn sub(self, rhs: Self) -> Self::Output {
let v = self.as_deg() - rhs.as_deg();
Angular::from_deg(v)
}
}
impl<'a, 'b> Sub<&'b Angular> for &'a Angular {
type Output = Angular;
fn sub(self, rhs: &'b Angular) -> Self::Output {
Angular::from_deg(self.as_deg() - rhs.as_deg())
}
}
impl<'a> Sub<&'a Angular> for Angular {
type Output = Angular;
fn sub(self, rhs: &'a Angular) -> Self::Output {
Angular::from_deg(self.as_deg() - rhs.as_deg())
}
}
impl<'a> Sub<Angular> for &'a Angular {
type Output = Angular;
fn sub(self, rhs: Angular) -> Self::Output {
Angular::from_deg(self.as_deg() - rhs.as_deg())
}
}
impl Sub<f64> for Angular {
type Output = Self;
fn sub(self, rhs: f64) -> Self::Output {
let v = self.v - rhs;
Angular {
v,
default_type: self.default_type,
}
}
}
impl Div<chrono::TimeDelta> for Angular {
type Output = AngularVelocity;
fn div(self, rhs: chrono::TimeDelta) -> Self::Output {
let v = self.as_rad() / time_delta_to_secs_f64(&rhs);
AngularVelocity::from_rad_per_second(v)
}
}
impl<'a> Div<chrono::TimeDelta> for &'a Angular {
type Output = AngularVelocity;
fn div(self, rhs: chrono::TimeDelta) -> Self::Output {
AngularVelocity::from_rad_per_second(self.as_rad() / time_delta_to_secs_f64(&rhs))
}
}
impl<'a> Div<&'a chrono::TimeDelta> for Angular {
type Output = AngularVelocity;
fn div(self, rhs: &'a chrono::TimeDelta) -> Self::Output {
AngularVelocity::from_rad_per_second(self.as_rad() / time_delta_to_secs_f64(rhs))
}
}
impl<'a, 'b> Div<&'b chrono::TimeDelta> for &'a Angular {
type Output = AngularVelocity;
fn div(self, rhs: &'b chrono::TimeDelta) -> Self::Output {
AngularVelocity::from_rad_per_second(self.as_rad() / time_delta_to_secs_f64(rhs))
}
}
impl Div<AngularVelocity> for Angular {
type Output = chrono::TimeDelta;
fn div(self, rhs: AngularVelocity) -> Self::Output {
time_delta_from_secs_f64(self.as_rad() / rhs.as_rad_per_second())
}
}
impl<'a, 'b> Div<&'b AngularVelocity> for &'a Angular {
type Output = chrono::TimeDelta;
fn div(self, rhs: &'b AngularVelocity) -> Self::Output {
time_delta_from_secs_f64(self.as_rad() / rhs.as_rad_per_second())
}
}
impl<'a> Div<&'a AngularVelocity> for Angular {
type Output = chrono::TimeDelta;
fn div(self, rhs: &'a AngularVelocity) -> Self::Output {
time_delta_from_secs_f64(self.as_rad() / rhs.as_rad_per_second())
}
}
impl<'a> Div<AngularVelocity> for &'a Angular {
type Output = chrono::TimeDelta;
fn div(self, rhs: AngularVelocity) -> Self::Output {
time_delta_from_secs_f64(self.as_rad() / rhs.as_rad_per_second())
}
}
impl Mul<f64> for Angular {
type Output = Self;
fn mul(self, rhs: f64) -> Self::Output {
let v = self.as_rad() * rhs;
Self::from_rad(v)
}
}
impl Div<f64> for Angular {
type Output = Self;
fn div(self, rhs: f64) -> Self::Output {
let v = self.as_rad() / rhs;
Self::from_rad(v)
}
}
impl Mul<Angular> for f64 {
type Output = Angular;
fn mul(self, rhs: Angular) -> Self::Output {
rhs * self
}
}
impl Div<Angular> for f64 {
type Output = Angular;
fn div(self, rhs: Angular) -> Self::Output {
let v = self / rhs.v;
Angular {
default_type: rhs.default_type,
v: v,
}
}
}
impl Mul<Coef> for Angular {
type Output = Self;
fn mul(self, rhs: Coef) -> Self::Output {
let v = self.as_rad() * rhs.get_value();
Self::from_rad(v)
}
}
impl Div<Coef> for Angular {
type Output = Self;
fn div(self, rhs: Coef) -> Self::Output {
let v = self.as_rad() / rhs.get_value();
Self::from_rad(v)
}
}
impl Neg for Angular {
type Output = Self;
fn neg(self) -> Self::Output {
let v = -self.as_rad();
Self::from_rad(v)
}
}
#[cfg(test)]
mod tests {
use super::*;
use chrono::TimeDelta;
use approx::assert_relative_eq;
#[test]
fn test_angular() {
let angular = Angular::from_rad(1.0);
assert_eq!(angular.v, 1.0);
assert_eq!(angular.default_type, AngularType::Rad);
let angular = Angular::from_rad(6.15 * PI);
assert_eq!(angular.default_type, AngularType::Rad);
assert_relative_eq!(angular.v, 6.15 * PI, epsilon = 1.0e-6);
let angular = Angular::from_deg(1.0);
assert_eq!(angular.default_type, AngularType::Deg);
assert_eq!(angular.v, 1.0);
}
#[test]
fn test_angular_convert() {
let angular = Angular::from_deg(1.0);
assert_relative_eq!(angular.as_deg(), 1.0);
assert_relative_eq!(angular.as_rad(), 0.0174532925, epsilon = 1e-8);
let angular = Angular::from_rad(2.0 * PI);
assert_relative_eq!(angular.as_deg(), 360.0);
assert_relative_eq!(angular.as_rad(), 2.0 * PI, epsilon = 1e-8);
let c: Coef = Coef::new(0.5 * PI);
let angular: Angular = Angular::from(c);
assert_relative_eq!(angular.as_deg(), 90.0);
}
#[test]
fn test_angular_sin_cos() {
let angular = Angular::from_deg(30.0);
assert_relative_eq!(angular.sin(), 0.5, epsilon = 1e-8);
assert_relative_eq!(angular.cos(), 0.866025403784, epsilon = 1e-8);
let angular = Angular::from_rad(PI / 6.0);
assert_relative_eq!(angular.sin(), 0.5, epsilon = 1e-8);
assert_relative_eq!(angular.cos(), 0.866025403784, epsilon = 1e-8);
let angular = Angular::from_rad(PI / 4.0);
assert_relative_eq!(angular.tan(), 1.0, epsilon = 1e-8);
}
#[test]
fn test_angular_add() {
{
let theta1 = Angular::from_rad(PI * 0.5);
let theta2 = Angular::from_rad(PI * 0.5);
let theta3 = theta1 + theta2;
assert_relative_eq!(PI, theta3.as_rad(), epsilon = 1e-8);
let theta1 = Angular::from_deg(30.0);
let theta2 = Angular::from_rad(PI / 6.0);
let theta3 = theta1 + theta2;
assert_relative_eq!(60.0, theta3.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_rad(PI / 6.0);
let theta2 = Angular::from_deg(30.0);
let theta3 = theta1 + theta2;
assert_relative_eq!(60.0, theta3.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_deg(30.0);
let theta2 = Angular::from_deg(90.0);
let theta3 = theta1 + theta2;
assert_relative_eq!(2.0 * PI / 3.0, theta3.as_rad(), epsilon = 1e-8);
}
{
let theta1 = Angular::from_deg(30.0);
let theta2 = theta1 + 30.0;
assert_relative_eq!(60.0, theta2.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_rad(PI * 0.25);
let theta2 = theta1 + PI * 0.25;
assert_relative_eq!(90.0, theta2.as_deg(), epsilon = 1e-8);
}
}
#[test]
fn test_angular_sub() {
{
let theta1 = Angular::from_rad(PI * 1.0);
let theta2 = Angular::from_rad(PI * 0.5);
let theta3 = theta1 - theta2;
assert_relative_eq!(PI * 0.5, theta3.as_rad(), epsilon = 1e-8);
let theta1 = Angular::from_deg(60.0);
let theta2 = Angular::from_rad(PI / 6.0);
let theta3 = theta1 - theta2;
assert_relative_eq!(30.0, theta3.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_rad(PI / 6.0);
let theta2 = Angular::from_deg(15.0);
let theta3 = theta1 - theta2;
assert_relative_eq!(15.0, theta3.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_deg(90.0);
let theta2 = Angular::from_deg(30.0);
let theta3 = theta1 - theta2;
assert_relative_eq!(PI / 3.0, theta3.as_rad(), epsilon = 1e-8);
}
{
let theta1 = Angular::from_deg(30.0);
let theta2 = theta1 - 10.0;
assert_relative_eq!(20.0, theta2.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_rad(PI * 0.5);
let theta2 = theta1 - PI * 0.25;
assert_relative_eq!(45.0, theta2.as_deg(), epsilon = 1e-8);
}
}
#[test]
fn test_angular_mod_round() {
let theta1 = Angular::from_deg(372.0);
let theta2 = theta1.mod_to_round_half();
assert_relative_eq!(12.0, theta2.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_deg(315.0);
let theta2 = theta1.mod_to_round_half();
assert_relative_eq!(-1.0 * PI / 4.0, theta2.as_rad(), epsilon = 1e-8);
let theta1 = Angular::from_deg(372.0);
let theta2 = theta1.mod_to_round();
assert_relative_eq!(12.0, theta2.as_deg(), epsilon = 1e-8);
let theta1 = Angular::from_deg(315.0);
let theta2 = theta1.mod_to_round();
assert_relative_eq!(315.0, theta2.as_deg(), epsilon = 1e-8);
}
#[test]
fn test_angular_to_angular_velocity() {
let theta1 = Angular::from_rad(PI * 0.5);
let omg1 = theta1 / TimeDelta::seconds(2);
assert_eq!(omg1.default_type, AngularVelocityType::RadperSecond);
assert_eq!(omg1.v, PI / 4.0);
}
#[test]
fn test_angular_to_angular_velocity_timedelta() {
let theta = Angular::from_rad(PI * 0.5);
let td = time_delta_from_secs_f64(2.0);
let omg = theta / td;
assert_eq!(omg.v, PI / 4.0);
let neg_td = time_delta_from_secs_f64(-2.0);
let omg_neg = theta / neg_td;
assert_eq!(omg_neg.as_rad_per_second(), -PI / 4.0);
}
#[test]
fn test_to_mul() {
let d1 = Angular::from_rad(1000.0);
let d2 = d1 * 2.0;
assert_eq!(d2.as_rad(), 2000.0);
let d1 = Angular::from_rad(1000.0);
let d2 = d1 * Coef::new(2.0);
assert_eq!(d2.as_rad(), 2000.0);
}
#[test]
fn test_to_div() {
let d1 = Angular::from_rad(1000.0);
let d2 = d1 / 2.0;
assert_eq!(d2.as_rad(), 500.0);
let d1 = Angular::from_rad(1000.0);
let d2 = d1 / Coef::new(2.0);
assert_eq!(d2.as_rad(), 500.0);
}
#[test]
fn test_atan() {
let sin = 0.5;
let cos = 0.8660254037844386;
let theta = Angular::atan2(sin, cos);
assert_relative_eq!(theta.as_deg(), 30.0, epsilon = 1e-8);
let value = 1.0f64;
let theta = Angular::atan(value);
assert_relative_eq!(theta.as_deg(), 45.0, epsilon = 1e-8);
let value = 0.5f64;
let theta = Angular::asin(value);
assert_relative_eq!(theta.as_deg(), 30.0, epsilon = 1e-8);
}
#[test]
fn test_acos() {
let cases = vec![
(1.0, 0.0), (0.5, PI / 3.0), (0.0, PI / 2.0), (-0.5, 2.0 * PI / 3.0), (-1.0, PI), ];
for (input, expected_rad) in cases {
let angle = Angular::acos(input);
assert_relative_eq!(angle.as_rad(), expected_rad, epsilon = 1e-8);
assert_relative_eq!(angle.as_deg(), expected_rad * 180.0 / PI, epsilon = 1e-8);
}
{
let angle = Angular::acos(1.0);
assert_relative_eq!(angle.as_rad(), 0.0, epsilon = 1e-8);
}
{
let angle = Angular::acos(-1.0);
assert_relative_eq!(angle.as_rad(), PI, epsilon = 1e-8);
}
let invalid_cases = vec![1.00001, -1.00001, 2.0, -2.0, f64::NAN, f64::INFINITY];
for invalid_input in invalid_cases {
let angle = Angular::acos(invalid_input);
assert_eq!(true, angle.v.is_nan());
}
let epsilon = 1e-10;
{
let angle = Angular::acos(1.0 - epsilon);
assert!(angle.as_rad() > 0.0 && angle.as_rad() < 1.5e-5);
}
{
let angle = Angular::acos(-1.0 + epsilon);
assert!(angle.as_rad() > PI - 1.5e-5 && angle.as_rad() < PI);
}
}
#[test]
fn test_f64_mul_angular() {
let a = Angular::from_rad(PI);
let result = 2.0 * a;
assert_relative_eq!(result.as_rad(), 2.0 * PI);
let a = Angular::from_deg(90.0);
let result = 2.0 * a;
assert_relative_eq!(result.as_deg(), 180.0);
}
#[test]
fn test_f64_div_angular() {
let a = Angular::from_rad(PI);
let result = 2.0 * PI / a;
assert_relative_eq!(result.as_rad(), 2.0);
let a = Angular::from_deg(180.0);
let result = 180.0 / a;
assert_relative_eq!(result.as_deg(), 1.0);
}
#[test]
fn test_angular_ref_ops() {
let a1 = Angular::from_deg(30.0);
let a2 = Angular::from_rad(std::f64::consts::PI / 6.0); let s = &a1 + &a2;
assert_relative_eq!(s.as_deg(), 60.0);
let d = &a1 - &a2;
assert_relative_eq!(d.as_deg(), 0.0, epsilon = 1e-10);
let omg = &a1 / &TimeDelta::seconds(2);
assert_relative_eq!(omg.as_rad_per_second(), (std::f64::consts::PI / 12.0));
let s2 = a1 + &a2;
assert_relative_eq!(s2.as_deg(), 60.0, epsilon = 1e-10);
let s3 = &a1 + a2;
assert_relative_eq!(s3.as_deg(), 60.0, epsilon = 1e-10);
let a2b = Angular::from_rad(std::f64::consts::PI / 6.0);
let d2 = a1 - &a2b;
assert_relative_eq!(d2.as_deg(), 0.0, epsilon = 1e-10);
let d3 = &a1 - a2b;
assert_relative_eq!(d3.as_deg(), 0.0, epsilon = 1e-10);
let dur = TimeDelta::seconds(2);
let w2 = a1 / &dur;
assert_relative_eq!(w2.as_rad_per_second(), (std::f64::consts::PI / 12.0));
let w3 = &a1 / dur;
assert_relative_eq!(w3.as_rad_per_second(), (std::f64::consts::PI / 12.0));
let td = time_delta_from_secs_f64(2.0);
let w4 = a1 / &td;
assert_relative_eq!(w4.as_rad_per_second(), (std::f64::consts::PI / 12.0));
let w5 = &a1 / td;
assert_relative_eq!(w5.as_rad_per_second(), (std::f64::consts::PI / 12.0));
let td2 = time_delta_from_secs_f64(2.0);
let w6 = &a1 / &td2;
assert_relative_eq!(w6.as_rad_per_second(), (std::f64::consts::PI / 12.0));
}
#[test]
fn test_angular_neg() {
let a1 = Angular::from_rad(PI);
let neg_a1 = -a1;
assert_relative_eq!(neg_a1.as_rad(), -PI);
let a2 = Angular::from_deg(-90.0);
let neg_a2 = -a2;
assert_relative_eq!(neg_a2.as_deg(), 90.0);
let a3 = Angular::from_rad(0.0);
let neg_a3 = -a3;
assert_relative_eq!(neg_a3.as_rad(), 0.0);
let a4 = Angular::from_deg(180.0);
let neg_a4 = -a4;
assert_relative_eq!(neg_a4.as_rad(), -PI);
let a5 = Angular::from_rad(2.0 * PI);
let neg_a5 = -a5;
assert_relative_eq!(neg_a5.as_deg(), -360.0);
}
#[test]
fn test_angular_div_angular_velocity() {
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_rad_per_second(0.5 * PI);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_deg(180.0);
let angular_velocity = AngularVelocity::from_deg_per_second(90.0);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_deg(180.0); let angular_velocity = AngularVelocity::from_rad_per_second(PI);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 1.0);
}
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_deg_per_second(180.0); let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 1.0);
}
{
let angle = Angular::from_rad(2.0 * PI);
let angular_velocity = AngularVelocity::from_rad_per_hour(3600.0 * PI); let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_deg(360.0);
let angular_velocity = AngularVelocity::from_deg_per_hour(180.0 * 3600.0); let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_rad_per_second(0.5 * PI);
let duration = &angle / &angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_rad_per_second(0.5 * PI);
let duration = angle / &angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_rad_per_second(0.5 * PI);
let duration = &angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 2.0);
}
{
let angle = Angular::from_deg(90.0);
let angular_velocity = AngularVelocity::from_rad_per_second(PI / 2.0);
let duration = &angle / &angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 1.0);
}
{
let angle = Angular::from_deg(90.0);
let angular_velocity = AngularVelocity::from_rad_per_second(PI / 2.0);
let duration = angle / &angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 1.0);
}
{
let angle = Angular::from_deg(90.0);
let angular_velocity = AngularVelocity::from_rad_per_second(PI / 2.0);
let duration = &angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 1.0);
}
{
let angle = Angular::from_rad(0.0);
let angular_velocity = AngularVelocity::from_rad_per_second(1.0);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 0.0);
}
{
let angle = Angular::from_rad(10.0 * PI);
let angular_velocity = AngularVelocity::from_rad_per_second(PI);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 10.0);
}
{
let angle = Angular::from_rad(PI);
let angular_velocity = AngularVelocity::from_rad_per_second(0.1);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), 10.0 * PI, epsilon = 1e-8);
}
{
let angle = Angular::from_rad(-PI);
let angular_velocity = AngularVelocity::from_rad_per_second(PI);
let duration = angle / angular_velocity;
assert_relative_eq!(time_delta_to_secs_f64(&duration), -1.0);
}
}
}