GetRotation Implementation (#72)

* Added implementetion to function based getRotation

* Added getAngularVelocity and associated tests.
Also added input test for getRotation

include/ale.h

@@ -34,10 +34,9 @@ namespace ale {
                                          std::vector<double> times,
                                          double time, interpolation interp);
 
-  std::vector<double> getRotation(std::string from, std::string to,
-                                  std::vector<double> coefficients, double time);
-  std::vector<double> getAngularVelocity(std::string from, std::string to,
-                                         std::vector<double> coefficients, double time);
+  std::vector<double> getRotation(std::vector<std::vector<double>> coeffs, double time);
+  std::vector<double> getAngularVelocity(std::vector<std::vector<double>> coeffs, double time);
+
   double evaluatePolynomial(std::vector<double> coeffs, double time, int d);
   double interpolate(std::vector<double> points, std::vector<double> times, double time, interpolation interp, int d);
   std::string load(std::string filename);

src/ale.cpp

@@ -174,16 +174,52 @@ namespace ale {
   }
 
   // Rotation Function Functions
-  vector<double> getRotation(string from, string to,
-                             vector<double> coefficients, double time) {
-    vector<double> coordinate = {0.0, 0.0, 0.0};
-    return coordinate;
+  std::vector<double> getRotation(vector<vector<double>> coeffs, double time) {
+
+    if (coeffs.size() != 3) {
+      throw invalid_argument("Invalid input coefficients, expected three vectors.");
     }
 
-  vector<double> getAngularVelocity(string from, string to,
-                                    vector<double> coefficients, double time) {
-    vector<double> coordinate = {0.0, 0.0, 0.0};
-    return coordinate;
+    vector<double> rotation = {0.0, 0.0, 0.0};
+
+    rotation[0] = evaluatePolynomial(coeffs[0], time, 0); // X
+    rotation[1] = evaluatePolynomial(coeffs[1], time, 0); // Y
+    rotation[2] = evaluatePolynomial(coeffs[2], time, 0); // Z
+
+    Eigen::Quaterniond quat;
+    quat = Eigen::AngleAxisd(rotation[0] * M_PI / 180, Eigen::Vector3d::UnitZ())
+                * Eigen::AngleAxisd(rotation[1] * M_PI / 180, Eigen::Vector3d::UnitX())
+                * Eigen::AngleAxisd(rotation[2] * M_PI / 180, Eigen::Vector3d::UnitZ());
+
+    quat.normalize();
+
+    vector<double> rotationQ = {quat.w(), quat.x(), quat.y(), quat.z()};
+    return rotationQ;
+  }
+
+  vector<double> getAngularVelocity(vector<vector<double>> coeffs, double time) {
+
+    if (coeffs.size() != 3) {
+      throw invalid_argument("Invalid input coefficients, expected three vectors.");
+    }
+
+    double phi = evaluatePolynomial(coeffs[0], time, 0); // X
+    double theta = evaluatePolynomial(coeffs[1], time, 0); // Y
+    double psi = evaluatePolynomial(coeffs[2], time, 0); // Z
+
+    double phi_dt = evaluatePolynomial(coeffs[0], time, 1);
+    double theta_dt = evaluatePolynomial(coeffs[1], time, 1);
+    double psi_dt = evaluatePolynomial(coeffs[2], time, 1);
+
+    Eigen::Quaterniond quat1, quat2;
+    quat1 = Eigen::AngleAxisd(phi * M_PI / 180, Eigen::Vector3d::UnitZ());
+    quat2 =  Eigen::AngleAxisd(theta * M_PI / 180, Eigen::Vector3d::UnitX());
+
+    Eigen::Vector3d velocity =  phi_dt * Eigen::Vector3d::UnitZ();
+    velocity += theta_dt * (quat1 *  Eigen::Vector3d::UnitX());
+    velocity += psi_dt * (quat1 * quat2 *  Eigen::Vector3d::UnitZ());
+
+    return {velocity[0], velocity[1], velocity[2]};
   }
 
   // Polynomial evaluation helper function

tests/ctests/AleTest.cpp

@@ -236,6 +236,48 @@ TEST(VelocityCoeffTest, InvalidInput) {
   EXPECT_THROW(ale::getVelocity(invalid_coeffs_sizes, valid_time), invalid_argument);
 }
 
+TEST(RotationCoeffTest, ZeroOrderPolynomial) {
+  double time = 1.0;
+  vector<vector<double>> coeffs = {{90},
+                                                            {0},
+                                                            {0}};
+
+  vector<double> coordinate = ale::getRotation(coeffs, time);
+
+  ASSERT_EQ(4, coordinate.size());
+  EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[0]);
+  EXPECT_DOUBLE_EQ(0, coordinate[1]);
+  EXPECT_DOUBLE_EQ(0, coordinate[2]);
+  EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[3]);
+}
+
+TEST(RotationCoeffTest, InvalidInput) {
+  double time = 1.0;
+  vector<vector<double>> coeffs = {{90},
+                                                            {0}};
+
+  EXPECT_THROW(ale::getRotation(coeffs, time), invalid_argument);
+}
+
+TEST(AngularVelocityCoeffTest, DefaultAngularExample) {
+  vector<vector<double>> coeffs = {{0, 90}, {0, 90}, {0, 90}};
+  double time = 1.0;
+
+  vector<double> av = ale::getAngularVelocity(coeffs, time);
+
+  ASSERT_EQ(3, av.size());
+  EXPECT_DOUBLE_EQ(90, av[0]);
+  EXPECT_DOUBLE_EQ(90, av[1]);
+  EXPECT_DOUBLE_EQ(90, av[2]);
+}
+
+TEST(AngularVelocityCoeffTest, InvalidInput) {
+  vector<vector<double>> coeffs = {{0, 90}, {0, 90}};
+  double time = 2.0;
+
+  EXPECT_THROW(ale::getAngularVelocity(coeffs, time), invalid_argument);
+}
+
 
 TEST(RotationInterpTest, ExampleGetRotation) {
   // simple test, only checks if API hit correctly and output is normalized