gunslinger/util/gs_physics.h

/*==============================================================================================================
	* Copyright (c) 2020 John Jackson
    * GSPhysics: Simple 3D/2D physics engine
    * File: gs_physics.h
    * Github: https://github.com/MrFrenik/gunslinger

    * All Rights Reserved

    * MIT License

    * May all those that this source may reach be blessed by the LORD and find peace and joy in life.

    * Everyone who drinks of this water will be thirsty again; but whoever drinks of the water
    * that I will give him shall never thirst; John 4:13-14

    * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
    * documentation files (the "Software"), to deal in the Software without restriction, including without limitation
    * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, 
    * and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 

    * The above copyright, blessing, biblical verse, notice and this permission notice shall be included in all 
    * copies or substantial portions of the Software.

    * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
    * TO THE WARRANTIES OF MECHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
    * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
    * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
    * IN THE SOFTWARE.  
    
=================================================================================================================*/

#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;
}

void gs_gjk_simplex_clear(gs_gjk_simplex_t* simplex)
{
    simplex->ct = 0;
}

void gs_gjk_simplex_set1(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t a)
{
    simplex->ct = 1;
    simplex->a = a;
}

void gs_gjk_simplex_set2(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t a, gs_gjk_support_point_t b)
{
    simplex->ct = 2;
    simplex->a = a;
    simplex->b = b;
}

void gs_gjk_simplex_set3(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t a, gs_gjk_support_point_t b, gs_gjk_support_point_t c)
{
    simplex->ct = 3;
    simplex->a = a;
    simplex->b = b;
    simplex->c = c;
}

void gs_gjk_simplex_set4(gs_gjk_simplex_t* simplex, gs_gjk_support_point_t a, gs_gjk_support_point_t b, gs_gjk_support_point_t c, gs_gjk_support_point_t d)
{
    simplex->ct = 4;
    simplex->a = a;
    simplex->b = b;
    simplex->c = c;
    simplex->d = d;
}

