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 can not 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.distance(a, b)
 116         local x = a.x - b.x
 117         local y = a.y - b.y
 118         local z = a.z - b.z
 119         return math.sqrt(x * x + y * y + z * z)
 120 end
 121
 122 function vector.direction(pos1, pos2)
 123         return vector.subtract(pos2, pos1):normalize()
 124 end
 125
 126 function vector.angle(a, b)
 127         local dotp = vector.dot(a, b)
 128         local cp = vector.cross(a, b)
 129         local crossplen = vector.length(cp)
 130         return math.atan2(crossplen, dotp)
 131 end
 132
 133 function vector.dot(a, b)
 134         return a.x * b.x + a.y * b.y + a.z * b.z
 135 end
 136
 137 function vector.cross(a, b)
 138         return fast_new(
 139                 a.y * b.z - a.z * b.y,
 140                 a.z * b.x - a.x * b.z,
 141                 a.x * b.y - a.y * b.x
 142         )
 143 end
 144
 145 function metatable.__unm(v)
 146         return fast_new(-v.x, -v.y, -v.z)
 147 end
 148
 149 -- add, sub, mul, div operations
 150
 151 function vector.add(a, b)
 152         if type(b) == "table" then
 153                 return fast_new(
 154                         a.x + b.x,
 155                         a.y + b.y,
 156                         a.z + b.z
 157                 )
 158         else
 159                 return fast_new(
 160                         a.x + b,
 161                         a.y + b,
 162                         a.z + b
 163                 )
 164         end
 165 end
 166 function metatable.__add(a, b)
 167         return fast_new(
 168                 a.x + b.x,
 169                 a.y + b.y,
 170                 a.z + b.z
 171         )
 172 end
 173
 174 function vector.subtract(a, b)
 175         if type(b) == "table" then
 176                 return fast_new(
 177                         a.x - b.x,
 178                         a.y - b.y,
 179                         a.z - b.z
 180                 )
 181         else
 182                 return fast_new(
 183                         a.x - b,
 184                         a.y - b,
 185                         a.z - b
 186                 )
 187         end
 188 end
 189 function metatable.__sub(a, b)
 190         return fast_new(
 191                 a.x - b.x,
 192                 a.y - b.y,
 193                 a.z - b.z
 194         )
 195 end
 196
 197 function vector.multiply(a, b)
 198         if type(b) == "table" then
 199                 return fast_new(
 200                         a.x * b.x,
 201                         a.y * b.y,
 202                         a.z * b.z
 203                 )
 204         else
 205                 return fast_new(
 206                         a.x * b,
 207                         a.y * b,
 208                         a.z * b
 209                 )
 210         end
 211 end
 212 function metatable.__mul(a, b)
 213         if type(a) == "table" then
 214                 return fast_new(
 215                         a.x * b,
 216                         a.y * b,
 217                         a.z * b
 218                 )
 219         else
 220                 return fast_new(
 221                         a * b.x,
 222                         a * b.y,
 223                         a * b.z
 224                 )
 225         end
 226 end
 227
 228 function vector.divide(a, b)
 229         if type(b) == "table" then
 230                 return fast_new(
 231                         a.x / b.x,
 232                         a.y / b.y,
 233                         a.z / b.z
 234                 )
 235         else
 236                 return fast_new(
 237                         a.x / b,
 238                         a.y / b,
 239                         a.z / b
 240                 )
 241         end
 242 end
 243 function metatable.__div(a, b)
 244         -- scalar/vector makes no sense
 245         return fast_new(
 246                 a.x / b,
 247                 a.y / b,
 248                 a.z / b
 249         )
 250 end
 251
 252 -- misc stuff
 253
 254 function vector.offset(v, x, y, z)
 255         return fast_new(
 256                 v.x + x,
 257                 v.y + y,
 258                 v.z + z
 259         )
 260 end
 261
 262 function vector.sort(a, b)
 263         return fast_new(math.min(a.x, b.x), math.min(a.y, b.y), math.min(a.z, b.z)),
 264                 fast_new(math.max(a.x, b.x), math.max(a.y, b.y), math.max(a.z, b.z))
 265 end
 266
 267 function vector.check(v)
 268         return getmetatable(v) == metatable
 269 end
 270
 271 local function sin(x)
 272         if x % math.pi == 0 then
 273                 return 0
 274         else
 275                 return math.sin(x)
 276         end
 277 end
 278
 279 local function cos(x)
 280         if x % math.pi == math.pi / 2 then
 281                 return 0
 282         else
 283                 return math.cos(x)
 284         end
 285 end
 286
 287 function vector.rotate_around_axis(v, axis, angle)
 288         local cosangle = cos(angle)
 289         local sinangle = sin(angle)
 290         axis = vector.normalize(axis)
 291         -- https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
 292         local dot_axis = vector.multiply(axis, vector.dot(axis, v))
 293         local cross = vector.cross(v, axis)
 294         return vector.new(
 295                 cross.x * sinangle + (v.x - dot_axis.x) * cosangle + dot_axis.x,
 296                 cross.y * sinangle + (v.y - dot_axis.y) * cosangle + dot_axis.y,
 297                 cross.z * sinangle + (v.z - dot_axis.z) * cosangle + dot_axis.z
 298         )
 299 end
 300
 301 function vector.rotate(v, rot)
 302         local sinpitch = sin(-rot.x)
 303         local sinyaw = sin(-rot.y)
 304         local sinroll = sin(-rot.z)
 305         local cospitch = cos(rot.x)
 306         local cosyaw = cos(rot.y)
 307         local cosroll = math.cos(rot.z)
 308         -- Rotation matrix that applies yaw, pitch and roll
 309         local matrix = {
 310                 {
 311                         sinyaw * sinpitch * sinroll + cosyaw * cosroll,
 312                         sinyaw * sinpitch * cosroll - cosyaw * sinroll,
 313                         sinyaw * cospitch,
 314                 },
 315                 {
 316                         cospitch * sinroll,
 317                         cospitch * cosroll,
 318                         -sinpitch,
 319                 },
 320                 {
 321                         cosyaw * sinpitch * sinroll - sinyaw * cosroll,
 322                         cosyaw * sinpitch * cosroll + sinyaw * sinroll,
 323                         cosyaw * cospitch,
 324                 },
 325         }
 326         -- Compute matrix multiplication: `matrix` * `v`
 327         return vector.new(
 328                 matrix[1][1] * v.x + matrix[1][2] * v.y + matrix[1][3] * v.z,
 329                 matrix[2][1] * v.x + matrix[2][2] * v.y + matrix[2][3] * v.z,
 330                 matrix[3][1] * v.x + matrix[3][2] * v.y + matrix[3][3] * v.z
 331         )
 332 end
 333
 334 function vector.dir_to_rotation(forward, up)
 335         forward = vector.normalize(forward)
 336         local rot = vector.new(math.asin(forward.y), -math.atan2(forward.x, forward.z), 0)
 337         if not up then
 338                 return rot
 339         end
 340         assert(vector.dot(forward, up) < 0.000001,
 341                         "Invalid vectors passed to vector.dir_to_rotation().")
 342         up = vector.normalize(up)
 343         -- Calculate vector pointing up with roll = 0, just based on forward vector.
 344         local forwup = vector.rotate(vector.new(0, 1, 0), rot)
 345         -- 'forwup' and 'up' are now in a plane with 'forward' as normal.
 346         -- The angle between them is the absolute of the roll value we're looking for.
 347         rot.z = vector.angle(forwup, up)
 348
 349         -- Since vector.angle never returns a negative value or a value greater
 350         -- than math.pi, rot.z has to be inverted sometimes.
 351         -- To determine wether this is the case, we rotate the up vector back around
 352         -- the forward vector and check if it worked out.
 353         local back = vector.rotate_around_axis(up, forward, -rot.z)
 354
 355         -- We don't use vector.equals for this because of floating point imprecision.
 356         if (back.x - forwup.x) * (back.x - forwup.x) +
 357                         (back.y - forwup.y) * (back.y - forwup.y) +
 358                         (back.z - forwup.z) * (back.z - forwup.z) > 0.0000001 then
 359                 rot.z = -rot.z
 360         end
 361         return rot
 362 end