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