From 6e90626febeeab57a8739aaa882b5dc737498823 Mon Sep 17 00:00:00 2001
From: Hleb Valoshka <375gnu@gmail.com>
Date: Mon, 21 Oct 2019 16:35:21 +0300
Subject: [PATCH] Add a method to transform Ray3 with 4x4 matrix

---
 src/celmath/ray.h | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/src/celmath/ray.h b/src/celmath/ray.h
index f7c87b29..756325e7 100644
--- a/src/celmath/ray.h
+++ b/src/celmath/ray.h
@@ -30,6 +30,15 @@ template<class T> class Ray3
         return Ray3<T>(m * origin, m * direction);
     }
 
+    Ray3<T> transform(const Eigen::Matrix<T, 4, 4>& m) const
+    {
+        Eigen::Matrix<T, 4, 1> o(Eigen::Matrix<T, 4, 1>::Ones());
+        o.head(3) = origin;
+        Eigen::Matrix<T, 4, 1> d(Eigen::Matrix<T, 4, 1>::Zero());
+        d.head(3) = direction;
+        return Ray3<T>((m * o).head(3), (m * d).head(3));
+    }
+
  public:
     Eigen::Matrix<T, 3, 1> origin;
     Eigen::Matrix<T, 3, 1> direction;