Some vector functions useful for working with rotations (#9572)

* added vector.rotate

* added vector.forward_from_rotation and vector.up_from_rotation

* added vector.forward_up_to_rotatiton

* fixed some bugs and formatting with vector functions

* shortened name of some new vector functions and added documentation

* made vector.rotate not require a unit vector as axis

* fixed crash with vector.forward_up_to_rot

* renamed new vector functions, made vector.rotate apply a rotation matrix, old vector.rotate is now called vector.rotate_around_axis

* documented vector function changes

* removed some whitespace to appease luacheck

* implemented and fixed optimization of vector.rotate_around_axis by SmallJoker

* added some unit tests for rotation vector functions

* clarified that rotation vectors are in radians and according to the left hand rule

* hopefully appeased luacheck

* renamed rotation_to_horizontal to forward_at_rotation, rotation_to_vertical to up_at_rotation

* handled cases where sin or cos are 0 in rotation vector functions

* added more comments

* clarified documentation of rotation vector functions

* added more unit tests

* changed way in which vector.rotate_around_axis is adjusted for left handed coordinate systems

* made vector.rotate_around_axis actually left handed

* unrolled matrix multiplication

* removed vector.forward_at_rotation and vector.up_at_rotation

* prettified vector.rotate_around_axis, made previous commits not break anything

* removed references to removed vector.forward_at_rotation and vector.up_at_rotation

* removed documentation of removed vector functions

* clarified documentation and fixed styling of rotation vector functions

* restyled comments minorly

* spelling fixes and some hopefully better comments

* allowed 'up' to be missing from vector.directions_to_rotation and removed requirement for unit vectors as arguments

* made vector.rotate_around_axis() right handed again for consistency

* documented previous changes

* made matrix multiplication actually multiply

* renamed vector.directions_to_rotation() to vector.dir_to_rotation()

* optimized a distance comparison

* Fixed potential false positive in unit tests.

Co-authored-by: NetherEran <nethereran@hotmail.com>

diff --git a/builtin/common/tests/vector_spec.lua b/builtin/common/tests/vector_spec.lua
index 79f032f28d61eab4a2d12bea84bf45ae79d43533..6f308a4a838968b49da01b717d298dbc9675839b 100644 (file)
--- a/builtin/common/tests/vector_spec.lua
+++ b/builtin/common/tests/vector_spec.lua
@@ -43,4 +43,146 @@ describe("vector", function()
        it("add()", function()
                assert.same({ x = 2, y = 4, z = 6 }, vector.add(vector.new(1, 2, 3), { x = 1, y = 2, z = 3 }))
        end)
+
+       -- This function is needed because of floating point imprecision.
+       local function almost_equal(a, b)
+               if type(a) == "number" then
+                       return math.abs(a - b) < 0.00000000001
+               end
+               return vector.distance(a, b) < 0.000000000001
+       end
+
+       describe("rotate_around_axis()", function()
+               it("rotates", function()
+                       assert.True(almost_equal({x = -1, y = 0, z = 0},
+                               vector.rotate_around_axis({x = 1, y = 0, z = 0}, {x = 0, y = 1, z = 0}, math.pi)))
+                       assert.True(almost_equal({x = 0, y = 1, z = 0},
+                               vector.rotate_around_axis({x = 0, y = 0, z = 1}, {x = 1, y = 0, z = 0}, math.pi / 2)))
+                       assert.True(almost_equal({x = 4, y = 1, z = 1},
+                               vector.rotate_around_axis({x = 4, y = 1, z = 1}, {x = 4, y = 1, z = 1}, math.pi / 6)))
+               end)
+               it("keeps distance to axis", function()
+                       local rotate1 = {x = 1, y = 3, z = 1}
+                       local axis1 = {x = 1, y = 3, z = 2}
+                       local rotated1 = vector.rotate_around_axis(rotate1, axis1, math.pi / 13)
+                       assert.True(almost_equal(vector.distance(axis1, rotate1), vector.distance(axis1, rotated1)))
+                       local rotate2 = {x = 1, y = 1, z = 3}
+                       local axis2 = {x = 2, y = 6, z = 100}
+                       local rotated2 = vector.rotate_around_axis(rotate2, axis2, math.pi / 23)
+                       assert.True(almost_equal(vector.distance(axis2, rotate2), vector.distance(axis2, rotated2)))
+                       local rotate3 = {x = 1, y = -1, z = 3}
+                       local axis3 = {x = 2, y = 6, z = 100}
+                       local rotated3 = vector.rotate_around_axis(rotate3, axis3, math.pi / 2)
+                       assert.True(almost_equal(vector.distance(axis3, rotate3), vector.distance(axis3, rotated3)))
+               end)
+               it("rotates back", function()
+                       local rotate1 = {x = 1, y = 3, z = 1}
+                       local axis1 = {x = 1, y = 3, z = 2}
+                       local rotated1 = vector.rotate_around_axis(rotate1, axis1, math.pi / 13)
+                       rotated1 = vector.rotate_around_axis(rotated1, axis1, -math.pi / 13)
+                       assert.True(almost_equal(rotate1, rotated1))
+                       local rotate2 = {x = 1, y = 1, z = 3}
+                       local axis2 = {x = 2, y = 6, z = 100}
+                       local rotated2 = vector.rotate_around_axis(rotate2, axis2, math.pi / 23)
+                       rotated2 = vector.rotate_around_axis(rotated2, axis2, -math.pi / 23)
+                       assert.True(almost_equal(rotate2, rotated2))
+                       local rotate3 = {x = 1, y = -1, z = 3}
+                       local axis3 = {x = 2, y = 6, z = 100}
+                       local rotated3 = vector.rotate_around_axis(rotate3, axis3, math.pi / 2)
+                       rotated3 = vector.rotate_around_axis(rotated3, axis3, -math.pi / 2)
+                       assert.True(almost_equal(rotate3, rotated3))
+               end)
+               it("is right handed", function()
+                       local v_before1 = {x = 0, y = 1, z = -1}
+                       local v_after1 = vector.rotate_around_axis(v_before1, {x = 1, y = 0, z = 0}, math.pi / 4)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after1, v_before1)), {x = 1, y = 0, z = 0}))
+
+                       local v_before2 = {x = 0, y = 3, z = 4}
+                       local v_after2 = vector.rotate_around_axis(v_before2, {x = 1, y = 0, z = 0},  2 * math.pi / 5)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after2, v_before2)), {x = 1, y = 0, z = 0}))
+
+                       local v_before3 = {x = 1, y = 0, z = -1}
+                       local v_after3 = vector.rotate_around_axis(v_before3, {x = 0, y = 1, z = 0}, math.pi / 4)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after3, v_before3)), {x = 0, y = 1, z = 0}))
+
+                       local v_before4 = {x = 3, y = 0, z = 4}
+                       local v_after4 = vector.rotate_around_axis(v_before4, {x = 0, y = 1, z = 0}, 2 * math.pi / 5)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after4, v_before4)), {x = 0, y = 1, z = 0}))
+
+                       local v_before5 = {x = 1, y = -1, z = 0}
+                       local v_after5 = vector.rotate_around_axis(v_before5, {x = 0, y = 0, z = 1}, math.pi / 4)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after5, v_before5)), {x = 0, y = 0, z = 1}))
+
+                       local v_before6 = {x = 3, y = 4, z = 0}
+                       local v_after6 = vector.rotate_around_axis(v_before6, {x = 0, y = 0, z = 1}, 2 * math.pi / 5)
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after6, v_before6)), {x = 0, y = 0, z = 1}))
+               end)
+       end)
+
+       describe("rotate()", function()
+               it("rotates", function()
+                       assert.True(almost_equal({x = -1, y = 0, z = 0},
+                               vector.rotate({x = 1, y = 0, z = 0}, {x = 0, y = math.pi, z = 0})))
+                       assert.True(almost_equal({x = 0, y = -1, z = 0},
+                               vector.rotate({x = 1, y = 0, z = 0}, {x = 0, y = 0, z = math.pi / 2})))
+                       assert.True(almost_equal({x = 1, y = 0, z = 0},
+                               vector.rotate({x = 1, y = 0, z = 0}, {x = math.pi / 123, y = 0, z = 0})))
+               end)
+               it("is counterclockwise", function()
+                       local v_before1 = {x = 0, y = 1, z = -1}
+                       local v_after1 = vector.rotate(v_before1, {x = math.pi / 4, y = 0, z = 0})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after1, v_before1)), {x = 1, y = 0, z = 0}))
+
+                       local v_before2 = {x = 0, y = 3, z = 4}
+                       local v_after2 = vector.rotate(v_before2, {x = 2 * math.pi / 5, y = 0, z = 0})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after2, v_before2)), {x = 1, y = 0, z = 0}))
+
+                       local v_before3 = {x = 1, y = 0, z = -1}
+                       local v_after3 = vector.rotate(v_before3, {x = 0, y = math.pi / 4, z = 0})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after3, v_before3)), {x = 0, y = 1, z = 0}))
+
+                       local v_before4 = {x = 3, y = 0, z = 4}
+                       local v_after4 = vector.rotate(v_before4, {x = 0, y = 2 * math.pi / 5, z = 0})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after4, v_before4)), {x = 0, y = 1, z = 0}))
+
+                       local v_before5 = {x = 1, y = -1, z = 0}
+                       local v_after5 = vector.rotate(v_before5, {x = 0, y = 0, z = math.pi / 4})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after5, v_before5)), {x = 0, y = 0, z = 1}))
+
+                       local v_before6 = {x = 3, y = 4, z = 0}
+                       local v_after6 = vector.rotate(v_before6, {x = 0, y = 0, z = 2 * math.pi / 5})
+                       assert.True(almost_equal(vector.normalize(vector.cross(v_after6, v_before6)), {x = 0, y = 0, z = 1}))
+               end)
+       end)
+
+       it("dir_to_rotation()", function()
+               -- Comparing rotations (pitch, yaw, roll) is hard because of certain ambiguities,
+               -- e.g. (pi, 0, pi) looks exactly the same as (0, pi, 0)
+               -- So instead we convert the rotation back to vectors and compare these.
+               local function forward_at_rot(rot)
+                       return vector.rotate(vector.new(0, 0, 1), rot)
+               end
+               local function up_at_rot(rot)
+                       return vector.rotate(vector.new(0, 1, 0), rot)
+               end
+               local rot1 = vector.dir_to_rotation({x = 1, y = 0, z = 0}, {x = 0, y = 1, z = 0})
+               assert.True(almost_equal({x = 1, y = 0, z = 0}, forward_at_rot(rot1)))
+               assert.True(almost_equal({x = 0, y = 1, z = 0}, up_at_rot(rot1)))
+               local rot2 = vector.dir_to_rotation({x = 1, y = 1, z = 0}, {x = 0, y = 0, z = 1})
+               assert.True(almost_equal({x = 1/math.sqrt(2), y = 1/math.sqrt(2), z = 0}, forward_at_rot(rot2)))
+               assert.True(almost_equal({x = 0, y = 0, z = 1}, up_at_rot(rot2)))
+               for i = 1, 1000 do
+                       local rand_vec = vector.new(math.random(), math.random(), math.random())
+                       if vector.length(rand_vec) ~= 0 then
+                               local rot_1 = vector.dir_to_rotation(rand_vec)
+                               local rot_2 = {
+                                       x = math.atan2(rand_vec.y, math.sqrt(rand_vec.z * rand_vec.z + rand_vec.x * rand_vec.x)),
+                                       y = -math.atan2(rand_vec.x, rand_vec.z),
+                                       z = 0
+                               }
+                               assert.True(almost_equal(rot_1, rot_2))
+                       end
+               end
+
+       end)
 end)

