-
Notifications
You must be signed in to change notification settings - Fork 79
/
gs_physics.h
1711 lines (1420 loc) · 66.1 KB
/
gs_physics.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
/*================================================================
* Copyright: 2020 John Jackson
* GSPhysics: Simple 3D/2D physics engine
* File: gs_physics.h
All Rights Reserved
=================================================================*/
#ifndef GS_PHYSICS_H
#define GS_PHYSICS_H
/*
USAGE: (IMPORTANT)
=================================================================================================================
Before including, define the gunslinger physics implementation like this:
#define GS_PHYSICS_IMPL
in EXACTLY ONE C or C++ file that includes this header, BEFORE the
include, like this:
#define GS_PHYSICS_IMPL
#include "gs_physics.h"
All other files should just #include "gs_physics.h" without the #define.
MUST include "gs.h" and declare GS_IMPL BEFORE this file, since this file relies on that:
#define GS_IMPL
#include "gs.h"
#define GS_PHYSICS_IMPL
#include "gs_physics.h"
Special Thanks for reference:
- https://github.com/r-lyeh (for collision algorithms)
- https://gist.github.com/vurtun/95f088e4889da2474ad1ce82d7911fee (for GJK impl)
- Randy Gaul (for physics system inspiration)
- https://github.com/Nightmask3/Physics-Framework
- https://github.com/IainWinter/IwEngine/tree/master/IwEngine/src/physics
+
================================================================================================================
*/
/*==== Interface ====*/
/** @defgroup gs_physics_util Physics Util
* Gunslinger Physics Util
* @{
*/
/*==== Collision Shapes ====*/
// 3D shapes
typedef struct gs_line_t
{
gs_vec3 a, b;
} gs_line_t;
typedef struct gs_aabb_t
{
gs_vec3 min, max;
} gs_aabb_t;
typedef struct gs_sphere_t
{
gs_vec3 c; float r;
} gs_sphere_t;
typedef struct gs_plane_t
{
union{
gs_vec3 p;
struct{float a, b, c;};
};
float d;
} gs_plane_t;
typedef struct gs_capsule_t
{
gs_vec3 base;
float r, height;
} gs_capsule_t;
typedef struct gs_ray_s
{
gs_vec3 p, d;
float len;
} gs_ray_t;
typedef struct gs_poly_t
{
gs_vec3* verts; int32_t cnt;
} gs_poly_t;
typedef union gs_frustum_t
{
struct {
gs_vec4 l, r, t, b, n, f;
};
gs_vec4 pl[6];
float v[24];
} gs_frustum_t;
typedef struct gs_cylinder_t
{
float r;
gs_vec3 base;
float height;
} gs_cylinder_t;
typedef struct gs_cone_t
{
float r;
gs_vec3 base;
float height;
} gs_cone_t;
// 2D shapes
typedef struct gs_circle_t {float r; gs_vec2 c; } gs_circle_t;
// typedef struct gs_rect_t {gs_vec2 min; gs_vec2 max; } gs_rect_t;
// typedef struct gs_triangle_t {gs_vec2 a,b,c; } gs_triangle_t;
// typedef struct gs_pill_t {gs_vec2 base; float r, height; } gs_pill_t;
/*
typedef struct _gs_collision_obj_handle_t
{
void* obj;
gs_support_func_t support;
gs_vqs* xform;
} _gs_collision_obj_handle_t;
// Wrap this, then expose collision callback to user through gs means
// Internal support function for all ccd callbacks (will pass in user function through this cb),
// this way I can keep everything consistent, only expose gs related APIs, wrap any 3rd party libs,
// and possibly change internals in the future to custom implementations without breaking anything.
void _gs_ccd_support_func(const void* _obj, const ccd_vec3_t* _dir, ccd_vec3_t* _out)
{
const _gs_collision_obj_handle_t* obj = (const _gs_collision_obj_handle_t)_obj;
if (obj->support)
{
// Call user support function
gs_vec3 dir = _gs_ccdv32gsv3(_dir);
gs_vec3 out = gs_default_val();
obj->support(obj->obj, obj->xform, &dir, &out);
// Copy over out result for ccd
_gs_gsv32ccdv3(&out, _out);
}
}
*/
// Need 2d collision shapes and responses with GJK+EPA
typedef struct gs_hit_t {
int32_t hit;
union {
// General case
float depth;
// Rays: Penetration (t0) and Extraction (t1) pts. along ray line
struct {float t0, t1;};
// GJK only
struct {int32_t hits, iterations; gs_vec3 p0, p1; float distance2;};
};
union {gs_vec3 p; gs_vec3 contact_point;};
union {gs_vec3 n; gs_vec3 normal;};
} gs_hit_t;
// Constructors with args
#define gs_line(...) gs_ctor(gs_line_t, __VA_ARGS__)
#define gs_sphere(...) gs_ctor(gs_sphere_t, __VA_ARGS__)
#define gs_aabb(...) gs_ctor(gs_aabb_t, __VA_ARGS__)
#define gs_plane(...) gs_ctor(gs_plane_t, __VA_ARGS__)
#define gs_capsule(...) gs_ctor(gs_capsule_t, __VA_ARGS__)
#define gs_cone(...) gs_ctor(gs_cone_t, __VA_ARGS__)
#define gs_ray(...) gs_ctor(gs_ray_t, __VA_ARGS__)
#define gs_poly(...) gs_ctor(gs_poly_t, __VA_ARGS__)
#define gs_frustum(...). gs_ctor(gs_frustum_t, __VA_ARGS__)
#define gs_cylinder(...) gs_ctor(gs_cylinder_t, __VA_ARGS__)
#define gs_hit(...) gs_ctor(gs_hit_t, __VA_ARGS__)
// Contact info
typedef struct gs_contact_info_t {
int32_t hit;
gs_vec3 normal;
float depth;
gs_vec3 point;
} gs_contact_info_t;
/* Line/Segment */
GS_API_DECL gs_line_t gs_line_closest_line(const gs_line_t* l, gs_vec3 p);
GS_API_DECL gs_vec3 gs_line_closest_point(const gs_line_t* l, gs_vec3 p);
GS_API_DECL gs_vec3 gs_line_direction(const gs_line_t* l);
/* Ray */
/* Plane */
GS_API_DECL gs_plane_t gs_plane_from_pt_normal(gs_vec3 pt, gs_vec3 n);
GS_API_DECL gs_plane_t gs_plane_from_pts(gs_vec3 a, gs_vec3 b, gs_vec3 c);
GS_API_DECL gs_vec3 gs_plane_normal(const gs_plane_t* p);
GS_API_DECL gs_vec3 gs_plane_closest_point(const gs_plane_t* p, gs_vec3 pt);
GS_API_DECL float gs_plane_signed_distance(const gs_plane_t* p, gs_vec3 pt);
GS_API_DECL float gs_plane_unsigned_distance(const gs_plane_t* p, gs_vec3 pt);
GS_API_DECL gs_plane_t gs_plane_normalized(const gs_plane_t* p);
/* Sphere */
GS_API_DECL int32_t gs_sphere_vs_sphere(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_aabb(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_poly(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_cylinder(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_cone(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_capsule(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_sphere_vs_ray(const gs_sphere_t* a, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
/* Box */
GS_API_DECL int32_t gs_aabb_vs_aabb(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_sphere(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_poly(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_cylinder(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_cone(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_capsule(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_aabb_vs_ray(const gs_aabb_t* a, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
/* Capsule */
GS_API_DECL int32_t gs_capsule_vs_aabb(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_sphere(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_poly(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_cylinder(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_cone(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_capsule(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_capsule_vs_ray(const gs_capsule_t* capsule, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
/* Poly */
GS_API_DECL int32_t gs_poly_vs_poly(const gs_poly_t* a, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_sphere(const gs_poly_t* a, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_aabb(const gs_poly_t* a, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_cylinder(const gs_poly_t* a, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_cone(const gs_poly_t* a, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_capsule(const gs_poly_t* a, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_poly_vs_ray(const gs_poly_t* a, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
/* Frustum */
/* Cylinder */
GS_API_DECL int32_t gs_cylinder_vs_cylinder(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_sphere(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_aabb(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_poly(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_cone(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_capsule(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cylinder_vs_ray(const gs_cylinder_t* a, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
/* Cone */
GS_API_DECL int32_t gs_cone_vs_cone(const gs_cone_t* a, const gs_vqs* xform_a, const gs_cone_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_sphere(const gs_cone_t* a, const gs_vqs* xform_a, const gs_sphere_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_aabb(const gs_cone_t* a, const gs_vqs* xform_a, const gs_aabb_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_poly(const gs_cone_t* a, const gs_vqs* xform_a, const gs_poly_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_cylinder(const gs_cone_t* a, const gs_vqs* xform_a, const gs_cylinder_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_capsule(const gs_cone_t* a, const gs_vqs* xform_a, const gs_capsule_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
GS_API_DECL int32_t gs_cone_vs_ray(const gs_cone_t* a, const gs_vqs* xform_a, const gs_ray_t* b, const gs_vqs* xform_b, gs_contact_info_t* res);
// 2D Shapes (eventually)
/* Hit */
/*==== Support Functions ====*/
// Support function typedef for GJK collision detection
typedef void (* gs_support_func_t)(const void* collider, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_poly(const void* p, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_sphere(const void* s, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_aabb(const void* a, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_cylinder(const void* c, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_cone(const void* c, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_capsule(const void* c, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
GS_API_DECL void gs_support_ray(const void* r, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out);
/*==== GJK ====*/
#define GS_GJK_FLT_MAX FLT_MAX // 3.40282347E+38F
#define GS_GJK_EPSILON FLT_EPSILON // 1.19209290E-07F
#define GS_GJK_MAX_ITERATIONS 64
#define GS_EPA_TOLERANCE 0.0001
#define GS_EPA_MAX_NUM_FACES 64
#define GS_EPA_MAX_NUM_LOOSE_EDGES 32
#define GS_EPA_MAX_NUM_ITERATIONS 64
typedef enum gs_gjk_dimension {
GS_GJK_DIMENSION_2D,
GS_GJK_DIMENSION_3D
} gs_gjk_dimension;
typedef struct gs_gjk_support_point_t {
gs_vec3 support_a;
gs_vec3 support_b;
gs_vec3 minkowski_hull_vert;
} gs_gjk_support_point_t;
typedef struct gs_gjk_simplex_t {
union {
gs_gjk_support_point_t points[4];
struct {
gs_gjk_support_point_t a;
gs_gjk_support_point_t b;
gs_gjk_support_point_t c;
gs_gjk_support_point_t d;
};
};
uint32_t ct;
} gs_gjk_simplex_t;
typedef struct gs_gjk_polytope_face_t {
gs_gjk_support_point_t points[3];
gs_vec3 normal;
} gs_gjk_polytope_face_t;
typedef struct gs_gjk_epa_edge_t {
gs_vec3 normal;
uint32_t index;
float distance;
gs_gjk_support_point_t a, b;
} gs_gjk_epa_edge_t;
// GS_API_DECL int32_t gs_gjk(const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1, gs_gjk_dimension dimension, gs_contact_info_t* res);
// GS_API_DECL gs_contact_info_t gs_gjk_epa(const gs_gjk_simplex_t* simplex, const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1);
// GS_API_DECL gs_contact_info_t gs_gjk_epa_2d(const gs_gjk_simplex_t* simplex, const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1);
// GS_API_DECL gs_gjk_collider_info_t gs_gjk_collider_info(void* c, gs_support_func_t f, gs_phys_xform_t* t);
//
GS_API_PRIVATE int32_t _gs_ccd_gjk_internal(const void* c0, const gs_vqs* xform_a, gs_support_func_t f0, const void* c1, const gs_vqs* xform_b, gs_support_func_t f1, gs_contact_info_t* res);
/*==== CCD ====*/
#ifndef GS_PHYSICS_NO_CCD
#include "../external/ccd/src/ccd/ccd_vec3.h"
// Internal collision object conversion handle
typedef struct _gs_collision_obj_handle_t
{
const void* obj;
gs_support_func_t support;
const gs_vqs* xform;
} _gs_collision_obj_handle_t;
// Internal support function for all ccd callbacks (will pass in user function through this cb),
// this way I can keep everything consistent, only expose gs related APIs, wrap any 3rd party libs,
// and possibly change internals in the future to custom implementations without breaking anything.
GS_API_DECL void _gs_ccd_support_func(const void* _obj, const ccd_vec3_t* _dir, ccd_vec3_t* _out);
GS_API_DECL int32_t _gs_ccd_gjk(const _gs_collision_obj_handle_t* c0, const _gs_collision_obj_handle_t* c1, gs_contact_info_t* res);
#endif // GS_PHYSICS_NO_CCD
/*==== Physics System ====*/
#define EMPTY_VAR(...) int32_t __empty
/* Rigid Body */
// Just want a single collision shape...not sure exactly how to handle this.
// Probably keep the data for the collision shape within the scene itself (or physics system)? Then give handles to users for the data.
typedef enum gs_rigid_body_type {
GS_RIGID_BODY_STATIC,
GS_RIGID_BODY_DYNAMIC,
GS_RIGID_BODY_KINEMATIC
} gs_rigid_body_type;
typedef enum gs_rigid_body_state_flags {
GS_RIGID_BODY_STATE_AWAKE = 0x001,
GS_RIGID_BODY_STATE_ACTIVE = 0x002,
GS_RIGID_BODY_STATE_ALLOW_SLEEP = 0x004,
GS_RIGID_BODY_STATE_ISLAND = 0x010,
GS_RIGID_BODY_STATE_STATIC = 0x020,
GS_RIGID_BODY_STATE_DYNAMIC = 0x040,
GS_RIGID_BODY_STATE_KINEMATIC = 0x080,
GS_RIGID_BODY_STATE_LOCK_AXIS_X = 0x100,
GS_RIGID_BODY_STATE_LOCK_AXIS_Y = 0x200,
GS_RIGID_BODY_STATE_LOCK_AXIS_Z = 0x400
} gs_rigid_body_state_flags;
typedef struct gs_rigid_body_t {
EMPTY_VAR();
float mass;
float inverve_mass;
gs_vec3 linear_velocity;
gs_vec3 angular_velocity;
gs_vec3 force;
gs_vec3 torque;
gs_quat rotation;
gs_vec3 local_center;
gs_vec3 world_center;
float sleep_time;
float gravity_scale;
uint32_t flags;
uint32_t island_index;
void * user_data;
} gs_rigid_body_t;
typedef struct gs_rigid_body_desc_t {
EMPTY_VAR();
} gs_rigid_body_desc_t;
/* Collision Shape */
typedef struct gs_collision_shape_t {
EMPTY_VAR();
} gs_collision_shape_t;
typedef struct gs_contact_constraint_t {
EMPTY_VAR();
} gs_contact_constraint_t;
typedef struct gs_raycast_data_t {
EMPTY_VAR();
} gs_raycast_data_t;
// The listener is used to gather information about two shapes colliding. Physics objects created in these callbacks
// Are not reported until following frame. Callbacks can be called quite frequently, so make them efficient.
typedef struct gs_contact_listener_t {
void (* begin_contact)(const gs_contact_constraint_t*);
void (* end_contact)(const gs_contact_constraint_t*);
} gs_contact_listener_t;
typedef struct gs_query_callback_t {
bool (* report_shape)(gs_collision_shape_t* shape);
} gs_query_callback_t;
// Contact Manager
typedef struct gs_physics_contact_manager_t {
EMPTY_VAR();
} gs_physics_contact_manager_t;
/* Physics Scene */
typedef struct gs_physics_scene_t {
gs_physics_contact_manager_t contact_manager;
gs_paged_allocator_t paged_allocator;
gs_stack_allocator_t stack_allocator;
gs_heap_allocator_t heap_allocator;
gs_vec3 gravity;
float delta_time;
uint32_t iterations;
} gs_physics_scene_t;
GS_API_DECL gs_physics_scene_t gs_physics_scene_new();
GS_API_DECL void gs_physics_scene_step(gs_physics_scene_t* scene);
GS_API_DECL uint32_t gs_physics_scene_create_body(gs_physics_scene_t* scene, gs_rigid_body_desc_t* desc);
GS_API_DECL void gs_physics_scene_destroy_body(gs_physics_scene_t* scene, uint32_t id);
GS_API_DECL void gs_physics_scene_destroy_all_bodies(gs_physics_scene_t* scene);
GS_API_DECL gs_raycast_data_t gs_physics_scene_raycast(gs_physics_scene_t* scene, gs_query_callback_t* cb);
/*
Scene
Contact Manager
Collision Solver
Dynamics Engine
Rigid Body
Collision Shape
*/
/** @} */ // end of gs_physics_util
/*==== Implementation ====*/
#ifdef GS_PHYSICS_IMPL
// Includes
#ifndef GS_PHYSICS_NO_CCD
#include "../external/ccd/libccd.c"
#endif
/*==== Support Functions =====*/
// Poly
GS_API_DECL void gs_support_poly(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
const gs_poly_t* p = (gs_poly_t*)_o;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
// Iterate over all points, find dot farthest in direction of d
double max_dot, dot = 0.0;
max_dot = (double)-FLT_MAX;
for (uint32_t i = 0; i < p->cnt; i++)
{
dot = (double)gs_vec3_dot(d, p->verts[i]);
if (dot > max_dot) {
*out = p->verts[i];
max_dot = dot;
}
}
// Transform support point by rotation and translation of object
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_mul(xform->scale, *out);
*out = gs_vec3_add(xform->position, *out);
}
// Sphere
GS_API_DECL void gs_support_sphere(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
// Support function is made according to Gino van den Bergen's paper
// A Fast and Robust CCD Implementation for Collision Detection of
// Convex Objects
const gs_sphere_t* s = (gs_sphere_t*)_o;
float scl = gs_max(xform->scale.x, gs_max(xform->scale.z, xform->scale.y));
*out = gs_vec3_add(gs_vec3_scale(gs_vec3_norm(*dir), scl * s->r), gs_vec3_add(xform->position, s->c));
}
// Ray
GS_API_DECL void gs_support_ray(const void* _r, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
const gs_ray_t* r = (gs_ray_t*)_r;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
gs_vec3 rs = r->p;
gs_vec3 re = gs_vec3_add(r->p, gs_vec3_scale(r->d, r->len));
if (gs_vec3_dot(rs, d) > gs_vec3_dot(re, d)) {
*out = rs;
} else {
*out = re;
}
// Transform support point by rotation and translation of object
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_mul(xform->scale, *out);
*out = gs_vec3_add(xform->position, *out);
}
// AABB
GS_API_DECL void gs_support_aabb(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
const gs_aabb_t* a = (gs_aabb_t*)_o;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
// Compute half coordinates and sign for aabb (scale by transform)
const float hx = (a->max.x - a->min.x) * 0.5f * xform->scale.x;
const float hy = (a->max.y - a->min.y) * 0.5f * xform->scale.y;
const float hz = (a->max.z - a->min.z) * 0.5f * xform->scale.z;
gs_vec3 s = gs_vec3_sign(d);
// Compure support for aabb
*out = gs_v3(s.x * hx, s.y * hy, s.z * hz);
// Transform support point by rotation and translation of object
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_add(xform->position, *out);
}
// Cylinder
GS_API_DECL void gs_support_cylinder(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
// Support function is made according to Gino van den Bergen's paper
// A Fast and Robust CCD Implementation for Collision Detection of
// Convex Objects
const gs_cylinder_t* c = (const gs_cylinder_t*)_o;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
// Compute support point (cylinder is standing on y axis, half height at origin)
double zdist = sqrt(d.x * d.x + d.z * d.z);
double hh = (double)c->height * 0.5 * xform->scale.y;
if (zdist == 0.0)
{
*out = gs_v3(0.0, gs_vec3_signY(d) * hh, 0.0);
}
else
{
double r = (double)c->r / zdist;
*out = gs_v3(r * d.x * xform->scale.x, gs_vec3_signY(d) * hh, r * d.z * xform->scale.z);
}
// Transform support point into world space
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_add(gs_vec3_add(xform->position, c->base), *out);
}
// Capsule
GS_API_DECL void gs_support_capsule(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
const gs_capsule_t* c = (const gs_capsule_t*)_o;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
// Compute support point (cone is standing on y axis, half height at origin)
const float s = gs_max(xform->scale.x, xform->scale.z);
*out = gs_vec3_scale(gs_vec3_norm(d), c->r * s);
double hh = (double)c->height * 0.5 * xform->scale.y;
if (gs_vec3_dot(d, GS_YAXIS) > 0.0) {
out->y += hh;
} else {
out->y -= hh;
}
// Transform support point into world space
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_add(gs_vec3_add(xform->position, c->base), *out);
}
// Cone
GS_API_DECL void gs_support_cone(const void* _o, const gs_vqs* xform, const gs_vec3* dir, gs_vec3* out)
{
const gs_cone_t* c = (const gs_cone_t*)_o;
// Bring direction vector into rotation space
gs_quat qinv = gs_quat_inverse(xform->rotation);
gs_vec3 d = gs_quat_rotate(qinv, *dir);
// Compute support point (cone is standing on y axis, half height at origin)
double sin_angle = c->r / sqrt((double)c->r * (double)c->r + (double)c->height * (double)c->height);
double hh = (double)c->height * 0.5 * xform->scale.y;
double len = sqrt(gs_vec3_len2(d));
if (d.y > len * sin_angle)
{
*out = gs_v3(0.0f, (float)hh, 0.0f);
}
else
{
double s = sqrt(d.x * d.x + d.z * d.z);
if (s > (double)GS_GJK_EPSILON)
{
double _d = (double)c->r / s;
*out = gs_v3(d.x * _d * xform->scale.x, (float)-hh, d.z * _d * xform->scale.z);
}
else
{
*out = gs_v3(0.0, (float)-hh, 0.0);
}
}
// Transform support point into world space
*out = gs_quat_rotate(xform->rotation, *out);
*out = gs_vec3_add(gs_vec3_add(xform->position, c->base), *out);
}
/*==== Collision Shapes ====*/
/* Line/Segment */
GS_API_DECL gs_line_t gs_line_closest_line(const gs_line_t* l, gs_vec3 p)
{
gs_vec3 cp = gs_line_closest_point(l, p);
return gs_line(p, cp);
}
GS_API_DECL gs_vec3 gs_line_closest_point(const gs_line_t* l, gs_vec3 p)
{
gs_vec3 pt = gs_default_val();
gs_vec3 ab = gs_vec3_sub(l->b, l->a);
gs_vec3 pa = gs_vec3_sub(p, l->a);
float t = gs_vec3_dot(pa, ab) / gs_vec3_dot(ab, ab);
t = gs_clamp(t, 0.f, 1.f);
pt = gs_vec3_add(l->a, gs_vec3_scale(ab, t));
return pt;
}
GS_API_DECL gs_vec3 gs_line_direction(const gs_line_t* l)
{
return gs_vec3_norm(gs_vec3_sub(l->b, l->a));
}
/* Plane */
// Modified from: https://graphics.stanford.edu/~mdfisher/Code/Engine/Plane.cpp.html
GS_API_DECL gs_plane_t gs_plane_from_pt_normal(gs_vec3 pt, gs_vec3 n)
{
gs_plane_t p = gs_default_val();
gs_vec3 nn = gs_vec3_norm(n);
p.a = nn.x; p.b = nn.y; p.c = nn.z;
p.d = -gs_vec3_dot(pt, nn);
return p;
}
GS_API_DECL gs_plane_t gs_plane_from_pts(gs_vec3 a, gs_vec3 b, gs_vec3 c)
{
gs_vec3 n = gs_vec3_norm(gs_vec3_cross(gs_vec3_sub(b, a), gs_vec3_sub(c, a)));
return gs_plane_from_pt_normal(a, n);
}
GS_API_DECL gs_vec3 gs_plane_normal(const gs_plane_t* p)
{
return gs_vec3_norm(gs_v3(p->a, p->b, p->c));
}
GS_API_DECL gs_vec3 gs_plane_closest_point(const gs_plane_t* p, gs_vec3 pt)
{
return gs_vec3_sub(pt, gs_vec3_scale(gs_plane_normal(p), gs_plane_signed_distance(p, pt)));
}
GS_API_DECL float gs_plane_signed_distance(const gs_plane_t* p, gs_vec3 pt)
{
return (p->a * pt.x + p->b * pt.y + p->c * pt.z + p->d);
}
GS_API_DECL float gs_plane_unsigned_distance(const gs_plane_t* p, gs_vec3 pt)
{
return fabsf(gs_plane_signed_distance(p, pt));
}
GS_API_DECL gs_plane_t gs_plane_normalized(const gs_plane_t* p)
{
gs_plane_t pn = gs_default_val();
float d = sqrtf(p->a * p->a + p->b * p->b + p->c * p->c);
pn.a = p->a / d; pn.b = p->b / d; pn.c = p->c / d; pn.d = p->d / d;
return pn;
}
GS_API_DECL int32_t gs_plane_vs_sphere(const gs_plane_t* a, gs_vqs* xform_a, const gs_sphere_t* b, gs_vqs* xform_b, struct gs_contact_info_t* res)
{
// Cache necesary transforms, matrices
gs_mat4 mat = xform_a ? gs_vqs_to_mat4(xform_a) : gs_mat4_identity();
gs_mat4 inv = xform_a ? gs_mat4_inverse(mat) : gs_mat4_identity();
gs_vqs local = gs_vqs_relative_transform(xform_a, xform_b);
// Transform sphere center into local cone space
gs_vec3 sc = xform_a ? gs_mat4_mul_vec3(inv, xform_b ? gs_vec3_add(xform_b->position, b->c) : b->c) : b->c;
// Determine closest point from sphere center to plane
gs_vec3 cp = gs_plane_closest_point(a, sc);
// Determine if sphere is intersecting this point
float sb = xform_b ? gs_max(local.scale.x, gs_max(local.scale.y, local.scale.z)) : 1.f;
gs_vec3 dir = gs_vec3_sub(cp, sc);
gs_vec3 n = gs_vec3_norm(dir);
float d = gs_vec3_len(dir);
float r = sb * b->r;
if (d > r)
{
return false;
}
// Construct contact information
if (res)
{
res->hit = true;
res->depth = (r - d);
res->normal = gs_mat4_mul_vec3(mat, n);
res->point = gs_mat4_mul_vec3(mat, cp);
}
return true;
}
/* Ray */
/* Frustum */
/* Hit */
#define _GS_COLLIDE_FUNC_IMPL(_TA, _TB, _F0, _F1)\
GS_API_DECL int32_t gs_##_TA##_vs_##_TB(const gs_##_TA##_t* a, const gs_vqs* xa, const gs_##_TB##_t* b, const gs_vqs* xb, gs_contact_info_t* r)\
{\
return _gs_ccd_gjk_internal(a, xa, (_F0), b, xb, (_F1), r);\
}
/* Sphere */
_GS_COLLIDE_FUNC_IMPL(sphere, sphere, gs_support_sphere, gs_support_sphere); // Sphere vs. Sphere
_GS_COLLIDE_FUNC_IMPL(sphere, cylinder, gs_support_sphere, gs_support_cylinder); // Sphere vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(sphere, cone, gs_support_sphere, gs_support_cone); // Sphere vs. Cone
_GS_COLLIDE_FUNC_IMPL(sphere, aabb, gs_support_sphere, gs_support_aabb); // Sphere vs. AABB
_GS_COLLIDE_FUNC_IMPL(sphere, capsule, gs_support_sphere, gs_support_capsule); // Sphere vs. Capsule
_GS_COLLIDE_FUNC_IMPL(sphere, poly, gs_support_sphere, gs_support_poly); // Sphere vs. Poly
_GS_COLLIDE_FUNC_IMPL(sphere, ray, gs_support_sphere, gs_support_ray); // Sphere vs. Ray
/* AABB */
_GS_COLLIDE_FUNC_IMPL(aabb, aabb, gs_support_aabb, gs_support_aabb); // AABB vs. AABB
_GS_COLLIDE_FUNC_IMPL(aabb, cylinder, gs_support_aabb, gs_support_cylinder); // AABB vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(aabb, cone, gs_support_aabb, gs_support_cone); // AABB vs. Cone
_GS_COLLIDE_FUNC_IMPL(aabb, sphere, gs_support_aabb, gs_support_sphere); // AABB vs. Sphere
_GS_COLLIDE_FUNC_IMPL(aabb, capsule, gs_support_aabb, gs_support_capsule); // AABB vs. Capsule
_GS_COLLIDE_FUNC_IMPL(aabb, poly, gs_support_aabb, gs_support_poly); // AABB vs. Poly
_GS_COLLIDE_FUNC_IMPL(aabb, ray, gs_support_aabb, gs_support_ray); // AABB vs. Ray
/* Capsule */
_GS_COLLIDE_FUNC_IMPL(capsule, capsule, gs_support_capsule, gs_support_capsule); // Capsule vs. Capsule
_GS_COLLIDE_FUNC_IMPL(capsule, cylinder, gs_support_capsule, gs_support_cylinder); // Capsule vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(capsule, cone, gs_support_capsule, gs_support_cone); // Capsule vs. Cone
_GS_COLLIDE_FUNC_IMPL(capsule, sphere, gs_support_capsule, gs_support_sphere); // Capsule vs. Sphere
_GS_COLLIDE_FUNC_IMPL(capsule, aabb, gs_support_capsule, gs_support_aabb); // Capsule vs. AABB
_GS_COLLIDE_FUNC_IMPL(capsule, poly, gs_support_capsule, gs_support_poly); // Capsule vs. Poly
_GS_COLLIDE_FUNC_IMPL(capsule, ray, gs_support_capsule, gs_support_ray); // Capsule vs. Ray
/* Poly */
_GS_COLLIDE_FUNC_IMPL(poly, poly, gs_support_poly, gs_support_poly); // Poly vs. Poly
_GS_COLLIDE_FUNC_IMPL(poly, cylinder, gs_support_poly, gs_support_cylinder); // Poly vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(poly, cone, gs_support_poly, gs_support_cone); // Poly vs. Cone
_GS_COLLIDE_FUNC_IMPL(poly, sphere, gs_support_poly, gs_support_sphere); // Poly vs. Sphere
_GS_COLLIDE_FUNC_IMPL(poly, aabb, gs_support_poly, gs_support_aabb); // Poly vs. AABB
_GS_COLLIDE_FUNC_IMPL(poly, capsule, gs_support_poly, gs_support_capsule); // Poly vs. Capsule
_GS_COLLIDE_FUNC_IMPL(poly, ray, gs_support_poly, gs_support_ray); // Poly vs. Ray
/* Cylinder */
_GS_COLLIDE_FUNC_IMPL(cylinder, cylinder, gs_support_cylinder, gs_support_cylinder); // Cylinder vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(cylinder, poly, gs_support_poly, gs_support_poly); // Cylinder vs. Poly
_GS_COLLIDE_FUNC_IMPL(cylinder, cone, gs_support_cylinder, gs_support_cone); // Cylinder vs. Cone
_GS_COLLIDE_FUNC_IMPL(cylinder, sphere, gs_support_cylinder, gs_support_sphere); // Cylinder vs. Sphere
_GS_COLLIDE_FUNC_IMPL(cylinder, aabb, gs_support_cylinder, gs_support_aabb); // Cylinder vs. AABB
_GS_COLLIDE_FUNC_IMPL(cylinder, capsule, gs_support_cylinder, gs_support_capsule); // Cylinder vs. Capsule
_GS_COLLIDE_FUNC_IMPL(cylinder, ray, gs_support_cylinder, gs_support_ray); // Cylinder vs. Ray
/* Cone */
_GS_COLLIDE_FUNC_IMPL(cone, cone, gs_support_cone, gs_support_cone); // Cone vs. Cone
_GS_COLLIDE_FUNC_IMPL(cone, poly, gs_support_poly, gs_support_poly); // Cone vs. Poly
_GS_COLLIDE_FUNC_IMPL(cone, cylinder, gs_support_cone, gs_support_cylinder); // Cone vs. Cylinder
_GS_COLLIDE_FUNC_IMPL(cone, sphere, gs_support_cone, gs_support_sphere); // Cone vs. Sphere
_GS_COLLIDE_FUNC_IMPL(cone, aabb, gs_support_cone, gs_support_aabb); // Cone vs. AABB
_GS_COLLIDE_FUNC_IMPL(cone, capsule, gs_support_cone, gs_support_capsule); // Cone vs. Capsule
_GS_COLLIDE_FUNC_IMPL(cone, ray, gs_support_cone, gs_support_ray); // Cone vs. Ray
/*==== GKJ ====*/
// Need 2D GJK/EPA impl in external (modify from chipmunk 2d)
// Internal functions
/*
bool gs_gjk_check_if_simplex_contains_origin(gs_gjk_simplex_t* simplex, gs_vec3* search_dir, gs_gjk_dimension dimension);
void gs_gjk_simplex_push(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p);
void gs_gjk_simplex_push_back(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p);
void gs_gjk_simplex_push_front(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p);
void gs_gjk_simplex_insert(gs_gjk_simplex_t* simplex, uint32_t idx, gs_gjk_support_point_t p);
void gs_gjk_bary(gs_vec3 p, gs_vec3 a, gs_vec3 b, gs_vec3 c, float* u, float* v, float* w);
// gs_gjk_epa_edge_t gs_gjk_epa_find_closest_edge(gs_gjk_simplex_t* simplex);
gs_gjk_support_point_t gs_gjk_generate_support_point(const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1, gs_vec3 dir);
gs_gjk_epa_edge_t gs_gjk_epa_find_closest_edge(gs_dyn_array(gs_gjk_support_point_t) polytope);
// Modified from: https://github.com/Nightmask3/Physics-Framework/blob/master/PhysicsFramework/PhysicsManager.cpp
int32_t gs_gjk(const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1, gs_gjk_dimension dimension, gs_contact_info_t* res)
{
// Simplex simplex;
gs_gjk_simplex_t simplex = gs_default_val();
gs_vec3 search_dir = gs_v3s(1.f);
gs_gjk_support_point_t new_pt = gs_gjk_generate_support_point(ci0, ci1, search_dir);
// Stability check
if (gs_vec3_dot(search_dir, new_pt.minkowski_hull_vert) >= gs_vec3_len(new_pt.minkowski_hull_vert) * 0.8f)
{
// the chosen direction is invalid, will produce (0,0,0) for a subsequent direction later
search_dir = gs_v3(0.f, 1.f, 0.f);
new_pt = gs_gjk_generate_support_point(ci0, ci1, search_dir);
}
gs_gjk_simplex_push(&simplex, new_pt);
// Invert the search direction for the next point
search_dir = gs_vec3_neg(search_dir);
uint32_t iterationCount = 0;
while (true)
{
if (iterationCount++ >= GS_GJK_MAX_ITERATIONS)
return false;
// Stability check
// Error, for some reason the direction vector is broken
if (gs_vec3_len(search_dir) <= 0.0001f)
return false;
// Add a new point to the simplex
gs_gjk_support_point_t new_pt = gs_gjk_generate_support_point(ci0, ci1, search_dir);
gs_gjk_simplex_push(&simplex, new_pt);
// If projection of newly added point along the search direction has not crossed the origin,
// the Minkowski Difference could not contain the origin, objects are not colliding
if (gs_vec3_dot(new_pt.minkowski_hull_vert, search_dir) < 0.0f)
{
return false;
}
else
{
// If the new point IS past the origin, check if the simplex contains the origin,
// If it doesn't modify search direction to point towards to origin
if (gs_gjk_check_if_simplex_contains_origin(&simplex, &search_dir, dimension))
{
// Capture collision data using EPA if requested by user
if (res)
{
switch (dimension) {
case GS_GJK_DIMENSION_3D: *res = gs_gjk_epa(&simplex, ci0, ci1); break;
case GS_GJK_DIMENSION_2D: *res = gs_gjk_epa_2d(&simplex, ci0, ci1); break;
}
return res->hit;
}
else
{
return true;
}
}
}
}
}
GS_API_DECL gs_gjk_support_point_t gs_gjk_generate_support_point(const gs_gjk_collider_info_t* ci0, const gs_gjk_collider_info_t* ci1, gs_vec3 dir)
{
gs_gjk_support_point_t sp = {0};
sp.support_a = ci0->func(ci0->collider, ci0->xform, dir);
sp.support_b = ci1->func(ci1->collider, ci1->xform, gs_vec3_neg(dir));
sp.minkowski_hull_vert = gs_vec3_sub(sp.support_a, sp.support_b);
return sp;
}
// Closest point method taken from Erin Catto's GDC 2010 slides
// Returns the closest point
gs_vec3 gs_closest_point_on_line_from_target_point(gs_line_t line, gs_vec3 point, float* u, float* v)
{
gs_vec3 line_seg = gs_vec3_sub(line.b, line.a);
gs_vec3 normalized = gs_vec3_norm(line_seg);
*v = gs_vec3_dot(gs_vec3_neg(line.a), normalized) / gs_vec3_len(line_seg);
*u = gs_vec3_dot(line.b, normalized) / gs_vec3_len(line_seg);
gs_vec3 closest_point;
if (*u <= 0.0f)
{
closest_point = line.b;
}
else if (*v <= 0.0f)
{
closest_point = line.a;
}
else
{
closest_point = gs_vec3_add(gs_vec3_scale(line.a, *u), gs_vec3_scale(line.b, *v));
}
return closest_point;
}
void gs_gjk_simplex_push(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p)
{
simplex->ct = gs_min(simplex->ct + 1, 4);
for (int32_t i = simplex->ct - 1; i > 0; i--)
simplex->points[i] = simplex->points[i - 1];
simplex->points[0] = p;
}
void gs_gjk_simplex_push_back(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p)
{
if (simplex->ct >= 4) return;
simplex->ct = gs_min(simplex->ct + 1, 4);
simplex->points[simplex->ct - 1] = p;
}
void gs_gjk_simplex_push_front(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t p)
{
if (simplex->ct == 3) {
simplex->points[3] = simplex->points[2];
simplex->points[2] = simplex->points[1];
simplex->points[1] = simplex->points[0];
simplex->points[0] = p;
}
else if (simplex->ct == 2) {
simplex->points[2] = simplex->points[1];
simplex->points[1] = simplex->points[0];
simplex->points[0] = p;
}
simplex->ct = gs_min(simplex->ct + 1, 4);
}
void gs_gjk_simplex_insert(gs_gjk_simplex_t* simplex, uint32_t idx, gs_gjk_support_point_t p)
{
// Need more points (this is where polytope comes into play, I think...)
// Splice the simplex by index
if (idx > 4) return;
simplex->ct = gs_min(simplex->ct + 1, 4);
for (int32_t i = simplex->ct - 1; i > idx; i--)
simplex->points[i] = simplex->points[i - 1];
simplex->points[idx] = p;