Merge pull request #17 from ichiro-its/15-add-direction-equation

Add Direction Equation

Author: threeal

README.md

@@ -1,8 +1,10 @@
 # Keisan
 
-[![latest version](https://img.shields.io/github/v/release/ichiro-its/keisan.svg)](https://github.com/ichiro-its/keisan/releases/)
-[![license](https://img.shields.io/github/license/ichiro-its/keisan.svg)](./LICENSE)
-[![build and test status](https://github.com/ichiro-its/keisan/actions/workflows/build-and-test.yml/badge.svg)](https://github.com/ichiro-its/keisan/actions)
+[![latest version](https://img.shields.io/github/v/release/ichiro-its/keisan)](https://github.com/ichiro-its/keisan/releases/)
+[![commits since latest version](https://img.shields.io/github/commits-since/ichiro-its/keisan/latest)](https://github.com/ichiro-its/keisan/commits/master)
+[![milestone](https://img.shields.io/github/milestones/progress/ichiro-its/keisan/1?label=milestone)](https://github.com/ichiro-its/keisan/milestone/1)
+[![license](https://img.shields.io/github/license/ichiro-its/keisan)](./LICENSE)
+[![test status](https://img.shields.io/github/workflow/status/ichiro-its/keisan/Build%20and%20Test?label=test)](https://github.com/ichiro-its/keisan/actions)
 
 Keisan is a [ROS 2](https://docs.ros.org/en/foxy/index.html) package that provides a math equations and algorithms library for a ROS 2 project.
 

include/keisan/point_2.hpp

@@ -48,8 +48,11 @@ struct Point2
   Point2 operator/(double value);
 
   static double distance_between(Point2 & point_a, Point2 & point_b);
+  static double angle_between(Point2 & point_a, Point2 & point_b);
 
   double magnitude();
+  double direction();
+
   Point2 normalize();
 
   double x;

include/keisan/point_3.hpp

@@ -48,8 +48,11 @@ struct Point3
   Point3 operator/(double value);
 
   static double distance_between(Point3 & point_a, Point3 & point_b);
+  static double angle_between(Point3 & point_a, Point3 & point_b);
 
   double magnitude();
+  double direction();
+
   Point3 normalize();
 
   double x;

src/angle.cpp

@@ -46,12 +46,12 @@ double deg_to_rad(double value)
 
 double delta_deg(double value1, double value2)
 {
-  return wrap_deg(value2 - value1);
+  return wrap_deg(wrap_deg(value2) - wrap_deg(value1));
 }
 
 double delta_rad(double value1, double value2)
 {
-  return wrap_rad(value2 - value1);
+  return wrap_rad(wrap_rad(value2) - wrap_rad(value1));
 }
 
 }  // namespace keisan

src/point_2.cpp

@@ -20,6 +20,7 @@
 
 #include <math.h>
 
+#include <keisan/angle.hpp>
 #include <keisan/point_2.hpp>
 
 namespace keisan
@@ -137,11 +138,24 @@ double Point2::distance_between(Point2 & point_a, Point2 & point_b)
   return sqrt(dx * dx + dy * dy);
 }
 
+double Point2::angle_between(Point2 & point_a, Point2 & point_b)
+{
+  double dot_product = point_a.x * point_b.x + point_a.y * point_b.y;
+  double mag = point_a.magnitude() * point_b.magnitude();
+
+  return wrap_rad(acos(dot_product / mag));
+}
+
 double Point2::magnitude()
 {
   return sqrt(x * x + y * y);
 }
 
+double Point2::direction()
+{
+  return wrap_rad(atan(y / x));
+}
+
 Point2 Point2::normalize()
 {
   double mag = magnitude();

src/point_3.cpp

@@ -20,6 +20,7 @@
 
 #include <math.h>
 
+#include <keisan/angle.hpp>
 #include <keisan/point_3.hpp>
 
 namespace keisan
@@ -148,11 +149,26 @@ double Point3::distance_between(Point3 & point_a, Point3 & point_b)
   return sqrt(dx * dx + dy * dy + dz * dz);
 }
 
+double Point3::angle_between(Point3 & point_a, Point3 & point_b)
+{
+  double dot_product = point_a.x * point_b.x + point_a.y * point_b.y + point_a.z * point_b.z;
+  double mag = point_a.magnitude() * point_b.magnitude();
+
+  return wrap_rad(acos(dot_product / mag));
+}
+
 double Point3::magnitude()
 {
   return sqrt(x * x + y * y + z * z);
 }
 
+double Point3::direction()
+{
+  double temp = sqrt(x * x + y * y);
+
+  return wrap_rad(atan(z / temp));
+}
+
 Point3 Point3::normalize()
 {
   double mag = magnitude();

test/point_2_test.cpp

@@ -119,3 +119,17 @@ TEST(Point2Test, Normalize)
   auto point = keisan::Point2(3.0, 4.0);
   ASSERT_POINT2_EQ(point.normalize(), 0.6, 0.8);
 }
+
+TEST(Point2Test, Direction)
+{
+  auto point = keisan::Point2(4.0, 0.0);
+  ASSERT_DOUBLE_EQ(point.direction(), keisan::deg_to_rad(0.0));
+}
+
+TEST(Point2Test, AngleBetween)
+{
+  auto point_a = keisan::Point2(4.0, 3.0);
+  auto point_b = keisan::Point2(-3.0, 4.0);
+
+  ASSERT_DOUBLE_EQ(keisan::Point2::angle_between(point_a, point_b), keisan::deg_to_rad(90.0));
+}

test/point_3_test.cpp

@@ -119,3 +119,17 @@ TEST(Point3Test, Normalize)
   auto point = keisan::Point3(3.0, 0.0, 4.0);
   ASSERT_POINT3_EQ(point.normalize(), 0.6, 0.0, 0.8);
 }
+
+TEST(Point3Test, Direction)
+{
+  auto point = keisan::Point3(3.0, 4.0, 0.0);
+  ASSERT_DOUBLE_EQ(point.direction(), keisan::deg_to_rad(0.0));
+}
+
+TEST(Point3Test, AngleBetween)
+{
+  auto point_a = keisan::Point3(2.0, -1.0, 7.0);
+  auto point_b = keisan::Point3(1.0, 2.0, 0.0);
+
+  ASSERT_DOUBLE_EQ(keisan::Point3::angle_between(point_a, point_b), keisan::deg_to_rad(90.0));
+}