builtin/common/vector.lua

   1
   2 vector = {}
   3
   4 function vector.new(a, b, c)
   5         if type(a) == "table" then
   6                 assert(a.x and a.y and a.z, "Invalid vector passed to vector.new()")
   7                 return {x=a.x, y=a.y, z=a.z}
   8         elseif a then
   9                 assert(b and c, "Invalid arguments for vector.new()")
  10                 return {x=a, y=b, z=c}
  11         end
  12         return {x=0, y=0, z=0}
  13 end
  14
  15 function vector.equals(a, b)
  16         return a.x == b.x and
  17                a.y == b.y and
  18                a.z == b.z
  19 end
  20
  21 function vector.length(v)
  22         return math.hypot(v.x, math.hypot(v.y, v.z))
  23 end
  24
  25 function vector.normalize(v)
  26         local len = vector.length(v)
  27         if len == 0 then
  28                 return {x=0, y=0, z=0}
  29         else
  30                 return vector.divide(v, len)
  31         end
  32 end
  33
  34 function vector.floor(v)
  35         return {
  36                 x = math.floor(v.x),
  37                 y = math.floor(v.y),
  38                 z = math.floor(v.z)
  39         }
  40 end
  41
  42 function vector.round(v)
  43         return {
  44                 x = math.floor(v.x + 0.5),
  45                 y = math.floor(v.y + 0.5),
  46                 z = math.floor(v.z + 0.5)
  47         }
  48 end
  49
  50 function vector.apply(v, func)
  51         return {
  52                 x = func(v.x),
  53                 y = func(v.y),
  54                 z = func(v.z)
  55         }
  56 end
  57
  58 function vector.distance(a, b)
  59         local x = a.x - b.x
  60         local y = a.y - b.y
  61         local z = a.z - b.z
  62         return math.hypot(x, math.hypot(y, z))
  63 end
  64
  65 function vector.direction(pos1, pos2)
  66         return vector.normalize({
  67                 x = pos2.x - pos1.x,
  68                 y = pos2.y - pos1.y,
  69                 z = pos2.z - pos1.z
  70         })
  71 end
  72
  73 function vector.angle(a, b)
  74         local dotp = vector.dot(a, b)
  75         local cp = vector.cross(a, b)
  76         local crossplen = vector.length(cp)
  77         return math.atan2(crossplen, dotp)
  78 end
  79
  80 function vector.dot(a, b)
  81         return a.x * b.x + a.y * b.y + a.z * b.z
  82 end
  83
  84 function vector.cross(a, b)
  85         return {
  86                 x = a.y * b.z - a.z * b.y,
  87                 y = a.z * b.x - a.x * b.z,
  88                 z = a.x * b.y - a.y * b.x
  89         }
  90 end
  91
  92 function vector.add(a, b)
  93         if type(b) == "table" then
  94                 return {x = a.x + b.x,
  95                         y = a.y + b.y,
  96                         z = a.z + b.z}
  97         else
  98                 return {x = a.x + b,
  99                         y = a.y + b,
 100                         z = a.z + b}
 101         end
 102 end
 103
 104 function vector.subtract(a, b)
 105         if type(b) == "table" then
 106                 return {x = a.x - b.x,
 107                         y = a.y - b.y,
 108                         z = a.z - b.z}
 109         else
 110                 return {x = a.x - b,
 111                         y = a.y - b,
 112                         z = a.z - b}
 113         end
 114 end
 115
 116 function vector.multiply(a, b)
 117         if type(b) == "table" then
 118                 return {x = a.x * b.x,
 119                         y = a.y * b.y,
 120                         z = a.z * b.z}
 121         else
 122                 return {x = a.x * b,
 123                         y = a.y * b,
 124                         z = a.z * b}
 125         end
 126 end
 127
 128 function vector.divide(a, b)
 129         if type(b) == "table" then
 130                 return {x = a.x / b.x,
 131                         y = a.y / b.y,
 132                         z = a.z / b.z}
 133         else
 134                 return {x = a.x / b,
 135                         y = a.y / b,
 136                         z = a.z / b}
 137         end
 138 end
 139
 140 function vector.offset(v, x, y, z)
 141         return {x = v.x + x,
 142                 y = v.y + y,
 143                 z = v.z + z}
 144 end
 145
 146 function vector.sort(a, b)
 147         return {x = math.min(a.x, b.x), y = math.min(a.y, b.y), z = math.min(a.z, b.z)},
 148                 {x = math.max(a.x, b.x), y = math.max(a.y, b.y), z = math.max(a.z, b.z)}
 149 end
 150
 151 local function sin(x)
 152         if x % math.pi == 0 then
 153                 return 0
 154         else
 155                 return math.sin(x)
 156         end
 157 end
 158
 159 local function cos(x)
 160         if x % math.pi == math.pi / 2 then
 161                 return 0
 162         else
 163                 return math.cos(x)
 164         end
 165 end
 166
 167 function vector.rotate_around_axis(v, axis, angle)
 168         local cosangle = cos(angle)
 169         local sinangle = sin(angle)
 170         axis = vector.normalize(axis)
 171         -- https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
 172         local dot_axis = vector.multiply(axis, vector.dot(axis, v))
 173         local cross = vector.cross(v, axis)
 174         return vector.new(
 175                 cross.x * sinangle + (v.x - dot_axis.x) * cosangle + dot_axis.x,
 176                 cross.y * sinangle + (v.y - dot_axis.y) * cosangle + dot_axis.y,
 177                 cross.z * sinangle + (v.z - dot_axis.z) * cosangle + dot_axis.z
 178         )
 179 end
 180
 181 function vector.rotate(v, rot)
 182         local sinpitch = sin(-rot.x)
 183         local sinyaw = sin(-rot.y)
 184         local sinroll = sin(-rot.z)
 185         local cospitch = cos(rot.x)
 186         local cosyaw = cos(rot.y)
 187         local cosroll = math.cos(rot.z)
 188         -- Rotation matrix that applies yaw, pitch and roll
 189         local matrix = {
 190                 {
 191                         sinyaw * sinpitch * sinroll + cosyaw * cosroll,
 192                         sinyaw * sinpitch * cosroll - cosyaw * sinroll,
 193                         sinyaw * cospitch,
 194                 },
 195                 {
 196                         cospitch * sinroll,
 197                         cospitch * cosroll,
 198                         -sinpitch,
 199                 },
 200                 {
 201                         cosyaw * sinpitch * sinroll - sinyaw * cosroll,
 202                         cosyaw * sinpitch * cosroll + sinyaw * sinroll,
 203                         cosyaw * cospitch,
 204                 },
 205         }
 206         -- Compute matrix multiplication: `matrix` * `v`
 207         return vector.new(
 208                 matrix[1][1] * v.x + matrix[1][2] * v.y + matrix[1][3] * v.z,
 209                 matrix[2][1] * v.x + matrix[2][2] * v.y + matrix[2][3] * v.z,
 210                 matrix[3][1] * v.x + matrix[3][2] * v.y + matrix[3][3] * v.z
 211         )
 212 end
 213
 214 function vector.dir_to_rotation(forward, up)
 215         forward = vector.normalize(forward)
 216         local rot = {x = math.asin(forward.y), y = -math.atan2(forward.x, forward.z), z = 0}
 217         if not up then
 218                 return rot
 219         end
 220         assert(vector.dot(forward, up) < 0.000001,
 221                         "Invalid vectors passed to vector.dir_to_rotation().")
 222         up = vector.normalize(up)
 223         -- Calculate vector pointing up with roll = 0, just based on forward vector.
 224         local forwup = vector.rotate({x = 0, y = 1, z = 0}, rot)
 225         -- 'forwup' and 'up' are now in a plane with 'forward' as normal.
 226         -- The angle between them is the absolute of the roll value we're looking for.
 227         rot.z = vector.angle(forwup, up)
 228
 229         -- Since vector.angle never returns a negative value or a value greater
 230         -- than math.pi, rot.z has to be inverted sometimes.
 231         -- To determine wether this is the case, we rotate the up vector back around
 232         -- the forward vector and check if it worked out.
 233         local back = vector.rotate_around_axis(up, forward, -rot.z)
 234
 235         -- We don't use vector.equals for this because of floating point imprecision.
 236         if (back.x - forwup.x) * (back.x - forwup.x) +
 237                         (back.y - forwup.y) * (back.y - forwup.y) +
 238                         (back.z - forwup.z) * (back.z - forwup.z) > 0.0000001 then
 239                 rot.z = -rot.z
 240         end
 241         return rot
 242 end