c3dlas.c 68 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708
  1. #include <stdio.h>
  2. #include <string.h>
  3. #include <float.h>
  4. #include <math.h>
  5. #include <limits.h>
  6. #include <float.h>
  7. #include <x86intrin.h>
  8. #include "c3dlas.h"
  9. #ifdef C3DLAS_USE_BUILTINS
  10. #define abs_double __builtin_fabs
  11. #define abs_float __builtin_fabsf
  12. #else
  13. #define abs_double fabs
  14. #define abs_float fabsf
  15. #endif
  16. #ifdef C3DLAS_NO_TGMATH
  17. // requires GCC probably
  18. /*
  19. #define FD_CHOOSE_1(a, b, fn_f, fn_d)\
  20. __builtin_choose_expr( \
  21. __builtin_types_compatible_p(__typeof__(a), double), \
  22. fn_d(a), \
  23. fn_f(a))
  24. #define FD_CHOOSE_2(a, b, fn_f, fn_d)\
  25. __builtin_choose_expr( \
  26. __builtin_types_compatible_p(__typeof__(a), double) || __builtin_types_compatible_p(__typeof__(b), double), \
  27. fn_d(a, b), \
  28. fn_f(a, b))
  29. #define fmax(a,b) FD_CHOOSE_2(a, b, fmaxf, fmax)
  30. #define fmin(a,b) FD_CHOOSE_2(a, b, fminf, fmin)
  31. #define fabs(a) FD_CHOOSE_1(a, fabsf, fabs)
  32. #define sqrt(a) FD_CHOOSE_1(a, sqrtf, sqrt)
  33. */
  34. #else
  35. #include <tgmath.h>
  36. #endif
  37. #ifndef _GNU_SOURCE
  38. static inline void sincosf(float x, float* s, float* c) {
  39. *s = sinf(x);
  40. *c = cosf(x);
  41. }
  42. #endif
  43. // utilities
  44. // reverses the argument
  45. uint32_t bitReverse32(uint32_t x) {
  46. x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1));
  47. x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2));
  48. x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4));
  49. x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8));
  50. return ((x >> 16) | (x << 16));
  51. }
  52. // reverses the least significant (len) bits, zeroing the top
  53. uint32_t reverseBits(uint32_t n, int len) {
  54. uint32_t rn = bitReverse32(n);
  55. return rn >> (32 - len);
  56. }
  57. // random numbers
  58. // returns a random number in (-1, 1) uninclusive
  59. // Thanks to Kaslai (https://github.com/Aslai) for fixing a nasty bug in the previous version
  60. float pcg_f(uint64_t* state, uint64_t stream) {
  61. union {
  62. uint32_t fu;
  63. float ff;
  64. } u;
  65. uint64_t last = *state;
  66. *state = (last * 6364136223846793005ULL) + (stream | 1);
  67. uint32_t xs = ((last >> 18) ^ last) >> 27;
  68. uint32_t rot = last >> 59;
  69. uint32_t fin = (xs >> rot) | (xs << ((-rot) & 31));
  70. uint32_t exp = (fin & 0x3F800000);
  71. exp = (0x7F + 33 - __builtin_clzl(exp)) << 23;
  72. u.fu = ((fin) & 0x807fffff) | exp;
  73. return u.ff;
  74. }
  75. // returns a random number in [0, UINT32_MAX] inclusive
  76. uint32_t pcg_u32(uint64_t* state, uint64_t stream) {
  77. uint64_t last = *state;
  78. *state = (last * 6364136223846793005ULL) + (stream | 1);
  79. uint32_t xs = ((last >> 18) ^ last) >> 27;
  80. uint32_t rot = last >> 59;
  81. uint32_t fin = (xs >> rot) | (xs << ((-rot) & 31));
  82. return fin;
  83. }
  84. // BUG: totally untested
  85. // SIMD and C versions do not return the same values.
  86. void pcg_f8(uint64_t* state, uint64_t stream, float* out) {
  87. #if defined(C3DLAS_USE_SIMD)
  88. __m256i s1, s2, xs1, xs2, xs, r, nra, q, f;
  89. s1 = _mm256_add_epi64(_mm256_set1_epi64x(*state), _mm256_set_epi64x(1,2,3,4));
  90. s2 = _mm256_add_epi64(_mm256_set1_epi64x(*state), _mm256_set_epi64x(5,6,7,8));
  91. // cycle the state
  92. *state = (*state * 6364136223846793005ULL) + (stream | 1);
  93. xs1 = _mm256_srli_epi64(_mm256_xor_si256(_mm256_srli_epi64(s1, 18), s1), 27);
  94. xs2 = _mm256_srli_epi64(_mm256_xor_si256(_mm256_srli_epi64(s2, 18), s2), 27);
  95. xs = _mm256_unpacklo_epi32(xs1, xs2);
  96. r = _mm256_srai_epi32(xs, 59);
  97. nra = _mm256_and_si256(_mm256_sign_epi32(r, _mm256_set1_epi32(-1)), _mm256_set1_epi32(31));
  98. q = _mm256_or_si256(_mm256_srav_epi32(xs, r), _mm256_sllv_epi32(xs, nra));
  99. // q is full of random 32bit integers now
  100. // convert to (-1, 1) floats by jamming in some exponent info
  101. f = _mm256_or_si256(_mm256_and_si256(q, _mm256_set1_epi32(0x807fffff)), _mm256_set1_epi32(0x3f000000));
  102. _mm256_storeu_si256((__m256i*)out, f);
  103. #else
  104. out[0] = pcg_f(state, stream);
  105. out[1] = pcg_f(state, stream);
  106. out[2] = pcg_f(state, stream);
  107. out[3] = pcg_f(state, stream);
  108. out[4] = pcg_f(state, stream);
  109. out[5] = pcg_f(state, stream);
  110. out[6] = pcg_f(state, stream);
  111. out[7] = pcg_f(state, stream);
  112. #endif
  113. }
  114. float frandPCG(float low, float high, PCG* pcg) {
  115. return low + ((high - low) * (pcg_f(&pcg->state, pcg->stream) * 0.5 + 0.5));
  116. }
  117. uint32_t urandPCG(uint32_t low, uint32_t high, PCG* pcg) {
  118. return pcg_u32(&pcg->state, pcg->stream) % (high - low) + low;
  119. }
  120. #define FN(sz, suf, ty, ft, sufft, pref, ...) \
  121. \
  122. int vEqExact##suf(const Vector##suf a, const Vector##suf b) { \
  123. return vEqExact##suf##p(&a, &b); \
  124. } \
  125. int vEqExact##suf##p(const Vector##suf const * a, const Vector##suf const * b) { \
  126. int tmp = 0; \
  127. for(int i = 0; i < sz; i++) \
  128. tmp += ((ty*)a)[i] == ((ty*)b)[i]; \
  129. return tmp == sz; \
  130. } \
  131. \
  132. int vEq##suf(const Vector##suf a, const Vector##suf b) { \
  133. return vEqEp##suf(a, b, pref##_CMP_EPSILON); \
  134. } \
  135. int vEq##suf##p(const Vector##suf* a, const Vector##suf* b) { \
  136. return vEqEp##suf(*a, *b, pref##_CMP_EPSILON); \
  137. } \
  138. \
  139. int vEqEp##suf(const Vector##suf a, const Vector##suf b, ft epsilon) { \
  140. return vEqEp##suf##p(&a, &b, epsilon); \
  141. } \
  142. int vEqEp##suf##p(const Vector##suf* a, const Vector##suf* b, ft epsilon) { \
  143. return vDistSq##suf(*a, *b) <= epsilon * epsilon; \
  144. } \
  145. \
  146. ft vDistSq##suf(const Vector##suf a, const Vector##suf b) { \
  147. return vDistSq##suf##p(&a, &b); \
  148. } \
  149. ft vDistSq##suf##p(const Vector##suf* a, const Vector##suf* b) { \
  150. ft tmp = 0; \
  151. for(int i = 0; i < sz; i++) { \
  152. ft q = ((ty*)a)[i] - ((ty*)b)[i]; \
  153. tmp += q * q; \
  154. } \
  155. return tmp;\
  156. } \
  157. ft vDist##suf(const Vector##suf a, const Vector##suf b) { \
  158. return sqrt(vDistSq##suf##p(&a, &b)); \
  159. } \
  160. ft vDist##suf##p(const Vector##suf* a, const Vector##suf* b) { \
  161. return sqrt(vDistSq##suf##p(a, b)); \
  162. } \
  163. \
  164. Vector##suf vAdd##suf(const Vector##suf a, const Vector##suf b) { \
  165. Vector##suf out; \
  166. vAdd##suf##p(&a, &b, &out); \
  167. return out; \
  168. } \
  169. void vAdd##suf##p(const Vector##suf* a, const Vector##suf* b, Vector##suf* out) { \
  170. for(int i = 0; i < sz; i++) { \
  171. ((ty*)out)[i] = ((ty*)a)[i] + ((ty*)b)[i]; \
  172. } \
  173. } \
  174. \
  175. Vector##suf vSub##suf(const Vector##suf a, const Vector##suf b) { \
  176. Vector##suf out; \
  177. vSub##suf##p(&a, &b, &out); \
  178. return out; \
  179. } \
  180. void vSub##suf##p(const Vector##suf const * a, const Vector##suf const * b, Vector##suf* out) { \
  181. for(int i = 0; i < sz; i++) { \
  182. ((ty*)out)[i] = ((ty*)a)[i] - ((ty*)b)[i]; \
  183. } \
  184. } \
  185. \
  186. Vector##suf vMul##suf(const Vector##suf a, const Vector##suf b) { \
  187. Vector##suf out; \
  188. vMul##suf##p(&a, &b, &out); \
  189. return out; \
  190. } \
  191. void vMul##suf##p(const Vector##suf const * a, const Vector##suf const * b, Vector##suf* out) { \
  192. for(int i = 0; i < sz; i++) { \
  193. ((ty*)out)[i] = ((ty*)a)[i] * ((ty*)b)[i]; \
  194. } \
  195. } \
  196. \
  197. Vector##suf vDiv##suf(const Vector##suf top, const Vector##suf bot) { \
  198. Vector##suf out; \
  199. vDiv##suf##p(&top, &bot, &out); \
  200. return out; \
  201. } \
  202. void vDiv##suf##p(const Vector##suf const * top, const Vector##suf const * bot, Vector##suf* out) { \
  203. for(int i = 0; i < sz; i++) { \
  204. ((ty*)out)[i] = ((ty*)top)[i] / ((ty*)bot)[i]; \
  205. } \
  206. } \
  207. \
  208. ft vDot##suf(const Vector##suf a, const Vector##suf b) { \
  209. return vDot##suf##p(&a, &b); \
  210. } \
  211. ft vDot##suf##p(const Vector##suf* a, const Vector##suf* b) { \
  212. ft tmp = 0; \
  213. for(int i = 0; i < sz; i++) { \
  214. tmp += ((ty*)a)[i] * ((ty*)b)[i]; \
  215. } \
  216. return tmp;\
  217. } \
  218. \
  219. Vector##sufft vScale##suf(const Vector##suf v, ft scalar) { \
  220. Vector##sufft out; \
  221. vScale##suf##p(&v, scalar, &out); \
  222. return out; \
  223. } \
  224. void vScale##suf##p(const Vector##suf* v, ft scalar, Vector##sufft* out) { \
  225. for(int i = 0; i < sz; i++) \
  226. ((ft*)out)[i] = (ft)((ty*)v)[i] * scalar; \
  227. } \
  228. \
  229. \
  230. Vector##sufft vAvg##suf(const Vector##suf a, const Vector##suf b) { \
  231. Vector##sufft out; \
  232. vAvg##suf##p(&a, &b, &out); \
  233. return out; \
  234. } \
  235. void vAvg##suf##p(const Vector##suf* a, const Vector##suf* b, Vector##sufft* out) { \
  236. for(int i = 0; i < sz; i++) { \
  237. ((ty*)out)[i] = (((ty*)a)[i] + ((ty*)b)[i]) / (ft)2.0; \
  238. } \
  239. } \
  240. \
  241. Vector##suf vNeg##suf(const Vector##suf v) { \
  242. Vector##suf out; \
  243. vNeg##suf##p(&v, &out); \
  244. return out; \
  245. } \
  246. void vNeg##suf##p(const Vector##suf* v, Vector##suf* out) { \
  247. for(int i = 0; i < sz; i++) \
  248. ((ty*)out)[i] = -((ty*)v)[i]; \
  249. } \
  250. \
  251. Vector##sufft vLerp##suf(const Vector##suf a, const Vector##suf b, ft t) { \
  252. Vector##sufft out; \
  253. vLerp##suf##p(&a, &b, t, &out); \
  254. return out; \
  255. } \
  256. void vLerp##suf##p(const Vector##suf* a, const Vector##suf* b, ft t, Vector##sufft* out) { \
  257. for(int i = 0; i < sz; i++) \
  258. ((ft*)out)[i] = (ft)((ty*)a)[i] + ((ft)(((ty*)b)[i] - ((ty*)a)[i]) * t) ; \
  259. } \
  260. \
  261. Vector##sufft vInv##suf(const Vector##suf v) { \
  262. Vector##sufft out; \
  263. vInv##suf##p(&v, &out); \
  264. return out; \
  265. } \
  266. void vInv##suf##p(const Vector##suf* v, Vector##sufft* out) { \
  267. for(int i = 0; i < sz; i++) \
  268. ((ft*)out)[i] = (((ty*)v)[i] == 0) ? pref##_MAX : ((ft)1.0 / (ft)((ty*)v)[i]); \
  269. } \
  270. \
  271. ft vLen##suf(const Vector##suf v) { \
  272. return vLen##suf##p(&v); \
  273. } \
  274. ft vLen##suf##p(const Vector##suf* v) { \
  275. ft tmp = 0.0; \
  276. for(int i = 0; i < sz; i++) \
  277. tmp += (ft)((ty*)v)[i] * (ft)((ty*)v)[i]; \
  278. return sqrt(tmp); \
  279. } \
  280. \
  281. ft vLenSq##suf(const Vector##suf v) { \
  282. return vLenSq##suf##p(&v); \
  283. } \
  284. ft vLenSq##suf##p(const Vector##suf* v) { \
  285. return vDot##suf##p(v, v); \
  286. } \
  287. \
  288. ft vMag##suf(const Vector##suf v) { \
  289. return vLen##suf##p(&v); \
  290. } \
  291. ft vMag##suf##p(const Vector##suf* v) { \
  292. return vLen##suf##p(v); \
  293. } \
  294. \
  295. ft vInvLen##suf(const Vector##suf v) { \
  296. ft tmp = vLen##suf(v); \
  297. return tmp == 0 ? pref##_MAX : (ft)1.0 / tmp; \
  298. } \
  299. ft vInvLen##suf##p(const Vector##suf* v) { \
  300. return vInvLen##suf(*v); \
  301. } \
  302. \
  303. Vector##sufft vNorm##suf(const Vector##suf v) { \
  304. Vector##sufft out; \
  305. vNorm##suf##p(&v, &out); \
  306. return out; \
  307. } \
  308. void vNorm##suf##p(const Vector##suf* v, Vector##sufft* out) { \
  309. ft n = vLenSq##suf(*v); \
  310. \
  311. if(n >= (ft)1.0f - pref##_CMP_EPSILON && n <= (ft)1.0 + pref##_CMP_EPSILON) { \
  312. for(int i = 0; i < sz; i++) \
  313. ((ft*)out)[i] = (ft)((ty*)v)[i]; \
  314. return; \
  315. } \
  316. else if(n == 0.0) { \
  317. for(int i = 0; i < sz; i++) \
  318. ((ft*)out)[i] = 0; \
  319. return; \
  320. } \
  321. \
  322. n = (ft)1.0 / sqrt(n); \
  323. for(int i = 0; i < sz; i++) \
  324. ((ft*)out)[i] = (ft)((ty*)v)[i] * n; \
  325. } \
  326. \
  327. Vector##sufft vUnit##suf(const Vector##suf v) { \
  328. return vNorm##suf(v); \
  329. } \
  330. void vUnit##suf##p(const Vector##suf* v, Vector##sufft* out) { \
  331. return vNorm##suf##p(v, out); \
  332. } \
  333. C3DLAS_VECTOR_LIST(FN)
  334. #undef FN
  335. // swap two vectors
  336. void vSwap2ip(Vector2i* a, Vector2i* b) {
  337. Vector2i t;
  338. t = *b;
  339. *b = *a;
  340. *a = t;
  341. }
  342. void vSwap2p(Vector2* a, Vector2* b) {
  343. Vector2 t;
  344. t = *b;
  345. *b = *a;
  346. *a = t;
  347. }
  348. void vSwap3p(Vector3* a, Vector3* b) {
  349. Vector3 t;
  350. t = *b;
  351. *b = *a;
  352. *a = t;
  353. }
  354. void vSwap4p(Vector4* a, Vector4* b) {
  355. Vector4 t;
  356. t = *b;
  357. *b = *a;
  358. *a = t;
  359. }
  360. // Cross product: out = a x b
  361. // Cross products *proper* only exist in 3 and 7 dimensions
  362. Vector3 vCross3(Vector3 a, Vector3 b) {
  363. Vector3 out;
  364. vCross3p(&a, &b, &out);
  365. return out;
  366. }
  367. void vCross3p(Vector3* a, Vector3* b, Vector3* out) {
  368. out->x = (a->y * b->z) - (a->z * b->y);
  369. out->y = (a->z * b->x) - (a->x * b->z);
  370. out->z = (a->x * b->y) - (a->y * b->x);
  371. }
  372. // ... however, if you apply it to two dimensions it yields
  373. // the sine of the angle between the two vectors, and the sign
  374. // of the value determines which side of a that b is on. If
  375. // the value is zero, the vectors are parallel.
  376. float vCross2(Vector2 a, Vector2 b) {
  377. return (a.x * b.y) - (a.y * b.x);
  378. }
  379. float vCross2p(Vector2* a, Vector2* b) {
  380. return (a->x * b->y) - (a->y * b->x);
  381. }
  382. // Scalar triple product: a . (b x c)
  383. float vScalarTriple3(Vector3 a, Vector3 b, Vector3 c) {
  384. return vScalarTriple3p(&a, &b, &c);
  385. }
  386. float vScalarTriple3p(Vector3* a, Vector3* b, Vector3* c) {
  387. return (float)((a->x * b->y * c->z) - (a->x * b->z * c->y) - (a->y * b->x * c->z)
  388. + (a->z * b->x * c->y) + (a->y * b->z * c->x) - (a->z * b->y * c->x));
  389. }
  390. // Vector Inverse. Returns FLT_MAX on div/0
  391. // Vector magnitude (length)
  392. // Squared distance from one point to another
  393. // Distance from one point to another
  394. // Vector normalize (scale to unit length)
  395. // vMin(a, b) Returns the minimum values of each component
  396. // vMin(a, b) Returns the maximum values of each component
  397. #define FN(sz, suf, t, maxval) \
  398. void vMin##sz##suf##p(const Vector##sz##suf* a, const Vector##sz##suf* b, Vector##sz##suf* out) { \
  399. for(int i = 0; i < sz; i++) \
  400. ((t*)out)[i] = fmin(((t*)a)[i], ((t*)b)[i]); \
  401. } \
  402. void vMax##sz##suf##p(const Vector##sz##suf* a, const Vector##sz##suf* b, Vector##sz##suf* out) { \
  403. for(int i = 0; i < sz; i++) \
  404. ((t*)out)[i] = fmax(((t*)a)[i], ((t*)b)[i]); \
  405. } \
  406. Vector##sz##suf vMin##sz##suf(Vector##sz##suf a, Vector##sz##suf b) { \
  407. Vector##sz##suf out; \
  408. vMin##sz##suf##p(&a, &b, &out); \
  409. return out; \
  410. } \
  411. Vector##sz##suf vMax##sz##suf(Vector##sz##suf a, Vector##sz##suf b) { \
  412. Vector##sz##suf out; \
  413. vMax##sz##suf##p(&a, &b, &out); \
  414. return out; \
  415. } \
  416. \
  417. int vMinComp##sz##suf##p(const Vector##sz##suf* a) { \
  418. t best = ((t*)a)[0]; \
  419. int best_ind = 0; \
  420. for(int i = 1; i < sz; i++) { \
  421. if(((t*)a)[i] < best) { \
  422. best = ((t*)a)[i]; \
  423. best_ind = i; \
  424. } \
  425. } \
  426. return best_ind; \
  427. } \
  428. \
  429. int vMaxComp##sz##suf##p(const Vector##sz##suf* a) { \
  430. t best = ((t*)a)[0]; \
  431. int best_ind = 0; \
  432. for(int i = 1; i < sz; i++) { \
  433. if(((t*)a)[i] > best) { \
  434. best = ((t*)a)[i]; \
  435. best_ind = i; \
  436. } \
  437. } \
  438. return best_ind; \
  439. } \
  440. \
  441. int vMinComp##sz##suf(const Vector##sz##suf a) { \
  442. return vMinComp##sz##suf##p(&a); \
  443. } \
  444. \
  445. int vMaxComp##sz##suf(const Vector##sz##suf a) { \
  446. return vMaxComp##sz##suf##p(&a); \
  447. } \
  448. \
  449. Vector##sz##suf vClamp##sz##suf(Vector##sz##suf in, Vector##sz##suf min, Vector##sz##suf max) { \
  450. Vector##sz##suf out; \
  451. for(int i = 0; i < sz; i++) \
  452. ((t*)&out)[i] = fmax(((t*)&min)[i], fmin(((t*)&in)[i], ((t*)&max)[i])); \
  453. return out; \
  454. } \
  455. Vector##sz##suf vAbs##sz##suf(const Vector##sz##suf v) { \
  456. Vector##sz##suf out; \
  457. vAbs##sz##suf##p(&v, &out); \
  458. return out; \
  459. } \
  460. void vAbs##sz##suf##p(const Vector##sz##suf* v, Vector##sz##suf* out) { \
  461. for(int i = 0; i < sz; i++) \
  462. ((t*)out)[i] = abs_##t( ((t*)v)[i] ); \
  463. } \
  464. Vector##sz##suf vSign##sz##suf(const Vector##sz##suf v) { \
  465. Vector##sz##suf out; \
  466. vSign##sz##suf##p(&v, &out); \
  467. return out; \
  468. } \
  469. void vSign##sz##suf##p(const Vector##sz##suf* v, Vector##sz##suf* out) { \
  470. for(int i = 0; i < sz; i++) \
  471. ((t*)out)[i] = copysign((t)1.0, ((t*)v)[i] ); \
  472. } \
  473. Vector##sz##suf vStep##sz##suf(const Vector##sz##suf edge, const Vector##sz##suf v) { \
  474. Vector##sz##suf out; \
  475. vStep##sz##suf##p(&edge, &v, &out); \
  476. return out; \
  477. } \
  478. void vStep##sz##suf##p(const Vector##sz##suf* edge, const Vector##sz##suf* v, Vector##sz##suf* out) { \
  479. for(int i = 0; i < sz; i++) \
  480. ((t*)out)[i] = ((t*)v)[i] < ((t*)edge)[i] ? 0.0 : 1.0; \
  481. } \
  482. FN(2, , float, FLT_MAX)
  483. FN(3, , float, FLT_MAX)
  484. FN(4, , float, FLT_MAX)
  485. FN(2, d, double, DBL_MAX)
  486. FN(3, d, double, DBL_MAX)
  487. FN(4, d, double, DBL_MAX)
  488. #undef FN
  489. #define FN(sz, suf, t, maxval) \
  490. void vMin##sz##suf##p(const Vector##sz##suf* a, const Vector##sz##suf* b, Vector##sz##suf* out) { \
  491. for(int i = 0; i < sz; i++) \
  492. ((t*)out)[i] = MIN(((t*)a)[i], ((t*)b)[i]); \
  493. } \
  494. void vMax##sz##suf##p(const Vector##sz##suf* a, const Vector##sz##suf* b, Vector##sz##suf* out) { \
  495. for(int i = 0; i < sz; i++) \
  496. ((t*)out)[i] = MAX(((t*)a)[i], ((t*)b)[i]); \
  497. } \
  498. Vector##sz##suf vMin##sz##suf(Vector##sz##suf a, Vector##sz##suf b) { \
  499. Vector##sz##suf out; \
  500. vMin##sz##suf##p(&a, &b, &out); \
  501. return out; \
  502. } \
  503. Vector##sz##suf vMax##sz##suf(Vector##sz##suf a, Vector##sz##suf b) { \
  504. Vector##sz##suf out; \
  505. vMax##sz##suf##p(&a, &b, &out); \
  506. return out; \
  507. } \
  508. Vector##sz##suf vClamp##sz##suf(Vector##sz##suf in, Vector##sz##suf min, Vector##sz##suf max) { \
  509. Vector##sz##suf out; \
  510. for(int i = 0; i < sz; i++) \
  511. ((t*)&out)[i] = MAX(((t*)&min)[i], MIN(((t*)&in)[i], ((t*)&max)[i])); \
  512. return out; \
  513. } \
  514. Vector##sz##suf vAbs##sz##suf(const Vector##sz##suf v) { \
  515. Vector##sz##suf out; \
  516. vAbs##sz##suf##p(&v, &out); \
  517. return out; \
  518. } \
  519. void vAbs##sz##suf##p(const Vector##sz##suf* v, Vector##sz##suf* out) { \
  520. for(int i = 0; i < sz; i++) \
  521. ((t*)out)[i] = labs( ((t*)v)[i] ); \
  522. } \
  523. Vector##sz##suf vSign##sz##suf(const Vector##sz##suf v) { \
  524. Vector##sz##suf out; \
  525. vSign##sz##suf##p(&v, &out); \
  526. return out; \
  527. } \
  528. void vSign##sz##suf##p(const Vector##sz##suf* v, Vector##sz##suf* out) { \
  529. for(int i = 0; i < sz; i++) \
  530. ((t*)out)[i] = ((t*)v)[i] < 0 ? -1 : 1; \
  531. } \
  532. Vector##sz##suf vStep##sz##suf(const Vector##sz##suf edge, const Vector##sz##suf v) { \
  533. Vector##sz##suf out; \
  534. vStep##sz##suf##p(&edge, &v, &out); \
  535. return out; \
  536. } \
  537. void vStep##sz##suf##p(const Vector##sz##suf* edge, const Vector##sz##suf* v, Vector##sz##suf* out) { \
  538. for(int i = 0; i < sz; i++) \
  539. ((t*)out)[i] = ((t*)v)[i] < ((t*)edge)[i] ? 0.0 : 1.0; \
  540. } \
  541. FN(2, i, int, DBL_MAX)
  542. FN(3, i, int, DBL_MAX)
  543. FN(4, i, int, DBL_MAX)
  544. FN(2, l, long, DBL_MAX)
  545. FN(3, l, long, DBL_MAX)
  546. FN(4, l, long, DBL_MAX)
  547. #undef FN
  548. // Returns an arbitrary unit vector perpendicular to the input
  549. // The input vector does not need to be normalized
  550. void vPerp3p(Vector3* n, Vector3* out) {
  551. *out = vPerp3(*n);
  552. }
  553. Vector3 vPerp3(Vector3 n) {
  554. float f, d;
  555. float absx = fabs(n.x);
  556. float absy = fabs(n.y);
  557. if(absx < absy) {
  558. if(n.x < n.z) goto MIN_X;
  559. goto MIN_Z;
  560. }
  561. if(absy < fabs(n.z)) goto MIN_Y;
  562. goto MIN_Z;
  563. MIN_X:
  564. d = 1.0f / sqrtf(n.z * n.z + n.y * n.y);
  565. f = n.z;
  566. n.z = n.y * d;
  567. n.y = -f * d;
  568. n.x = 0;
  569. return n;
  570. MIN_Y:
  571. d = 1.0f / sqrtf(n.z * n.z + n.x * n.x);
  572. f = n.x;
  573. n.x = n.z * d;
  574. n.z = -f * d;
  575. n.y = 0;
  576. return n;
  577. MIN_Z:
  578. d = 1.0f / sqrtf(n.x * n.x + n.y * n.y);
  579. f = n.y;
  580. n.y = n.x * d;
  581. n.x = -f * d;
  582. n.z = 0;
  583. return n;
  584. }
  585. // Returns an arbitrary unit vector perpendicular to the input
  586. // The input vector does not need to be normalized
  587. void vPerp2p(Vector2* n, Vector2* out) {
  588. *out = vPerp2(*n);
  589. }
  590. Vector2 vPerp2(Vector2 n) {
  591. return vNorm2((Vector2){.x = -n.y, .y = n.x});
  592. }
  593. // Coordinate system conversions
  594. // Does not check for degenerate vectors
  595. // Cartesian to Spherical
  596. Vector3 vC2S3(Vector3 cart) {
  597. Vector3 sp;
  598. sp.rho = vMag3(cart);
  599. sp.theta = atan2f(cart.x, cart.y);
  600. sp.phi = acosf(cart.z / sp.rho);
  601. return sp;
  602. }
  603. // Spherical to Cartesian
  604. Vector3 vS2C3(Vector3 s) {
  605. float st, ct, sp, cp;
  606. // as of July 2022, gcc trunk is smart enough to automatically optimize to sincos, but clang isn't.
  607. sincosf(s.phi, &sp, &cp);
  608. sincosf(s.theta, &st, &ct);
  609. return (Vector3){
  610. .x = s.rho * sp * ct,
  611. .y = s.rho * sp * st,
  612. .z = s.rho * cp
  613. };
  614. }
  615. Vector3 closestPointToRay3(Vector3 p, Ray3 r) {
  616. Vector3 po = vSub3(p, r.o); // vector from the starting point to p
  617. float t = vDot3(po, r.d); // project the pa onto the ray direction
  618. fclamp(t, 0.0, 1.0); // clamp t to between the endpoints of the line segment
  619. return vSub3(po, vScale3(r.d, t));
  620. }
  621. // completely untested.
  622. // can probably be optimized
  623. // This function is poorly named. It is designed to check if a bounding sphere intersects a cone surrounding a viewing frustum.
  624. int distanceSphereToCone(Vector3 spc, float spr, Vector3 c1, Vector3 c2, float cr1, float cr2) {
  625. Vector3 cnorm = vNorm(vSub(c2, c1)); // normal pointing down the center of the cone
  626. Vector3 sp_c1 = vSub(spc, c1); // vector pointing from c1 to the sphere center
  627. Vector3 up = vCross3(spc, cnorm); // vector perpendicular to the plane containing the cone's centerline and the sphere center.
  628. Vector3 perp_toward_sp = vNorm(vCross3(cnorm, up)); // vector perpendicular to the cone's centerline within the plane, towards the sphere
  629. Vector3 outer_c1 = vAdd(c1, vScale(perp_toward_sp, cr1)); // point in the plane on the outer edge of the cone
  630. Vector3 outer_c2 = vAdd(c2, vScale(perp_toward_sp, cr2)); // point in the plane on the outer edge of the cone
  631. Vector3 closest = closestPointToRay3(spc, (Ray3){.o = outer_c1, .d = vNorm(vSub(outer_c2, outer_c1))}); // point on the cone closest to the sphere
  632. // this part is probably wrong
  633. if(vDot(perp_toward_sp, vSub(spc, closest)) < 0) return 1; // is the sphere center inside the cone?
  634. return (vDist(closest, spc) - spr) <= 0;
  635. }
  636. float distTPointRay3(Vector3 p, Ray3 r, float* T) {
  637. Vector3 pa = vSub3(p, r.o);
  638. Vector3 ba = vNeg3(r.d);// vSub3(ls.end, ls.start);
  639. float t = vDot3(pa, ba) / vDot3(ba, ba);
  640. if(T) *T = t;
  641. return vLen3(vSub3(pa, vScale3(ba, t)));
  642. }
  643. float dist2TPointRay3(Vector3 p, Ray3 r, float* T) {
  644. Vector3 pa = vSub3(p, r.o);
  645. Vector3 ba = vNeg3(r.d);// vSub3(ls.end, ls.start);
  646. float t = vDot3(pa, ba) / vDot3(ba, ba);
  647. if(T) *T = t;
  648. return vLenSq3(vSub3(pa, vScale3(ba, t)));
  649. }
  650. int vInsidePolygon(Vector2 p, Polygon* poly) {
  651. int inside = 0;
  652. int cnt = poly->pointCount;
  653. if(poly->maxRadiusSq < vDot2(poly->centroid, p)) return 0;
  654. for(int i = 0; i < cnt; i++) {
  655. Vector2 a = poly->points[i];
  656. Vector2 b = poly->points[(i + 1) % cnt];
  657. if(a.y == b.y) continue; // horizontal edges are ignored
  658. // we're testing a ray going to the right
  659. if(a.x < p.x && b.x < p.x) continue; // segment is entirely left of the point
  660. if(a.y >= p.y && b.y >= p.y) continue; // segment entirely above the point
  661. if(a.y < p.y && b.y < p.y) continue; // segment entirely below the point
  662. // segment is in the same vertical band as the point
  663. float sx = a.x + (b.x - a.x) * ((p.y - a.y) / (b.y - a.y));
  664. if(p.x > sx) continue;
  665. inside = !inside;
  666. }
  667. return inside;
  668. }
  669. // Muchas gracias, Inigo.
  670. // https://iquilezles.org/articles/distfunctions2d/
  671. float vDistPolygon(Vector2 p, Polygon* poly) {
  672. float d = vDot2(vSub2(p, poly->points[0]), vSub2(p, poly->points[0]));
  673. float s = 1.0;
  674. for(int i = 0, j = poly->pointCount - 1; i < poly->pointCount; j = i, i++) {
  675. Vector2 A = poly->points[i];
  676. Vector2 B = poly->points[j];
  677. Vector2 e = vSub2(B, A);
  678. Vector2 w = vSub2(p, A);
  679. Vector2 b = vSub2(w, vScale2(e, fclamp(vDot2(w, e) / vDot2(e, e), 0.0, 1.0)));
  680. d = fminf(d, vDot2(b, b));
  681. int c1 = p.y >= A.y;
  682. int c2 = p.y < B.y;
  683. int c3 = e.x * w.y > e.y * w.x;
  684. if((c1 && c2 && c3) || (!c1 && !c2 && !c3)) s *= -1.0;
  685. }
  686. return s * sqrtf(d);
  687. }
  688. // ----
  689. void polyCalcCentroid(Polygon* poly) {
  690. int cnt = poly->pointCount;
  691. Vector2 centroid = {0,0};
  692. for(int i = 0; i < cnt; i++) {
  693. Vector2 a = poly->points[i];
  694. centroid = vAdd2(centroid, a);
  695. }
  696. poly->centroid = vScale2(centroid, 1.0 / poly->pointCount);
  697. }
  698. void polyCalcRadiusSq(Polygon* poly) {
  699. int cnt = poly->pointCount;
  700. float d = 0;
  701. for(int i = 0; i < cnt; i++) {
  702. Vector2 a = poly->points[i];
  703. d = fmaxf(d, vDot2(poly->centroid, a));
  704. }
  705. poly->maxRadiusSq = d;
  706. }
  707. // feeding a zero vector into this will cause div/0 and you will deserve it
  708. void vProject3p(Vector3* what, Vector3* onto, Vector3* out) { // slower; onto may not be normalized
  709. float wdo = vDot3p(what, onto);
  710. float odo = vDot3p(onto, onto);
  711. vScale3p(onto, wdo / odo, out);
  712. }
  713. void vProjectNorm3p(Vector3* what, Vector3* onto, Vector3* out) { // faster; onto must be normalized
  714. float wdo = vDot3p(what, onto);
  715. vScale3p(onto, wdo, out);
  716. }
  717. void vRandomPCG2p(Vector2* end1, Vector2* end2, PCG* pcg, Vector2* out) {
  718. out->x = frandPCG(fminf(end1->x, end2->x), fmaxf(end1->x, end2->x), pcg);
  719. out->y = frandPCG(fminf(end1->y, end2->y), fmaxf(end1->y, end2->y), pcg);
  720. }
  721. Vector2 vRandomPCG2(Vector2 end1, Vector2 end2, PCG* pcg) {
  722. Vector2 o;
  723. vRandomPCG2p(&end1, &end2, pcg, &o);
  724. return o;
  725. }
  726. void vRandomNormPCG2p(PCG* pcg, Vector2* out) {
  727. float th = frandPCG(0, 2.0 * F_PI, pcg);
  728. float sth, cth;
  729. sincosf(th, &sth, &cth);
  730. out->x = cth;
  731. out->y = sth;
  732. }
  733. Vector2 vRandomNormPCG2(PCG* pcg) {
  734. Vector2 o;
  735. vRandomNormPCG2p(pcg, &o);
  736. return o;
  737. }
  738. void vRandomPCG3p(Vector3* end1, Vector3* end2, PCG* pcg, Vector3* out) {
  739. out->x = frandPCG(fminf(end1->x, end2->x), fmaxf(end1->x, end2->x), pcg);
  740. out->y = frandPCG(fminf(end1->y, end2->y), fmaxf(end1->y, end2->y), pcg);
  741. out->z = frandPCG(fminf(end1->z, end2->z), fmaxf(end1->z, end2->z), pcg);
  742. }
  743. Vector3 vRandomPCG3(Vector3 end1, Vector3 end2, PCG* pcg) {
  744. Vector3 o;
  745. vRandomPCG3p(&end1, &end2, pcg, &o);
  746. return o;
  747. }
  748. // This algorithm is uniformly distributed over the surface of a sphere. There is no clustering at the poles.
  749. void vRandomNormPCG3p(PCG* pcg, Vector3* out) {
  750. float u = frandPCG(-1.f, 1.f, pcg);
  751. float th = frandPCG(0.f, 2.f * F_PI, pcg);
  752. float q = sqrtf(1.f - u * u);
  753. float sth, cth;
  754. sincosf(th, &sth, &cth);
  755. out->x = u * cth;
  756. out->y = u * sth;
  757. out->z = u;
  758. }
  759. Vector3 vRandomNormPCG3(PCG* pcg) {
  760. Vector3 o;
  761. vRandomNormPCG3p(pcg, &o);
  762. return o;
  763. }
  764. void vRandom3p(Vector3* end1, Vector3* end2, Vector3* out) {
  765. out->x = frand(fminf(end1->x, end2->x), fmaxf(end1->x, end2->x));
  766. out->y = frand(fminf(end1->y, end2->y), fmaxf(end1->y, end2->y));
  767. out->z = frand(fminf(end1->z, end2->z), fmaxf(end1->z, end2->z));
  768. }
  769. Vector3 vRandom3(Vector3 end1, Vector3 end2) {
  770. return (Vector3){
  771. .x = frand(fminf(end1.x, end2.x), fmaxf(end1.x, end2.x)),
  772. .y = frand(fminf(end1.y, end2.y), fmaxf(end1.y, end2.y)),
  773. .z = frand(fminf(end1.z, end2.z), fmaxf(end1.z, end2.z))
  774. };
  775. }
  776. // Uniformly distributed around the unit sphere; ie, no clustering at the poles.
  777. Vector3 vRandomNorm3() {
  778. Vector3 out;
  779. vRandomNorm3p(&out);
  780. return out;
  781. }
  782. void vRandomNorm3p(Vector3* out) {
  783. float u = frand(-1.0, 1.0);
  784. float th = frand(0, 2.0 * F_PI);
  785. float q = sqrtf(1.0 - u * u);
  786. float sth, cth;
  787. sincosf(th, &sth, &cth);
  788. out->x = u * cth;
  789. out->y = u * sth;
  790. out->z = u;
  791. }
  792. Vector4i vFloor4(const Vector4 v) {
  793. return (Vector4i){.x = floorf(v.x), .y = floorf(v.y), .z = floorf(v.z), .w = floorf(v.w)};
  794. }
  795. Vector4i vCeil4(const Vector4 v) {
  796. return (Vector4i){.x = ceilf(v.x), .y = ceilf(v.y), .z = ceilf(v.z), .w = ceilf(v.w)};
  797. }
  798. Vector3i vFloor3(const Vector3 v) {
  799. return (Vector3i){.x = floorf(v.x), .y = floorf(v.y), .z = floorf(v.z)};
  800. }
  801. Vector3i vCeil3(const Vector3 v) {
  802. return (Vector3i){.x = ceilf(v.x), .y = ceilf(v.y), .z = ceilf(v.z)};
  803. }
  804. Vector2i vFloor2(const Vector2 v) {
  805. return (Vector2i){.x = floorf(v.x), .y = floorf(v.y)};
  806. }
  807. Vector2i vCeil2(const Vector2 v) {
  808. return (Vector2i){.x = ceilf(v.x), .y = ceilf(v.y)};
  809. }
  810. Vector4l vFloor4d(const Vector4d v) {
  811. return (Vector4l){.x = floor(v.x), .y = floor(v.y), .z = floor(v.z), .w = floor(v.w)};
  812. }
  813. Vector4l vCeil4d(const Vector4d v) {
  814. return (Vector4l){.x = ceil(v.x), .y = ceil(v.y), .z = ceil(v.z), .w = ceil(v.w)};
  815. }
  816. Vector3l vFloor3d(const Vector3d v) {
  817. return (Vector3l){.x = floor(v.x), .y = floor(v.y), .z = floor(v.z)};
  818. }
  819. Vector3l vCeil3d(const Vector3d v) {
  820. return (Vector3l){.x = ceil(v.x), .y = ceil(v.y), .z = ceil(v.z)};
  821. }
  822. Vector2l vFloor2d(const Vector2d v) {
  823. return (Vector2l){.x = floor(v.x), .y = floor(v.y)};
  824. }
  825. Vector2l vCeil2d(const Vector2d v) {
  826. return (Vector2l){.x = ceil(v.x), .y = ceil(v.y)};
  827. }
  828. Vector2 vModPositive2(Vector2 v, Vector2 m) {
  829. return (Vector2){
  830. .x = fmodf(fmodf(v.x, m.x) + m.x, m.x),
  831. .y = fmodf(fmodf(v.y, m.y) + m.y, m.y),
  832. };
  833. }
  834. Vector3 vModPositive3(Vector3 v, Vector3 m) {
  835. return (Vector3){
  836. .x = fmodf(fmodf(v.x, m.x) + m.x, m.x),
  837. .y = fmodf(fmodf(v.y, m.y) + m.y, m.y),
  838. .z = fmodf(fmodf(v.z, m.z) + m.z, m.z),
  839. };
  840. }
  841. Vector4 vModPositive4(Vector4 v, Vector4 m) {
  842. return (Vector4){
  843. .x = fmodf(fmodf(v.x, m.x) + m.x, m.x),
  844. .y = fmodf(fmodf(v.y, m.y) + m.y, m.y),
  845. .z = fmodf(fmodf(v.z, m.z) + m.z, m.z),
  846. .w = fmodf(fmodf(v.w, m.w) + m.w, m.w),
  847. };
  848. }
  849. // reflects the distance from v to pivot across pivot.
  850. // out, pivot, and v will all be in the same plane, with pivot half way between v and out
  851. void vReflectAcross3p(Vector3* v, Vector3* pivot, Vector3* out) {
  852. Vector3 v2 = vScale3(*v, -1);
  853. float d = vDot3(v2, *pivot) * 2.0;
  854. *out = vSub3(v2, vScale3(*pivot, d));
  855. }
  856. Vector3 vReflectAcross3(Vector3 v, Vector3 pivot) {
  857. Vector3 o;
  858. vReflectAcross3p(&v, &pivot, &o);
  859. return o;
  860. }
  861. // calculate a unit vector normal to a triangle's face.
  862. void vTriFaceNormal3p(Vector3* a, Vector3* b, Vector3* c, Vector3* out) {
  863. Vector3 b_a, c_a;
  864. vSub3p(b, a, &b_a);
  865. vSub3p(c, a, &c_a);
  866. vCross3p(&b_a, &c_a, out);
  867. vNorm3p(out, out);
  868. }
  869. // calculate a unit vector normal to a triangle's face.
  870. Vector3 vTriFaceNormal3(Vector3 a, Vector3 b, Vector3 c) {
  871. Vector3 b_a, c_a, out;
  872. b_a = vSub3(b, a);
  873. c_a = vSub3(c, a);
  874. return vNorm3(vCross3(b_a, c_a));
  875. }
  876. // calculate a unit vector normal to a triangle's face.
  877. Vector3 vTriFaceNormalArea3(Vector3 a, Vector3 b, Vector3 c, float* area) {
  878. Vector3 b_a, c_a, out;
  879. b_a = vSub3(b, a);
  880. c_a = vSub3(c, a);
  881. Vector3 n = vCross3(b_a, c_a);
  882. if(area) *area = vLen(n) * .5f;
  883. return vNorm3(n);
  884. }
  885. // calculate a unit vector normal to a triangle's face.
  886. void vpTriFaceNormal3p(Vector3* tri, Vector3* out) {
  887. vTriFaceNormal3p(tri+0, tri+1, tri+2, out);
  888. }
  889. void vProjectOntoPlane3p(Vector3* v, Plane* p, Vector3* out) {
  890. Vector3 v_ortho;
  891. // get the component of v that's perpendicular to the plane
  892. vProjectNorm3p(v, &p->n, &v_ortho);
  893. // subtract it from v
  894. vSub3p(v, &v_ortho, out);
  895. }
  896. void vProjectOntoPlaneNormalized3p(Vector3* v, Plane* p, Vector3* out) {
  897. vProjectOntoPlane3p(v, p, out);
  898. vNorm3p(out, out);
  899. }
  900. void planeFromPointNormal(Vector3* p, Vector3* norm, Plane* out) {
  901. out->n = *norm;
  902. out->d = vDot3p(norm, p);
  903. }
  904. // calculates a plane from a triangle
  905. void planeFromTriangle3p(Vector3* v1, Vector3* v2, Vector3* v3, Plane* out) {
  906. vTriFaceNormal3p(v1, v2, v3, &out->n);
  907. out->d = vDot3p(&out->n, v1);
  908. }
  909. // copy a plane
  910. void planeCopy3p(Plane* in, Plane* out) {
  911. out->n = in->n;
  912. out->d = in->d;
  913. }
  914. // reverses the plane's direction
  915. void planeInverse3p(Plane* in, Plane* out) {
  916. vInv3p(&in->n, &out->n);
  917. out->d = -in->d;
  918. }
  919. // classifies a point by which side of the plane it's on, default espilon
  920. int planeClassifyPoint3p(Plane* p, Vector3* pt) {
  921. return planeClassifyPointEps3p(p, pt, FLT_CMP_EPSILON);
  922. }
  923. // classifies a point by which side of the plane it's on, custom espilon
  924. int planeClassifyPointEps3p(Plane* p, Vector3* pt, float epsilon) {
  925. float dist = vDot3p(&p->n, pt);
  926. if(fabs(dist - p->d) < epsilon)
  927. return C3DLAS_COPLANAR;
  928. else if (dist < p->d)
  929. return C3DLAS_BACK;
  930. else
  931. return C3DLAS_FRONT;
  932. }
  933. // https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
  934. // returns _INTERSECT or _DISJOINT
  935. int rayTriangleIntersect(
  936. Vector3* a, Vector3* b, Vector3* c, // triangle
  937. Vector3* ray_origin, Vector3* ray_dir, // ray
  938. float* u, float* v, float* t // barycentric out coords, t of intersection point along ray
  939. ) {
  940. Vector3 ab = vSub3(*b, *a);
  941. Vector3 ac = vSub3(*c, *a);
  942. Vector3 n = vCross3(ab, ac);
  943. float det = -vDot3(*ray_dir, n);
  944. if(fabsf(det) <= FLT_CMP_EPSILON) {
  945. return C3DLAS_DISJOINT; // the ray is parallel to the triangle
  946. }
  947. float idet = 1.0f / det;
  948. Vector3 ao = vSub3(*ray_origin, *a);
  949. Vector3 dao = vCross3(ao, *ray_dir);
  950. *u = vDot3(ac, dao) * idet;
  951. if(*u < 0.f) return C3DLAS_DISJOINT; // barycentric coord is outside the triangle
  952. *v = -vDot3(ab, dao) * idet;
  953. if(*v < 0.f || *u + *v > 1.f) return C3DLAS_DISJOINT; // barycentric coord is outside the triangle
  954. *t = vDot3(ao, n) * idet;
  955. // if(*t < 0.0f) return C3DLAS_DISJOINT; // the ray intersects the triangle behind the origin
  956. return C3DLAS_INTERSECT;
  957. }
  958. // returns _INTERSECT or _DISJOINT
  959. Vector3 triangleClosestPoint_Reference(
  960. Vector3* a, Vector3* b, Vector3* c, // triangle
  961. Vector3* p, // test point
  962. float* out_u, float* out_v // barycentric out coords of closest point
  963. ) {
  964. Vector3 ab = vSub3(*b, *a);
  965. Vector3 ac = vSub3(*c, *a);
  966. Vector3 n = vCross3(ab, ac);
  967. Vector3 ray_dir = vNeg3(vNorm(n));
  968. float idet = 1.0f / -vDot3(ray_dir, n);
  969. Vector3 ao = vSub3(*p, *a);
  970. Vector3 dao = vCross3(ao, ray_dir);
  971. // printf("idet = %f, n = %f,%f,%f\n", idet, n.x, n.y, n.z);
  972. float u = vDot3(ac, dao) * idet;
  973. float v = -vDot3(ab, dao) * idet;
  974. // printf("u,v = %f, %f\n", u, v);
  975. if(u >= 0 && v >= 0.f && u + v <= 1.f) {
  976. float nt = vDot3(ao, n);
  977. Vector3 planep = vAdd3(*p, vScale3(vNeg3(n), nt));
  978. return planep; // the ray intersects the triangle
  979. }
  980. float t_ab, t_bc, t_ca;
  981. // collect all the possible locations
  982. float dist[6];
  983. dist[0] = vDistTPointLine3(*p, (Line3){*a, *b}, &t_ab);
  984. dist[1] = vDistTPointLine3(*p, (Line3){*b, *c}, &t_bc);
  985. dist[2] = vDistTPointLine3(*p, (Line3){*c, *a}, &t_ca);
  986. dist[3] = vDist(*a, *p);
  987. dist[4] = vDist(*b, *p);
  988. dist[5] = vDist(*c, *p);
  989. // find the smallest distance
  990. float min = dist[0];
  991. int mini = 0;
  992. for(int i = 1; i < 6; i++) {
  993. if(dist[i] < min) {
  994. min = dist[i];
  995. mini = i;
  996. }
  997. }
  998. switch(mini) {
  999. case 0: return vLerp(*a, *b, t_ab);
  1000. case 1: return vLerp(*b, *c, t_bc);
  1001. case 2: return vLerp(*c, *a, t_ca);
  1002. case 3: return *a;
  1003. case 4: return *b;
  1004. case 5: return *c;
  1005. }
  1006. return (Vector3){0,0,0}; // HACK just return something
  1007. }
  1008. /*
  1009. // https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
  1010. // returns _INTERSECT or _DISJOINT
  1011. vec3 triangleClosestPoint(
  1012. Vector3* a, Vector3* b, Vector3* c, // triangle
  1013. Vector3* p, // test point
  1014. float* out_u, float* out_v // barycentric out coords of closest point
  1015. ) {
  1016. Vector3 ab = vSub3(*b, *a);
  1017. Vector3 ac = vSub3(*c, *a);
  1018. Vector3 n = vCross3(ab, ac); // triangle plane normal
  1019. Vector3 ap = vSub3(*p, *a);
  1020. Vector3 dap = vCross3(ap, vNeg(n)); // p projected onto the triangle's plane, relative to a
  1021. // p = w*a + u*b + v*c;
  1022. float u = vDot3(ac, dap); // inversely proportional to distance from _b_, aka "beta"
  1023. // u < 0 means outside the triangle past the a/c edge
  1024. // u > 1 means outside the triangle past b
  1025. // u == 0 means somewhere on the a/c edge
  1026. // if(*u < 0.f) return C3DLAS_DISJOINT; // barycentric coord is outside the triangle
  1027. float v = -vDot3(ab, dap); // inversely proportional to distance from _c_, aka "gamma"
  1028. // v < 0 means outside the triangle past the a/b edge
  1029. // v > 1 means outside the triangle past c
  1030. // v == 0 means somewhere on the a/b edge
  1031. // if(*u < 0.f || *u + *v < 1.0f) return C3DLAS_DISJOINT; // barycentric coord is outside the triangle
  1032. float w = 1.0f - u - v; // inversely proportional to distance from _a_, aka "alpha"
  1033. // w < 0 means outside the triangle past the b/c edge
  1034. // w > 1 means outside the triangle past a
  1035. // w == 0 means somewhere on the b/c edge
  1036. if(u > 0 && v > 0 && w > 0) // point inside triangle
  1037. float t = vDot3(ap, n);
  1038. vec3 closest = vAdd3(*p, vScale3(vNeg(n), t));
  1039. return closest;
  1040. }
  1041. float new_u = 0, new_v = 0, new_w = 0;
  1042. if(w < 0) {
  1043. float t = fclamp(0f, 1f, vDot3() / vDot3());
  1044. new_v = 1.f - t;
  1045. new_w = t;
  1046. }
  1047. else if(v < 0) {
  1048. float t = fclamp(0f, 1f, vDot3() / vDot3());
  1049. new_u = t;
  1050. new_w = 1.f - t;
  1051. }
  1052. else if(u < 0) {
  1053. float t = fclamp(0f, 1f, vDot3() / vDot3());
  1054. new_u = 1.f - t;
  1055. new_v = t;
  1056. }
  1057. // if(*t < 0.0f) return C3DLAS_DISJOINT; // the ray intersects the triangle behind the origin
  1058. return C3DLAS_INTERSECT;
  1059. }
  1060. */
  1061. // C3DLAS_COPLANAR, _INTERSECT, or _DISJOINT
  1062. int triPlaneTestIntersect3p(Vector3* pTri, Plane* pl) {
  1063. Vector3 a, b, c;
  1064. float da, db, dc;
  1065. // get distance of each vertex from the plane
  1066. // bail early if any of them are coplanar
  1067. a = pTri[0];
  1068. da = vDot3p(&a, &pl->n) - pl->d;
  1069. if(fabs(da) < FLT_CMP_EPSILON) {
  1070. return C3DLAS_COPLANAR;
  1071. }
  1072. b = pTri[1];
  1073. db = vDot3p(&b, &pl->n) - pl->d;
  1074. if(fabs(db) < FLT_CMP_EPSILON) {
  1075. return C3DLAS_COPLANAR;
  1076. }
  1077. c = pTri[2];
  1078. dc = vDot3p(&c, &pl->n) - pl->d;
  1079. if(fabs(dc) < FLT_CMP_EPSILON) {
  1080. return C3DLAS_COPLANAR;
  1081. }
  1082. // the triangle intersects if the sign of all the distances does not match,
  1083. // ie, on vertex is on the opposite side of the plane from the others
  1084. return (signbit(da) == signbit(db) && signbit(db) == signbit(dc)) ? C3DLAS_DISJOINT : C3DLAS_INTERSECT;
  1085. }
  1086. // C3DLAS_COPLANAR, _INTERSECT, or _DISJOINT
  1087. int triPlaneClip3p(
  1088. Vector3* pTri,
  1089. Plane* pl,
  1090. Vector3* aboveOut,
  1091. Vector3* belowOut,
  1092. int* aboveCnt,
  1093. int* belowCnt
  1094. ) {
  1095. Vector3 v0, v1, v2;
  1096. float vp_d0, vp_d1, vp_d2;
  1097. v0 = pTri[0];
  1098. v1 = pTri[1];
  1099. v2 = pTri[2];
  1100. // get distance of each vertex from the plane
  1101. vp_d0 = vDot3p(&v0, &pl->n) - pl->d;
  1102. vp_d1 = vDot3p(&v1, &pl->n) - pl->d;
  1103. vp_d2 = vDot3p(&v2, &pl->n) - pl->d;
  1104. // bail early if just one is coplanar
  1105. // split in half with single-edge intersections
  1106. if(fabs(vp_d0) < FLT_CMP_EPSILON) {
  1107. if( // single edge intersection
  1108. signbit(vp_d1) != signbit(vp_d2) &&
  1109. fabs(vp_d1) > FLT_CMP_EPSILON &&
  1110. fabs(vp_d2) > FLT_CMP_EPSILON
  1111. ) {
  1112. // get intersection point
  1113. Vector3 c;
  1114. planeLineFindIntersectFast3p(pl, &v1, &v2, &c);
  1115. if(vp_d1 > 0) { // v1 is above the plane
  1116. aboveOut[0] = c; // correct winding
  1117. aboveOut[1] = v0;
  1118. aboveOut[2] = v1;
  1119. belowOut[0] = c;
  1120. belowOut[1] = v2;
  1121. belowOut[2] = v0;
  1122. }
  1123. else {
  1124. belowOut[0] = c; // correct winding
  1125. belowOut[1] = v0;
  1126. belowOut[2] = v1;
  1127. aboveOut[0] = c;
  1128. aboveOut[1] = v2;
  1129. aboveOut[2] = v0;
  1130. }
  1131. *aboveCnt = 1;
  1132. *belowCnt = 1;
  1133. return C3DLAS_INTERSECT;
  1134. }
  1135. return C3DLAS_COPLANAR; // one vertex is on the plane, the others all above or below
  1136. }
  1137. if(fabs(vp_d1) < FLT_CMP_EPSILON) {
  1138. if( // single edge intersection
  1139. signbit(vp_d0) != signbit(vp_d2) &&
  1140. fabs(vp_d0) > FLT_CMP_EPSILON &&
  1141. fabs(vp_d2) > FLT_CMP_EPSILON
  1142. ) {
  1143. // get intersection point
  1144. Vector3 c;
  1145. planeLineFindIntersectFast3p(pl, &v0, &v2, &c);
  1146. if(vp_d0 > 0) { // v0 is above the plane
  1147. aboveOut[0] = c; // correct winding
  1148. aboveOut[1] = v0;
  1149. aboveOut[2] = v1;
  1150. belowOut[0] = c;
  1151. belowOut[1] = v1;
  1152. belowOut[2] = v2;
  1153. }
  1154. else {
  1155. belowOut[0] = c; // correct winding
  1156. belowOut[1] = v0;
  1157. belowOut[2] = v1;
  1158. aboveOut[0] = c;
  1159. aboveOut[1] = v1;
  1160. aboveOut[2] = v2;
  1161. }
  1162. *aboveCnt = 1;
  1163. *belowCnt = 1;
  1164. return C3DLAS_INTERSECT;
  1165. }
  1166. return C3DLAS_COPLANAR; // one vertex is on the plane, the others all above or below
  1167. }
  1168. if(fabs(vp_d2) < FLT_CMP_EPSILON) {
  1169. if( // single edge intersection
  1170. signbit(vp_d0) != signbit(vp_d1) &&
  1171. fabs(vp_d0) > FLT_CMP_EPSILON &&
  1172. fabs(vp_d1) > FLT_CMP_EPSILON
  1173. ) {
  1174. // get intersection point
  1175. Vector3 c;
  1176. planeLineFindIntersectFast3p(pl, &v0, &v1, &c);
  1177. if(vp_d0 > 0) { // v0 is above the plane
  1178. aboveOut[0] = c; // correct winding
  1179. aboveOut[1] = v2;
  1180. aboveOut[2] = v0;
  1181. belowOut[0] = c;
  1182. belowOut[1] = v1;
  1183. belowOut[2] = v2;
  1184. }
  1185. else {
  1186. belowOut[0] = c; // correct winding
  1187. belowOut[1] = v2;
  1188. belowOut[2] = v0;
  1189. aboveOut[0] = c;
  1190. aboveOut[1] = v1;
  1191. aboveOut[2] = v2;
  1192. }
  1193. *aboveCnt = 1;
  1194. *belowCnt = 1;
  1195. return C3DLAS_INTERSECT;
  1196. }
  1197. return C3DLAS_COPLANAR; // one vertex is on the plane, the others all above or below
  1198. }
  1199. // the triangle intersects if the sign of all the distances does not match,
  1200. // ie, on vertex is on the opposite side of the plane from the others
  1201. // bail if disjoint
  1202. if(signbit(vp_d0) == signbit(vp_d1) && signbit(vp_d1) == signbit(vp_d2)) {
  1203. return C3DLAS_DISJOINT;
  1204. }
  1205. // split on which edges intersect the plane
  1206. if(signbit(vp_d0) == signbit(vp_d1)) {
  1207. // vertex 2 is isolated; edges 0,2 and 1,2 intersect
  1208. Vector3 c0, c1;
  1209. planeLineFindIntersectFast3p(pl, &v0, &v2, &c0);
  1210. planeLineFindIntersectFast3p(pl, &v1, &v2, &c1);
  1211. if(vp_d2 > 0) { // v2 is above the plane
  1212. aboveOut[0] = v2; // correct winding
  1213. aboveOut[1] = c0;
  1214. aboveOut[2] = c1;
  1215. belowOut[0] = c1;
  1216. belowOut[1] = v0;
  1217. belowOut[2] = v1;
  1218. belowOut[3] = c1;
  1219. belowOut[4] = c0;
  1220. belowOut[5] = v1;
  1221. *aboveCnt = 1;
  1222. *belowCnt = 2;
  1223. }
  1224. else {
  1225. belowOut[0] = v2; // correct winding
  1226. belowOut[1] = c0;
  1227. belowOut[2] = c1;
  1228. aboveOut[0] = c1;
  1229. aboveOut[1] = v0;
  1230. aboveOut[2] = v1;
  1231. aboveOut[3] = c1;
  1232. aboveOut[4] = c0;
  1233. aboveOut[5] = v1;
  1234. *aboveCnt = 2;
  1235. *belowCnt = 1;
  1236. }
  1237. }
  1238. else if(signbit(vp_d1) == signbit(vp_d2)) {
  1239. // vertex 0 is isolated; edges 1,0 and 2,0 intersect
  1240. Vector3 c0, c1;
  1241. planeLineFindIntersectFast3p(pl, &v1, &v0, &c0);
  1242. planeLineFindIntersectFast3p(pl, &v2, &v0, &c1);
  1243. if(vp_d0 > 0) { // v0 is above the plane
  1244. aboveOut[0] = v0; // correct winding
  1245. aboveOut[1] = c0;
  1246. aboveOut[2] = c1;
  1247. belowOut[0] = c1;
  1248. belowOut[1] = v1;
  1249. belowOut[2] = v2;
  1250. belowOut[3] = c1;
  1251. belowOut[4] = c0;
  1252. belowOut[5] = v1;
  1253. *aboveCnt = 1;
  1254. *belowCnt = 2;
  1255. }
  1256. else {
  1257. belowOut[0] = v0; // correct winding
  1258. belowOut[1] = c0;
  1259. belowOut[2] = c1;
  1260. aboveOut[0] = c1;
  1261. aboveOut[1] = v1;
  1262. aboveOut[2] = v2;
  1263. aboveOut[3] = c1;
  1264. aboveOut[4] = c0;
  1265. aboveOut[5] = v1;
  1266. *aboveCnt = 2;
  1267. *belowCnt = 1;
  1268. }
  1269. }
  1270. else {
  1271. // vertex 1 is isolated; edges 0,1 and 2,1 intersect
  1272. Vector3 c0, c1;
  1273. planeLineFindIntersectFast3p(pl, &v0, &v1, &c0);
  1274. planeLineFindIntersectFast3p(pl, &v2, &v1, &c1);
  1275. if(vp_d1 > 0) { // v1 is above the plane
  1276. aboveOut[0] = v1; // correct winding
  1277. aboveOut[1] = c1;
  1278. aboveOut[2] = c0;
  1279. belowOut[0] = c1;
  1280. belowOut[1] = v2;
  1281. belowOut[2] = v0;
  1282. belowOut[3] = c0;
  1283. belowOut[4] = c1;
  1284. belowOut[5] = v0;
  1285. *aboveCnt = 1;
  1286. *belowCnt = 2;
  1287. }
  1288. else {
  1289. belowOut[0] = v1; // correct winding
  1290. belowOut[1] = c1;
  1291. belowOut[2] = c0;
  1292. aboveOut[0] = c1;
  1293. aboveOut[1] = v2;
  1294. aboveOut[2] = v0;
  1295. aboveOut[3] = c0;
  1296. aboveOut[4] = c1;
  1297. aboveOut[5] = v0;
  1298. *aboveCnt = 2;
  1299. *belowCnt = 1;
  1300. }
  1301. }
  1302. return C3DLAS_INTERSECT;
  1303. }
  1304. // http://geomalgorithms.com/a07-_distance.html
  1305. // _PARALLEL with no output on parallel lines
  1306. // _INTERSECT with one point of output on intersection
  1307. // _DISJOINT with two outputs otherwise
  1308. int shortestLineFromRayToRay3p(Ray3* r1, Ray3* r2, Vector3* pOut) {
  1309. Vector3 u, v, w, ps, pt;
  1310. float a, b, c, d, e, s, t;
  1311. u = r1->d;
  1312. v = r2->d;
  1313. vSub3p(&r1->o, &r2->o, &w);
  1314. a = vDot3p(&u, &u);
  1315. b = vDot3p(&u, &v);
  1316. c = vDot3p(&v, &v);
  1317. d = vDot3p(&u, &w);
  1318. e = vDot3p(&v, &w);
  1319. float ac_bb = a * c - b * b;
  1320. if(fabs(ac_bb) < FLT_CMP_EPSILON) {
  1321. return C3DLAS_PARALLEL;
  1322. }
  1323. s = (b * e - c * d) / ac_bb;
  1324. t = (a * e - b * d) / ac_bb;
  1325. vScale3p(&u, s, &ps);
  1326. vScale3p(&v, t, &pt);
  1327. vAdd3p(&r1->o, &ps, &ps);
  1328. vAdd3p(&r2->o, &pt, &pt);
  1329. pOut[0] = ps;
  1330. pOut[1] = pt;
  1331. if(vDistSq3p(&ps, &pt) < FLT_CMP_EPSILON_SQ) {
  1332. return C3DLAS_INTERSECT;
  1333. }
  1334. return C3DLAS_DISJOINT;
  1335. }
  1336. void frustumFromMatrix(Matrix* m, Frustum* out) {
  1337. Matrix inv;
  1338. mInverse(m, &inv);
  1339. // first the points
  1340. // these MUST be in this order
  1341. // near
  1342. vMatrixMulf3p(-1,-1,-1, &inv, &out->points[0]);
  1343. vMatrixMulf3p(-1, 1,-1, &inv, &out->points[1]);
  1344. vMatrixMulf3p( 1,-1,-1, &inv, &out->points[2]);
  1345. vMatrixMulf3p( 1, 1,-1, &inv, &out->points[3]);
  1346. // far
  1347. vMatrixMulf3p(-1,-1, 1, &inv, &out->points[4]);
  1348. vMatrixMulf3p(-1, 1, 1, &inv, &out->points[5]);
  1349. vMatrixMulf3p( 1,-1, 1, &inv, &out->points[6]);
  1350. vMatrixMulf3p( 1, 1, 1, &inv, &out->points[7]);
  1351. // now the planes
  1352. // near and far
  1353. planeFromTriangle3p(&out->points[0], &out->points[1], &out->points[2], &out->planes[0]);
  1354. planeFromTriangle3p(&out->points[4], &out->points[5], &out->points[6], &out->planes[1]);
  1355. // sides
  1356. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[1], &out->planes[2]);
  1357. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[2], &out->planes[3]);
  1358. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[1], &out->planes[4]);
  1359. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[2], &out->planes[5]);
  1360. }
  1361. void frustumFromMatrixVK(Matrix* m, Frustum* out) {
  1362. Matrix inv;
  1363. mInverse(m, &inv);
  1364. // first the points
  1365. // these MUST be in this order
  1366. // near
  1367. vMatrixMulf3p(-1,-1, 0, &inv, &out->points[0]);
  1368. vMatrixMulf3p(-1, 1, 0, &inv, &out->points[1]);
  1369. vMatrixMulf3p( 1,-1, 0, &inv, &out->points[2]);
  1370. vMatrixMulf3p( 1, 1, 0, &inv, &out->points[3]);
  1371. // far
  1372. vMatrixMulf3p(-1,-1, 1, &inv, &out->points[4]);
  1373. vMatrixMulf3p(-1, 1, 1, &inv, &out->points[5]);
  1374. vMatrixMulf3p( 1,-1, 1, &inv, &out->points[6]);
  1375. vMatrixMulf3p( 1, 1, 1, &inv, &out->points[7]);
  1376. // now the planes
  1377. // near and far
  1378. planeFromTriangle3p(&out->points[0], &out->points[1], &out->points[2], &out->planes[0]);
  1379. planeFromTriangle3p(&out->points[4], &out->points[5], &out->points[6], &out->planes[1]);
  1380. // sides
  1381. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[1], &out->planes[2]);
  1382. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[2], &out->planes[3]);
  1383. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[1], &out->planes[4]);
  1384. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[2], &out->planes[5]);
  1385. }
  1386. void frustumFromMatrixVK_ZUP(Matrix* m, Frustum* out) {
  1387. Matrix inv;
  1388. mInverse(m, &inv);
  1389. *out = (Frustum){0};
  1390. // first the points
  1391. // these MUST be in this order
  1392. // near
  1393. vMatrixMulf3p(-1,-1, 0, &inv, &out->points[0]); // BUG this order is likely wrong for the planes but results in a sane wireframe.
  1394. vMatrixMulf3p( 1,-1, 0, &inv, &out->points[1]);
  1395. vMatrixMulf3p(-1, 1, 0, &inv, &out->points[2]);
  1396. vMatrixMulf3p( 1, 1, 0, &inv, &out->points[3]);
  1397. // far
  1398. vMatrixMulf3p(-1,-1, 1, &inv, &out->points[4]);
  1399. vMatrixMulf3p(1, -1, 1, &inv, &out->points[5]);
  1400. vMatrixMulf3p( -1,1, 1, &inv, &out->points[6]);
  1401. vMatrixMulf3p( 1, 1, 1, &inv, &out->points[7]);
  1402. // now the planes
  1403. // near and far
  1404. planeFromTriangle3p(&out->points[0], &out->points[1], &out->points[2], &out->planes[0]);
  1405. planeFromTriangle3p(&out->points[4], &out->points[5], &out->points[6], &out->planes[1]);
  1406. // sides
  1407. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[1], &out->planes[2]);
  1408. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[2], &out->planes[3]);
  1409. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[1], &out->planes[4]);
  1410. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[2], &out->planes[5]);
  1411. }
  1412. void frustumFromMatrixVK_RDepth(Matrix* m, Frustum* out) {
  1413. Matrix inv;
  1414. mInverse(m, &inv);
  1415. // first the points
  1416. // these MUST be in this order
  1417. // near
  1418. vMatrixMulf3p(-1,-1, 1, &inv, &out->points[0]);
  1419. vMatrixMulf3p(-1, 1, 1, &inv, &out->points[1]);
  1420. vMatrixMulf3p( 1,-1, 1, &inv, &out->points[2]);
  1421. vMatrixMulf3p( 1, 1, 1, &inv, &out->points[3]);
  1422. // far
  1423. vMatrixMulf3p(-1,-1, 0, &inv, &out->points[4]);
  1424. vMatrixMulf3p(-1, 1, 0, &inv, &out->points[5]);
  1425. vMatrixMulf3p( 1,-1, 0, &inv, &out->points[6]);
  1426. vMatrixMulf3p( 1, 1, 0, &inv, &out->points[7]);
  1427. // now the planes
  1428. // near and far
  1429. planeFromTriangle3p(&out->points[0], &out->points[1], &out->points[2], &out->planes[0]);
  1430. planeFromTriangle3p(&out->points[4], &out->points[5], &out->points[6], &out->planes[1]);
  1431. // sides
  1432. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[1], &out->planes[2]);
  1433. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[2], &out->planes[3]);
  1434. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[1], &out->planes[4]);
  1435. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[2], &out->planes[5]);
  1436. }
  1437. void frustumFromMatrixVK_ZUP_RDepth(Matrix* m, Frustum* out) {
  1438. Matrix inv;
  1439. mInverse(m, &inv);
  1440. *out = (Frustum){0};
  1441. // first the points
  1442. // these MUST be in this order
  1443. // near
  1444. vMatrixMulf3p(-1,-1, 1, &inv, &out->points[0]); // BUG this order is likely wrong for the planes but results in a sane wireframe.
  1445. vMatrixMulf3p( 1,-1, 1, &inv, &out->points[1]);
  1446. vMatrixMulf3p(-1, 1, 1, &inv, &out->points[2]);
  1447. vMatrixMulf3p( 1, 1, 1, &inv, &out->points[3]);
  1448. // far
  1449. vMatrixMulf3p(-1,-1, 0, &inv, &out->points[4]);
  1450. vMatrixMulf3p(1, -1, 0, &inv, &out->points[5]);
  1451. vMatrixMulf3p( -1,1, 0, &inv, &out->points[6]);
  1452. vMatrixMulf3p( 1, 1, 0, &inv, &out->points[7]);
  1453. // now the planes
  1454. // near and far
  1455. planeFromTriangle3p(&out->points[0], &out->points[1], &out->points[2], &out->planes[0]);
  1456. planeFromTriangle3p(&out->points[4], &out->points[5], &out->points[6], &out->planes[1]);
  1457. // sides
  1458. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[1], &out->planes[2]);
  1459. planeFromTriangle3p(&out->points[0], &out->points[4], &out->points[2], &out->planes[3]);
  1460. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[1], &out->planes[4]);
  1461. planeFromTriangle3p(&out->points[3], &out->points[7], &out->points[2], &out->planes[5]);
  1462. }
  1463. void frustumCenter(Frustum* f, Vector3* out) {
  1464. Vector3 sum = {0.0f,0.0f,0.0f};
  1465. for(int i = 0; i < 8; i++) vAdd3p(&f->points[i], &sum, &sum);
  1466. vScale3p(&sum, 1.0f/8.0f, out);
  1467. }
  1468. // General idea of the algorithm:
  1469. // https://lxjk.github.io/2017/04/15/Calculate-Minimal-Bounding-Sphere-of-Frustum.html
  1470. // http://archive.is/YACj2
  1471. void frustumBoundingSphere(Frustum* f, Sphere* out) {
  1472. Vector3 f0, n0;
  1473. vPointAvg3p(&f->points[0], &f->points[3], &n0);
  1474. vPointAvg3p(&f->points[4], &f->points[7], &f0);
  1475. float Dn2 = vDistSq3p(&n0, &f->points[0]);
  1476. float Df2 = vDistSq3p(&f0, &f->points[4]);
  1477. // check for ortho
  1478. if(Dn2 - Df2 < 0.00001) {
  1479. frustumCenter(f, &out->center);
  1480. out->r = vDist3p(&out->center, &f->points[0]);
  1481. return;
  1482. }
  1483. float Dnf = vDist3p(&f0, &n0);
  1484. float Dnc = (Dn2 - Df2 - Df2) / (2 * Dnf);
  1485. // printf("\n f: %f,%f,%f\n", f->points[4].x,f->points[4].y,f->points[4].z);
  1486. // printf(" n: %f,%f,%f\n", f->points[0].x,f->points[0].y,f->points[0].z);
  1487. // printf(" f0: %f,%f,%f\n", f0.x,f0.y,f0.z);
  1488. // printf(" n0: %f,%f,%f\n", n0.x,n0.y,n0.z);
  1489. // printf(" dn2, df2, dnf, dnc: %f,%f,%f,%f\n", Dn2, Df2, Dnf, Dnc);
  1490. if(Dnc > 0 && Dnc < Dnf) {
  1491. vLerp3p(&f0, &n0, Dnc / Dnf, &out->center);
  1492. out->r = sqrt(Dnc * Dnc + Dn2);
  1493. }
  1494. else {
  1495. out->center = f0;
  1496. out->r = sqrt(Df2);
  1497. }
  1498. }
  1499. void frustumInscribeSphere(Frustum* f, Sphere* out) {
  1500. Vector3 fx, nx;
  1501. vPointAvg3p(&f->points[0], &f->points[3], &nx);
  1502. vPointAvg3p(&f->points[4], &f->points[7], &fx);
  1503. /*
  1504. float Dn2 = vDistSq3p(&n0, &f->points[0]);
  1505. float Df2 = vDistSq3p(&f0, &f->points[4]);
  1506. float Dnf = vDist3p(&f0, n0);
  1507. float Dnc = (Dn2 - Df2 - Df2) / (2 * Dnf);*/
  1508. }
  1509. void quadCenterp3p(Vector3* a, Vector3* b, Vector3* c, Vector3* d, Vector3* out) {
  1510. Vector3 sum;
  1511. vAdd3p(a, b, &sum);
  1512. vAdd3p(&sum, c, &sum);
  1513. vAdd3p(&sum, d, &sum);
  1514. vScale3p(&sum, 0.25f, out);
  1515. }
  1516. void vPointAvg3p(Vector3* a, Vector3* b, Vector3* out) {
  1517. Vector3 sum;
  1518. vAdd3p(a, b, &sum);
  1519. vScale3p(&sum, 0.5f, out);
  1520. }
  1521. // reflects the distance from v to pivot across pivot.
  1522. // out, pivot, and v will form a straight line with pivot exactly in the middle.
  1523. void vReflectAcross2p(Vector2* v, Vector2* pivot, Vector2* out) {
  1524. Vector2 diff;
  1525. vSub2p(pivot, v, &diff);
  1526. vAdd2p(pivot, &diff, out);
  1527. }
  1528. // degenerate cases may not give desired results. GIGO.
  1529. void vRoundAway2p(const Vector2* in, const Vector2* center, Vector2i* out) {
  1530. if(in->x > center->x) out->x = ceilf(in->x);
  1531. else out->x = floorf(in->x);
  1532. if(in->y > center->y) out->y = ceilf(in->y);
  1533. else out->y = floorf(in->y);
  1534. }
  1535. // degenerate cases may not give desired results. GIGO.
  1536. void vRoundToward2p(const Vector2* in, const Vector2* center, Vector2i* out) {
  1537. if(in->x > center->x) out->x = floorf(in->x);
  1538. else out->x = ceilf(in->x);
  1539. if(in->y > center->y) out->y = floorf(in->y);
  1540. else out->y = ceilf(in->y);
  1541. }
  1542. // returns the *signed* area of a triangle. useful for determining winding
  1543. // positive values mean a clockwise triangle
  1544. float triArea2p(Vector2* a, Vector2* b, Vector2* c) {
  1545. return 0.5 * (
  1546. ((b->x - a->x) * (b->y + a->y)) +
  1547. ((c->x - b->x) * (c->y + b->y)) +
  1548. ((a->x - c->x) * (a->y + c->y)));
  1549. }
  1550. // determines if a point is inside a triangle
  1551. int triPointInside2p(Vector2* p, Vector2* a, Vector2* b, Vector2* c) {
  1552. int d = signbit((p->x - b->x) * (a->y - b->y) - (a->x - b->x) * (p->y - b->y));
  1553. int e = signbit((p->x - c->x) * (b->y - c->y) - (b->x - c->x) * (p->y - c->y));
  1554. if(d != e) return 0;
  1555. int f = signbit((p->x - a->x) * (c->y - a->y) - (c->x - a->x) * (p->y - a->y));
  1556. return e == f;
  1557. }
  1558. // plane-vector operations
  1559. // distance from point to plane
  1560. float pvDist3p(Plane* p, Vector3* v) {
  1561. return vDot3p(v, &p->n) + p->d;
  1562. }
  1563. // matrix-vector operations
  1564. Vector3 vMatrixMulProjectedMagic3(Vector3 in, Matrix* m) {
  1565. Vector4 v;
  1566. v.x = in.x * m->m[0+0] + in.y * m->m[4+0] + in.z * m->m[8+0] + 1.0 * m->m[12+0];
  1567. v.y = in.x * m->m[0+1] + in.y * m->m[4+1] + in.z * m->m[8+1] + 1.0 * m->m[12+1];
  1568. v.z = in.x * m->m[0+2] + in.y * m->m[4+2] + in.z * m->m[8+2] + 1.0 * m->m[12+2];
  1569. v.w = in.x * m->m[0+3] + in.y * m->m[4+3] + in.z * m->m[8+3] + 1.0 * m->m[12+3];
  1570. if(v.w == 0) return (Vector3){0,0,0};
  1571. if(v.w < 0) v.w = -v.w;
  1572. return (Vector3){.x = v.x / v.w, .y = v.y / v.w, .z = v.z / v.w};
  1573. }
  1574. Vector3 vMatrixMul3(Vector3 in, Matrix* m) {
  1575. Vector4 v;
  1576. v.x = in.x * m->m[0+0] + in.y * m->m[4+0] + in.z * m->m[8+0] + 1.0 * m->m[12+0];
  1577. v.y = in.x * m->m[0+1] + in.y * m->m[4+1] + in.z * m->m[8+1] + 1.0 * m->m[12+1];
  1578. v.z = in.x * m->m[0+2] + in.y * m->m[4+2] + in.z * m->m[8+2] + 1.0 * m->m[12+2];
  1579. v.w = in.x * m->m[0+3] + in.y * m->m[4+3] + in.z * m->m[8+3] + 1.0 * m->m[12+3];
  1580. if(v.w == 0) return (Vector3){0,0,0};
  1581. return (Vector3){.x = v.x / v.w, .y = v.y / v.w, .z = v.z / v.w};
  1582. }
  1583. Vector4 vMatrixMul4(Vector4 in, Matrix* m) {
  1584. Vector4 v;
  1585. v.x = in.x * m->m[0+0] + in.y * m->m[4+0] + in.z * m->m[8+0] + in.w * m->m[12+0];
  1586. v.y = in.x * m->m[0+1] + in.y * m->m[4+1] + in.z * m->m[8+1] + in.w * m->m[12+1];
  1587. v.z = in.x * m->m[0+2] + in.y * m->m[4+2] + in.z * m->m[8+2] + in.w * m->m[12+2];
  1588. v.w = in.x * m->m[0+3] + in.y * m->m[4+3] + in.z * m->m[8+3] + in.w * m->m[12+3];
  1589. return v;
  1590. }
  1591. // multiply a vector by a matrix
  1592. void vMatrixMul3p(Vector3* restrict in, Matrix* restrict m, Vector3* restrict out) {
  1593. vMatrixMulf3p(in->x, in->y, in->z, m, out);
  1594. }
  1595. void vMatrixMulf3p(float x, float y, float z, Matrix* restrict m, Vector3* restrict out) {
  1596. Vector4 v;
  1597. v.x = x * m->m[0+0] + y * m->m[4+0] + z * m->m[8+0] + 1 * m->m[12+0];
  1598. v.y = x * m->m[0+1] + y * m->m[4+1] + z * m->m[8+1] + 1 * m->m[12+1];
  1599. v.z = x * m->m[0+2] + y * m->m[4+2] + z * m->m[8+2] + 1 * m->m[12+2];
  1600. v.w = x * m->m[0+3] + y * m->m[4+3] + z * m->m[8+3] + 1 * m->m[12+3];
  1601. out->x = v.x / v.w;
  1602. out->y = v.y / v.w;
  1603. out->z = v.z / v.w;
  1604. }
  1605. void evalBezier3p(Vector3* e1, Vector3* e2, Vector3* c1, Vector3* c2, float t, Vector3* out) {
  1606. out->x = evalBezier1D(e1->x, e2->x, c1->x, c2->x, t);
  1607. out->y = evalBezier1D(e1->y, e2->y, c1->y, c2->y, t);
  1608. out->z = evalBezier1D(e1->z, e2->z, c1->z, c2->z, t);
  1609. }
  1610. void evalBezier2D(Vector2* e1, Vector2* e2, Vector2* c1, Vector2* c2, float t, Vector2* out) {
  1611. out->x = evalBezier1D(e1->x, e2->x, c1->x, c2->x, t);
  1612. out->y = evalBezier1D(e1->y, e2->y, c1->y, c2->y, t);
  1613. }
  1614. float evalBezier1D(float e1, float e2, float c1, float c2, float t) {
  1615. float mt, mt2, t2;
  1616. mt = 1 - t;
  1617. mt2 = mt * mt;
  1618. t2 = t * t;
  1619. return (mt2 * mt * e1) + (3 * mt2 * t * c1) + (3 * t2 * mt * c2) + (t2 * t * e2);
  1620. }
  1621. void evalBezierTangent3p(Vector3* e1, Vector3* e2, Vector3* c1, Vector3* c2, float t, Vector3* out) {
  1622. out->x = evalBezier1D_dt(e1->x, e2->x, c1->x, c2->x, t);
  1623. out->y = evalBezier1D_dt(e1->y, e2->y, c1->y, c2->y, t);
  1624. out->z = evalBezier1D_dt(e1->z, e2->z, c1->z, c2->z, t);
  1625. }
  1626. float evalBezier1D_dt(float e1, float e2, float c1, float c2, float t) {
  1627. float mt, mt2, t2;
  1628. mt = 1 - t;
  1629. mt2 = mt * mt;
  1630. t2 = t * t;
  1631. return (3 * mt2 * (c1 - e1)) + (6 * mt * t * (c2 - c1)) + (3 * t2 * (e2 - c2));
  1632. }
  1633. float evalBezier1D_ddt(float e1, float e2, float c1, float c2, float t) {
  1634. return (6 * (1 - t) * (c2 - c1 - c1 + e1)) + (6 * t * (e2 - c2 - c2 - c1));
  1635. }
  1636. void evalBezierNorm3p(Vector3* e1, Vector3* e2, Vector3* c1, Vector3* c2, float t, Vector3* out) {
  1637. out->x = evalBezier1D_ddt(e1->x, e2->x, c1->x, c2->x, t);
  1638. out->y = evalBezier1D_ddt(e1->y, e2->y, c1->y, c2->y, t);
  1639. out->z = evalBezier1D_ddt(e1->z, e2->z, c1->z, c2->z, t);
  1640. }
  1641. // Quadratic bezier functions
  1642. float evalQBezier1D(float e1, float e2, float c1, float t) {
  1643. float mt, mt2;
  1644. mt = 1 - t;
  1645. mt2 = mt * mt;
  1646. return (mt2 * e1) + (2 * mt * t * c1) + (t * t * e2);
  1647. }
  1648. void evalQBezier2D3p(Vector2* e1, Vector2* e2, Vector2* c1, float t, Vector2* out) {
  1649. out->x = evalQBezier1D(e1->x, e2->x, c1->x, t);
  1650. out->y = evalQBezier1D(e1->y, e2->y, c1->y, t);
  1651. }
  1652. void evalQBezier3p(Vector3* e1, Vector3* e2, Vector3* c1, float t, Vector3* out) {
  1653. out->x = evalQBezier1D(e1->x, e2->x, c1->x, t);
  1654. out->y = evalQBezier1D(e1->y, e2->y, c1->y, t);
  1655. out->z = evalQBezier1D(e1->z, e2->z, c1->z, t);
  1656. }
  1657. ///// bounding box functions
  1658. // 3D versions
  1659. int boxDisjoint3p(const AABB3* a, const AABB3* b) {
  1660. return a->max.x < b->min.x || b->max.x < a->min.x
  1661. || a->max.y < b->min.y || b->max.y < a->min.y
  1662. || a->max.z < b->min.z || b->max.z < a->min.z;
  1663. }
  1664. int boxOverlaps3p(const AABB3* a, const AABB3* b) {
  1665. return !boxDisjoint3p(a, b);
  1666. }
  1667. int boxContainsPoint3p(const AABB3* b, const Vector3* p) {
  1668. return b->min.x <= p->x && b->max.x >= p->x
  1669. && b->min.y <= p->y && b->max.y >= p->y
  1670. && b->min.z <= p->z && b->max.z >= p->z;
  1671. }
  1672. void boxCenter3p(const AABB3* b, Vector3* out) {
  1673. out->x = (b->max.x + b->min.x) * .5f;
  1674. out->y = (b->max.y + b->min.y) * .5f;
  1675. out->z = (b->max.z + b->min.z) * .5f;
  1676. }
  1677. Vector3 boxCenter3(const AABB3 b) {
  1678. return (Vector3) {
  1679. (b.max.x + b.min.x) * .5f,
  1680. (b.max.y + b.min.y) * .5f,
  1681. (b.max.z + b.min.z) * .5f
  1682. };
  1683. }
  1684. void boxSize3p(const AABB3* b, Vector3* out) {
  1685. out->x = b->max.x - b->min.x;
  1686. out->y = b->max.y - b->min.y;
  1687. out->z = b->max.z - b->min.z;
  1688. }
  1689. Vector3 boxSize3(const AABB3 b) {
  1690. return (Vector3){
  1691. b.max.x - b.min.x,
  1692. b.max.y - b.min.y,
  1693. b.max.z - b.min.z
  1694. };
  1695. }
  1696. void boxExpandTo3p(AABB3* b, Vector3* p) {
  1697. b->min.x = fminf(b->min.x, p->x);
  1698. b->min.y = fminf(b->min.y, p->y);
  1699. b->min.z = fminf(b->min.z, p->z);
  1700. b->max.x = fmaxf(b->max.x, p->x);
  1701. b->max.y = fmaxf(b->max.y, p->y);
  1702. b->max.z = fmaxf(b->max.z, p->z);
  1703. }
  1704. void boxExpandTo3(AABB3* b, Vector3 p) {
  1705. boxExpandTo3p(b, &p);
  1706. }
  1707. // 2D versions
  1708. int boxDisjoint2p(const AABB2* a, const AABB2* b) {
  1709. return a->max.x < b->min.x || b->max.x < a->min.x
  1710. || a->max.y < b->min.y || b->max.y < a->min.y;
  1711. }
  1712. int boxOverlaps2p(const AABB2* a, const AABB2* b) {
  1713. return !boxDisjoint2p(a, b);
  1714. }
  1715. int boxContainsPoint2p(const AABB2* b, const Vector2* p) {
  1716. return b->min.x <= p->x && b->max.x >= p->x
  1717. && b->min.y <= p->y && b->max.y >= p->y;
  1718. }
  1719. void boxCenter2p(const AABB2* b, Vector2* out) {
  1720. out->x = (b->max.x + b->min.x) / 2.0;
  1721. out->y = (b->max.y + b->min.y) / 2.0;
  1722. }
  1723. Vector2 boxSize2(const AABB2 b) {
  1724. return (Vector2){
  1725. b.max.x - b.min.x,
  1726. b.max.y - b.min.y
  1727. };
  1728. }
  1729. void boxSize2p(const AABB2* b, Vector2* out) {
  1730. out->x = b->max.x - b->min.x;
  1731. out->y = b->max.y - b->min.y;
  1732. }
  1733. void boxQuadrant2p(const AABB2* in, char ix, char iy, AABB2* out) {
  1734. Vector2 sz, c;
  1735. boxCenter2p(in, &c);
  1736. boxSize2p(in, &sz);
  1737. sz.x *= .5;
  1738. sz.y *= .5;
  1739. out->min.x = c.x - (ix ? 0.0f : sz.x);
  1740. out->min.y = c.y - (iy ? 0.0f : sz.y);
  1741. out->max.x = c.x + (ix ? sz.x : 0.0f);
  1742. out->max.y = c.y + (iy ? sz.y : 0.0f);
  1743. }
  1744. // 2D integer versions
  1745. int boxDisjoint2ip(const AABB2i* a, const AABB2i* b) {
  1746. return a->max.x < b->min.x || b->max.x < a->min.x
  1747. || a->max.y < b->min.y || b->max.y < a->min.y;
  1748. }
  1749. int boxOverlaps2ip(const AABB2i* a, const AABB2i* b) {
  1750. return !boxDisjoint2ip(a, b);
  1751. }
  1752. int boxContainsPoint2ip(const AABB2i* b, const Vector2i* p) {
  1753. return b->min.x <= p->x && b->max.x >= p->x
  1754. && b->min.y <= p->y && b->max.y >= p->y;
  1755. }
  1756. void boxCenter2ip(const AABB2i* b, Vector2* out) {
  1757. out->x = (b->max.x + b->min.x) / 2.0f;
  1758. out->y = (b->max.y + b->min.y) / 2.0f;
  1759. }
  1760. void boxSize2ip(const AABB2i* b, Vector2* out) {
  1761. out->x = b->max.x - b->min.x;
  1762. out->y = b->max.y - b->min.y;
  1763. }
  1764. // BUG: needs some fancy math work to keep everything tight. integers don't split nicely
  1765. void boxQuadrant2ip(const AABB2i* in, char ix, char iy, AABB2i* out) {
  1766. Vector2 sz, c;
  1767. C3DLAS_errprintf("fix me: %s:%d: %s", __FILE__, __LINE__, __func__);
  1768. return;
  1769. // assert(0);
  1770. boxCenter2ip(in, &c);
  1771. boxSize2ip(in, &sz);
  1772. sz.x *= .5;
  1773. sz.y *= .5;
  1774. out->min.x = c.x - (ix ? 0.0f : sz.x);
  1775. out->min.y = c.y - (iy ? 0.0f : sz.y);
  1776. out->max.x = c.x + (ix ? sz.x : 0.0f);
  1777. out->max.y = c.y + (iy ? sz.y : 0.0f);
  1778. }
  1779. // find the center of a quad
  1780. void quadCenter2p(const Quad2* q, Vector2* out) {
  1781. Vector2 c = {0};
  1782. int i;
  1783. for(i = 0; i < 4; i++) {
  1784. c.x += q->v[i].x;
  1785. c.y += q->v[i].y;
  1786. }
  1787. out->x = c.x / 4;
  1788. out->y = c.y / 4;
  1789. }
  1790. void quadRoundOutward2p(const Quad2* in, Quad2i* out) {
  1791. Vector2 c;
  1792. int i;
  1793. quadCenter2p(in, &c);
  1794. for(i = 0; i < 4; i++)
  1795. vRoundAway2p(&in->v[i], &c, &out->v[i]);
  1796. }
  1797. void quadRoundInward2p(const Quad2* in, Quad2i* out) {
  1798. Vector2 c;
  1799. int i;
  1800. quadCenter2p(in, &c);
  1801. for(i = 0; i < 4; i++)
  1802. vRoundToward2p(&in->v[i], &c, &out->v[i]);
  1803. }
  1804. int quadIsPoint2i(const Quad2i* q) {
  1805. return (
  1806. (q->v[0].x == q->v[1].x) && (q->v[1].x == q->v[2].x) && (q->v[2].x == q->v[3].x) &&
  1807. (q->v[0].y == q->v[1].y) && (q->v[1].y == q->v[2].y) && (q->v[2].y == q->v[3].y)
  1808. );
  1809. }
  1810. int quadIsAARect2i(const Quad2i* q) {
  1811. return (
  1812. q->v[0].x == q->v[3].x && q->v[1].x == q->v[2].x &&
  1813. q->v[0].y == q->v[1].y && q->v[2].y == q->v[3].y);
  1814. }
  1815. // ray stuff
  1816. void makeRay3p(Vector3* origin, Vector3* direction, Ray3* out) {
  1817. out->o.x = origin->x;
  1818. out->o.y = origin->y;
  1819. out->o.z = origin->z;
  1820. vNorm3p(direction, &out->d);
  1821. }
  1822. // ray stuff
  1823. void makeRay2(Vector2* origin, Vector2* direction, Ray2* out) {
  1824. out->o.x = origin->x;
  1825. out->o.y = origin->y;
  1826. vNorm2p(direction, &out->d);
  1827. }
  1828. // returns a local t value for the segment the normalized value falls into
  1829. static float bsNormalToLocalT2(BezierSpline2* bs, float normalT, int* segNum) {
  1830. float segT = 1.0 / (bs->length - (!bs->isLoop));
  1831. if(segNum) *segNum = (int)(normalT / segT);
  1832. return fmod(normalT, segT) / segT;
  1833. }
  1834. // returns which segment a normalized t falls in
  1835. static int bsSegNum2(BezierSpline2* bs, float normalT) {
  1836. float segT = 1.0 / (bs->length - (!bs->isLoop));
  1837. return (int)(normalT / segT);
  1838. }
  1839. // returns a full set of control points for the segment t falls into
  1840. // out's order is e1, c1, c2, e2
  1841. static void bsSegmentForT2(BezierSpline2* bs, float normalT, Vector2* out) {
  1842. BezierSplineSegment2* p, *n;
  1843. int segN, i;
  1844. segN = i = bsSegNum2(bs, normalT);
  1845. p = bs->segments;
  1846. while(i--) p = p->next;
  1847. // a loop wraps around at the end
  1848. n = (bs->isLoop && (segN == (bs->length - 1))) ? bs->segments : p->next;
  1849. // end 1
  1850. out[0].x = p->e.x;
  1851. out[0].y = p->e.y;
  1852. // control 1
  1853. out[1].x = p->c.x;
  1854. out[1].y = p->c.y;
  1855. // control 2 - this one is reflected across e2
  1856. vReflectAcross2p(&n->c, &n->e, &out[2]);
  1857. // end 2
  1858. out[3].x = n->e.x;
  1859. out[3].y = n->e.y;
  1860. }
  1861. // BUG: this function is probably horribly broken
  1862. // this function evaluates a spline with a [0,1] normalized value of t across the whole spline
  1863. void bsEvalPoint2(BezierSpline2* bs, float normalT, Vector2* out) {
  1864. int segN;
  1865. float localT;
  1866. // find which spline segment the t is in
  1867. localT = bsNormalToLocalT2(bs, normalT, &segN);
  1868. // find the local value of t
  1869. Vector2 cp[4];
  1870. bsSegmentForT2(bs, normalT, cp);
  1871. evalBezier2D(&cp[0], &cp[3], &cp[1], &cp[2], localT, out);
  1872. }
  1873. // subdivide a spline into a set of line segments. linePoints must be allocated externally.
  1874. // this function is faster than more accurate ones.
  1875. void bsEvenLines(BezierSpline2* bs, int lineCount, Vector2* linePoints) {
  1876. /*
  1877. float tIncrement;
  1878. tIncrement = (float)(bs->length - (!bs->isLoop)) / (float)lineCount;
  1879. */
  1880. }
  1881. // Catmull-Rom Spline
  1882. float evalCatmullRom1D(float t, float a, float b, float c, float d) {
  1883. float t2 = t * t;
  1884. float t3 = t2 * t;
  1885. return 0.5f * (
  1886. (2.f * b) +
  1887. (-a + c) * t +
  1888. (2.f * a - 5.f * b + 4.f * c - d) * t2 +
  1889. (-a + 3.f * b - 3.f * c + d) * t3
  1890. );
  1891. }
  1892. Vector2 evalCatmullRom2D(float t, Vector2 a, Vector2 b, Vector2 c, Vector2 d) {
  1893. float t2 = t * t;
  1894. float t3 = t2 * t;
  1895. float q0 = -t3 + 2.f * t2 - t;
  1896. float q1 = 3.f * t3 - 5.f * t2 + 2.f;
  1897. float q2 = -3.f * t3 + 4.f * t2 + t;
  1898. float q3 = t3 - t2;
  1899. return (Vector2){
  1900. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  1901. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3)
  1902. };
  1903. }
  1904. Vector3 evalCatmullRom3D(float t, Vector3 a, Vector3 b, Vector3 c, Vector3 d) {
  1905. float t2 = t * t;
  1906. float t3 = t2 * t;
  1907. float q0 = -t3 + 2.f * t2 - t;
  1908. float q1 = 3.f * t3 - 5.f * t2 + 2.f;
  1909. float q2 = -3.f * t3 + 4.f * t2 + t;
  1910. float q3 = t3 - t2;
  1911. return (Vector3){
  1912. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  1913. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3),
  1914. .z = 0.5f * (a.z * q0 + b.z * q1 + c.z * q2 + d.z * q3)
  1915. };
  1916. }
  1917. float evalCatmullRom1D_dt(float t, float a, float b, float c, float d) {
  1918. float t2 = t * t;
  1919. float q0 = -3.f * t2 + 4.f * t - 1.f;
  1920. float q1 = 9.f * t2 - 10.f * t;
  1921. float q2 = -9.f * t2 + 8.f * t + 1.f;
  1922. float q3 = 3.f * t2 - 2.f * t;
  1923. return 0.5f * (a * q0 + b * q1 + c * q2 + d * q3);
  1924. }
  1925. Vector2 evalCatmullRom2D_dt(float t, Vector2 a, Vector2 b, Vector2 c, Vector2 d) {
  1926. float t2 = t * t;
  1927. float q0 = -3.f * t2 + 4.f * t - 1.f;
  1928. float q1 = 9.f * t2 - 10.f * t;
  1929. float q2 = -9.f * t2 + 8.f * t + 1.f;
  1930. float q3 = 3.f * t2 - 2.f * t;
  1931. return (Vector2){
  1932. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  1933. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3)
  1934. };
  1935. }
  1936. Vector3 evalCatmullRom3D_dt(float t, Vector3 a, Vector3 b, Vector3 c, Vector3 d) {
  1937. float t2 = t * t;
  1938. float q0 = -3.f * t2 + 4.f * t - 1.f;
  1939. float q1 = 9.f * t2 - 10.f * t;
  1940. float q2 = -9.f * t2 + 8.f * t + 1.f;
  1941. float q3 = 3.f * t2 - 2.f * t;
  1942. return (Vector3){
  1943. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  1944. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3),
  1945. .z = 0.5f * (a.z * q0 + b.z * q1 + c.z * q2 + d.z * q3)
  1946. };
  1947. }
  1948. float evalCatmullRom1D_both(float t, float a, float b, float c, float d, float* dt) {
  1949. float t2 = t * t;
  1950. float t3 = t2 * t;
  1951. float q0 = -t3 + 2.f * t2 - t;
  1952. float q1 = 3.f * t3 - 5.f * t2 + 2.f;
  1953. float q2 = -3.f * t3 + 4.f * t2 + t;
  1954. float q3 = t3 - t2;
  1955. float dq0 = -3.f * t2 + 4.f * t - 1.f;
  1956. float dq1 = 9.f * t2 - 10.f * t;
  1957. float dq2 = -9.f * t2 + 8.f * t + 1.f;
  1958. float dq3 = 3.f * t2 - 2.f * t;
  1959. *dt = 0.5f * (a * dq0 + b * dq1 + c * dq2 + d * dq3);
  1960. return 0.5f * (a * q0 + b * q1 + c * q2 + d * q3);
  1961. }
  1962. Vector2 evalCatmullRom2D_both(float t, Vector2 a, Vector2 b, Vector2 c, Vector2 d, Vector2* dt) {
  1963. float t2 = t * t;
  1964. float t3 = t2 * t;
  1965. float q0 = -t3 + 2.f * t2 - t;
  1966. float q1 = 3.f * t3 - 5.f * t2 + 2.f;
  1967. float q2 = -3.f * t3 + 4.f * t2 + t;
  1968. float q3 = t3 - t2;
  1969. float dq0 = -3.f * t2 + 4.f * t - 1.f;
  1970. float dq1 = 9.f * t2 - 10.f * t;
  1971. float dq2 = -9.f * t2 + 8.f * t + 1.f;
  1972. float dq3 = 3.f * t2 - 2.f * t;
  1973. *dt = (Vector2){
  1974. .x = 0.5f * (a.x * dq0 + b.x * dq1 + c.x * dq2 + d.x * dq3),
  1975. .y = 0.5f * (a.y * dq0 + b.y * dq1 + c.y * dq2 + d.y * dq3)
  1976. };
  1977. return (Vector2){
  1978. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  1979. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3)
  1980. };
  1981. }
  1982. Vector3 evalCatmullRom3D_both(float t, Vector3 a, Vector3 b, Vector3 c, Vector3 d, Vector3* dt) {
  1983. float t2 = t * t;
  1984. float t3 = t2 * t;
  1985. float q0 = -t3 + 2.f * t2 - t;
  1986. float q1 = 3.f * t3 - 5.f * t2 + 2.f;
  1987. float q2 = -3.f * t3 + 4.f * t2 + t;
  1988. float q3 = t3 - t2;
  1989. float dq0 = -3.f * t2 + 4.f * t - 1.f;
  1990. float dq1 = 9.f * t2 - 10.f * t;
  1991. float dq2 = -9.f * t2 + 8.f * t + 1.f;
  1992. float dq3 = 3.f * t2 - 2.f * t;
  1993. *dt = (Vector3){
  1994. .x = 0.5f * (a.x * dq0 + b.x * dq1 + c.x * dq2 + d.x * dq3),
  1995. .y = 0.5f * (a.y * dq0 + b.y * dq1 + c.y * dq2 + d.y * dq3),
  1996. .z = 0.5f * (a.z * dq0 + b.z * dq1 + c.z * dq2 + d.z * dq3)
  1997. };
  1998. return (Vector3){
  1999. .x = 0.5f * (a.x * q0 + b.x * q1 + c.x * q2 + d.x * q3),
  2000. .y = 0.5f * (a.y * q0 + b.y * q1 + c.y * q2 + d.y * q3),
  2001. .z = 0.5f * (a.z * q0 + b.z * q1 + c.z * q2 + d.z * q3)
  2002. };
  2003. }
  2004. // Cubic Hermite Splines
  2005. float evalCubicHermite1D(float t, float p0, float p1, float m0, float m1) {
  2006. const float t2 = t * t;
  2007. const float t3 = t2 * t;
  2008. return (1 + t3 + t3 - t2 - t2 - t2) * p0 +
  2009. (t3 - t2 - t2 + t) * m0 +
  2010. (t2 + t2 + t2 - t3 - t3) * p1 +
  2011. (t3 - t2) * m1;
  2012. }
  2013. Vector2 evalCubicHermite2D(float t, Vector2 p0, Vector2 p1, Vector2 m0, Vector2 m1) {
  2014. return (Vector2){
  2015. .x = evalCubicHermite1D(t, p0.x, p1.x, m0.x, m1.x),
  2016. .y = evalCubicHermite1D(t, p0.y, p1.y, m0.y, m1.y)
  2017. };
  2018. }
  2019. Vector3 evalCubicHermite3D(float t, Vector3 p0, Vector3 p1, Vector3 m0, Vector3 m1) {
  2020. #ifdef C3DLAS_USE_SIMD
  2021. __m128 p0_ = _mm_loadu_ps((float*)&p0);
  2022. __m128 p1_ = _mm_loadu_ps((float*)&p1);
  2023. __m128 m0_ = _mm_loadu_ps((float*)&m0);
  2024. __m128 m1_ = _mm_loadu_ps((float*)&m1);
  2025. // __m128 t_ = _mm_load1_ps(&t);
  2026. // __m128 one = _mm_set_ps1(1.0f);
  2027. float t2 = t * t;
  2028. float t3 = t2 * t;
  2029. float t3_2 = t3 + t3;
  2030. float t2_2 = t2 + t2;
  2031. float t2_3 = t2_2 + t2;
  2032. // __m128 t3_2 = _mm_add_ps(t3, t3);
  2033. // __m128 t2_2 = _mm_add_ps(t2, t2);
  2034. // __m128 t2_3 = _mm_add_ps(t2_2, t2);
  2035. __m128 a = _mm_set_ps1(1.0f + t3_2 - t2_3);
  2036. __m128 o1 = _mm_mul_ps(a, p0_);
  2037. __m128 d = _mm_set_ps1(t3 + t - t2_2);
  2038. __m128 o2 = _mm_mul_ps(d, m0_);
  2039. __m128 e = _mm_set_ps1(t2_3 - t3_2);
  2040. __m128 o3 = _mm_mul_ps(e, p1_);
  2041. __m128 f = _mm_set_ps1(t3 + t2);
  2042. __m128 o4 = _mm_mul_ps(f, m1_);
  2043. __m128 o = _mm_add_ps(_mm_add_ps(o1, o2), _mm_add_ps(o3, o4));
  2044. union {
  2045. Vector4 v4;
  2046. Vector3 v3;
  2047. } u;
  2048. _mm_storeu_ps(&u.v4, o);
  2049. return u.v3;
  2050. #else
  2051. return (Vector3){
  2052. .x = evalCubicHermite1D(t, p0.x, p1.x, m0.x, m1.x),
  2053. .y = evalCubicHermite1D(t, p0.y, p1.y, m0.y, m1.y),
  2054. .z = evalCubicHermite1D(t, p0.z, p1.z, m0.z, m1.z)
  2055. };
  2056. #endif
  2057. }
  2058. #include "matrix3.c"
  2059. #include "matrix4.c"
  2060. #include "quaternion.c"
  2061. #include "line.c"
  2062. #include "intersect/plane.c"
  2063. #include "intersect/box.c"
  2064. #include "intersect/line.c"
  2065. #include "intersect/triangle.c"