diff --git a/builtin/common/vector.lua b/builtin/common/vector.lua
index ca6541eb46362c6aff681f2f54ab5e9f85a9f807..1fd784ce28884d45c42b9ae6c27a7e67ae18844e 100644 (file)
--- a/builtin/common/vector.lua
+++ b/builtin/common/vector.lua
@@ -141,3 +141,96 @@ function vector.sort(a, b)
        return {x = math.min(a.x, b.x), y = math.min(a.y, b.y), z = math.min(a.z, b.z)},
                {x = math.max(a.x, b.x), y = math.max(a.y, b.y), z = math.max(a.z, b.z)}
 end
+
+local function sin(x)
+       if x % math.pi == 0 then
+               return 0
+       else
+               return math.sin(x)
+       end
+end
+
+local function cos(x)
+       if x % math.pi == math.pi / 2 then
+               return 0
+       else
+               return math.cos(x)
+       end
+end
+
+function vector.rotate_around_axis(v, axis, angle)
+       local cosangle = cos(angle)
+       local sinangle = sin(angle)
+       axis = vector.normalize(axis)
+       -- https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
+       local dot_axis = vector.multiply(axis, vector.dot(axis, v))
+       local cross = vector.cross(v, axis)
+       return vector.new(
+               cross.x * sinangle + (v.x - dot_axis.x) * cosangle + dot_axis.x,
+               cross.y * sinangle + (v.y - dot_axis.y) * cosangle + dot_axis.y,
+               cross.z * sinangle + (v.z - dot_axis.z) * cosangle + dot_axis.z
+       )
+end
+
+function vector.rotate(v, rot)
+       local sinpitch = sin(-rot.x)
+       local sinyaw = sin(-rot.y)
+       local sinroll = sin(-rot.z)
+       local cospitch = cos(rot.x)
+       local cosyaw = cos(rot.y)
+       local cosroll = math.cos(rot.z)
+       -- Rotation matrix that applies yaw, pitch and roll
+       local matrix = {
+               {
+                       sinyaw * sinpitch * sinroll + cosyaw * cosroll,
+                       sinyaw * sinpitch * cosroll - cosyaw * sinroll,
+                       sinyaw * cospitch,
+               },
+               {
+                       cospitch * sinroll,
+                       cospitch * cosroll,
+                       -sinpitch,
+               },
+               {
+                       cosyaw * sinpitch * sinroll - sinyaw * cosroll,
+                       cosyaw * sinpitch * cosroll + sinyaw * sinroll,
+                       cosyaw * cospitch,
+               },
+       }
+       -- Compute matrix multiplication: `matrix` * `v`
+       return vector.new(
+               matrix[1][1] * v.x + matrix[1][2] * v.y + matrix[1][3] * v.z,
+               matrix[2][1] * v.x + matrix[2][2] * v.y + matrix[2][3] * v.z,
+               matrix[3][1] * v.x + matrix[3][2] * v.y + matrix[3][3] * v.z
+       )
+end
+
+function vector.dir_to_rotation(forward, up)
+       forward = vector.normalize(forward)
+       local rot = {x = math.asin(forward.y), y = -math.atan2(forward.x, forward.z), z = 0}
+       if not up then
+               return rot
+       end
+       assert(vector.dot(forward, up) < 0.000001,
+                       "Invalid vectors passed to vector.dir_to_rotation().")
+       up = vector.normalize(up)
+       -- Calculate vector pointing up with roll = 0, just based on forward vector.
+       local forwup = vector.rotate({x = 0, y = 1, z = 0}, rot)
+       -- 'forwup' and 'up' are now in a plane with 'forward' as normal.
+       -- The angle between them is the absolute of the roll value we're looking for.
+       rot.z = vector.angle(forwup, up)
+
+       -- Since vector.angle never returns a negative value or a value greater
+       -- than math.pi, rot.z has to be inverted sometimes.
+       -- To determine wether this is the case, we rotate the up vector back around
+       -- the forward vector and check if it worked out.
+       local back = vector.rotate_around_axis(up, forward, -rot.z)
+
+       -- We don't use vector.equals for this because of floating point imprecision.
+       if (back.x - forwup.x) * (back.x - forwup.x) +
+                       (back.y - forwup.y) * (back.y - forwup.y) +
+                       (back.z - forwup.z) * (back.z - forwup.z) > 0.0000001 then
+               rot.z = -rot.z
+       end
+       return rot
+end

