diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index b0055e9a..7958bc73 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -832,7 +832,14 @@ def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3: :return: vector projection of v1 onto v2 (parrallel to v2) :rtype: ndarray(n) """ - return np.dot(v1, v2) * v2 + v1 = getvector(v1) + v2 = getvector(v2) + scale = np.max(np.abs(v2)) + if scale == 0: + return np.zeros_like(v2) + # Normalize the scale before dot products to avoid over/underflow. + v2 = v2 / scale + return (np.dot(v1, v2) / np.dot(v2, v2)) * v2 def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3: diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 15c6a451..e8360ee8 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -33,6 +33,23 @@ class TestVector(unittest.TestCase): def tearDownClass(cls): plt.close("all") + def test_project_nonunit_direction(self): + for scale in (1, 2, -3, 1e-200, 1e200): + direction = np.array([scale, scale, 0]) + nt.assert_allclose(project([3, 1, 4], direction), [2, 2, 0]) + nt.assert_allclose(project((3, 1, 4), (2, 2, 0)), [2, 2, 0]) + + def test_orthogonalize_nonunit_direction(self): + result = orthogonalize([3, 1, 4], [2, 2, 0], normalize=False) + nt.assert_allclose(result, [1, -1, 4]) + self.assertAlmostEqual(np.dot(result, [2, 2, 0]), 0) + normalized = orthogonalize([3, 1, 4], [2, 2, 0]) + self.assertAlmostEqual(np.linalg.norm(normalized), 1) + self.assertAlmostEqual(np.dot(normalized, [2, 2, 0]), 0) + + def test_project_zero_direction(self): + nt.assert_array_equal(project([1, 2, 3], [0, 0, 0]), [0, 0, 0]) + def test_unit(self): nt.assert_array_almost_equal(unitvec([1, 0, 0]), np.r_[1, 0, 0]) nt.assert_array_almost_equal(unitvec([0, 1, 0]), np.r_[0, 1, 0])