vector.lua 8.8 KB

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