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