commit 4148d6a353bf6726a667f64bde358e84454b1b2c
author Sergiu Deitsch <sergiu.deitsch@gmail.com> Tue Jun 10 23:14:28 2025 +0200
committer Sergiu Deitsch <sergiu.deitsch@gmail.com> Tue Jun 10 23:14:28 2025 +0200
tree 3132cb9495480ae367e3a81287b52c2868814553
parent 9c22db814cf7d2f688d7a8b716b112bdb07d0be1

Allow AngleAxisRotatePoint to be applied in-place

Thanks to @CatInTheRain for the suggestion.

Fixes #1163

Change-Id: I4e148c62891979e1ec01b91f95d3e2d04501c8ef

include/ceres/rotation.h

diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index 261d3a0..c04278e 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -282,8 +282,7 @@
 
 // y = R(angle_axis) * x;
 //
-// Inplace rotation is not supported. pt and result must point to different
-// memory locations, otherwise the result will be undefined.
+// Inplace rotation is supported.
 template <typename T>
 inline void AngleAxisRotatePoint(const T angle_axis[3],
                                  const T pt[3],
@@ -791,9 +790,10 @@
 inline void AngleAxisRotatePoint(const T angle_axis[3],
                                  const T pt[3],
                                  T result[3]) {
-  DCHECK_NE(pt, result) << "Inplace rotation is not supported.";
+  using std::cos;
   using std::fpclassify;
   using std::hypot;
+  using std::sin;
 
   const T theta = hypot(angle_axis[0], angle_axis[1], angle_axis[2]);
 

internal/ceres/rotation_test.cc

diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 0dfcf71..85c8b78 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -1,5 +1,5 @@
 // Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2023 Google Inc. All rights reserved.
+// Copyright 2025 Google Inc. All rights reserved.
 // http://ceres-solver.org/
 //
 // Redistribution and use in source and binary forms, with or without
@@ -1732,18 +1732,22 @@
       rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
       rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
 
-      AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
-      for (int k = 0; k < 3; ++k) {
-        // clang-format off
-        EXPECT_NEAR(rotation_matrix_rotated_p[k],
-                    angle_axis_rotated_p[k],
-                    kTolerance) << "p: " << p[0]
-                                << " " << p[1]
-                                << " " << p[2]
-                                << " angle_axis: " << angle_axis[0]
-                                << " " << angle_axis[1]
-                                << " " << angle_axis[2];
-        // clang-format on
+      // Rotate point and write the result to a different and the same
+      // destination
+      for (double* const dst : {angle_axis_rotated_p, angle_axis}) {
+        AngleAxisRotatePoint(angle_axis, p, dst);
+        for (int k = 0; k < 3; ++k) {
+          // clang-format off
+          EXPECT_NEAR(rotation_matrix_rotated_p[k],
+                      dst[k],
+                      kTolerance) << "p: " << p[0]
+                                  << " " << p[1]
+                                  << " " << p[2]
+                                  << " angle_axis: " << angle_axis[0]
+                                  << " " << angle_axis[1]
+                                  << " " << angle_axis[2];
+          // clang-format on
+        }
       }
     }
   }
@@ -1790,18 +1794,22 @@
     rotation_matrix_rotated_p[1] = R[1] * p[0] + R[4] * p[1] + R[7] * p[2];
     rotation_matrix_rotated_p[2] = R[2] * p[0] + R[5] * p[1] + R[8] * p[2];
 
-    AngleAxisRotatePoint(angle_axis, p, angle_axis_rotated_p);
-    for (int k = 0; k < 3; ++k) {
-      // clang-format off
-      EXPECT_NEAR(rotation_matrix_rotated_p[k],
-                  angle_axis_rotated_p[k],
-                  kTolerance) << "p: " << p[0]
-                              << " " << p[1]
-                              << " " << p[2]
-                              << " angle_axis: " << angle_axis[0]
-                              << " " << angle_axis[1]
-                              << " " << angle_axis[2];
-      // clang-format on
+    // Rotate point and write the result to a different and the same
+    // destination
+    for (double* const dst : {angle_axis_rotated_p, angle_axis}) {
+      AngleAxisRotatePoint(angle_axis, p, dst);
+      for (int k = 0; k < 3; ++k) {
+        // clang-format off
+        EXPECT_NEAR(rotation_matrix_rotated_p[k],
+                    dst[k],
+                    kTolerance) << "p: " << p[0]
+                                << " " << p[1]
+                                << " " << p[2]
+                                << " angle_axis: " << angle_axis[0]
+                                << " " << angle_axis[1]
+                                << " " << angle_axis[2];
+        // clang-format on
+      }
     }
   }
 }