vector.lua 7.4 KB

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