diff options
Diffstat (limited to 'builtin/common')
| -rw-r--r-- | builtin/common/tests/vector_spec.lua | 142 | ||||
| -rw-r--r-- | builtin/common/vector.lua | 93 |
2 files changed, 235 insertions, 0 deletions
diff --git a/builtin/common/tests/vector_spec.lua b/builtin/common/tests/vector_spec.lua index 79f032f28..6f308a4a8 100644 --- 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 ca6541eb4..1fd784ce2 100644 --- 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 |