diff --git a/math/genvector/inc/Math/GenVector/Transform3D.h b/math/genvector/inc/Math/GenVector/Transform3D.h index bef4b01f900ae..7cf6436a26c71 100644 --- a/math/genvector/inc/Math/GenVector/Transform3D.h +++ b/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 + PositionVector3D ApplyInverse(const PositionVector3D &p) const + { + return PositionVector3D(ApplyInverse(Point(p))); + } + + /** + Directly apply the inverse affine transformation on an arbitrary + coordinate-system vector. + Involves casting to Vector(p) type. + */ + template + DisplacementVector3D ApplyInverse(const DisplacementVector3D &p) const + { + return DisplacementVector3D(ApplyInverse(Vector(p))); + } + /** Transformation operation for points between different coordinate system tags */ diff --git a/math/genvector/test/testGenVector.cxx b/math/genvector/test/testGenVector.cxx index abe082c1f7779..76a7c09dec1c5 100644 --- a/math/genvector/test/testGenVector.cxx +++ b/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";