/*============================================================================================================== * 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