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 Ray3 return Ray3(m * origin, m * direction); } + Ray3 transform(const Eigen::Matrix& m) const + { + Eigen::Matrix o(Eigen::Matrix::Ones()); + o.head(3) = origin; + Eigen::Matrix d(Eigen::Matrix::Zero()); + d.head(3) = direction; + return Ray3((m * o).head(3), (m * d).head(3)); + } + public: Eigen::Matrix origin; Eigen::Matrix direction;