123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424 |
- --[[
- Vector helpers
- Note: The vector.*-functions must be able to accept old vectors that had no metatables
- ]]
- -- localize functions
- local setmetatable = setmetatable
- local math = math
- vector = {}
- local metatable = {}
- vector.metatable = metatable
- local xyz = {"x", "y", "z"}
- -- only called when rawget(v, key) returns nil
- function metatable.__index(v, key)
- return rawget(v, xyz[key]) or vector[key]
- end
- -- only called when rawget(v, key) returns nil
- function metatable.__newindex(v, key, value)
- rawset(v, xyz[key] or key, value)
- end
- -- constructors
- local function fast_new(x, y, z)
- return setmetatable({x = x, y = y, z = z}, metatable)
- end
- function vector.new(a, b, c)
- if a and b and c then
- return fast_new(a, b, c)
- end
- -- deprecated, use vector.copy and vector.zero directly
- if type(a) == "table" then
- return vector.copy(a)
- else
- assert(not a, "Invalid arguments for vector.new()")
- return vector.zero()
- end
- end
- function vector.zero()
- return fast_new(0, 0, 0)
- end
- function vector.copy(v)
- assert(v.x and v.y and v.z, "Invalid vector passed to vector.copy()")
- return fast_new(v.x, v.y, v.z)
- end
- function vector.from_string(s, init)
- local x, y, z, np = string.match(s, "^%s*%(%s*([^%s,]+)%s*[,%s]%s*([^%s,]+)%s*[,%s]" ..
- "%s*([^%s,]+)%s*[,%s]?%s*%)()", init)
- x = tonumber(x)
- y = tonumber(y)
- z = tonumber(z)
- if not (x and y and z) then
- return nil
- end
- return fast_new(x, y, z), np
- end
- function vector.to_string(v)
- return string.format("(%g, %g, %g)", v.x, v.y, v.z)
- end
- metatable.__tostring = vector.to_string
- function vector.equals(a, b)
- return a.x == b.x and
- a.y == b.y and
- a.z == b.z
- end
- metatable.__eq = vector.equals
- -- unary operations
- function vector.length(v)
- return math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z)
- end
- -- Note: we cannot use __len because it is already used for primitive table length
- function vector.normalize(v)
- local len = vector.length(v)
- if len == 0 then
- return fast_new(0, 0, 0)
- else
- return vector.divide(v, len)
- end
- end
- function vector.floor(v)
- return vector.apply(v, math.floor)
- end
- function vector.round(v)
- return vector.apply(v, math.round)
- end
- function vector.ceil(v)
- return vector.apply(v, math.ceil)
- end
- function vector.sign(v, tolerance)
- return vector.apply(v, math.sign, tolerance)
- end
- function vector.abs(v)
- return vector.apply(v, math.abs)
- end
- function vector.apply(v, func, ...)
- return fast_new(
- func(v.x, ...),
- func(v.y, ...),
- func(v.z, ...)
- )
- end
- function vector.combine(a, b, func)
- return fast_new(
- func(a.x, b.x),
- func(a.y, b.y),
- func(a.z, b.z)
- )
- end
- function vector.distance(a, b)
- local x = a.x - b.x
- local y = a.y - b.y
- local z = a.z - b.z
- return math.sqrt(x * x + y * y + z * z)
- end
- function vector.direction(pos1, pos2)
- return vector.subtract(pos2, pos1):normalize()
- end
- function vector.angle(a, b)
- local dotp = vector.dot(a, b)
- local cp = vector.cross(a, b)
- local crossplen = vector.length(cp)
- return math.atan2(crossplen, dotp)
- end
- function vector.dot(a, b)
- return a.x * b.x + a.y * b.y + a.z * b.z
- end
- function vector.cross(a, b)
- return fast_new(
- a.y * b.z - a.z * b.y,
- a.z * b.x - a.x * b.z,
- a.x * b.y - a.y * b.x
- )
- end
- function metatable.__unm(v)
- return fast_new(-v.x, -v.y, -v.z)
- end
- -- add, sub, mul, div operations
- function vector.add(a, b)
- if type(b) == "table" then
- return fast_new(
- a.x + b.x,
- a.y + b.y,
- a.z + b.z
- )
- else
- return fast_new(
- a.x + b,
- a.y + b,
- a.z + b
- )
- end
- end
- function metatable.__add(a, b)
- return fast_new(
- a.x + b.x,
- a.y + b.y,
- a.z + b.z
- )
- end
- function vector.subtract(a, b)
- if type(b) == "table" then
- return fast_new(
- a.x - b.x,
- a.y - b.y,
- a.z - b.z
- )
- else
- return fast_new(
- a.x - b,
- a.y - b,
- a.z - b
- )
- end
- end
- function metatable.__sub(a, b)
- return fast_new(
- a.x - b.x,
- a.y - b.y,
- a.z - b.z
- )
- end
- function vector.multiply(a, b)
- if type(b) == "table" then
- return fast_new(
- a.x * b.x,
- a.y * b.y,
- a.z * b.z
- )
- else
- return fast_new(
- a.x * b,
- a.y * b,
- a.z * b
- )
- end
- end
- function metatable.__mul(a, b)
- if type(a) == "table" then
- return fast_new(
- a.x * b,
- a.y * b,
- a.z * b
- )
- else
- return fast_new(
- a * b.x,
- a * b.y,
- a * b.z
- )
- end
- end
- function vector.divide(a, b)
- if type(b) == "table" then
- return fast_new(
- a.x / b.x,
- a.y / b.y,
- a.z / b.z
- )
- else
- return fast_new(
- a.x / b,
- a.y / b,
- a.z / b
- )
- end
- end
- function metatable.__div(a, b)
- -- scalar/vector makes no sense
- return fast_new(
- a.x / b,
- a.y / b,
- a.z / b
- )
- end
- -- misc stuff
- function vector.offset(v, x, y, z)
- return fast_new(
- v.x + x,
- v.y + y,
- v.z + z
- )
- end
- function vector.sort(a, b)
- return fast_new(math.min(a.x, b.x), math.min(a.y, b.y), math.min(a.z, b.z)),
- fast_new(math.max(a.x, b.x), math.max(a.y, b.y), math.max(a.z, b.z))
- end
- function vector.check(v)
- return getmetatable(v) == metatable
- 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 = vector.new(math.asin(forward.y), -math.atan2(forward.x, forward.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(vector.new(0, 1, 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 whether 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
- function vector.in_area(pos, min, max)
- return (pos.x >= min.x) and (pos.x <= max.x) and
- (pos.y >= min.y) and (pos.y <= max.y) and
- (pos.z >= min.z) and (pos.z <= max.z)
- end
- function vector.random_direction()
- -- Generate a random direction of unit length, via rejection sampling
- local x, y, z, l2
- repeat -- expected less than two attempts on average (volume sphere vs. cube)
- x, y, z = math.random() * 2 - 1, math.random() * 2 - 1, math.random() * 2 - 1
- l2 = x*x + y*y + z*z
- until l2 <= 1 and l2 >= 1e-6
- -- normalize
- local l = math.sqrt(l2)
- return fast_new(x/l, y/l, z/l)
- end
- function vector.random_in_area(min, max)
- return fast_new(
- math.random(min.x, max.x),
- math.random(min.y, max.y),
- math.random(min.z, max.z)
- )
- end
- if rawget(_G, "core") and core.set_read_vector and core.set_push_vector then
- local function read_vector(v)
- return v.x, v.y, v.z
- end
- core.set_read_vector(read_vector)
- core.set_read_vector = nil
- if rawget(_G, "jit") then
- -- This is necessary to prevent trace aborts.
- local function push_vector(x, y, z)
- return (fast_new(x, y, z))
- end
- core.set_push_vector(push_vector)
- else
- core.set_push_vector(fast_new)
- end
- core.set_push_vector = nil
- end
|