2
0

vector.lua 8.0 KB

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