diff --git a/doc/lua_api.txt b/doc/lua_api.txt
index 898531880aecd5b2e2b03fda4f95523ffb96e8ab..ed060c4ad9dde4c89ff4f79e2c16ef485c234b7e 100644 (file)
--- a/doc/lua_api.txt
+++ b/doc/lua_api.txt
@@ -3007,6 +3007,24 @@ For the following functions `x` can be either a vector or a number:
 * `vector.divide(v, x)`:
     * Returns a scaled vector or Schur quotient.
 
+For the following functions `a` is an angle in radians and `r` is a rotation
+vector ({x = <pitch>, y = <yaw>, z = <roll>}) where pitch, yaw and roll are
+angles in radians.
+
+* `vector.rotate(v, r)`:
+    * Applies the rotation `r` to `v` and returns the result.
+    * `vector.rotate({x = 0, y = 0, z = 1}, r)` and
+      `vector.rotate({x = 0, y = 1, z = 0}, r)` return vectors pointing
+      forward and up relative to an entity's rotation `r`.
+* `vector.rotate_around_axis(v1, v2, a)`:
+    * Returns `v1` rotated around axis `v2` by `a` radians according to
+      the right hand rule.
+* `vector.dir_to_rotation(direction[, up])`:
+    * Returns a rotation vector for `direction` pointing forward using `up`
+      as the up vector.
+    * If `up` is omitted, the roll of the returned vector defaults to zero.
+    * Otherwise `direction` and `up` need to be vectors in a 90 degree angle to each other.
+