Adds functions for rotation matrices about primary axes.

pazvirtual · Jul 12, 2024 · 708607e · 708607e
1 parent 2ddecaf
commit 708607e
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 0 deletions.
diff --git a/PAZ_Math b/PAZ_Math
@@ -379,6 +379,10 @@ namespace paz
         means, const std::vector<Mat>& cholCovs);
 
     double jv(const Mat& costMat, std::vector<std::size_t>& rowSols);
+
+    Mat rot1(double angle);
+    Mat rot2(double angle);
+    Mat rot3(double angle);
 }
 
 #endif
diff --git a/basics.cpp b/basics.cpp
@@ -109,3 +109,30 @@ void paz::normalize_weights(std::vector<double>& weights)
     }
     normalize_log_weights(weights);
 }
+
+paz::Mat rot1(double angle)
+{
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {{1., 0., 0.},
+            {0.,  c,  s},
+            {0., -s,  c}};
+}
+
+paz::Mat rot2(double angle)
+{
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {{ c, 0., -s},
+            {0., 1., 0.},
+            { s, 0.,  c}};
+}
+
+paz::Mat rot3(double angle)
+{
+    const double c = std::cos(angle);
+    const double s = std::sin(angle);
+    return {{ c,  s, 0.},
+            {-s,  c, 0.},
+            {0., 0., 1.}};
+}