bool gs_gjk_check_if_simplex_contains_origin(gs_gjk_simplex_t* simplex, gs_vec3* search_dir, gs_gjk_dimension dimension)
{
    // Line case
    if (simplex->ct == 2)
    {
        // Line cannot contain the origin, only search for the direction to it 
        gs_vec3 new_point_to_old_point = gs_vec3_sub(simplex->b.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
        gs_vec3 new_point_to_origin = gs_vec3_neg(simplex->a.minkowski_hull_vert);

        // Method given by Erin Catto in GDC 2010 presentation
        float u = 0.0f, v = 0.0f;
        gs_line_t line = gs_line(simplex->a.minkowski_hull_vert, simplex->b.minkowski_hull_vert);
        gs_vec3 origin = gs_v3s(0.f);
        gs_vec3 closest_point = gs_closest_point_on_line_from_target_point(line, origin, &u, &v);
    
        // Test vertex region of new simplex point first as highest chance to be there
        if (v <= 0.0f)
        {
            gs_gjk_simplex_set1(simplex, simplex->a);
            *search_dir = gs_vec3_neg(closest_point);
            return false;
        }
        else if (u <= 0.0f)
        {
            gs_gjk_simplex_set1(simplex, simplex->b);
            *search_dir = gs_vec3_neg(closest_point);
            return false;
        }
        else
        {
            *search_dir = gs_vec3_neg(closest_point);
            return false;
        }
    }

    // Triangle case
    else if (simplex->ct == 3)
    {
        // Find the newly added features
        gs_vec3 new_point_to_origin = gs_vec3_neg(simplex->a.minkowski_hull_vert);
        gs_vec3 edge1 = gs_vec3_sub(simplex->b.minkowski_hull_vert, simplex->a.minkowski_hull_vert);    // AB
        gs_vec3 edge2 = gs_vec3_sub(simplex->c.minkowski_hull_vert, simplex->a.minkowski_hull_vert);    // AC
        // Find the normals to the triangle and the two edges
        gs_vec3 triangle_normal = gs_vec3_cross(edge1, edge2);  // ABC
        gs_vec3 edge1_normal = gs_vec3_cross(edge1, triangle_normal);
        gs_vec3 edge2_normal = gs_vec3_cross(triangle_normal, edge2);

        // If origin is closer to the area along the second edge normal (if same_dir(AB X ABC, -A))
        if (gs_vec3_dot(edge2_normal, new_point_to_origin) > 0.0f)
        {
            // If closer to the edge then find the normal that points towards the origin
            if (gs_vec3_dot(edge2, new_point_to_origin) > 0.0f)
            {
                // [AC]
                *search_dir = gs_vec3_triple_cross_product(edge2, new_point_to_origin, edge2);
                gs_gjk_simplex_clear(simplex);
                gs_gjk_simplex_set2(simplex, simplex->a, simplex->c);
                return false;
            }
            // If closer to the new simplex point 
            else
            {
                // The "Star case" from the Muratori lecture
                // If new search direction should be along edge normal 
                if (gs_vec3_dot(edge1, new_point_to_origin) > 0.0f)
                {
                    // [AB]
                    *search_dir = gs_vec3_triple_cross_product(edge1, new_point_to_origin, edge1);
                    gs_gjk_simplex_clear(simplex);
                    gs_gjk_simplex_set2(simplex, simplex->a, simplex->b);
                    return false;
                }
                else // If new search direction should be along the new Simplex point
                {
                    // Return new simplex point alone [A]
                    *search_dir = new_point_to_origin;
                    gs_gjk_simplex_clear(simplex);
                    gs_gjk_simplex_set1(simplex, simplex->a);
                    return false;
                }
            }
        }
        // In 2D, this is a "success" case, otherwise keep going for 3D
        else
        {
            // Max simplex dimension is a triangle
            if (dimension == GS_GJK_DIMENSION_2D) 
            {
                return true;
            }

            // The "Star case" from the Muratori lecture
            // If closer to the first edge normal
            if (gs_vec3_dot(edge1_normal, new_point_to_origin) > 0.0f)
            {
                // If new search direction should be along edge normal
                if (gs_vec3_dot(edge1, new_point_to_origin) > 0.0f)
                {
                    // Return it as [A, B]
                    *search_dir = gs_vec3_triple_cross_product(edge1, new_point_to_origin, edge1);
                    gs_gjk_simplex_clear(simplex);
                    gs_gjk_simplex_set2(simplex, simplex->a, simplex->b);
                    return false;
                }
                else // If new search direction should be along the new Simplex point
                {
                    // Return new simplex point alone [A]
                    *search_dir = new_point_to_origin;
                    gs_gjk_simplex_clear(simplex);
                    gs_gjk_simplex_set1(simplex, simplex->a);
                    return false;
                }
            }
            else
            {
                // Check if it is above the triangle
                if (gs_vec3_dot(triangle_normal, new_point_to_origin) > 0.0f)
                {
                    // No need to change the simplex, return as [A, B, C]
                    *search_dir = triangle_normal;
                    return false;
                }
                else // Has to be below the triangle, all 4 other possible regions checked
                {
                    // Return simplex as [A, C, B]
                    *search_dir = gs_vec3_neg(triangle_normal);
                    gs_gjk_simplex_set3(simplex, simplex->a, simplex->c, simplex->b);
                    return false;
                }
            }
        }
    }
    // Tetrahedron for 3D case
    else if (simplex->ct == 4)
    {
        // the simplex is a tetrahedron, must check if it is outside any of the side triangles,
        // if it is then set the simplex equal to that triangle and continue, otherwise we know
        // there is an intersection and exit

        gs_vec3 edge1 = gs_vec3_sub(simplex->b.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
        gs_vec3 edge2 = gs_vec3_sub(simplex->c.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
        gs_vec3 edge3 = gs_vec3_sub(simplex->d.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
        
        gs_vec3 face1_normal = gs_vec3_cross(edge1, edge2);
        gs_vec3 face2_normal = gs_vec3_cross(edge2, edge3);
        gs_vec3 face3_normal = gs_vec3_cross(edge3, edge1);
        
        gs_vec3 new_point_to_origin = gs_vec3_neg(simplex->a.minkowski_hull_vert);

        bool contains = true;
        if (gs_vec3_dot(face1_normal, new_point_to_origin) > 0.0f)
        {
            // Origin is in front of first face, simplex is correct already
            // goto tag;
            contains = false;
        }

        if (!contains && gs_vec3_dot(face2_normal, new_point_to_origin) > 0.0f)
        {
            // Origin is in front of second face, simplex is set to this triangle [A, C, D]
            gs_gjk_simplex_clear(simplex);
            gs_gjk_simplex_set3(simplex, simplex->a, simplex->c, simplex->d);
            contains = false;
        }

        if (!contains && gs_vec3_dot(face3_normal, new_point_to_origin) > 0.0f)
        {
            // Origin is in front of third face, simplex is set to this triangle [A, D, B]
            gs_gjk_simplex_clear(simplex);
            gs_gjk_simplex_set3(simplex, simplex->a, simplex->d, simplex->b);
            contains = false;
        }

        // If reached here it means the simplex MUST contain the origin, intersection confirmed
        if (contains) {
            return true;
        }

        gs_vec3 edge1_normal = gs_vec3_cross(edge1, face1_normal);
        if (gs_vec3_dot(edge1_normal, new_point_to_origin) > 0.0f)
        {
            // Origin is along the normal of edge1, set the simplex to that edge [A, B]
            *search_dir = gs_vec3_triple_cross_product(edge1, new_point_to_origin, edge1);
            gs_gjk_simplex_clear(simplex);
            gs_gjk_simplex_set2(simplex, simplex->a, simplex->b);
            return false;
        }

        gs_vec3 edge2Normal = gs_vec3_cross(face1_normal, edge2);
        if (gs_vec3_dot(edge2Normal, new_point_to_origin) > 0.0f)
        {
            // Origin is along the normal of edge2, set the simplex to that edge [A, C]
            *search_dir = gs_vec3_triple_cross_product(edge2, new_point_to_origin, edge2);
            gs_gjk_simplex_clear(simplex);
            gs_gjk_simplex_set2(simplex, simplex->a, simplex->c);
            return false;
        }

        // If reached here the origin is along the first face normal, set the simplex to this face [A, B, C]
        *search_dir = face1_normal;
        gs_gjk_simplex_clear(simplex);
        gs_gjk_simplex_set3(simplex, simplex->a, simplex->b, simplex->c);
        return false;
    }
    return false;
}


// Find barycentric coordinates of triangle with respect to p
GS_API_DECL void gs_gjk_bary(gs_vec3 p, gs_vec3 a, gs_vec3 b, gs_vec3 c, float* u, float* v, float* w)
{
    gs_vec3 v0 = gs_vec3_sub(b, a), v1 = gs_vec3_sub(c, a), v2 = gs_vec3_sub(p, a);
    float d00 = gs_vec3_dot(v0, v0);
    float d01 = gs_vec3_dot(v0, v1);
    float d11 = gs_vec3_dot(v1, v1);
    float d20 = gs_vec3_dot(v2, v0);
    float d21 = gs_vec3_dot(v2, v1);
    float denom = d00 * d11 - d01 * d01;
    *v = (d11 * d20 - d01 * d21) / denom;
    *w = (d00 * d21 - d01 * d20) / denom;
    *u = 1.0f - *v - *w;
}

//Expanding Polytope Algorithm
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_contact_info_t res = {0};

    // Cache pointers for collider 0
    void* c0 = ci0->collider;
    gs_gjk_support_func_t f0 = ci0->func;
    gs_phys_xform_t* xform_0 = ci0->xform;

    // Cache pointers for collider 1
    void* c1 = ci1->collider;
    gs_gjk_support_func_t f1 = ci1->func;
    gs_phys_xform_t* xform_1 = ci1->xform;

    // Array of polytope faces, each with 3 support points and a normal
    gs_gjk_polytope_face_t faces[GS_EPA_MAX_NUM_FACES] = {0};
    gs_vec3 bma = gs_vec3_sub(simplex->b.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
    gs_vec3 cma = gs_vec3_sub(simplex->c.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
    gs_vec3 dma = gs_vec3_sub(simplex->d.minkowski_hull_vert, simplex->a.minkowski_hull_vert);
    gs_vec3 cmb = gs_vec3_sub(simplex->c.minkowski_hull_vert, simplex->b.minkowski_hull_vert);
    gs_vec3 dmb = gs_vec3_sub(simplex->d.minkowski_hull_vert, simplex->b.minkowski_hull_vert);
    
    faces[0] = gs_ctor(gs_gjk_polytope_face_t, {simplex->a, simplex->b, simplex->c}, gs_vec3_norm(gs_vec3_cross(bma, cma)));    // ABC
    faces[1] = gs_ctor(gs_gjk_polytope_face_t, {simplex->a, simplex->c, simplex->d}, gs_vec3_norm(gs_vec3_cross(cma, dma)));    // ACD
    faces[2] = gs_ctor(gs_gjk_polytope_face_t, {simplex->a, simplex->d, simplex->b}, gs_vec3_norm(gs_vec3_cross(dma, bma)));    // ADB
    faces[3] = gs_ctor(gs_gjk_polytope_face_t, {simplex->b, simplex->d, simplex->c}, gs_vec3_norm(gs_vec3_cross(dmb, cmb)));    // BDC

    int32_t num_faces = 4;
    int32_t closest_face;

    for (int32_t iterations = 0; iterations < GS_EPA_MAX_NUM_ITERATIONS; ++iterations) 
    {
        //Find face that's closest to origin
        float min_dist = gs_vec3_dot(faces[0].points[0].minkowski_hull_vert, faces[0].normal);
        closest_face = 0;
        for (int32_t i = 1; i < num_faces; ++i) 
        {
            float dist = gs_vec3_dot(faces[i].points[0].minkowski_hull_vert, faces[i].normal);
            if (dist < min_dist) 
            {
                min_dist = dist;
                closest_face = i;
            }
        }

        // Search normal to face that's closest to origin
        gs_vec3 search_dir = faces[closest_face].normal; 
        gs_gjk_support_point_t p = gs_gjk_generate_support_point(ci0, ci1, search_dir);
        float depth = gs_vec3_dot(p.minkowski_hull_vert, search_dir);

        // Within tolerance, so extract contact information from hull
        if (depth - min_dist < GS_EPA_TOLERANCE) 
        {
            // Cache local pointers
            gs_gjk_polytope_face_t* f = &faces[closest_face];
            gs_vec3* n = &f->normal;
            gs_gjk_support_point_t* sp0 = &f->points[0];
            gs_gjk_support_point_t* sp1 = &f->points[1];
            gs_gjk_support_point_t* sp2 = &f->points[2];
            gs_vec3* p0 = &sp0->minkowski_hull_vert;
            gs_vec3* p1 = &sp1->minkowski_hull_vert;
            gs_vec3* p2 = &sp2->minkowski_hull_vert;

            // Normal and depth information
            res.hit = true;
            res.normal = gs_vec3_norm(*n);
            res.depth = depth;

            // Get barycentric coordinates of resulting triangle face
            float u = 0.f, v = 0.f, w = 0.f;
            gs_gjk_bary(*n, *p0, *p1, *p2, &u, &v, &w);

            gs_vec3* support_0, *support_1, *support_2;

            // A Contact points
            support_0 = &sp0->support_a;
            support_1 = &sp1->support_a;
            support_2 = &sp2->support_a;

            // Contact point on collider a
            res.points[0] = gs_vec3_add(gs_vec3_add(gs_vec3_scale(*support_0, u), gs_vec3_scale(*support_1, v)), gs_vec3_scale(*support_2, w));

            // B Contact points
            support_0 = &sp0->support_b;
            support_1 = &sp1->support_b;
            support_2 = &sp2->support_b;

            // Contact point on collider b
            res.points[1] = gs_vec3_add(gs_vec3_add(gs_vec3_scale(*support_0, u), gs_vec3_scale(*support_1, v)), gs_vec3_scale(*support_2, w));

            return res;
        }

        // Fix Edges
        gs_gjk_support_point_t loose_edges[GS_EPA_MAX_NUM_LOOSE_EDGES][2]; 
        int32_t num_loose_edges = 0;

        // Find all triangles that are facing p
        for (int32_t i = 0; i < num_faces; ++i)
        {  
            // Grab direction from first point on face at p to i
            gs_vec3 dir = gs_vec3_sub(p.minkowski_hull_vert, faces[i].points[0].minkowski_hull_vert);

            // Triangle i faces p, remove it
            if (gs_vec3_same_dir(faces[i].normal, dir))
            {
                // Add removed triangle's edges to loose edge list.
                // If it's already there, remove it (both triangles it belonged to are gone)
                for (int32_t j = 0; j < 3; ++j)
                {
                    gs_gjk_support_point_t current_edge[2] = {faces[i].points[j], faces[i].points[(j + 1) % 3]};
                    bool found_edge = false;
                    for (int32_t k = 0; k < num_loose_edges; ++k) //Check if current edge is already in list
                    {
                        //Edge is already in the list, remove it
                        if (gs_vec3_eq(loose_edges[k][1].minkowski_hull_vert, current_edge[0].minkowski_hull_vert) 
                                && gs_vec3_eq(loose_edges[k][0].minkowski_hull_vert, current_edge[1].minkowski_hull_vert)) 
                        {
                            // Overwrite current edge with last edge in list
                            loose_edges[k][0] = loose_edges[num_loose_edges - 1][0];
                            loose_edges[k][1] = loose_edges[num_loose_edges - 1][1];
                            num_loose_edges--;

                             // Exit loop because edge can only be shared once
                            found_edge = true;
                            k = num_loose_edges;
                        }
                    }
                    
                    // Add current edge to list (is unique)
                    if (!found_edge) 
                    { 
                        if (num_loose_edges >= GS_EPA_MAX_NUM_LOOSE_EDGES) break;
                        loose_edges[num_loose_edges][0] = current_edge[0];
                        loose_edges[num_loose_edges][1] = current_edge[1];
                        num_loose_edges++;
                    }
                }

                // Remove triangle i from list
                faces[i].points[0] = faces[num_faces - 1].points[0];
                faces[i].points[1] = faces[num_faces - 1].points[1];
                faces[i].points[2] = faces[num_faces - 1].points[2];
                faces[i].normal = faces[num_faces - 1].normal;
                num_faces--;
                i--;
            }
        }
        
        // Reconstruct polytope with p now added
        for (int32_t i = 0; i < num_loose_edges; ++i)
        {
            if (num_faces >= GS_EPA_MAX_NUM_FACES) break;
            faces[num_faces].points[0] = loose_edges[i][0];
            faces[num_faces].points[1] = loose_edges[i][1];
            faces[num_faces].points[2] = p;
            gs_vec3 zmo = gs_vec3_sub(loose_edges[i][0].minkowski_hull_vert, loose_edges[i][1].minkowski_hull_vert);
            gs_vec3 zmp = gs_vec3_sub(loose_edges[i][0].minkowski_hull_vert, p.minkowski_hull_vert);
            faces[num_faces].normal = gs_vec3_norm(gs_vec3_cross(zmo, zmp)); 

            //Check for wrong normal to maintain CCW winding
            // In case dot result is only slightly < 0 (because origin is on face)
            float bias = 0.000001;
            if (gs_vec3_dot(faces[num_faces].points[0].minkowski_hull_vert, faces[num_faces].normal) + bias < 0.f){
                gs_gjk_support_point_t temp = faces[num_faces].points[0];
                faces[num_faces].points[0] = faces[num_faces].points[1];
                faces[num_faces].points[1] = temp;
                faces[num_faces].normal = gs_vec3_scale(faces[num_faces].normal, -1.f);
            }
            num_faces++;
        }
    }

    // Return most recent closest point
    float dot = gs_vec3_dot(faces[closest_face].points[0].minkowski_hull_vert, faces[closest_face].normal);
    gs_vec3 norm = gs_vec3_scale(faces[closest_face].normal, dot);
    res.hit = false;
    res.normal = gs_vec3_norm(norm);
    res.depth = gs_vec3_len(norm);
    return res;
}

// Expanding Polytope Algorithm 2D
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_dyn_array(gs_gjk_support_point_t) polytope = NULL;
    gs_contact_info_t res = gs_default_val();
    gs_gjk_support_point_t p = gs_default_val();

    // Copy over simplex into polytope array
    for (uint32_t i = 0; i < simplex->ct; ++i) {
        gs_dyn_array_push(polytope, simplex->points[i]);
    }

    // p = gs_gjk_generate_support_point(ci0, ci1, gs_v3s(1.f));
    // gs_gjk_simplex_push_front(simplex, p);
    gs_gjk_epa_edge_t e = gs_default_val();
    uint32_t iterations = 0;
    while (iterations < GS_EPA_MAX_NUM_ITERATIONS)
    {
        // Find closest edge to origin from simplex
        e = gs_gjk_epa_find_closest_edge(polytope);
        p = gs_gjk_generate_support_point(ci0, ci1, e.normal);

        double d = (double)gs_vec3_dot(p.minkowski_hull_vert, e.normal);

        // Success, calculate results
        if (d - e.distance < GS_EPA_TOLERANCE)
        {
            // Normal and depth information
            res.normal = gs_vec3_norm(e.normal);
            res.depth = d;
            break;
        }
        else
        {
            // Need to think about this more. This is fucked.
            // Need an "expanding" simplex, that only allows for unique points to be included.
            // This is just overwriting. I need to actually insert then push back.
            // gs_gjk_simplex_insert(simplex, e.index + 1, p);
            // Insert into polytope array at idx + 1
            for (uint32_t i = 0; i < gs_dyn_array_size(polytope); ++i)
            {
               gs_vec3* p = &polytope[i].minkowski_hull_vert; 
               gs_printf("<%.2f, %.2f>, ", p->x, p->y);
            }
            gs_dyn_array_push(polytope, p);

            for (int32_t i = gs_dyn_array_size(polytope) - 1; i > e.index + 1; i--)
                polytope[i] = polytope[i - 1];

            polytope[e.index + 1] = p;

            gs_println("pts after: ");
            for (uint32_t i = 0; i < gs_dyn_array_size(polytope); ++i)
            {
               gs_vec3* p = &polytope[i].minkowski_hull_vert; 
               gs_printf("<%.2f, %.2f>, ", p->x, p->y);
            }
        }

        // Increment iterations
        iterations++;
    }

    // gs_vec3 line_vec = gs_vec3_sub(e.a.minkowski_hull_vert, e.b.minkowski_hull_vert);
    // gs_vec3 projO = gs_vec3_scale(gs_vec3_scale(line_vec, 1.f / gs_vec3_len2(line_vec)), gs_vec3_dot(line_vec, gs_vec3_neg(e.b.minkowski_hull_vert)));
    // float s, t;
    // s = sqrt(gs_vec3_len2(projO) / gs_vec3_len2(line_vec));
    // t = 1.f - s;
    // int32_t next_idx = (e.index + 1) % simplex->ct;
    res.hit = true;
    // res.points[0] = gs_vec3_add(gs_vec3_scale(simplex->points[e.index].support_a, s), gs_vec3_scale(simplex->points[next_idx].support_a, t));
    // res.points[1] = gs_vec3_add(gs_vec3_scale(simplex->points[e.index].support_b, s), gs_vec3_scale(simplex->points[next_idx].support_b, t));

    return res;

}

gs_gjk_epa_edge_t gs_gjk_epa_find_closest_edge(gs_dyn_array(gs_gjk_support_point_t) polytope)
{
   gs_gjk_epa_edge_t res = gs_default_val();
   float min_dist = FLT_MAX;
   uint32_t min_index = 0;
   gs_vec3 min_normal = gs_default_val();
   for (uint32_t i = 0; i < gs_dyn_array_size(polytope); ++i)
   {
        uint32_t j = (i + 1) % gs_dyn_array_size(polytope);
        gs_gjk_support_point_t* a = &polytope[0];
        gs_gjk_support_point_t* b = &polytope[1];
        gs_vec3 dir = gs_vec3_sub(b->minkowski_hull_vert, a->minkowski_hull_vert);
        gs_vec3 norm = gs_vec3_norm(gs_v3(dir.y, -dir.x, 0.f));
        float dist = gs_vec3_dot(norm, a->minkowski_hull_vert);

        // If distance is negative, then we need to correct for winding order mistakes
        if (dist < 0) {
            dist *= -1;
            norm = gs_vec3_neg(norm);
        }

        // Check for min distance
        if (dist < min_dist) {
            min_dist = dist;
            min_normal = norm;
            min_index = j;
        }
   }
   res.index = min_index;
   res.normal = min_normal;
   res.distance = min_dist;
   return res;
}

gs_gjk_epa_edge_t gs_gjk_epa_find_closest_edge2(gs_gjk_simplex_t* simplex)
{
    gs_gjk_epa_edge_t result = gs_default_val();
    uint32_t next_idx = 0;
    float min_dist = FLT_MAX, curr_dist = 0.f;
    gs_vec3 norm, edge;
    gs_vec3 norm_3d;

    for (int32_t i = 0; i < simplex->ct; ++i)
    {
        next_idx = (i + 1) % simplex->ct;
        edge = gs_vec3_sub(simplex->points[next_idx].minkowski_hull_vert, simplex->points[i].minkowski_hull_vert);
        norm_3d = gs_vec3_triple_cross_product(gs_v3(edge.x, edge.y, 0), 
            gs_v3(simplex->points[i].minkowski_hull_vert.x, simplex->points[i].minkowski_hull_vert.y, 0.f), 
            gs_v3(edge.x, edge.y, 0.f));
        norm.x = norm_3d.x;
        norm.y = norm_3d.y;

        norm = gs_vec3_norm(norm);
        curr_dist = gs_vec3_dot(norm, simplex->points[i].minkowski_hull_vert);
        if (curr_dist < min_dist)
        { 
            min_dist = curr_dist;
            result.a = simplex->points[i];
            result.b = simplex->points[next_idx];
            result.normal = norm;
            result.distance = curr_dist;
            result.index = i;
        }
    }

    return result;
}
*/

/*===== CCD ======*/

// Useful CCD conversions
void _gs_ccdv32gsv3(const ccd_vec3_t* _in, gs_vec3* _out)
{
    // Safe check against NaNs
    if (gs_is_nan(_in->v[0]) || gs_is_nan(_in->v[1]) || gs_is_nan(_in->v[2])) return;
    *_out = gs_ctor(gs_vec3, (float)_in->v[0], (float)_in->v[1], (float)_in->v[2]);
}

void _gs_gsv32ccdv3(const gs_vec3* _in, ccd_vec3_t* _out)
{
    ccdVec3Set(_out, CCD_REAL(_in->x), CCD_REAL(_in->y), CCD_REAL(_in->z));
}

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

)
{
    // Convert to appropriate gjk internals, then call ccd
    ccd_t ccd = gs_default_val();
    CCD_INIT(&ccd);

    // set up ccd_t struct
    ccd.support1       = _gs_ccd_support_func;  // support function for first object
    ccd.support2       = _gs_ccd_support_func;  // support function for second object
    ccd.max_iterations = 100;                   // maximal number of iterations
    ccd.epa_tolerance  = 0.0001;                // maximal tolerance for epa to succeed

    // Default transforms
    gs_vqs _xa = gs_vqs_default(), _xb = gs_vqs_default();

    // Collision object 1
    _gs_collision_obj_handle_t h0 = gs_default_val();
    h0.support = f0;
    h0.obj = c0;
    h0.xform = xform_a ? xform_a : &_xa;

    // Collision object 2
    _gs_collision_obj_handle_t h1 = gs_default_val();
    h1.support = f1;
    h1.obj = c1;
    h1.xform = xform_b ? xform_b : &_xb;

    // Call ccd, cache results into res for user
    ccd_real_t depth = CCD_REAL(0.0);
    ccd_vec3_t n = gs_ctor(ccd_vec3_t, 0.f, 0.f, 0.f), p = gs_ctor(ccd_vec3_t, 0.f, 0.f, 0.f);
    int32_t r = ccdGJKPenetration(&h0, &h1, &ccd, &depth, &n, &p);
    bool32 hit = r >= 0 && !gs_is_nan(n.v[0]) && !gs_is_nan(n.v[1]) && !gs_is_nan(n.v[2]);

    if (hit && res)
    {
        res->hit = true;
        res->depth = (float)depth;
        _gs_ccdv32gsv3(&p, &res->point);
        _gs_ccdv32gsv3(&n, &res->normal);
    }

    return r;
}

// typedef void (*ccd_support_fn)(const void *obj, const ccd_vec3_t *dir,
//                                ccd_vec3_t *vec);

// Internal support function for gs -> ccd
GS_API_DECL 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) 
    {
        // Temp copy conversion for direction vector
        gs_vec3 dir = gs_default_val(), out = gs_default_val();
        _gs_ccdv32gsv3((const ccd_vec3_t*)_dir, &dir);

        // Call user provided support function
        // Think I found it...
        obj->support(obj->obj, obj->xform, &dir, &out);

        // Copy over out result for ccd
        _gs_gsv32ccdv3(&out, (ccd_vec3_t*)_out);
    }
}

#endif // GS_PHYSICS_IMPL
#endif // GS_PHYSICS_H