builtin/common/vector.lua

   1 --[[
   2 Vector helpers
   3 Note: The vector.*-functions must be able to accept old vectors that had no metatables
   4 ]]
   5
   6 -- localize functions
   7 local setmetatable = setmetatable
   8
   9 vector = {}
  10
  11 local metatable = {}
  12 vector.metatable = metatable
  13
  14 local xyz = {"x", "y", "z"}
  15
  16 -- only called when rawget(v, key) returns nil
  17 function metatable.__index(v, key)
  18         return rawget(v, xyz[key]) or vector[key]
  19 end
  20
  21 -- only called when rawget(v, key) returns nil
  22 function metatable.__newindex(v, key, value)
  23         rawset(v, xyz[key] or key, value)
  24 end
  25
  26 -- constructors
  27
  28 local function fast_new(x, y, z)
  29         return setmetatable({x = x, y = y, z = z}, metatable)
  30 end
  31
  32 function vector.new(a, b, c)
  33         if type(a) == "table" then
  34                 assert(a.x and a.y and a.z, "Invalid vector passed to vector.new()")
  35                 return fast_new(a.x, a.y, a.z)
  36         elseif a then
  37                 assert(b and c, "Invalid arguments for vector.new()")
  38                 return fast_new(a, b, c)
  39         end
  40         return fast_new(0, 0, 0)
  41 end
  42
  43 function vector.from_string(s, init)
  44         local x, y, z, np = string.match(s, "^%s*%(%s*([^%s,]+)%s*[,%s]%s*([^%s,]+)%s*[,%s]" ..
  45                         "%s*([^%s,]+)%s*[,%s]?%s*%)()", init)
  46         x = tonumber(x)
  47         y = tonumber(y)
  48         z = tonumber(z)
  49         if not (x and y and z) then
  50                 return nil
  51         end
  52         return {x = x, y = y, z = z}, np
  53 end
  54
  55 function vector.to_string(v)
  56         return string.format("(%g, %g, %g)", v.x, v.y, v.z)
  57 end
  58 metatable.__tostring = vector.to_string
  59
  60 function vector.equals(a, b)
  61         return a.x == b.x and
  62                a.y == b.y and
  63                a.z == b.z
  64 end
  65 metatable.__eq = vector.equals
  66
  67 -- unary operations
  68
  69 function vector.length(v)
  70         return math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z)
  71 end
  72 -- Note: we can not use __len because it is already used for primitive table length
  73
  74 function vector.normalize(v)
  75         local len = vector.length(v)
  76         if len == 0 then
  77                 return fast_new(0, 0, 0)
  78         else
  79                 return vector.divide(v, len)
  80         end
  81 end
  82
  83 function vector.floor(v)
  84         return vector.apply(v, math.floor)
  85 end
  86
  87 function vector.round(v)
  88         return fast_new(
  89                 math.round(v.x),
  90                 math.round(v.y),
  91                 math.round(v.z)
  92         )
  93 end
  94
  95 function vector.apply(v, func)
  96         return fast_new(
  97                 func(v.x),
  98                 func(v.y),
  99                 func(v.z)
 100         )
 101 end
 102
 103 function vector.distance(a, b)
 104         local x = a.x - b.x
 105         local y = a.y - b.y
 106         local z = a.z - b.z
 107         return math.sqrt(x * x + y * y + z * z)
 108 end
 109
 110 function vector.direction(pos1, pos2)
 111         return vector.subtract(pos2, pos1):normalize()
 112 end
 113
 114 function vector.angle(a, b)
 115         local dotp = vector.dot(a, b)
 116         local cp = vector.cross(a, b)
 117         local crossplen = vector.length(cp)
 118         return math.atan2(crossplen, dotp)
 119 end
 120
 121 function vector.dot(a, b)
 122         return a.x * b.x + a.y * b.y + a.z * b.z
 123 end
 124
 125 function vector.cross(a, b)
 126         return fast_new(
 127                 a.y * b.z - a.z * b.y,
 128                 a.z * b.x - a.x * b.z,
 129                 a.x * b.y - a.y * b.x
 130         )
 131 end
 132
 133 function metatable.__unm(v)
 134         return fast_new(-v.x, -v.y, -v.z)
 135 end
 136
 137 -- add, sub, mul, div operations
 138
 139 function vector.add(a, b)
 140         if type(b) == "table" then
 141                 return fast_new(
 142                         a.x + b.x,
 143                         a.y + b.y,
 144                         a.z + b.z
 145                 )
 146         else
 147                 return fast_new(
 148                         a.x + b,
 149                         a.y + b,
 150                         a.z + b
 151                 )
 152         end
 153 end
 154 function metatable.__add(a, b)
 155         return fast_new(
 156                 a.x + b.x,
 157                 a.y + b.y,
 158                 a.z + b.z
 159         )
 160 end
 161
 162 function vector.subtract(a, b)
 163         if type(b) == "table" then
 164                 return fast_new(
 165                         a.x - b.x,
 166                         a.y - b.y,
 167                         a.z - b.z
 168                 )
 169         else
 170                 return fast_new(
 171                         a.x - b,
 172                         a.y - b,
 173                         a.z - b
 174                 )
 175         end
 176 end
 177 function metatable.__sub(a, b)
 178         return fast_new(
 179                 a.x - b.x,
 180                 a.y - b.y,
 181                 a.z - b.z
 182         )
 183 end
 184
 185 function vector.multiply(a, b)
 186         if type(b) == "table" then
 187                 return fast_new(
 188                         a.x * b.x,
 189                         a.y * b.y,
 190                         a.z * b.z
 191                 )
 192         else
 193                 return fast_new(
 194                         a.x * b,
 195                         a.y * b,
 196                         a.z * b
 197                 )
 198         end
 199 end
 200 function metatable.__mul(a, b)
 201         if type(a) == "table" then
 202                 return fast_new(
 203                         a.x * b,
 204                         a.y * b,
 205                         a.z * b
 206                 )
 207         else
 208                 return fast_new(
 209                         a * b.x,
 210                         a * b.y,
 211                         a * b.z
 212                 )
 213         end
 214 end
 215
 216 function vector.divide(a, b)
 217         if type(b) == "table" then
 218                 return fast_new(
 219                         a.x / b.x,
 220                         a.y / b.y,
 221                         a.z / b.z
 222                 )
 223         else
 224                 return fast_new(
 225                         a.x / b,
 226                         a.y / b,
 227                         a.z / b
 228                 )
 229         end
 230 end
 231 function metatable.__div(a, b)
 232         -- scalar/vector makes no sense
 233         return fast_new(
 234                 a.x / b,
 235                 a.y / b,
 236                 a.z / b
 237         )
 238 end
 239
 240 -- misc stuff
 241
 242 function vector.offset(v, x, y, z)
 243         return fast_new(
 244                 v.x + x,
 245                 v.y + y,
 246                 v.z + z
 247         )
 248 end
 249
 250 function vector.sort(a, b)
 251         return fast_new(math.min(a.x, b.x), math.min(a.y, b.y), math.min(a.z, b.z)),
 252                 fast_new(math.max(a.x, b.x), math.max(a.y, b.y), math.max(a.z, b.z))
 253 end
 254
 255 function vector.check(v)
 256         return getmetatable(v) == metatable
 257 end
 258
 259 local function sin(x)
 260         if x % math.pi == 0 then
 261                 return 0
 262         else
 263                 return math.sin(x)
 264         end
 265 end
 266
 267 local function cos(x)
 268         if x % math.pi == math.pi / 2 then
 269                 return 0
 270         else
 271                 return math.cos(x)
 272         end
 273 end
 274
 275 function vector.rotate_around_axis(v, axis, angle)
 276         local cosangle = cos(angle)
 277         local sinangle = sin(angle)
 278         axis = vector.normalize(axis)
 279         -- https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
 280         local dot_axis = vector.multiply(axis, vector.dot(axis, v))
 281         local cross = vector.cross(v, axis)
 282         return vector.new(
 283                 cross.x * sinangle + (v.x - dot_axis.x) * cosangle + dot_axis.x,
 284                 cross.y * sinangle + (v.y - dot_axis.y) * cosangle + dot_axis.y,
 285                 cross.z * sinangle + (v.z - dot_axis.z) * cosangle + dot_axis.z
 286         )
 287 end
 288
 289 function vector.rotate(v, rot)
 290         local sinpitch = sin(-rot.x)
 291         local sinyaw = sin(-rot.y)
 292         local sinroll = sin(-rot.z)
 293         local cospitch = cos(rot.x)
 294         local cosyaw = cos(rot.y)
 295         local cosroll = math.cos(rot.z)
 296         -- Rotation matrix that applies yaw, pitch and roll
 297         local matrix = {
 298                 {
 299                         sinyaw * sinpitch * sinroll + cosyaw * cosroll,
 300                         sinyaw * sinpitch * cosroll - cosyaw * sinroll,
 301                         sinyaw * cospitch,
 302                 },
 303                 {
 304                         cospitch * sinroll,
 305                         cospitch * cosroll,
 306                         -sinpitch,
 307                 },
 308                 {
 309                         cosyaw * sinpitch * sinroll - sinyaw * cosroll,
 310                         cosyaw * sinpitch * cosroll + sinyaw * sinroll,
 311                         cosyaw * cospitch,
 312                 },
 313         }
 314         -- Compute matrix multiplication: `matrix` * `v`
 315         return vector.new(
 316                 matrix[1][1] * v.x + matrix[1][2] * v.y + matrix[1][3] * v.z,
 317                 matrix[2][1] * v.x + matrix[2][2] * v.y + matrix[2][3] * v.z,
 318                 matrix[3][1] * v.x + matrix[3][2] * v.y + matrix[3][3] * v.z
 319         )
 320 end
 321
 322 function vector.dir_to_rotation(forward, up)
 323         forward = vector.normalize(forward)
 324         local rot = vector.new(math.asin(forward.y), -math.atan2(forward.x, forward.z), 0)
 325         if not up then
 326                 return rot
 327         end
 328         assert(vector.dot(forward, up) < 0.000001,
 329                         "Invalid vectors passed to vector.dir_to_rotation().")
 330         up = vector.normalize(up)
 331         -- Calculate vector pointing up with roll = 0, just based on forward vector.
 332         local forwup = vector.rotate(vector.new(0, 1, 0), rot)
 333         -- 'forwup' and 'up' are now in a plane with 'forward' as normal.
 334         -- The angle between them is the absolute of the roll value we're looking for.
 335         rot.z = vector.angle(forwup, up)
 336
 337         -- Since vector.angle never returns a negative value or a value greater
 338         -- than math.pi, rot.z has to be inverted sometimes.
 339         -- To determine wether this is the case, we rotate the up vector back around
 340         -- the forward vector and check if it worked out.
 341         local back = vector.rotate_around_axis(up, forward, -rot.z)
 342
 343         -- We don't use vector.equals for this because of floating point imprecision.
 344         if (back.x - forwup.x) * (back.x - forwup.x) +
 345                         (back.y - forwup.y) * (back.y - forwup.y) +
 346                         (back.z - forwup.z) * (back.z - forwup.z) > 0.0000001 then
 347                 rot.z = -rot.z
 348         end
 349         return rot
 350 end