A (fast) ApplyInverse() method for 3D affine transformations

math/genvector/inc/Math/GenVector/Transform3D.h

@@ -671,6 +671,54 @@ class Transform3D {
       return operator()(v);
    }
 
+   /**
+      Directly apply the inverse affine transformation on vectors.
+      Avoids having to calculate the inverse as an intermediate result.
+      This is possible since the inverse of a rotation is its transpose.
+   */
+   Vector ApplyInverse(const Vector &v) const
+   {
+      return Vector(fM[kXX] * v.X() + fM[kYX] * v.Y() + fM[kZX] * v.Z(),
+                    fM[kXY] * v.X() + fM[kYY] * v.Y() + fM[kZY] * v.Z(),
+                    fM[kXZ] * v.X() + fM[kYZ] * v.Y() + fM[kZZ] * v.Z());
+   }
+
+   /**
+      Directly apply the inverse affine transformation on points
+      (first inverse translation then inverse rotation).
+      Avoids having to calculate the inverse as an intermediate result.
+      This is possible since the inverse of a rotation is its transpose.
+   */
+   Point ApplyInverse(const Point &p) const
+   {
+      Point tmp(p.X() - fM[kDX], p.Y() - fM[kDY], p.Z() - fM[kDZ]);
+      return Point(fM[kXX] * tmp.X() + fM[kYX] * tmp.Y() + fM[kZX] * tmp.Z(),
+                   fM[kXY] * tmp.X() + fM[kYY] * tmp.Y() + fM[kZY] * tmp.Z(),
+                   fM[kXZ] * tmp.X() + fM[kYZ] * tmp.Y() + fM[kZZ] * tmp.Z());
+   }
+
+   /**
+      Directly apply the inverse affine transformation on an arbitrary
+      coordinate-system point.
+      Involves casting to Point(p) type.
+   */
+   template <class CoordSystem>
+   PositionVector3D<CoordSystem> ApplyInverse(const PositionVector3D<CoordSystem> &p) const
+   {
+      return PositionVector3D<CoordSystem>(ApplyInverse(Point(p)));
+   }
+
+   /**
+      Directly apply the inverse affine transformation on an arbitrary
+      coordinate-system vector.
+      Involves casting to Vector(p) type.
+   */
+   template <class CoordSystem>
+   DisplacementVector3D<CoordSystem> ApplyInverse(const DisplacementVector3D<CoordSystem> &p) const
+   {
+      return DisplacementVector3D<CoordSystem>(ApplyInverse(Vector(p)));
+   }
+
    /**
       Transformation operation for points between different coordinate system tags
    */

math/genvector/test/testGenVector.cxx

@@ -671,6 +671,38 @@ int testTransform3D() {
   iret |= compare( (lr==lr2),true,"Get/SetLRotMatrix");
 #endif
 
+  {
+     // testing ApplyInverse on Point
+     XYZPoint point(1., -2., 3.);
+     Transform3D tr(EulerAngles(10, -10, 10), XYZVector(10, -10, 0));
+
+     // test that applying transformation + Inverse is identity
+     auto r0 = tr.ApplyInverse(tr(point));
+     auto r0_2 = tr.Inverse()(tr(point));
+
+     iret |= compare(r0.X(), point.X(), "ApplyInverse/PointX", 100);
+     iret |= compare(r0_2.X(), point.X(), "ApplyInverse/PointX", 100);
+     iret |= compare(r0.Y(), point.Y(), "ApplyInverse/PointY", 10);
+     iret |= compare(r0_2.Y(), point.Y(), "ApplyInverse/PointY", 10);
+     iret |= compare(r0.Z(), point.Z(), "ApplyInverse/PointZ", 10);
+     iret |= compare(r0_2.Z(), point.Z(), "ApplyInverse/PointZ", 10);
+
+     // compare ApplyInverse with Inverse()
+     auto r1 = tr.ApplyInverse(point);
+     auto r2 = tr.Inverse()(point);
+     iret |= compare(r1.X(), r2.X(), "ApplyInverse/Point", 10);
+     iret |= compare(r1.Y(), r2.Y(), "ApplyInverse/Point", 10);
+     iret |= compare(r1.Z(), r2.Z(), "ApplyInverse/Point", 10);
+  }
+
+  {
+     // testing ApplyInverse on Vector
+     XYZVector vector(1, -2., 3);
+     Transform3D tr(EulerAngles(10, -10, 10), XYZVector(10, -10, 0));
+     auto r1 = tr.ApplyInverse(vector);
+     auto r2 = tr.Inverse()(vector);
+     iret |= compare((r1 == r2), true, "ApplyInverse/Vector");
+  }
 
   if (iret == 0) std::cout << "OK\n";
   else std::cout << "\t\t\tFAILED\n";