-
Notifications
You must be signed in to change notification settings - Fork 11
/
maths.h
1981 lines (1698 loc) · 71 KB
/
maths.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// maths.h
// Copyright 2014 - 2020 Alex Dixon.
// License: https://github.com/polymonster/maths/blob/master/license.md
#pragma once
#include "mat.h"
#include "quat.h"
#include "util.h"
#include "vec.h"
constexpr double M_PI_OVER_180 = 3.1415926535897932384626433832795 / 180.0;
constexpr double M_180_OVER_PI = 180.0 / 3.1415926535897932384626433832795;
constexpr double M_TWO_PI = M_PI * 2.0;
constexpr double M_PHI = 1.61803398875;
constexpr double M_INV_PHI = 0.61803398875;
namespace maths
{
enum e_classifications
{
INTERSECTS = 0,
BEHIND = 1,
INFRONT = 2,
};
typedef u32 classification;
struct transform
{
vec3f translation = vec3f::zero();
quat rotation = quat();
vec3f scale = vec3f::one();
};
// a collection of tests and useful maths functions
// see inline implementation below file for explanation of args and return values.
// .. consider moving large functions into a cpp instead of keeping them inline, just leaving them inline here for
// convenience and to keep the library header only
// Basis
vec3f get_normal(const vec3f& v1, const vec3f& v2, const vec3f& v3);
void get_orthonormal_basis_hughes_moeller(const vec3f& n, vec3f& b1, vec3f& b2);
void get_orthonormal_basis_frisvad(const vec3f& n, vec3f& b1, vec3f& b2);
void get_frustum_planes_from_matrix(const mat4f& view_projection, vec4f* planes_out);
void get_frustum_corners_from_matrix(const mat4f& view_projection, vec3f* corners);
transform get_transform_from_matrix(const mat4f& mat);
template<size_t N, typename T>
Vec<3, T> barycentric(const Vec<N, T>& p, const Vec<N, T>& a, const Vec<N, T>& b, const Vec<N, T>& c);
// Angles
f32 deg_to_rad(f32 degree_angle);
f32 rad_to_deg(f32 radian_angle);
vec3f azimuth_altitude_to_xyz(f32 azimuth, f32 altitude);
void xyz_to_azimuth_altitude(vec3f v, f32& azimuth, f32& altitude);
f32 focal_length_to_fov(f32 focal_length, f32 aperture_width);
f32 fov_to_focal_length(f32 fov, f32 aperture_width);
// Colours
vec3f rgb_to_hsv(vec3f rgb);
vec3f hsv_to_rgb(vec3f hsv);
vec4f rgba8_to_vec4f(u32 rgba);
u32 vec4f_to_rgba8(vec4f);
// Projection
// ndc = normalized device coordinates (-1 to 1)
// sc = screen coordinates (viewport (0,0) to (width, height)
// vdown = y.0 = top, y.height = bottom
// vup (no suffix) = y.0 bottom, y.height = top
vec3f project_to_ndc(const vec3f& p, const mat4f& view_projection);
vec3f project_to_sc(const vec3f& p, const mat4f& view_projection, const vec2i& viewport);
vec3f project_to_sc_vdown(const vec3f& p, const mat4f& view_projection, const vec2i& viewport);
vec3f unproject_ndc(const vec3f& p, const mat4f& view_projection);
vec3f unproject_sc(const vec3f& p, const mat4f& view_projection, const vec2i& viewport);
vec3f unproject_sc_vdown(const vec3f& p, const mat4f& view_projection, const vec2i& viewport);
// Plane Classification
classification point_vs_plane(const vec3f& p, const vec3f& x, const vec3f& n);
classification aabb_vs_plane(const vec3f& aabb_min, const vec3f& aabb_max, const vec3f& x0, const vec3f& xN);
classification sphere_vs_plane(const vec3f& s, f32 r, const vec3f& x0, const vec3f& xN);
classification capsule_vs_plane(const vec3f& c1, const vec3f& c2, f32 r, const vec3f& x, const vec3f& n);
classification cone_vs_plane(const vec3f& cp, const vec3f& cv, f32 h, f32 r, const vec3f& x, const vec3f& n);
// Overlaps
bool sphere_vs_sphere(const vec3f& s0, f32 r0, const vec3f& s1, f32 r1);
bool sphere_vs_aabb(const vec3f& s0, f32 r0, const vec3f& aabb_min, const vec3f& aabb_max);
bool sphere_vs_obb(const vec3f& s0, f32 r0, const mat4f& obb);
bool aabb_vs_aabb(const vec3f& min0, const vec3f& max0, const vec3f& min1, const vec3f& max1);
bool aabb_vs_obb(const vec3f& aabb_min, const vec3f& aabb_max, const mat4f& obb);
bool aabb_vs_frustum(const vec3f& aabb_pos, const vec3f& aabb_extent, vec4f* planes);
bool sphere_vs_frustum(const vec3f& pos, f32 radius, vec4f* planes);
bool sphere_vs_capsule(const vec3f s0, f32 sr, const vec3f& cp0, const vec3f& cp1, f32 cr);
bool capsule_vs_capsule(const vec3f& cp0, const vec3f& cp1, f32 cr0, const vec3f& cp2, const vec3f& cp3, f32 cr1);
bool obb_vs_obb(const mat4f& obb0, const mat4f& obb1);
bool convex_hull_vs_convex_hull(const std::vector<vec2f>& hull0, const std::vector<vec2f>& hull1);
bool gjk_2d(const std::vector<vec2f>& convex0, const std::vector<vec2f>& convex1);
bool gjk_3d(const std::vector<vec3f>& convex0, const std::vector<vec3f>& convex1);
// Point Test
template<size_t N, typename T>
bool point_inside_aabb(const Vec<N, T>& min, const Vec<N, T>& max, const Vec<N, T>& p0);
bool point_inside_sphere(const vec3f& s0, f32 r0, const vec3f& p0);
bool point_inside_obb(const mat4f& mat, const vec3f& p);
bool point_inside_triangle(const vec3f& p, const vec3f& v1, const vec3f& v2, const vec3f& v3);
bool point_inside_cone(const vec3f& p, const vec3f& cp, const vec3f& cv, f32 h, f32 r);
bool point_inside_convex_hull(const vec2f& p, const std::vector<vec2f>& hull);
bool point_inside_poly(const vec2f& p, const std::vector<vec2f>& poly);
bool point_inside_frustum(const vec3f& p, vec4f* planes);
// Closest Point
template<size_t N, typename T>
Vec<N, T> closest_point_on_aabb(const Vec<N, T>& p0, const Vec<N, T>& aabb_min, const Vec<N, T>& aabb_max);
template<size_t N, typename T>
Vec<N, T> closest_point_on_line(const Vec<N, T>& l1, const Vec<N, T>& l2, const Vec<N, T>& p);
vec3f closest_point_on_plane(const vec3f& p, const vec3f& x, const vec3f& n);
vec3f closest_point_on_obb(const mat4f& mat, const vec3f& p);
vec3f closest_point_on_sphere(const vec3f& s0, f32 r0, const vec3f& p0);
vec3f closest_point_on_ray(const vec3f& r0, const vec3f& rV, const vec3f& p);
vec3f closest_point_on_triangle(const vec3f& p, const vec3f& v1, const vec3f& v2, const vec3f& v3, f32& side);
vec2f closest_point_on_polygon(const vec2f& p, std::vector<vec2f> poly);
vec2f closest_point_on_convex_hull(const vec2f& p, std::vector<vec2f> hull);
vec3f closest_point_on_cone(const vec3f& p, const vec3f& cp, const vec3f& cv, f32 h, f32 r);
// Point Distance
template<size_t N, typename T>
T point_aabb_distance(const Vec<N, T>& p0, const Vec<N, T>& aabb_min, const Vec<N, T>& aabb_max);
template<size_t N, typename T>
T point_segment_distance(const Vec<N, T>& x0, const Vec<N, T>& x1, const Vec<N, T>& x2);
f32 point_triangle_distance(const vec3f& x0, const vec3f& x1, const vec3f& x2, const vec3f& x3);
template<size_t N, typename T>
T distance_on_line(const Vec<N, T> & l1, const Vec<N, T> & l2, const Vec<N, T> & p);
f32 point_plane_distance(const vec3f& p0, const vec3f& x0, const vec3f& xN);
f32 plane_distance(const vec3f& x0, const vec3f& xN);
f32 point_sphere_distance(const vec3f& p0, const vec3f& s0, f32 r);
f32 point_polygon_distance(const vec2f& p, std::vector<vec2f> poly);
f32 point_convex_hull_distance(const vec2f& p, std::vector<vec2f> hull);
f32 point_cone_distance(const vec3f& p, const vec3f& cp, const vec3f& cv, f32 h, f32 r);
f32 point_obb_distance(const vec3f& p, const mat4f& obb);
// Ray / Line
vec3f ray_vs_plane(const vec3f& r0, const vec3f& rV, const vec3f& x0, const vec3f& xN);
bool ray_vs_triangle(const vec3f& r0, const vec3f& rv, const vec3f& t0, const vec3f& t1, const vec3f& t2, vec3f& ip);
bool ray_vs_sphere(const vec3f& r0, const vec3f& rv, const vec3f& s0, f32 r, vec3f& ip);
bool ray_vs_line_segment(const vec3f& l1, const vec3f& l2, const vec3f& r0, const vec3f& rV, vec3f& ip);
bool ray_vs_aabb(const vec3f& min, const vec3f& max, const vec3f& r1, const vec3f& rv, vec3f& ip);
bool ray_vs_obb(const mat4f& mat, const vec3f& r1, const vec3f& rv, vec3f& ip);
bool ray_vs_capsule(const vec3f& r0, const vec3f& rv, const vec3f& c1, const vec3f& c2, f32 r, vec3f& ip);
bool ray_vs_cylinder(const vec3f& r0, const vec3f& rv, const vec3f& c0, const vec3f& c1, f32 cr, vec3f& ip);
bool line_vs_line(const vec3f& l1, const vec3f& l2, const vec3f& s1, const vec3f& s2, vec3f& ip);
bool line_vs_poly(const vec2f& l1, const vec2f& l2, const std::vector<vec2f>& poly, std::vector<vec2f>& ips);
bool shortest_line_segment_between_lines(const vec3f& p1, const vec3f& p2, const vec3f& p3, const vec3f& p4, vec3f& r0, vec3f& r1);
bool shortest_line_segment_between_line_segments(const vec3f& p1, const vec3f& p2, const vec3f& p3, const vec3f& p4, vec3f& r0, vec3f& r1);
// Convex Hull
void convex_hull_from_points(std::vector<vec2f>& hull, const std::vector<vec2f>& p);
template<size_t N, typename T>
Vec<N, T> get_convex_hull_centre(const std::vector<Vec<N, T>>& hull);
//
// Deprecated Functions (they still exist but have been renamed for api consitency)
//
maths_deprecated vec3f ray_plane_intersect(const vec3f& r0, const vec3f& rV, const vec3f& x0, const vec3f& xN);
maths_deprecated bool ray_triangle_intersect(const vec3f& r0, const vec3f& rv, const vec3f& t0, const vec3f& t1, const vec3f& t2, vec3f& ip);
maths_deprecated bool line_vs_ray(const vec3f& l1, const vec3f& l2, const vec3f& r0, const vec3f& rV, vec3f& ip);
//
// Implementation
//
maths_inline f32 deg_to_rad(f32 degree_angle)
{
return (degree_angle * (f32)M_PI_OVER_180);
}
maths_inline f32 rad_to_deg(f32 radian_angle)
{
return (radian_angle * (f32)M_180_OVER_PI);
}
// Convert rgb [0-1] to hsv [0-1]
inline vec3f rgb_to_hsv(vec3f rgb)
{
// from Foley & van Dam p592
// optimized: http://lolengine.net/blog/2013/01/13/fast-rgb-to-hsv
f32 r = rgb.r;
f32 g = rgb.g;
f32 b = rgb.b;
vec3f out_hsv;
f32 K = 0.f;
if (g < b)
{
std::swap(g, b);
K = -1.f;
}
if (r < g)
{
std::swap(r, g);
K = -2.f / 6.f - K;
}
const f32 chroma = r - (g < b ? g : b);
out_hsv.r = fabsf(K + (g - b) / (6.f * chroma + 1e-20f));
out_hsv.g = chroma / (r + 1e-20f);
out_hsv.b = r;
return out_hsv;
}
// Convert hsv [0-1] to rgb [0-1]
inline vec3f hsv_to_rgb(vec3f hsv)
{
// from Foley & van Dam p593: http://en.wikipedia.org/wiki/HSL_and_HSV
f32 h = hsv.r;
f32 s = hsv.g;
f32 v = hsv.b;
vec3f out_rgb;
if (s == 0.0f)
{
// gray
out_rgb.r = out_rgb.g = out_rgb.b = v;
return out_rgb;
}
h = fmodf(h, 1.0f) / (60.0f/360.0f);
int i = (int)h;
f32 f = h - (f32)i;
f32 p = v * (1.0f - s);
f32 q = v * (1.0f - s * f);
f32 t = v * (1.0f - s * (1.0f - f));
switch (i)
{
case 0: out_rgb.r = v; out_rgb.g = t; out_rgb.b = p; break;
case 1: out_rgb.r = q; out_rgb.g = v; out_rgb.b = p; break;
case 2: out_rgb.r = p; out_rgb.g = v; out_rgb.b = t; break;
case 3: out_rgb.r = p; out_rgb.g = q; out_rgb.b = v; break;
case 4: out_rgb.r = t; out_rgb.g = p; out_rgb.b = v; break;
case 5: default: out_rgb.r = v; out_rgb.g = p; out_rgb.b = q; break;
}
return out_rgb;
}
// convert rgb8 packed in u32 to vec4 (f32) rgba
inline vec4f rgba8_to_vec4f(u32 rgba)
{
constexpr f32 k_one_over_255 = 1.0f/255.0f;
return vec4f(
((rgba >> 0) & 0xff) * k_one_over_255,
((rgba >> 8) & 0xff) * k_one_over_255,
((rgba >> 16) & 0xff) * k_one_over_255,
((rgba >> 24) & 0xff) * k_one_over_255
);
}
// convert vec4 (f32) rgba into a packed u32 containing rgba8
inline u32 vec4f_to_rgba8(vec4f v)
{
u32 rgba = 0;
rgba |= ((u32)(v[0] * 255.0f));
rgba |= ((u32)(v[1] * 255.0f)) << 8;
rgba |= ((u32)(v[2] * 255.0f)) << 16;
rgba |= ((u32)(v[3] * 255.0f)) << 24;
return rgba;
}
// given the normalized vector n, constructs an orthonormal basis returned in n, b1, b2
inline void get_orthonormal_basis_hughes_moeller(const vec3f& n, vec3f& b1, vec3f& b2)
{
// choose a vector orthogonal to n as the direction of b2.
if(fabs(n.x) > fabs(n.z))
{
b2 = vec3f(-n.y, n.x, 0.0f);
}
else
{
b2 = vec3f(0.0f, -n.z, n.y);
}
// normalize b2
b2 *= rsqrt(dot(b2, b2));
// construct b1 using cross product
b1 = cross(b2, n);
}
// given the normalized vector n construct an orthonormal basis without sqrt..
inline void get_orthonormal_basis_frisvad(const vec3f& n, vec3f& b1, vec3f& b2)
{
constexpr f32 k_singularity = -0.99999999f;
if(n.z < k_singularity)
{
b1 = vec3f(0.0f, -1.0f, 0.0f);
b2 = vec3f(-1.0f, 0.0f, 0.0f);
return;
}
f32 a = 1.0f/(1.0f + n.z);
f32 b = -n.x * n.y * a;
b1 = vec3f(1.0f - n.x * n.x * a, b, -n.x);
b2 = vec3f(b, 1.0f - n.y * n.y * a, -n.y);
}
// returns the barycentric coordinates of point p within triangle t0-t1-t2
// with the result packed into a vec (u = x, v = y, w = z)
template<size_t N, typename T>
Vec<3, T> barycentric(const Vec<N, T>& p, const Vec<N, T>& t0, const Vec<N, T>& t1, const Vec<N, T>& t2)
{
Vec<N, T> v0 = t1 - t0, v1 = t2 - t0, v2 = p - t0;
T d00 = dot(v0, v0);
T d01 = dot(v0, v1);
T d11 = dot(v1, v1);
T d20 = dot(v2, v0);
T d21 = dot(v2, v1);
T denom = d00 * d11 - d01 * d01;
T v = (d11 * d20 - d01 * d21) / denom;
T w = (d00 * d21 - d01 * d20) / denom;
T u = 1.0f - v - w;
return {u, v, w};
}
// project point p by view_projection to normalized device coordinates, perfroming homogeneous divide
inline vec3f project_to_ndc(const vec3f& p, const mat4f& view_projection)
{
vec4f ndc = view_projection.transform_vector(vec4f(p, 1.0f));
ndc /= ndc.w;
return ndc.xyz;
}
// project point p to screen coordinates of viewport after projecting to normalized device coordinates first
// coordinates are vup in the y-axis y.0 = bottom y.height = top
inline vec3f project_to_sc(const vec3f& p, const mat4f& view_projection, const vec2i& viewport)
{
vec3f ndc = project_to_ndc(p, view_projection);
vec3f sc = ndc * 0.5f + 0.5f;
sc.xy *= vec2f((f32)viewport.x, (f32)viewport.y);
return sc;
}
// project point p to screen coordinates of viewport after projecting to normalized device coordinates first
// coordinates are vdown in the y-axis vdown = y.0 = top y.height = bottom
inline vec3f project_to_sc_vdown(const vec3f& p, const mat4f& view_projection, const vec2i& viewport)
{
vec3f ndc = project_to_ndc(p, view_projection);
ndc.y *= -1.0f;
vec3f sc = ndc * 0.5f + 0.5f;
sc.xy *= vec2f((f32)viewport.x, (f32)viewport.y);
return sc;
}
// unproject normalized device coordinate p with viewport using inverse view_projection
inline vec3f unproject_ndc(const vec3f& p, const mat4f& view_projection)
{
mat4f inv = mat::inverse4x4(view_projection);
vec4f ppc = inv.transform_vector(vec4f(p, 1.0f));
return ppc.xyz / ppc.w;
}
// unproject screen coordinate p with viewport using inverse view_projection
// coordinates are vup in the y-axis y.0 = bottom y.height = top
// this function expects z in -1 to 1 range
inline vec3f unproject_sc(const vec3f& p, const mat4f& view_projection, const vec2i& viewport)
{
vec2f ndc_xy = (p.xy / (vec2f)viewport) * vec2f(2.0) - vec2f(1.0);
vec3f ndc = vec3f(ndc_xy, p.z);
return unproject_ndc(ndc, view_projection);
}
// unproject screen coordinate p with viewport using inverse view_projection
// coordinates are vdown in the y-axis vdown = y.0 = top y.height = bottom
inline vec3f unproject_sc_vdown(const vec3f& p, const mat4f& view_projection, const vec2i& viewport)
{
vec2f ndc_xy = (p.xy / (vec2f)viewport) * vec2f(2.0) - vec2f(1.0);
ndc_xy.y *= -1.0f;
vec3f ndc = vec3f(ndc_xy, p.z);
return unproject_ndc(ndc, view_projection);
}
// convert azimuth / altitude to vec3f xyz
inline vec3f azimuth_altitude_to_xyz(f32 azimuth, f32 altitude)
{
f32 z = sin(altitude);
f32 hyp = cos(altitude);
f32 y = hyp * cos(azimuth);
f32 x = hyp * sin(azimuth);
return vec3f(x, z, y);
}
// convert vector xyz to azimuth, altitude
inline void xyz_to_azimuth_altitude(vec3f v, f32& azimuth, f32& altitude)
{
azimuth = atan2(v.y, v.x);
altitude = atan2(v.z, sqrt(v.x * v.x + v.y * v.y));
}
// convert focal length to field of view with the specified aperture_width
inline f32 focal_length_to_fov(f32 focal_length, f32 aperture_width)
{
return (2.0f * rad_to_deg(atan((aperture_width * 25.4f) / (2.0f * focal_length))));
}
// convert field of view to focal length with the specified aperture_width
inline f32 fov_to_focal_length(f32 fov, f32 aperture_width)
{
return (aperture_width * 25.4f) / (2.0f * tan(maths::deg_to_rad(fov / 2.0f)));
}
// get distance to plane x defined by point on plane x0 and normal of plane xN
maths_inline f32 plane_distance(const vec3f& x0, const vec3f& xN)
{
return dot(xN, x0) * -1.0f;
}
// get distance from point p0 to plane defined by point x0 and normal xN
maths_inline f32 point_plane_distance(const vec3f& p0, const vec3f& x0, const vec3f& xN)
{
f32 d = plane_distance(x0, xN);
return dot(p0, xN) + d;
}
// returns the unsigned distance from point p0 to the sphere centred at s0 with radius r
maths_inline f32 point_sphere_distance(const vec3f& p0, const vec3f& s0, f32 r)
{
vec3f cp = closest_point_on_sphere(s0, r, p0);
return dist(p0, cp);
}
// returns the unsigned distance from point p to polygon defined by pairs of points which define the polygons edges
maths_inline f32 point_polygon_distance(const vec2f& p, std::vector<vec2f> poly)
{
return dist(p, closest_point_on_polygon(p, poly));
}
// returns the unsigned distance from point p to convex hull defined by pairs of points which define the hulls edges
maths_inline f32 point_convex_hull_distance(const vec2f& p, std::vector<vec2f> hull)
{
return dist(p, closest_point_on_polygon(p, hull));
}
// returns the unsigned distance from point p to the edge of the cone defined start position cp, direction cv with height h and radius r
maths_inline f32 point_cone_distance(const vec3f& p, const vec3f& cp, const vec3f& cv, f32 h, f32 r)
{
return dist(closest_point_on_cone(p, cp, cv, h, r), p);
}
// returns the unsigned distance from point p to the obb defined by matrix obb, where the matrix transforms a unit cube
// from -1 to 1 into an obb
inline f32 point_obb_distance(const vec3f& p, const mat4f& obb)
{
return dist(p, closest_point_on_obb(obb, p));
}
// returns the intersection point of ray defined by origin r0 and direction rV,
// with plane defined by point on plane x0 normal of plane xN
inline vec3f ray_vs_plane(const vec3f& r0, const vec3f& rV, const vec3f& x0, const vec3f& xN)
{
f32 d = plane_distance(x0, xN);
f32 t = -(dot(r0, xN) + d) / dot(rV, xN);
return r0 + (rV * t);
}
// deprecated: use ray_vs_plane
inline vec3f ray_plane_intersect(const vec3f& r0, const vec3f& rV, const vec3f& x0, const vec3f& xN)
{
return ray_vs_plane(r0, rV, x0, xN);
}
// returns true if the ray (origin r0, direction rv) intersects with the triangle (t0,t1,t2)
// if it does intersect, ip is set to the intersection point
inline bool ray_vs_triangle(const vec3f& r0, const vec3f& rv, const vec3f& t0, const vec3f& t1, const vec3f& t2, vec3f& ip)
{
vec3f n = get_normal(t0, t1, t2);
vec3f p = ray_vs_plane(r0, rv, t0, n);
bool hit = point_inside_triangle(p, t0, t1, t2);
if(hit)
ip = p;
return hit;
}
// deprecated: use ray_vs_triangle
inline bool ray_triangle_intersect(const vec3f& r0, const vec3f& rv, const vec3f& t0, const vec3f& t1, const vec3f& t2, vec3f& ip)
{
return ray_vs_triangle(r0, rv, t0, t1, t2, ip);
}
// returns true if the ray (origin r0, direction rv) intersects with the sphere at s0 with radius r
// if it does intersect, ip is set to the intersection point
inline bool ray_vs_sphere(const vec3f& r0, const vec3f& rv, const vec3f& s0, f32 r, vec3f& ip)
{
vec3f oc = r0 - s0;
f32 a = dot(rv, rv);
f32 b = 2.0f * dot(oc, rv);
f32 c = dot(oc,oc) - r*r;
f32 discriminant = b*b - 4*a*c;
bool hit = discriminant > 0.0f;
if(hit)
{
f32 t1 = (-b - sqrt(discriminant)) / (2.0f*a);
f32 t2 = (-b + sqrt(discriminant)) / (2.0f*a);
f32 t;
if (t1 > 0.0f && t2 > 0.0f)
{
// shooting from outside
// get nearest
t = std::min(t1, t2);
}
else
{
// shooting from inside
// get hit in ray dir
t = (t1 > 0.0f ? t1 : t2);
}
ip = r0 + rv * t;
}
return hit;
}
// returns the classification of the point p vs the plane defined by point on plane x and normal n
inline classification point_vs_plane(const vec3f& p, const vec3f& x, const vec3f& n)
{
f32 d = point_plane_distance(p, x, n);
if(d < 0.0f)
{
return BEHIND;
}
else if(d > 0.0f)
{
return INFRONT;
}
// point is on the plane
return INTERSECTS;
}
// returns the classification of an aabb vs a plane aabb defined by min and max
// plane defined by point on plane x0 and normal of plane xN
inline classification aabb_vs_plane(const vec3f& aabb_min, const vec3f& aabb_max, const vec3f& x0, const vec3f& xN)
{
vec3f e = (aabb_max - aabb_min) / 2.0f;
vec3f centre = aabb_min + e;
f32 radius = fabs(xN.x * e.x) + fabs(xN.y * e.y) + fabs(xN.z * e.z);
f32 pd = plane_distance(x0, xN);
f32 d = dot(xN, centre) + pd;
if (d > radius)
return INFRONT;
if (d < -radius)
return BEHIND;
return INTERSECTS;
}
// returns the classification of a sphere vs a plane
// sphere defined by centre s, and radius r
// plane defined by point on plane x0 and normal of plane xN
inline classification sphere_vs_plane(const vec3f& s, f32 r, const vec3f& x0, const vec3f& xN)
{
f32 pd = plane_distance(x0, xN);
f32 d = dot(xN, s) + pd;
if (d > r)
return INFRONT;
if (d < -r)
return BEHIND;
return INTERSECTS;
}
// returns the classification of a capsule defined by line c1-c2 with radius r vs a plane defined by point on plane x and normal n
inline classification capsule_vs_plane(const vec3f& c1, const vec3f& c2, f32 r, const vec3f& x, const vec3f& n)
{
auto pd = plane_distance(x, n);
// classify both spheres at the ends of the capsule
// sphere 1
auto d1 = dot(n, c1) + pd;
auto r1 = INTERSECTS;
if(d1 > r)
{
r1 = INFRONT;
}
else if (d1 < -r)
{
r1 = BEHIND;
}
// sphere 2
auto d2 = dot(n, c2) + pd;
auto r2 = INTERSECTS;
if(d2 > r)
{
r2 = INFRONT;
}
else if (d2 < -r)
{
r2 = BEHIND;
}
// ..
if(r1 == r2) {
// if both speheres are the same, we return their classification this could give us infront, behind or intersects
return r1;
}
else {
// the else case means r1 != r2 and this means we are on opposite side of the plane or one of them intersects
return INTERSECTS;
}
}
// return the classification of cone defined by position cp, direction cv with height h and radius at the base of r. vs the plane defined by point x and normal n
inline classification cone_vs_plane(const vec3f& cp, const vec3f& cv, f32 h, f32 r, const vec3f& x, const vec3f& n)
{
auto l2 = cp + cv * h;
auto pd = maths::plane_distance(x, n);
// check if the tip and cones extent are on different sides of the plane
auto d1 = dot(n, cp) + pd;
// extent from the tip is at the base centre point perp of cv at the radius edge
auto perp = normalize(cross(cross(n, cv), cv));
auto extent = l2 + perp * r;
auto extent2 = l2 + perp * r * -1.0f;
// take left and right extent.
auto d2 = dot(n, extent) + pd;
auto d3 = dot(n, extent2) + pd;
// if tip and both extents lie on the same side, we are either infront or behind
if(d1 < 0.0f && d2 < 0.0f && d3 < 0.0f)
{
return maths::BEHIND;
}
else if(d1 > 0.0f && d2 > 0.0f && d3 > 0.0f)
{
return maths::INFRONT;
}
// otherwise we have points on either side of the plane meaning we intersect
return maths::INTERSECTS;
}
// returns true if sphere with centre s0 and radius r0 overlaps
// sphere with centre s1 and radius r1
inline bool sphere_vs_sphere(const vec3f& s0, f32 r0, const vec3f& s1, f32 r1)
{
f32 rr = r0 + r1;
f32 d = dist(s0, s1);
if (d < rr)
return true;
return false;
}
// returns true if sphere with centre s0 and radius r0 overlaps
// AABB defined by aabb_min and aabb_max extents
inline bool sphere_vs_aabb(const vec3f& s0, f32 r0, const vec3f& aabb_min, const vec3f& aabb_max)
{
vec3f cp = closest_point_on_aabb(s0, aabb_min, aabb_max);
f32 d = dist(cp, s0);
return d < r0;
}
// returns true if the sphere with centre s0 and radius r0 overlaps obb defined by matrix obb, where the matrix
// transforms a unit cube with extents -1 to 1 into an obb
inline bool sphere_vs_obb(const vec3f& s0, f32 r0, const mat4f& obb)
{
// test the distance to the closest point on the obb
vec3f cp = closest_point_on_obb(obb, s0);
if(dist2(s0, cp) < r0 * r0)
{
return true;
}
return false;
}
// returns true if the aabb's defined by min0,max0 and min1,max1 overlap
inline bool aabb_vs_aabb(const vec3f& min0, const vec3f& max0, const vec3f& min1, const vec3f& max1)
{
// discard non overlaps quickly
for (u32 i = 0; i < 3; ++i)
{
if (min0[i] > max1[i])
return false;
if (max0[i] < min1[i])
return false;
}
return true;
}
// returns true if the 3d aabb defined by aabb_min-aabb_max overlaps with the obb defined by matrix obb, where
// the matrix transforms a unit aabb with extents -1 to 1 into an obb
inline bool aabb_vs_obb(const vec3f& aabb_min, const vec3f& aabb_max, const mat4f& obb)
{
// this function is for convenience, you can extract vertices and pass to gjk_3d yourself
static const vec3f corners[8] = {
vec3f(-1.0f, -1.0f, -1.0f),
vec3f( 1.0f, -1.0f, -1.0f),
vec3f( 1.0f, 1.0f, -1.0f),
vec3f(-1.0f, 1.0f, -1.0f),
vec3f(-1.0f, -1.0f, 1.0f),
vec3f( 1.0f, -1.0f, 1.0f),
vec3f( 1.0f, 1.0f, 1.0f),
vec3f(-1.0f, 1.0f, 1.0f),
};
std::vector<vec3f> verts0 = {
aabb_min,
vec3f(aabb_min.x, aabb_min.y, aabb_max.z),
vec3f(aabb_max.x, aabb_min.y, aabb_min.z),
vec3f(aabb_max.x, aabb_min.y, aabb_max.z),
vec3f(aabb_min.x, aabb_max.y, aabb_min.z),
vec3f(aabb_min.x, aabb_max.y, aabb_max.z),
vec3f(aabb_max.x, aabb_max.y, aabb_min.z),
aabb_max
};
std::vector<vec3f> verts1;
for(u32 i = 0; i < 8; ++i)
{
verts1.push_back(obb.transform_vector(corners[i]));
}
return gjk_3d(verts0, verts1);
}
// returns true if the 3d obb defined by matrix obb0 and obb defined by matrix obb1 overlap, matrices will transform
// unit aabb with extents -1 to 1 into an obb
inline bool obb_vs_obb(const mat4f& obb0, const mat4f& obb1)
{
// this function is for convenience, you can extract vertices and pass to gjk_3d yourself
static const vec3f corners[8] = {
vec3f(-1.0f, -1.0f, -1.0f),
vec3f( 1.0f, -1.0f, -1.0f),
vec3f( 1.0f, 1.0f, -1.0f),
vec3f(-1.0f, 1.0f, -1.0f),
vec3f(-1.0f, -1.0f, 1.0f),
vec3f( 1.0f, -1.0f, 1.0f),
vec3f( 1.0f, 1.0f, 1.0f),
vec3f(-1.0f, 1.0f, 1.0f),
};
std::vector<vec3f> verts0;
std::vector<vec3f> verts1;
for(u32 i = 0; i < 8; ++i)
{
verts0.push_back(obb0.transform_vector(corners[i]));
verts1.push_back(obb1.transform_vector(corners[i]));
}
return gjk_3d(verts0, verts1);
}
// returns true if an aabb defined by aabb_pos (centre) and aabb_extent (half extent) is inside or intersecting the frustum
// defined by 6 planes (xyz = plane normal, w = plane constant / distance from origin)
// implemented via info detailed in this insightful blog post: https://fgiesen.wordpress.com/2010/10/17/view-frustum-culling
// sse/avx simd optimised variations can be found here: https://github.com/polymonster/pmtech/blob/master/core/put/source/ecs/ecs_cull.cpp
inline bool aabb_vs_frustum(const vec3f& aabb_pos, const vec3f& aabb_extent, vec4f* planes)
{
bool inside = true;
for (size_t p = 0; p < 6; ++p)
{
vec3f sign_flip = sgn(planes[p].xyz) * -1.0f;
f32 pd = planes[p].w;
f32 d2 = dot(aabb_pos + aabb_extent * sign_flip, planes[p].xyz);
if (d2 > -pd)
{
inside = false;
}
}
return inside;
}
// returns true if the sphere defined by pos and radius is inside or intersecting frustum
// defined by 6 planes (xyz = plane normal, w = plane constant / distance)
// sse/avx simd optimised variations can be found here: https://github.com/polymonster/pmtech/blob/master/core/put/source/ecs/ecs_cull.cpp
inline bool sphere_vs_frustum(const vec3f& pos, f32 radius, vec4f* planes)
{
for (size_t p = 0; p < 6; ++p)
{
f32 d = dot(pos, planes[p].xyz) + planes[p].w;
if (d > radius)
{
return false;
}
}
return true;
}
// returns true if the sphere with centre s0 and radius sr overlaps the capsule with line c0-c1 and radius cr
inline bool sphere_vs_capsule(const vec3f s0, f32 sr, const vec3f& cp0, const vec3f& cp1, f32 cr)
{
auto cp = closest_point_on_line(cp0, cp1, s0);
auto r2 = sqr(sr + cr);
return dist2(s0, cp) < r2;
}
// returns true if the capsule cp0-cp1 with radius cr0 overlaps the capsule cp2-cp3 with radius cr1
inline bool capsule_vs_capsule(const vec3f& cp0, const vec3f& cp1, f32 cr0, const vec3f& cp2, const vec3f& cp3, f32 cr1)
{
f32 r2 = (cr0 + cr1) * (cr0 + cr1);
// check shortest distance between the 2 capsule line segments, if less than the sq radius we overlap
vec3f start, end;
if(shortest_line_segment_between_line_segments(cp0, cp1, cp2, cp3, start, end))
{
f32 m = dist2(start, end);
if(m < r2)
{
return true;
}
}
else
{
// we must be orthogonal, which means there is no one single closest point between the 2 lines
// find the distance between the 2 axes
vec3f l0 = normalize(cp1 - cp0);
f32 t0 = dot(cp2 - cp0, l0);
vec3f ip0 = cp0 + l0 * t0;
f32 m0 = dist2(cp2, ip0);
// check axes afre within distance
if(m0 < r2)
{
vec3f l1 = normalize(cp3 - cp2);
f32 t1 = dot(cp0 - cp2, l1);
// now check if the axes overlap
if(t0 >= 0.0f && t0*t0 < dist2(cp1, cp0))
{
return true;
}
else if(t1 > 0.0f && t1*t1 < dist2(cp2, cp3))
{
return true;
}
}
}
return false;
}
// returns true if the convex hull hull0 overlaps hull1
inline bool convex_hull_vs_convex_hull(const std::vector<vec2f>& hull0, const std::vector<vec2f>& hull1)
{
return gjk_2d(hull0, hull1);
}
// returns true if sphere with centre s0 and radius r0 contains point p0
inline bool point_inside_sphere(const vec3f& s0, f32 r0, const vec3f& p0)
{
return dist2(p0, s0) < r0 * r0;
}
// returns true if point p0 is inside aabb defined by min and max extents
template<size_t N, typename T>
inline bool point_inside_aabb(const Vec<N, T>& min, const Vec<N, T>& max, const Vec<N, T>& p0)
{
for(size_t i = 0; i < N; ++i)
if(p0.v[i] < min.v[i] || p0.v[i] > max.v[i])
return false;
return true;
}
// returns if the point p is inside the obb defined by mat
// mat will transform an aabb centred at 0 with extents -1 to 1 into an obb
inline bool point_inside_obb(const mat4f& mat, const vec3f& p)
{
mat4f invm = mat::inverse4x4(mat);
vec3f tp = invm.transform_vector(vec4f(p, 1.0f)).xyz;
return point_inside_aabb(-vec3f::one(), vec3f::one(), tp);
}
// return true if point p is inside cone defined by position cp facing direction cv with height h and radius r
inline bool point_inside_cone(const vec3f& p, const vec3f& cp, const vec3f& cv, f32 h, f32 r)
{
vec3f l2 = cp + cv * h;
f32 dh = distance_on_line(cp, l2, p) / h;
vec3f x0 = closest_point_on_line(cp, l2, p);
f32 d = dist(x0, p);
if (d < dh * r && dh < 1.0f)
return true;
return false;
}
// return true if point p is inside convex hull defined by point list 'hull', with clockwise winding
// ... use convex_hull_from_points to generate a compatible convex hull from point cloud.
inline bool point_inside_convex_hull(const vec2f& p, const std::vector<vec2f>& hull)
{
vec3f p0 = vec3f(p.xy, 0.0f);
size_t ncp = hull.size();
for(size_t i = 0; i < ncp; ++i)
{
size_t i2 = (i+1)%ncp;
vec3f p1 = vec3f(hull[i].xy, 0.0f);
vec3f p2 = vec3f(hull[i2].xy, 0.0f);
vec3f v1 = p2 - p1;
vec3f v2 = p0 - p1;
if(cross(v2,v1).z > 0.0f)
return false;
}
return true;
}
// returns true if the point p is inside the polygon, which may be concave
// it even supports self intersections!
inline bool point_inside_poly(const vec2f& p, const std::vector<vec2f>& poly)
{
// copyright (c) 1970-2003, Wm. Randolph Franklin
// https://wrf.ecse.rpi.edu/Research/Short_Notes/pnpoly.html
intptr_t npol = (intptr_t)poly.size();
intptr_t i, j;
bool c = false;
for (i = 0, j = npol-1; i < npol; j = i++) {
if ((((poly[i].y <= p.y) && (p.y < poly[j].y)) ||
((poly[j].y <= p.y) && (p.y < poly[i].y))) &&
(p.x < (poly[j].x - poly[i].x) * (p.y - poly[i].y) / (poly[j].y - poly[i].y) + poly[i].x))
c = !c;
}
return c;
}
// returns true if point pos inside the frustum defined by 6 planes, xyz = normal, w = plane constant (distance)
inline bool point_inside_frustum(const vec3f& pos, vec4f* planes)
{
for (size_t p = 0; p < 6; ++p)
{
f32 d = dot(pos, planes[p].xyz) + planes[p].w;
if (d > 0.0f)
{
return false;
}
}
return true;
}
// returns the closest point from p0 on sphere s0 with radius r0
inline vec3f closest_point_on_sphere(const vec3f& s0, f32 r0, const vec3f& p0)
{
vec3f v = normalize(p0 - s0);
vec3f cp = s0 + v * r0;
return cp;
}
// returns closest point on aabb defined by aabb_min -> aabb_max to the point p0
template<size_t N, typename T>
inline Vec<N, T> closest_point_on_aabb(const Vec<N, T>& p0, const Vec<N, T>& aabb_min, const Vec<N, T>& aabb_max)
{
Vec<N, T> t1 = max_union(p0, aabb_min);
return min_union(t1, aabb_max);
}
// returns the closest point to p on the line segment l1-l2
template<size_t N, typename T>
inline Vec<N, T> closest_point_on_line(const Vec<N, T>& l1, const Vec<N, T>& l2, const Vec<N, T>& p)
{
Vec<N, T> v1 = p - l1;
Vec<N, T> v2 = normalize(l2 - l1);
T d = dist(l1, l2);
T t = dot(v2, v1);
if (t <= 0)
return l1;
if (t >= d)
return l2;
return l1 + v2 * t;
}
// returns the distance (t) of p along the line l1-l2
template<size_t N, typename T>
inline T distance_on_line(const Vec<N, T> & l1, const Vec<N, T> & l2, const Vec<N, T> & p)
{
Vec<N, T> v1 = p - l1;
Vec<N, T> v2 = normalize(l2 - l1);
return dot(v2, v1);
}