From 90a71ff4e3d421c73d39a2a48ad289fa467f3422 Mon Sep 17 00:00:00 2001 From: hgn Date: Mon, 15 Sep 2025 22:33:45 +0000 Subject: [PATCH] idiot --- bootstrap.sh | 32 + build-compat/workaround-27653.c | 85 - foundation/build.sh | 6 + include/common_api.h | 291 ++ include/common_thread_api.h | 3 + include/maths/common_maths.h | 1327 ++++++++ include/maths/rigidbody.h | 188 ++ metacompiler/build.sh | 3 + metacompiler/main.c | 8 + vg_audio.c => source/engine/audio_mixer.c | 0 vg_profiler.c => source/engine/profiler.c | 0 vg_shader.c => source/engine/shader.c | 0 vg_steam2.c => source/engine/steamworks.c | 0 vg_tex.c => source/engine/texture.c | 0 source/foundation/allocator_heap.c | 21 + source/foundation/allocator_pool.c | 88 + source/foundation/allocator_queue.c | 1 + source/foundation/allocator_stack.c | 92 + source/foundation/allocator_stretchy.c | 52 + source/foundation/buffer_operations.c | 75 + source/foundation/exit.c | 42 + source/foundation/io.c | 93 + source/foundation/keyvalues.c | 575 ++++ source/foundation/logging.c | 37 + source/foundation/options.c | 267 ++ source/foundation/stream.c | 163 + source/foundation/string.c | 261 ++ vg_bvh.c => source/maths/bvh.c | 0 source/maths/common_maths.c | 998 ++++++ vg_perlin.c => source/maths/perlin.c | 0 vg_rigidbody.c => source/maths/rigidbody.c | 0 .../maths/rigidbody_collision.c | 0 .../maths/rigidbody_constraints.c | 0 source/tools/metacompiler.c | 115 + src/metacompiler.c | 4 + vg.hconf | 10 +- vg_io.c | 3 + vg_io.h | 8 +- vg_log.h | 6 - vg_m.h | 2683 ----------------- vg_mem.c | 2 + vg_mem.h | 8 +- vg_platform.h | 25 +- vg_rigidbody.h | 88 - vg_rigidbody_collision.h | 74 - vg_rigidbody_constraints.h | 50 - 46 files changed, 4771 insertions(+), 3013 deletions(-) create mode 100755 bootstrap.sh delete mode 100644 build-compat/workaround-27653.c create mode 100644 foundation/build.sh create mode 100644 include/common_api.h create mode 100644 include/common_thread_api.h create mode 100644 include/maths/common_maths.h create mode 100644 include/maths/rigidbody.h create mode 100755 metacompiler/build.sh create mode 100644 metacompiler/main.c rename vg_audio.c => source/engine/audio_mixer.c (100%) rename vg_profiler.c => source/engine/profiler.c (100%) rename vg_shader.c => source/engine/shader.c (100%) rename vg_steam2.c => source/engine/steamworks.c (100%) rename vg_tex.c => source/engine/texture.c (100%) create mode 100644 source/foundation/allocator_heap.c create mode 100644 source/foundation/allocator_pool.c create mode 100644 source/foundation/allocator_queue.c create mode 100644 source/foundation/allocator_stack.c create mode 100644 source/foundation/allocator_stretchy.c create mode 100644 source/foundation/buffer_operations.c create mode 100644 source/foundation/exit.c create mode 100644 source/foundation/io.c create mode 100644 source/foundation/keyvalues.c create mode 100644 source/foundation/logging.c create mode 100644 source/foundation/options.c create mode 100644 source/foundation/stream.c create mode 100644 source/foundation/string.c rename vg_bvh.c => source/maths/bvh.c (100%) create mode 100644 source/maths/common_maths.c rename vg_perlin.c => source/maths/perlin.c (100%) rename vg_rigidbody.c => source/maths/rigidbody.c (100%) rename vg_rigidbody_collision.c => source/maths/rigidbody_collision.c (100%) rename vg_rigidbody_constraints.c => source/maths/rigidbody_constraints.c (100%) create mode 100644 source/tools/metacompiler.c create mode 100644 src/metacompiler.c delete mode 100644 vg_m.h delete mode 100644 vg_rigidbody.h delete mode 100644 vg_rigidbody_collision.h delete mode 100644 vg_rigidbody_constraints.h diff --git a/bootstrap.sh b/bootstrap.sh new file mode 100755 index 0000000..1c166c6 --- /dev/null +++ b/bootstrap.sh @@ -0,0 +1,32 @@ +mkdir -p bin/vg.metacompiler-linux-x86_64-zig-cc/ +zig cc -I. -I./include/ -fsanitize=address -lasan -Wall -Wno-unused-function -O0 -ggdb -std=c99 \ + source/foundation/options.c \ + source/foundation/logging.c \ + source/foundation/allocator_heap.c \ + source/foundation/allocator_stack.c \ + source/foundation/allocator_pool.c \ + source/foundation/allocator_queue.c \ + source/foundation/allocator_stretchy.c \ + source/foundation/stream.c \ + source/foundation/string.c \ + source/foundation/keyvalues.c \ + source/foundation/exit.c \ + source/foundation/io.c \ + source/foundation/buffer_operations.c \ + \ + source/tools/metacompiler.c \ + -o bin/vg.metacompiler-linux-x86_64-zig-cc/vg.metacompiler + +sudo ln -sf $(realpath ./bin/vg.metacompiler-linux-x86_64-zig-cc/vg.metacompiler) /usr/bin/vgc + +# Foundatin Engine +# Y source/foundation/options.c source/foundation/options.c +# Y source/foundation/logging.c source/foundation/logging__ENGINE.c +# Y source/foundation/allocator_heap.c source/foundation/allocator_heap.c +# Y source/foundation/allocator_stack.c source/foundation/allocator_stack.c +# Y source/foundation/allocator_pool.c source/foundation/allocator_pool.c +# source/foundation/allocator_queue.c source/foundation/allocator_queue.c +# Y source/foundation/stream.c source/foundation/stream.c +# Y source/foundation/string.c source/foundation/string.c +# source/foundation/keyvalues.c source/foundation/keyvalues.c +# Y source/foundation/exit.c source/foundation/exit__ENGINE.c diff --git a/build-compat/workaround-27653.c b/build-compat/workaround-27653.c deleted file mode 100644 index e625b2c..0000000 --- a/build-compat/workaround-27653.c +++ /dev/null @@ -1,85 +0,0 @@ -/* - * SPDX-License-Identifier: LGPL-2.1-or-later - * - * Copyright (C) 2021 Mathieu Desnoyers - * - * Workaround for malloc interceptors hang caused by glibc i18n lookup deadlock. - * Bypass i18n lookup when nested in glibc's intl code. - * - * https://sourceware.org/bugzilla/show_bug.cgi?id=27653 - * - * Build with: - * - * gcc -shared -fPIC -o workaround-27653.so workaround-27653.c - * - * Then use with (e.g.): - * - * LD_PRELOAD=/usr/lib/gcc/x86_64-linux-gnu/11/libasan.so:./workaround-27653.so free - */ - -#define _GNU_SOURCE -#include -#include -#include - -static __thread int textdomain_nesting __attribute__((tls_model("initial-exec"))); - -static char *(*textdomain_fct)(const char *domainname); -static char *(*bindtextdomain_fct)(const char *domainname, const char *dirname); -static char *(*bind_textdomain_codeset_fct)(const char *domainname, const char *codeset); -static char *(*__dcgettext_fct)(const char *msgid); - -char *textdomain(const char *domainname) -{ - char *str; - - textdomain_nesting++; - if (!textdomain_fct) - textdomain_fct = dlsym(RTLD_NEXT, "textdomain"); - if (!textdomain_fct) - abort(); - str = textdomain_fct(domainname); - textdomain_nesting--; - return str; -} - -char *bindtextdomain(const char *domainname, const char *dirname) -{ - printf( "FUCKYOU\n""FUCKYOU\n""FUCKYOU\n""FUCKYOU\n""FUCKYOU\n""FUCKYOU\n" ); - char *str; - - textdomain_nesting++; - if (!bindtextdomain_fct) - bindtextdomain_fct = dlsym(RTLD_NEXT, "bindtextdomain"); - if (!bindtextdomain_fct) - abort(); - str = bindtextdomain_fct(domainname, dirname); - textdomain_nesting--; - return str; -} - -char *bind_textdomain_codeset(const char *domainname, const char *codeset) -{ - char *str; - - textdomain_nesting++; - if (!bind_textdomain_codeset_fct) - bind_textdomain_codeset_fct = dlsym(RTLD_NEXT, "bind_textdomain_codeset"); - if (!bind_textdomain_codeset_fct) - abort(); - str = bind_textdomain_codeset_fct(domainname, codeset); - textdomain_nesting--; - return str; -} - -char *__dcgettext(const char *msgid) -{ - if (textdomain_nesting) - return (char *) msgid; - - if (!__dcgettext_fct) - __dcgettext_fct = dlsym(RTLD_NEXT, "__dcgettext"); - if (!__dcgettext_fct) - abort(); - return __dcgettext_fct(msgid); -} diff --git a/foundation/build.sh b/foundation/build.sh new file mode 100644 index 0000000..11acd34 --- /dev/null +++ b/foundation/build.sh @@ -0,0 +1,6 @@ +zig cc -I. -fsanitize=address -lasan -Wall -Wno-unused-function -O0 -ggdb -std=c99 \ + -include ../foundation/common.h \ + -c \ + options.c logging.c allocator_heap.c \ + allocator_stack.c allocator_pool.c allocator_queue.c \ + stream.c string.c keyvalues.c diff --git a/include/common_api.h b/include/common_api.h new file mode 100644 index 0000000..a032458 --- /dev/null +++ b/include/common_api.h @@ -0,0 +1,291 @@ +/* Voyager common application interface */ + +/* Types + * ------------------------------------------------------------------------------------------------------------------ */ +typedef unsigned char u8; +typedef char c8; +typedef unsigned short int u16; +typedef unsigned int u32; +typedef unsigned long int u64; +typedef char i8; +typedef signed short int i16; +typedef signed int i32; +typedef signed long int i64; +typedef float f32; +typedef double f64; +typedef unsigned char bool; + +#define NULL 0 +#define BYTES_KB( X ) X*1024 +#define BYTES_MB( X ) X*1024*1024 +#define BYTES_GB( X ) X*1024*1024*1024 +#define ARRAY_COUNT( X ) (sizeof((X))/sizeof((X)[0])) +#define i8_MAX 0x7F +#define u8_MAX 0XFF +#define i16_MAX 0x7FFF +#define u16_MAX 0xFFFF +#define i32_MAX 0x7FFFFFFF +#define u32_MAX 0xFFFFFFFF +#define i64_MAX 0x7FFFFFFFFFFFFFFF +#define u64_MAX 0xFFFFFFFFFFFFFFFF + +static inline f32 f32_min( f32 a, f32 b ){ return a < b? a: b; } +static inline f32 f32_max( f32 a, f32 b ){ return a > b? a: b; } +static inline i32 i32_min( i32 a, i32 b ){ return a < b? a: b; } +static inline i32 i32_max( i32 a, i32 b ){ return a > b? a: b; } +static inline f32 f32_clamp( f32 a, f32 min, f32 max ) { return f32_min( max, f32_max( a, min ) ); } +static inline f32 f32_sign( f32 a ) { return a < 0.0f? -1.0f: 1.0f; } + +void _exit_init(void); +void _fatal_exit( const c8 *reason ); +void _normal_exit( const c8 *reason ); +#define ASSERT_CRITICAL( CONDITION ) \ + if( !(CONDITION) ) \ + _fatal_exit( "("__BECOME_STRING__(CONDITION) ") evaluated to 0\nLocation: " __LINE_STRING__ "\n" ); + +/* Command line options + * ------------------------------------------------------------------------------------------------------------------ */ +void _options_init( i32 argc, const c8 *argv[] ); +void _options_check_end( void ); +bool _option_flag( c8 c, const c8 *desc ); +const c8 *_option_argument( char c, const c8 *desc ); +bool _option_long( c8 *name, const c8 *desc ); +const c8 *_option_long_argument( char *name, const c8 *desc ); +const c8 *_option( u32 index ); + +/* Heap allocation + * ------------------------------------------------------------------------------------------------------------------ */ +void *_heap_allocate( u64 size ); +void *_heap_reallocate( void *buf, u64 size ); +void _heap_free( void *buf ); + +void zero_buffer( void *buffer, u32 length ); + +/* if max_length is 0, the operation runs until a 0 is found in the buffer */ +u32 buffer_djb2( const void *buffer, i32 max_length ); +bool compare_buffers( const void *buffer_a, i32 a_max, const void *buffer_b, i32 b_max ); +i32 buffer_first_index( const void *buffer, u8 match, i32 max_length ); +i32 buffer_last_index( const void *buffer, u8 match, i32 max_length ); +void buffer_copy( const void *buffer_src, void *buffer_dest, i32 length ); + +struct stretchy_allocator +{ + i32 count, element_size; + void *segments[26]; +}; +void stretchy_init( struct stretchy_allocator *stretchy, u32 element_size ); +void *stretchy_append( struct stretchy_allocator *stretchy ); +void *stretchy_get( struct stretchy_allocator *stretchy, u32 index ); +u32 stretchy_count( struct stretchy_allocator *stretchy ); +void stretchy_free( struct stretchy_allocator *stretchy ); + +/* Stack + * ------------------------------------------------------------------------------------------------------------------ */ +struct stack_allocator +{ + u32 capacity, offset; /* bytes */ + void *data; +}; +void stack_init( struct stack_allocator *stack, void *buffer, u32 capacity, const c8 *debug_name ); +void *stack_allocate( struct stack_allocator *stack, u32 size, u32 alignment, const c8 *debug_name ); +void stack_clear( struct stack_allocator *stack ); +void stack_extend_last( struct stack_allocator *stack, i32 extra_bytes ); +u32 stack_offset( struct stack_allocator *stack, void *pointer ); +void *stack_pointer( struct stack_allocator *stack, u32 offset ); + +u32 _start_temporary_frame(void); +void _end_temporary_frame( u32 whence ); +void *_temporary_allocate( u32 bytes, u32 alignment ); +struct stack_allocator *_temporary_stack_allocator(void); + +/* Pool + * ------------------------------------------------------------------------------------------------------------------ */ +struct pool_node +{ + u16 l, r, refcount, unused0; +}; +struct pool_chain +{ + u16 head, tail, count, unused0; +}; +struct pool_allocator +{ + struct pool_node *nodes; + u32 count; +}; +void pool_init( struct pool_allocator *pool, struct pool_node *nodes, u16 node_count, struct pool_chain *full_chain ); +u32 pool_index( struct pool_allocator *pool, u16 pool_id ); +u16 pool_reference( struct pool_allocator *pool, u16 pool_id, bool increment ); +u16 pool_next( struct pool_allocator *pool, u16 pool_id, bool right ); +void pool_switch( struct pool_allocator *pool, struct pool_chain *source, struct pool_chain *dest, u16 which ); + +/* Queue + * ------------------------------------------------------------------------------------------------------------------ */ +struct queue_item +{ + u32 alloc_size,prev_size; + u8 data[]; +}; +struct queue_allocator +{ + void *buffer; + u32 size; + u32 head_offset, tail_offset; + u32 allocation_count; +}; +void queue_init( struct queue_allocator *queue, void *buffer, u32 buffer_size ); +void *queue_alloc( struct queue_allocator *queue, u32 size ); +void *queue_data( struct queue_allocator *queue, u32 offset ); +void *queue_tail_data( struct queue_allocator *queue ); +u32 queue_offset( struct queue_allocator *queue, void *pointer ); +/* warning: this is not the size but the allocation size (may be padded) */ +u32 queue_item_size( struct queue_allocator *queue, u32 item_id ); +bool queue_next( struct queue_allocator *queue, u32 item_id, u32 *out_next ); +bool queue_previous( struct queue_allocator *queue, u32 item_id, u32 *out_prev ); +void queue_pop( struct queue_allocator *queue ); +void queue_copy_buffer( struct queue_allocator *queue, void *dst, u32 start, u32 size ); +u32 queue_usage( struct queue_allocator *queue ); +void queue_clear( struct queue_allocator *queue ); + +/* Stream + * ------------------------------------------------------------------------------------------------------------------ */ +enum stream_flag +{ + k_stream_overflow_error = 0x20, + k_stream_text = 0x10, /* SEMANTIC, not logical. */ + k_stream_procedural = 0x8, + k_stream_posix = 0x4, + k_stream_write = 0x2, + k_stream_read = 0x1 +}; +struct stream +{ + u32 flags, buffer_length, offset; + union + { + void *posix_stream; + u8 *buffer; + + struct + { + u32 (*procedure)( u32 flags, u32 offset, void *buffer, u32 length ); + void *procedure_userdata; + }; + }; +}; +void stream_open_buffer( struct stream *stream, void *buffer, u32 buffer_length, u32 flags ); +bool stream_open_file( struct stream *stream, const c8 *path, u32 flags ); +void stream_close( struct stream *stream ); +u32 stream_read( struct stream *stream, void *buffer, u32 length ); +u32 stream_write( struct stream *stream, const void *buffer, u32 length ); +u32 stream_offset( struct stream *stream ); +void stream_seek( struct stream *stream, u32 offset ); +bool stream_error( struct stream *stream ); + +/* String (Stream subset) + * ------------------------------------------------------------------------------------------------------------------ */ +void string_init( struct stream *string, c8 *buffer, u32 buffer_length ); +void string_clip( struct stream *string, i32 length ); +void string_append( struct stream *string, const c8 *substring ); +void string_append_c8( struct stream *string, c8 c ); +void string_append_i64( struct stream *string, i64 value, u64 base ); +void string_append_i64r( struct stream *string, i64 value, u64 base, u32 width, c8 blank_c8acter ); +void string_append_u64( struct stream *string, u64 value, u64 base ); +void string_append_f64( struct stream *string, f64 value, u64 base, u32 decimal_places ); + +enum string_parse_result +{ + k_string_parse_ok, + k_string_parse_eof, + k_string_parse_error, + k_string_parse_whitespace +}; +enum string_parse_result string_parse_c8 ( struct stream *string, c8 *c ); +enum string_parse_result string_parse_u64( struct stream *string, u64 *value ); +enum string_parse_result string_parse_i64( struct stream *string, i64 *value ); +enum string_parse_result string_parse_f64( struct stream *string, f64 *value ); + +/* Logging + * ------------------------------------------------------------------------------------------------------------------ */ +enum log_event +{ + k_log_low = 0x1, + k_log_info = 0x2, + k_log_ok = 0x4, + k_log_warning = 0x8, + k_log_error = 0x10 +}; + +/* One day we will replace this shit with a good pre-processor. */ +#define __BECOME_STRING__(S) __BECOME_STRING_REALLY__(S) +#define __BECOME_STRING_REALLY__(S) #S +#define __LINE_STRING__ __FILE__ ":" __BECOME_STRING__(__LINE__) +#define _log( TYPE, TEXT ) _log_event( TYPE, TEXT, __LINE_STRING__ ) + +struct stream *_log_event( enum log_event type, const c8 *text, const c8 *code_location ); +void _log_append_errno( struct stream *stream ); +struct stream *_get_console_stream(); + +/* Keyvalues + * ------------------------------------------------------------------------------------------------------------------ */ +struct keyvalues +{ + struct stack_allocator *stack; + u32 root_offset; + u32 kv_page_offset, kv_page_count; +}; +void keyvalues_init( struct keyvalues *kvs, struct stack_allocator *stack ); +void keyvalues_parse_stream( struct keyvalues *kvs, u32 root_offset, struct stream *in_stream ); +void keyvalues_write_stream( struct keyvalues *kvs, struct stream *out_stream ); + +bool keyvalues_read_file( struct keyvalues *kvs, const char *path, struct stack_allocator *stack ); +bool keyvalues_write_file( struct keyvalues *kvs, const char *path ); + +enum keyvalue_type +{ + k_keyvalue_type_frame = 0, + k_keyvalue_type_pair = 1, + k_keyvalue_type_unused2 = 2, + k_keyvalue_type_unused3 = 3 +}; +u32 keyvalues_type( struct keyvalues *kvs, u32 kv_offset ); +c8 *keyvalues_key( struct keyvalues *kvs, u32 kv_offset, u32 *out_length ); +c8 *keyvalues_value( struct keyvalues *kvs, u32 kv_offset, u32 *out_length ); + +u32 keyvalues_get( struct keyvalues *kvs, u32 root_offset, const c8 *key ); +u32 keyvalues_get_next( struct keyvalues *kvs, u32 kv_offset ); +u32 keyvalues_get_child( struct keyvalues *kvs, u32 root_offset, u32 index ); + +const c8 *keyvalues_read_string( struct keyvalues *kvs, u32 root_offset, const c8 *key, const c8 *default_value ); +bool keyvalues_read_i32s( struct keyvalues *kvs, u32 root_offset, const c8 *key, i32 *default_values, i32 *out_values, u32 len ); +bool keyvalues_read_u32s( struct keyvalues *kvs, u32 root_offset, const c8 *key, u32 *default_values, u32 *out_values, u32 len ); +bool keyvalues_read_f32s( struct keyvalues *kvs, u32 root_offset, const c8 *key, f32 *default_values, f32 *out_values, u32 len ); +u32 keyvalues_append_string( struct keyvalues *kvs, u32 parent_offset, const c8 *key, const c8 *value ); +u32 keyvalues_append_i32s( struct keyvalues *kvs, u32 parent_offset, const c8 *key, i32 *values, u32 len ); +u32 keyvalues_append_u32s( struct keyvalues *kvs, u32 parent_offset, const c8 *key, u32 *values, u32 len ); +u32 keyvalues_append_f32s( struct keyvalues *kvs, u32 parent_offset, const c8 *key, f32 *values, u32 len ); + +/* IO */ +struct directory; +enum directory_entry_type +{ + k_directory_entry_type_unknown, + k_directory_entry_type_file, + k_directory_entry_type_dir +}; + +enum directory_status +{ + k_directory_status_none, + k_directory_status_ok, + k_directory_status_path_too_long, + k_directory_status_invalid_path, + k_directory_status_is_file +}; + +struct directory *directory_open( const c8 *path, struct stack_allocator *stack ); +enum directory_status directory_status( struct directory *directory ); +const c8 *directory_entry_name( struct directory *directory ); +bool directory_next_entry( struct directory *directory ); +enum directory_entry_type directory_entry_type( struct directory *directory ); +void directory_close( struct directory *directory ); diff --git a/include/common_thread_api.h b/include/common_thread_api.h new file mode 100644 index 0000000..ab115aa --- /dev/null +++ b/include/common_thread_api.h @@ -0,0 +1,3 @@ +bool _thread_has_flags( u32 flags ); +void _thread_set_flags( u32 flags ); +const c8 *_thread_prefix(void); diff --git a/include/maths/common_maths.h b/include/maths/common_maths.h new file mode 100644 index 0000000..cec1c02 --- /dev/null +++ b/include/maths/common_maths.h @@ -0,0 +1,1327 @@ +#include +#define VG_PIf 3.141592 /* fuck you */ +#define VG_TAUf 6.283 /* fuckk you */ + +static inline u32 f32_raw_bits( f32 a ) { return *((u32 *)(&a)); } +static inline bool f32_is_infinite( f32 a ) { return ((f32_raw_bits(a)) & 0x7FFFFFFFU) == 0x7F800000U; } +static inline bool f32_is_nan( f32 a ) { return !f32_is_infinite(a) && ((f32_raw_bits(a)) & 0x7F800000U) == 0x7F800000U;} +static inline bool f32_is_number( f32 a ) { return ((f32_raw_bits(a)) & 0x7F800000U) != 0x7F800000U; } + +/* Scalars ---------------------------------------------------------------------------------------------------------- */ + +static inline f32 f32_fractional_part( f32 a ) { return a - floorf( a ); } +static inline f64 f64_fractional_part( f64 a ) { return a - floor( a ); } +static inline f32 f32_degrees_to_radians( f32 degrees ) { return degrees * VG_PIf/180.0f; } +static inline f32 f32_friction( f32 velocity, f32 F ) +{ + return -f32_sign(velocity) * f32_min( F, fabsf(velocity) ); +} +static inline f32 f32_lerp( f32 a, f32 b, f32 t ) { return a + t*(b-a); } +static inline f64 f64_lerp( f64 a, f64 b, f64 t ) { return a + t*(b-a); } +static inline void vg_slewf( f32 *a, f32 b, f32 speed ) +{ + f32 d = f32_sign( b-*a ), + c = *a + d*speed; + *a = f32_min( b*d, c*d ) * d; +} +static inline f32 f32_smoothstep( f32 x ) { return x*x*(3.0f - 2.0f*x); } + +static inline f32 f32_radian_difference( f32 a, f32 b ) +{ + f32 d = fmodf(b,VG_TAUf)-fmodf(a,VG_TAUf); + if( fabsf(d) > VG_PIf ) + d = -f32_sign(d) * (VG_TAUf - fabsf(d)); + return d; +} +static inline f32 f32_radian_lerp( f32 a, f32 b, f32 t ) +{ + f32 d = fmodf( b-a, VG_TAUf ), + s = fmodf( 2.0f*d, VG_TAUf ) - d; + return a + s*t; +} + +static inline u32 f32_quantize( f32 a, u32 bits, f32 min, f32 max ) +{ + u32 mask = (0x1 << bits) - 1; + return f32_clamp((a - min) * ((f32)mask/(max-min)), 0.0f, mask ); +} +static inline f32 f32_unquantize( u32 q, u32 bits, f32 min, f32 max ) +{ + return min + (f32)q * ((max-min) / (f32)((0x1 << bits) - 1)); +} +/* https://iquilezles.org/articles/functions/ + * + * Use k to control the stretching of the function. Its maximum, which is 1, + * happens at exactly x = 1/k. + */ +static inline f32 f32_exponential_impulse( f32 x, f32 k ) +{ + f32 h = k*x; + return h*expf(1.0f-h); +} + +/* f32[2] ----------------------------------------------------------------------------------------------------------- */ +static inline void v2_copy( f32 a[2], f32 d[2] ) +{ + d[0] = a[0]; + d[1] = a[1]; +} +static inline void v2_add( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; +} +static inline void v2_sub( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = a[0]-b[0]; + d[1] = a[1]-b[1]; +} +static inline void v2_min( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = f32_min(a[0], b[0]); + d[1] = f32_min(a[1], b[1]); +} +static inline void v2_max( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = f32_max(a[0], b[0]); + d[1] = f32_max(a[1], b[1]); +} +static inline f32 v2_dot( f32 a[2], f32 b[2] ) +{ + return a[0] * b[0] + a[1] * b[1]; +} +static inline f32 v2_cross( f32 a[2], f32 b[2] ) +{ + return a[0]*b[1] - a[1]*b[0]; +} +static inline void v2_abs( f32 a[2], f32 d[2] ) +{ + d[0] = fabsf( a[0] ); + d[1] = fabsf( a[1] ); +} +static inline void v2_muls( f32 a[2], f32 s, f32 d[2] ) +{ + d[0] = a[0]*s; + d[1] = a[1]*s; +} +static inline void v2_divs( f32 a[2], f32 s, f32 d[2] ) +{ + d[0] = a[0]/s; + d[1] = a[1]/s; +} +static inline void v2_mul( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = a[0]*b[0]; + d[1] = a[1]*b[1]; +} +static inline void v2_div( f32 a[2], f32 b[2], f32 d[2] ) +{ + d[0] = a[0]/b[0]; + d[1] = a[1]/b[1]; +} +static inline void v2_muladd( f32 a[2], f32 b[2], f32 s[2], f32 d[2] ) +{ + d[0] = a[0]+b[0]*s[0]; + d[1] = a[1]+b[1]*s[1]; +} +static inline void v2_muladds( f32 a[2], f32 b[2], f32 s, f32 d[2] ) +{ + d[0] = a[0]+b[0]*s; + d[1] = a[1]+b[1]*s; +} +static inline f32 v2_length2( f32 a[2] ) +{ + return a[0]*a[0] + a[1]*a[1]; +} +static inline f32 v2_length( f32 a[2] ) +{ + return sqrtf( v2_length2( a ) ); +} +static inline f32 v2_dist2( f32 a[2], f32 b[2] ) +{ + f32 delta[2]; + v2_sub( a, b, delta ); + return v2_length2( delta ); +} +static inline f32 v2_dist( f32 a[2], f32 b[2] ) +{ + return sqrtf( v2_dist2( a, b ) ); +} +static inline void v2_lerp( f32 a[2], f32 b[2], f32 t, f32 d[2] ) +{ + d[0] = a[0] + t*(b[0]-a[0]); + d[1] = a[1] + t*(b[1]-a[1]); +} +static inline void v2_normalize( f32 a[2] ) +{ + v2_muls( a, 1.0f / v2_length( a ), a ); +} +static void v2_normalize_clamp( f32 a[2] ) +{ + f32 l2 = v2_length2( a ); + if( l2 > 1.0f ) + v2_muls( a, 1.0f/sqrtf(l2), a ); +} +static inline void v2_floor( f32 a[2], f32 b[2] ) +{ + b[0] = floorf( a[0] ); + b[1] = floorf( a[1] ); +} +static inline void v2_fill( f32 a[2], f32 v ) +{ + a[0] = v; + a[1] = v; +} +static inline void v2_copysign( f32 a[2], f32 b[2] ) +{ + a[0] = copysignf( a[0], b[0] ); + a[1] = copysignf( a[1], b[1] ); +} +/* integer variants */ +static inline void v2i_copy( i32 a[2], i32 b[2] ) +{ + b[0] = a[0]; + b[1] = a[1]; +} +static inline int v2i_eq( i32 a[2], i32 b[2] ) +{ + return ((a[0] == b[0]) && (a[1] == b[1])); +} +static inline void v2i_add( i32 a[2], i32 b[2], i32 d[2] ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; +} +static inline void v2i_sub( i32 a[2], i32 b[2], i32 d[2] ) +{ + d[0] = a[0]-b[0]; + d[1] = a[1]-b[1]; +} + +/* f32[3] ----------------------------------------------------------------------------------------------------------- */ +static inline bool v3_is_numbers( f32 a[3] ) +{ + return f32_is_number( a[0] ) && f32_is_number( a[1] ) && f32_is_number( a[2] ); +} +static inline void v3_copy( f32 a[3], f32 b[3] ) +{ + b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; +} +static inline void v3_add( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; + d[2] = a[2]+b[2]; +} +static inline void v3i_add( i32 a[3], i32 b[3], i32 d[3] ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; + d[2] = a[2]+b[2]; +} +static inline void v3_sub( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = a[0]-b[0]; + d[1] = a[1]-b[1]; + d[2] = a[2]-b[2]; +} +static inline void v3i_sub( i32 a[3], i32 b[3], i32 d[3] ) +{ + d[0] = a[0]-b[0]; + d[1] = a[1]-b[1]; + d[2] = a[2]-b[2]; +} +static inline void v3_mul( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = a[0]*b[0]; + d[1] = a[1]*b[1]; + d[2] = a[2]*b[2]; +} +static inline void v3_div( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = b[0]!=0.0f? a[0]/b[0]: INFINITY; + d[1] = b[1]!=0.0f? a[1]/b[1]: INFINITY; + d[2] = b[2]!=0.0f? a[2]/b[2]: INFINITY; +} +static inline void v3_muls( f32 a[3], f32 s, f32 d[3] ) +{ + d[0] = a[0]*s; + d[1] = a[1]*s; + d[2] = a[2]*s; +} +static inline void v3_fill( f32 a[3], f32 v ) +{ + a[0] = v; + a[1] = v; + a[2] = v; +} +static inline void v4_fill( f32 a[4], f32 v ) +{ + a[0] = v; + a[1] = v; + a[2] = v; + a[3] = v; +} +static inline void v3_divs( f32 a[3], f32 s, f32 d[3] ) +{ + if( s == 0.0f ) + v3_fill( d, INFINITY ); + else + { + d[0] = a[0]/s; + d[1] = a[1]/s; + d[2] = a[2]/s; + } +} +static inline void v3_muladds( f32 a[3], f32 b[3], f32 s, f32 d[3] ) +{ + d[0] = a[0]+b[0]*s; + d[1] = a[1]+b[1]*s; + d[2] = a[2]+b[2]*s; +} +static inline void v3_muladd( f32 a[3], f32 b[3], f32 s[3], f32 d[3] ) +{ + d[0] = a[0]+b[0]*s[0]; + d[1] = a[1]+b[1]*s[1]; + d[2] = a[2]+b[2]*s[2]; +} +static inline f32 v3_dot( f32 a[3], f32 b[3] ) +{ + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; +} +static inline void v3_cross( f32 a[3], f32 b[3], f32 dest[3] ) +{ + f32 d[3]; + d[0] = a[1]*b[2] - a[2]*b[1]; + d[1] = a[2]*b[0] - a[0]*b[2]; + d[2] = a[0]*b[1] - a[1]*b[0]; + v3_copy( d, dest ); +} +static inline f32 v3_length2( f32 a[3] ) +{ + return v3_dot( a, a ); +} +static inline f32 v3_length( f32 a[3] ) +{ + return sqrtf( v3_length2( a ) ); +} +static inline f32 v3_dist2( f32 a[3], f32 b[3] ) +{ + f32 delta[3]; + v3_sub( a, b, delta ); + return v3_length2( delta ); +} +static inline f32 v3_dist( f32 a[3], f32 b[3] ) +{ + return sqrtf( v3_dist2( a, b ) ); +} +static inline void v3_normalize( f32 a[3] ) +{ + v3_muls( a, 1.f / v3_length( a ), a ); +} +static inline void v3_lerp( f32 a[3], f32 b[3], f32 t, f32 d[3] ) +{ + d[0] = a[0] + t*(b[0]-a[0]); + d[1] = a[1] + t*(b[1]-a[1]); + d[2] = a[2] + t*(b[2]-a[2]); +} +static inline void v3_min( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = f32_min(a[0], b[0]); + d[1] = f32_min(a[1], b[1]); + d[2] = f32_min(a[2], b[2]); +} +static inline void v3_max( f32 a[3], f32 b[3], f32 d[3] ) +{ + d[0] = f32_max(a[0], b[0]); + d[1] = f32_max(a[1], b[1]); + d[2] = f32_max(a[2], b[2]); +} +static inline f32 v3_min_element( f32 a[3] ) +{ + return f32_min( f32_min( a[0], a[1] ), a[2] ); +} +static inline f32 v3_max_element( f32 a[3] ) +{ + return f32_max( f32_max( a[0], a[1] ), a[2] ); +} +static inline void v3_floor( f32 a[3], f32 b[3] ) +{ + b[0] = floorf( a[0] ); + b[1] = floorf( a[1] ); + b[2] = floorf( a[2] ); +} +static inline void v3_ceil( f32 a[3], f32 b[3] ) +{ + b[0] = ceilf( a[0] ); + b[1] = ceilf( a[1] ); + b[2] = ceilf( a[2] ); +} +static inline void v3_negate( f32 a[3], f32 b[3] ) +{ + b[0] = -a[0]; + b[1] = -a[1]; + b[2] = -a[2]; +} +static inline void v3_rotate( f32 v[3], f32 angle, f32 axis[3], f32 d[3] ) +{ + f32 v1[3], v2[3], k[3], c, s; + c = cosf( angle ); + s = sinf( angle ); + v3_copy( axis, k ); + v3_normalize( k ); + v3_muls( v, c, v1 ); + v3_cross( k, v, v2 ); + v3_muls( v2, s, v2 ); + v3_add( v1, v2, v1 ); + v3_muls( k, v3_dot(k, v) * (1.0f - c), v2); + v3_add( v1, v2, d ); +} +static void v3_tangent_basis( f32 n[3], f32 out_tx[3], f32 out_ty[3] ) +{ + /* Compute tangent basis (box2d) */ + if( fabsf( n[0] ) >= 0.57735027f ) + { + out_tx[0] = n[1]; + out_tx[1] = -n[0]; + out_tx[2] = 0.0f; + } + else + { + out_tx[0] = 0.0f; + out_tx[1] = n[2]; + out_tx[2] = -n[1]; + } + v3_normalize( out_tx ); + v3_cross( n, out_tx, out_ty ); +} +static void v3_vector_to_angles( f32 v[3], f32 out_angles[3] ) +{ + f32 yaw = atan2f( v[0], -v[2] ), + pitch = atan2f( -v[1], sqrtf( v[0]*v[0] + v[2]*v[2] ) ); + out_angles[0] = yaw; + out_angles[1] = pitch; + out_angles[2] = 0.0f; +} +static void v3_angles_to_vector( f32 angles[3], f32 out_vector[3] ) +{ + out_vector[0] = sinf( angles[0] ) * cosf( angles[1] ); + out_vector[1] = -sinf( angles[1] ); + out_vector[2] = -cosf( angles[0] ) * cosf( angles[1] ); +} + +/* f32[4] ----------------------------------------------------------------------------------------------------------- */ +static inline void v4_copy( f32 a[4], f32 d[4] ) +{ + d[0] = a[0]; + d[1] = a[1]; + d[2] = a[2]; + d[3] = a[3]; +} +static inline void v4_add( f32 a[4], f32 b[3], f32 d[4] ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; + d[2] = a[2]+b[2]; + d[3] = a[3]+b[3]; +} +static inline void v4_muls( f32 a[4], f32 s, f32 d[4] ) +{ + d[0] = a[0]*s; + d[1] = a[1]*s; + d[2] = a[2]*s; + d[3] = a[3]*s; +} +static inline void v4_muladds( f32 a[4], f32 b[4], f32 s, f32 d[4] ) +{ + d[0] = a[0]+b[0]*s; + d[1] = a[1]+b[1]*s; + d[2] = a[2]+b[2]*s; + d[3] = a[3]+b[3]*s; +} +static inline void v4_lerp( f32 a[4], f32 b[4], f32 t, f32 d[4] ) +{ + d[0] = a[0] + t*(b[0]-a[0]); + d[1] = a[1] + t*(b[1]-a[1]); + d[2] = a[2] + t*(b[2]-a[2]); + d[3] = a[3] + t*(b[3]-a[3]); +} +static inline f32 v4_dot( f32 a[4], f32 b[4] ) +{ + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; +} +static inline f32 v4_length( f32 a[4] ) +{ + return sqrtf( v4_dot(a,a) ); +} + +/* f32[4] ----------------------------------------------------------------------------------------------------------- */ +static inline void q_identity( f32 q[4] ) +{ + q[0] = 0.0f; + q[1] = 0.0f; + q[2] = 0.0f; + q[3] = 1.0f; +} +static inline void q_axis_angle( f32 q[4], f32 axis[3], f32 angle ) +{ + f32 a = angle*0.5f, + c = cosf(a), + s = sinf(a); + q[0] = s*axis[0]; + q[1] = s*axis[1]; + q[2] = s*axis[2]; + q[3] = c; +} +static inline void q_mul( f32 q[4], f32 q1[4], f32 d[4] ) +{ + f32 t[4]; + t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1]; + t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0]; + t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3]; + t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2]; + v4_copy( t, d ); +} +static inline void q_normalize( f32 q[4] ) +{ + f32 l2 = v4_dot(q,q); + if( l2 < 0.00001f ) + q_identity( q ); + else + { + f32 s = 1.0f/sqrtf(l2); + q[0] *= s; + q[1] *= s; + q[2] *= s; + q[3] *= s; + } +} +static inline void q_inv( f32 q[4], f32 d[4] ) +{ + f32 s = 1.0f / v4_dot(q,q); + d[0] = -q[0]*s; + d[1] = -q[1]*s; + d[2] = -q[2]*s; + d[3] = q[3]*s; +} +static inline void q_nlerp( f32 a[4], f32 b[4], f32 t, f32 d[4] ) +{ + if( v4_dot(a,b) < 0.0f ) + { + f32 c[4]; + v4_muls( b, -1.0f, c ); + v4_lerp( a, c, t, d ); + } + else + v4_lerp( a, b, t, d ); + q_normalize( d ); +} +static inline void q_m3x3( f32 q[4], f32 d[3][3] ) +{ + f32 l = v4_length(q), + s = l > 0.0f? 2.0f/l: 0.0f, + xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0], + yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1], + zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2]; + d[0][0] = 1.0f - yy - zz; + d[1][1] = 1.0f - xx - zz; + d[2][2] = 1.0f - xx - yy; + d[0][1] = xy + wz; + d[1][2] = yz + wx; + d[2][0] = xz + wy; + d[1][0] = xy - wz; + d[2][1] = yz - wx; + d[0][2] = xz - wy; +} +static inline void q_mulv( f32 q[4], f32 v[3], f32 d[3] ) +{ + f32 v1[3], v2[3]; + v3_muls( q, 2.0f*v3_dot(q,v), v1 ); + v3_muls( v, q[3]*q[3] - v3_dot(q,q), v2 ); + v3_add( v1, v2, v1 ); + v3_cross( q, v, v2 ); + v3_muls( v2, 2.0f*q[3], v2 ); + v3_add( v1, v2, d ); +} +static inline f32 q_dist( f32 q0[4], f32 q1[4] ) +{ + return acosf( 2.0f * v4_dot(q0,q1) -1.0f ); +} +static inline void q_copy( f32 a[4], f32 b[4] ) +{ + b[0] = a[0]; + b[1] = a[1]; + b[2] = a[2]; + b[3] = a[3]; +} +static inline void m2x2_copy( f32 a[2][2], f32 b[2][2] ) +{ + v2_copy( a[0], b[0] ); + v2_copy( a[1], b[1] ); +} +static inline void m2x2_identity( f32 a[2][2] ) +{ + m2x2_copy( (f32[2][2]){{1,0}, + {0,1}}, a ); +} +static inline void m2x2_create_rotation( f32 a[2][2], f32 theta ) +{ + f32 s = sinf( theta ), + c = cosf( theta ); + a[0][0] = c; + a[0][1] = -s; + a[1][0] = s; + a[1][1] = c; +} +static inline void m2x2_mulv( f32 m[2][2], f32 v[2], f32 d[2] ) +{ + f32 res[2]; + res[0] = m[0][0]*v[0] + m[1][0]*v[1]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1]; + v2_copy( res, d ); +} + +/* f32[3][3] -------------------------------------------------------------------------------------------------------- */ +static inline void euler_m3x3( f32 angles[3], f32 d[3][3] ) +{ + f32 cosY = cosf( angles[0] ), + sinY = sinf( angles[0] ), + cosP = cosf( angles[1] ), + sinP = sinf( angles[1] ), + cosR = cosf( angles[2] ), + sinR = sinf( angles[2] ); + d[2][0] = -sinY * cosP; + d[2][1] = sinP; + d[2][2] = cosY * cosP; + d[0][0] = cosY * cosR; + d[0][1] = sinR; + d[0][2] = sinY * cosR; + v3_cross( d[0], d[2], d[1] ); +} +static inline void m3x3_q( f32 m[3][3], f32 q[4] ) +{ + f32 diag, r, rinv; + diag = m[0][0] + m[1][1] + m[2][2]; + if( diag >= 0.0f ) + { + r = sqrtf( 1.0f + diag ); + rinv = 0.5f / r; + q[0] = rinv * (m[1][2] - m[2][1]); + q[1] = rinv * (m[2][0] - m[0][2]); + q[2] = rinv * (m[0][1] - m[1][0]); + q[3] = r * 0.5f; + } + else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] ) + { + r = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] ); + rinv = 0.5f / r; + q[0] = r * 0.5f; + q[1] = rinv * (m[0][1] + m[1][0]); + q[2] = rinv * (m[0][2] + m[2][0]); + q[3] = rinv * (m[1][2] - m[2][1]); + } + else if( m[1][1] >= m[2][2] ) + { + r = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] ); + rinv = 0.5f / r; + q[0] = rinv * (m[0][1] + m[1][0]); + q[1] = r * 0.5f; + q[2] = rinv * (m[1][2] + m[2][1]); + q[3] = rinv * (m[2][0] - m[0][2]); + } + else + { + r = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] ); + rinv = 0.5f / r; + q[0] = rinv * (m[0][2] + m[2][0]); + q[1] = rinv * (m[1][2] + m[2][1]); + q[2] = r * 0.5f; + q[3] = rinv * (m[0][1] - m[1][0]); + } +} +/* a X b == [b]T a == ...*/ +static inline void m3x3_skew_symetric( f32 a[3][3], f32 v[3] ) +{ + a[0][0] = 0.0f; + a[0][1] = v[2]; + a[0][2] = -v[1]; + a[1][0] = -v[2]; + a[1][1] = 0.0f; + a[1][2] = v[0]; + a[2][0] = v[1]; + a[2][1] = -v[0]; + a[2][2] = 0.0f; +} +/* aka kronecker product */ +static inline void m3x3_outer_product( f32 out_m[3][3], f32 a[3], f32 b[3] ) +{ + out_m[0][0] = a[0]*b[0]; + out_m[0][1] = a[0]*b[1]; + out_m[0][2] = a[0]*b[2]; + out_m[1][0] = a[1]*b[0]; + out_m[1][1] = a[1]*b[1]; + out_m[1][2] = a[1]*b[2]; + out_m[2][0] = a[2]*b[0]; + out_m[2][1] = a[2]*b[1]; + out_m[2][2] = a[2]*b[2]; +} +static inline void m3x3_add( f32 a[3][3], f32 b[3][3], f32 d[3][3] ) +{ + v3_add( a[0], b[0], d[0] ); + v3_add( a[1], b[1], d[1] ); + v3_add( a[2], b[2], d[2] ); +} +static inline void m3x3_sub( f32 a[3][3], f32 b[3][3], f32 d[3][3] ) +{ + v3_sub( a[0], b[0], d[0] ); + v3_sub( a[1], b[1], d[1] ); + v3_sub( a[2], b[2], d[2] ); +} +static inline void m3x3_copy( f32 a[3][3], f32 b[3][3] ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); + v3_copy( a[2], b[2] ); +} +static inline void m3x3_identity( f32 a[3][3] ) +{ + m3x3_copy( (f32[3][3]){{1,0,0}, + {0,1,0}, + {0,0,1}}, a ); +} +static inline void m3x3_zero( f32 a[3][3] ) +{ + m3x3_copy( (f32[3][3]){{0,0,0}, + {0,0,0}, + {0,0,0}}, a ); +} +static inline void m3x3_diagonal( f32 out_a[3][3], f32 t ) +{ + m3x3_identity( out_a ); + out_a[0][0] = t; + out_a[1][1] = t; + out_a[2][2] = t; +} +static inline void m3x3_set_diagonal_v3( f32 a[3][3], f32 v[3] ) +{ + a[0][0] = v[0]; + a[1][1] = v[1]; + a[2][2] = v[2]; +} +static inline void m3x3_inv( f32 src[3][3], f32 dest[3][3] ) +{ + f32 a = src[0][0], b = src[0][1], c = src[0][2], + d = src[1][0], e = src[1][1], f = src[1][2], + g = src[2][0], h = src[2][1], i = src[2][2], + det = 1.f / (+a*(e*i-h*f) + -b*(d*i-f*g) + +c*(d*h-e*g)); + dest[0][0] = (e*i-h*f)*det; + dest[0][1] = -(b*i-c*h)*det; + dest[0][2] = (b*f-c*e)*det; + dest[1][0] = -(d*i-f*g)*det; + dest[1][1] = (a*i-c*g)*det; + dest[1][2] = -(a*f-d*c)*det; + dest[2][0] = (d*h-g*e)*det; + dest[2][1] = -(a*h-g*b)*det; + dest[2][2] = (a*e-d*b)*det; +} +static inline f32 m3x3_det( f32 m[3][3] ) +{ + return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); +} +static inline void m3x3_transpose( f32 src[3][3], f32 dest[3][3] ) +{ + f32 a = src[0][0], b = src[0][1], c = src[0][2], + d = src[1][0], e = src[1][1], f = src[1][2], + g = src[2][0], h = src[2][1], i = src[2][2]; + dest[0][0] = a; + dest[0][1] = d; + dest[0][2] = g; + dest[1][0] = b; + dest[1][1] = e; + dest[1][2] = h; + dest[2][0] = c; + dest[2][1] = f; + dest[2][2] = i; +} +static inline void m3x3_mul( f32 a[3][3], f32 b[3][3], f32 d[3][3] ) +{ + f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], + b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], + b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], + b20 = b[2][0], b21 = b[2][1], b22 = b[2][2]; + d[0][0] = a00*b00 + a10*b01 + a20*b02; + d[0][1] = a01*b00 + a11*b01 + a21*b02; + d[0][2] = a02*b00 + a12*b01 + a22*b02; + d[1][0] = a00*b10 + a10*b11 + a20*b12; + d[1][1] = a01*b10 + a11*b11 + a21*b12; + d[1][2] = a02*b10 + a12*b11 + a22*b12; + d[2][0] = a00*b20 + a10*b21 + a20*b22; + d[2][1] = a01*b20 + a11*b21 + a21*b22; + d[2][2] = a02*b20 + a12*b21 + a22*b22; +} +static inline void m3x3_mulv( f32 m[3][3], f32 v[3], f32 d[3] ) +{ + f32 res[3]; + res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2]; + res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2]; + v3_copy( res, d ); +} +static inline void m3x3_projection( f32 d[3][3], f32 left, f32 right, f32 bottom, f32 top ) +{ + f32 rl, tb; + m3x3_zero( d ); + rl = 1.0f / (right - left); + tb = 1.0f / (top - bottom); + d[0][0] = 2.0f * rl; + d[1][1] = 2.0f * tb; + d[2][2] = 1.0f; +} +static inline void m3x3_translate( f32 m[3][3], f32 v[3] ) +{ + m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0]; + m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1]; + m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2]; +} +static inline void m3x3_scale( f32 m[3][3], f32 v[3] ) +{ + v3_muls( m[0], v[0], m[0] ); + v3_muls( m[1], v[1], m[1] ); + v3_muls( m[2], v[2], m[2] ); +} +static inline void m3x3_scalef( f32 m[3][3], f32 f ) +{ + f32 v[3]; + v3_fill( v, f ); + m3x3_scale( m, v ); +} +static inline void m3x3_rotate( f32 m[3][3], f32 angle ) +{ + f32 m00 = m[0][0], m10 = m[1][0], + m01 = m[0][1], m11 = m[1][1], + m02 = m[0][2], m12 = m[1][2], c, s; + s = sinf( angle ); + c = cosf( angle ); + m[0][0] = m00 * c + m10 * s; + m[0][1] = m01 * c + m11 * s; + m[0][2] = m02 * c + m12 * s; + m[1][0] = m00 * -s + m10 * c; + m[1][1] = m01 * -s + m11 * c; + m[1][2] = m02 * -s + m12 * c; +} + +/* f32[4][3] -------------------------------------------------------------------------------------------------------- */ +static inline void m4x3_to_3x3( f32 a[4][3], f32 b[3][3] ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); + v3_copy( a[2], b[2] ); +} +static inline void m4x3_invert_affine( f32 a[4][3], f32 b[4][3] ) +{ + m3x3_transpose( a, b ); + m3x3_mulv( b, a[3], b[3] ); + v3_negate( b[3], b[3] ); +} +static inline void m4x3_invert_full( f32 src[4][3], f32 dst[4][3] ) +{ + f32 t2, t4, t5, + det, + a = src[0][0], b = src[0][1], c = src[0][2], + e = src[1][0], f = src[1][1], g = src[1][2], + i = src[2][0], j = src[2][1], k = src[2][2], + m = src[3][0], n = src[3][1], o = src[3][2]; + t2 = j*o - n*k; + t4 = i*o - m*k; + t5 = i*n - m*j; + + dst[0][0] = f*k - g*j; + dst[1][0] =-(e*k - g*i); + dst[2][0] = e*j - f*i; + dst[3][0] =-(e*t2 - f*t4 + g*t5); + + dst[0][1] =-(b*k - c*j); + dst[1][1] = a*k - c*i; + dst[2][1] =-(a*j - b*i); + dst[3][1] = a*t2 - b*t4 + c*t5; + + t2 = f*o - n*g; + t4 = e*o - m*g; + t5 = e*n - m*f; + + dst[0][2] = b*g - c*f ; + dst[1][2] =-(a*g - c*e ); + dst[2][2] = a*f - b*e ; + dst[3][2] =-(a*t2 - b*t4 + c * t5); + + det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]); + v3_muls( dst[0], det, dst[0] ); + v3_muls( dst[1], det, dst[1] ); + v3_muls( dst[2], det, dst[2] ); + v3_muls( dst[3], det, dst[3] ); +} +static inline void m4x3_copy( f32 a[4][3], f32 b[4][3] ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); + v3_copy( a[2], b[2] ); + v3_copy( a[3], b[3] ); +} +static inline void m4x3_identity( f32 a[4][3] ) +{ + m4x3_copy( (f32[4][3]){{1,0,0}, + {0,1,0}, + {0,0,1}, + {0,0,0}}, a ); +} +static inline void m4x3_mul( f32 a[4][3], f32 b[4][3], f32 d[4][3] ) +{ + f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], + a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], + b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], + b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], + b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], + b30 = b[3][0], b31 = b[3][1], b32 = b[3][2]; + d[0][0] = a00*b00 + a10*b01 + a20*b02; + d[0][1] = a01*b00 + a11*b01 + a21*b02; + d[0][2] = a02*b00 + a12*b01 + a22*b02; + d[1][0] = a00*b10 + a10*b11 + a20*b12; + d[1][1] = a01*b10 + a11*b11 + a21*b12; + d[1][2] = a02*b10 + a12*b11 + a22*b12; + d[2][0] = a00*b20 + a10*b21 + a20*b22; + d[2][1] = a01*b20 + a11*b21 + a21*b22; + d[2][2] = a02*b20 + a12*b21 + a22*b22; + d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30; + d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31; + d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32; +} +static inline void m4x3_mulv( f32 m[4][3], f32 v[3], f32 d[3] ) +{ + f32 result[3]; + result[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]; + result[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]; + result[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]; + v3_copy( result, d ); +} +static inline void m4x3_mulp( f32 m[4][3], f32 p[4], f32 d[4] ) +{ + f32 o[3]; + v3_muls( p, p[3], o ); + m4x3_mulv( m, o, o ); + m3x3_mulv( m, p, d ); + d[3] = v3_dot( o, d ); +} +static inline void m4x3_translate( f32 m[4][3], f32 v[3] ) +{ + v3_muladds( m[3], m[0], v[0], m[3] ); + v3_muladds( m[3], m[1], v[1], m[3] ); + v3_muladds( m[3], m[2], v[2], m[3] ); +} +static inline void m4x3_rotate_x( f32 m[4][3], f32 angle ) +{ + f32 t[4][3], + c = cosf( angle ), + s = sinf( angle ); + m4x3_identity( t ); + t[1][1] = c; + t[1][2] = s; + t[2][1] = -s; + t[2][2] = c; + m4x3_mul( m, t, m ); +} +static inline void m4x3_rotate_y( f32 m[4][3], f32 angle ) +{ + f32 t[4][3], + c = cosf( angle ), + s = sinf( angle ); + m4x3_identity( t ); + t[0][0] = c; + t[0][2] = -s; + t[2][0] = s; + t[2][2] = c; + m4x3_mul( m, t, m ); +} +static inline void m4x3_rotate_z( f32 m[4][3], f32 angle ) +{ + f32 t[4][3], + c = cosf( angle ), + s = sinf( angle ); + m4x3_identity( t ); + t[0][0] = c; + t[0][1] = s; + t[1][0] = -s; + t[1][1] = c; + m4x3_mul( m, t, m ); +} +static inline void m4x3_expand( f32 m[4][3], f32 d[4][4] ) +{ + v3_copy( m[0], d[0] ); + v3_copy( m[1], d[1] ); + v3_copy( m[2], d[2] ); + v3_copy( m[3], d[3] ); + d[0][3] = 0.0f; + d[1][3] = 0.0f; + d[2][3] = 0.0f; + d[3][3] = 1.0f; +} +static inline void m4x3_decompose( f32 m[4][3], f32 co[3], f32 q[4], f32 s[3] ) +{ + v3_copy( m[3], co ); + s[0] = v3_length(m[0]); + s[1] = v3_length(m[1]); + s[2] = v3_length(m[2]); + f32 rot[3][3]; + v3_divs( m[0], s[0], rot[0] ); + v3_divs( m[1], s[1], rot[1] ); + v3_divs( m[2], s[2], rot[2] ); + m3x3_q( rot, q ); +} +static inline void m4x3_expand_aabb_point( f32 m[4][3], f32 box[2][3], f32 point[3] ) +{ + f32 v[3]; + m4x3_mulv( m, point, v ); + v3_min( box[0], v, box[0] ); + v3_max( box[1], v, box[1] ); +} +static inline void m4x3_expand_aabb_aabb( f32 m[4][3], f32 boxa[2][3], f32 boxb[2][3] ) +{ + f32 a[3]; f32 b[3]; + v3_copy( boxb[0], a ); + v3_copy( boxb[1], b ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ a[0], a[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ a[0], b[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ b[0], b[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ b[0], a[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ a[0], a[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ a[0], b[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ b[0], b[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (f32 [3]){ b[0], a[1], b[2] } ); +} +static inline void m4x3_lookat( f32 m[4][3], f32 pos[3], f32 target[3], f32 up[3] ) +{ + f32 dir[3]; + v3_sub( target, pos, dir ); + v3_normalize( dir ); + v3_copy( dir, m[2] ); + v3_cross( up, m[2], m[0] ); + v3_normalize( m[0] ); + v3_cross( m[2], m[0], m[1] ); + v3_copy( pos, m[3] ); +} + +/* f32[4][4] -------------------------------------------------------------------------------------------------------- */ +static inline void m4x4_projection( f32 m[4][4], f32 angle, f32 ratio, f32 fnear, f32 ffar ) +{ + f32 scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear, + r = ratio * scale, + l = -r, + t = scale, + b = -t; + m[0][0] = 2.0f * fnear / (r - l); + m[0][1] = 0.0f; + m[0][2] = 0.0f; + m[0][3] = 0.0f; + m[1][0] = 0.0f; + m[1][1] = 2.0f * fnear / (t - b); + m[1][2] = 0.0f; + m[1][3] = 0.0f; + m[2][0] = (r + l) / (r - l); + m[2][1] = (t + b) / (t - b); + m[2][2] = -(ffar + fnear) / (ffar - fnear); + m[2][3] = -1.0f; + m[3][0] = 0.0f; + m[3][1] = 0.0f; + m[3][2] = -2.0f * ffar * fnear / (ffar - fnear); + m[3][3] = 0.0f; +} +static inline void m4x4_translate( f32 m[4][4], f32 v[3] ) +{ + v4_muladds( m[3], m[0], v[0], m[3] ); + v4_muladds( m[3], m[1], v[1], m[3] ); + v4_muladds( m[3], m[2], v[2], m[3] ); +} +static inline void m4x4_copy( f32 a[4][4], f32 b[4][4] ) +{ + v4_copy( a[0], b[0] ); + v4_copy( a[1], b[1] ); + v4_copy( a[2], b[2] ); + v4_copy( a[3], b[3] ); +} +static inline void m4x4_identity( f32 a[4][4] ) +{ + m4x4_copy( (f32[4][4]){{1,0,0,0}, + {0,1,0,0}, + {0,0,1,0}, + {0,0,0,1}}, a ); +} +static inline void m4x4_zero( f32 a[4][4] ) +{ + m4x4_copy( (f32[4][4]){{0,0,0,0}, + {0,0,0,0}, + {0,0,0,0}, + {0,0,0,0}}, a ); +} +static inline void m4x4_mul( f32 a[4][4], f32 b[4][4], f32 d[4][4] ) +{ + f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3], + a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3], + b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], b03 = b[0][3], + b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], b13 = b[1][3], + b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], b23 = b[2][3], + b30 = b[3][0], b31 = b[3][1], b32 = b[3][2], b33 = b[3][3]; + d[0][0] = a00*b00 + a10*b01 + a20*b02 + a30*b03; + d[0][1] = a01*b00 + a11*b01 + a21*b02 + a31*b03; + d[0][2] = a02*b00 + a12*b01 + a22*b02 + a32*b03; + d[0][3] = a03*b00 + a13*b01 + a23*b02 + a33*b03; + d[1][0] = a00*b10 + a10*b11 + a20*b12 + a30*b13; + d[1][1] = a01*b10 + a11*b11 + a21*b12 + a31*b13; + d[1][2] = a02*b10 + a12*b11 + a22*b12 + a32*b13; + d[1][3] = a03*b10 + a13*b11 + a23*b12 + a33*b13; + d[2][0] = a00*b20 + a10*b21 + a20*b22 + a30*b23; + d[2][1] = a01*b20 + a11*b21 + a21*b22 + a31*b23; + d[2][2] = a02*b20 + a12*b21 + a22*b22 + a32*b23; + d[2][3] = a03*b20 + a13*b21 + a23*b22 + a33*b23; + d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30*b33; + d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31*b33; + d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32*b33; + d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33; +} +static inline void m4x4_mulv( f32 m[4][4], f32 v[4], f32 d[4] ) +{ + f32 res[4]; + res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3]; + res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3]; + res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3]; + v4_copy( res, d ); +} +static inline void m4x4_inv( f32 a[4][4], f32 d[4][4] ) +{ + f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3], + a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3], + det, + t[6]; + + t[0] = a22*a33 - a32*a23; + t[1] = a21*a33 - a31*a23; + t[2] = a21*a32 - a31*a22; + t[3] = a20*a33 - a30*a23; + t[4] = a20*a32 - a30*a22; + t[5] = a20*a31 - a30*a21; + + d[0][0] = a11*t[0] - a12*t[1] + a13*t[2]; + d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]); + d[2][0] = a10*t[1] - a11*t[3] + a13*t[5]; + d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]); + + d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]); + d[1][1] = a00*t[0] - a02*t[3] + a03*t[4]; + d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]); + d[3][1] = a00*t[2] - a01*t[4] + a02*t[5]; + + t[0] = a12*a33 - a32*a13; + t[1] = a11*a33 - a31*a13; + t[2] = a11*a32 - a31*a12; + t[3] = a10*a33 - a30*a13; + t[4] = a10*a32 - a30*a12; + t[5] = a10*a31 - a30*a11; + + d[0][2] = a01*t[0] - a02*t[1] + a03*t[2]; + d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]); + d[2][2] = a00*t[1] - a01*t[3] + a03*t[5]; + d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]); + + t[0] = a12*a23 - a22*a13; + t[1] = a11*a23 - a21*a13; + t[2] = a11*a22 - a21*a12; + t[3] = a10*a23 - a20*a13; + t[4] = a10*a22 - a20*a12; + t[5] = a10*a21 - a20*a11; + + d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]); + d[1][3] = a00*t[0] - a02*t[3] + a03*t[4]; + d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]); + d[3][3] = a00*t[2] - a01*t[4] + a02*t[5]; + + det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]); + v4_muls( d[0], det, d[0] ); + v4_muls( d[1], det, d[1] ); + v4_muls( d[2], det, d[2] ); + v4_muls( d[3], det, d[3] ); +} +/* + * http://www.terathon.com/lengyel/Lengyel-Oblique.pdf + */ +static inline void m4x4_clip_projection( f32 mat[4][4], f32 plane[4] ) +{ + f32 c[4] = + { + (f32_sign(plane[0]) + mat[2][0]) / mat[0][0], + (f32_sign(plane[1]) + mat[2][1]) / mat[1][1], + -1.0f, + (1.0f + mat[2][2]) / mat[3][2] + }; + v4_muls( plane, 2.0f / v4_dot(plane,c), c ); + mat[0][2] = c[0]; + mat[1][2] = c[1]; + mat[2][2] = c[2] + 1.0f; + mat[3][2] = c[3]; +} +static inline void m4x4_reset_clipping( f32 mat[4][4], f32 far, f32 near ) +{ + mat[0][2] = 0.0f; + mat[1][2] = 0.0f; + mat[2][2] = -(far + near) / (far - near); + mat[3][2] = -2.0f * far * near / (far - near); +} + +/* box -------------------------------------------------------------------------------------------------------------- */ +static inline void box_addpt( f32 a[2][3], f32 pt[3] ) +{ + v3_min( a[0], pt, a[0] ); + v3_max( a[1], pt, a[1] ); +} +static inline void box_concat( f32 a[2][3], f32 b[2][3] ) +{ + v3_min( a[0], b[0], a[0] ); + v3_max( a[1], b[1], a[1] ); +} +static inline void box_copy( f32 a[2][3], f32 b[2][3] ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); +} +static inline bool box_overlap( f32 a[2][3], f32 b[2][3] ) +{ + return ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) && + ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) && + ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] ); +} +static inline bool box_within_pt( f32 box[2][3], f32 pt[3] ) +{ + return (pt[0] >= box[0][0]) && (pt[1] >= box[0][1]) && (pt[2] >= box[0][2]) && + (pt[0] <= box[1][0]) && (pt[1] <= box[1][1]) && (pt[2] <= box[1][2]); +} +static inline bool box_within( f32 greater[2][3], f32 lesser[2][3] ) +{ + f32 a[3], b[3]; + v3_sub( lesser[0], greater[0], a ); + v3_sub( lesser[1], greater[1], b ); + return (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) && + (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f); +} +static inline void box_init_inf( f32 box[2][3] ) +{ + v3_fill( box[0], INFINITY ); + v3_fill( box[1], -INFINITY ); +} + +/* planes ----------------------------------------------------------------------------------------------------------- */ +static inline void tri_to_plane( f64 a[3], f64 b[3], f64 c[3], f64 p[4] ) +{ + f64 edge0[3]; + f64 edge1[3]; + f64 l; + + edge0[0] = b[0] - a[0]; + edge0[1] = b[1] - a[1]; + edge0[2] = b[2] - a[2]; + + edge1[0] = c[0] - a[0]; + edge1[1] = c[1] - a[1]; + edge1[2] = c[2] - a[2]; + + p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1]; + p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2]; + p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0]; + + l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); + p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l; + + p[0] = p[0] / l; + p[1] = p[1] / l; + p[2] = p[2] / l; +} + +// TODO: What is going on here? +static inline bool plane_intersect3( f32 a[4], f32 b[4], f32 c[4], f32 p[3] ) +{ + f32 const epsilon = 1e-6f; + f32 x[3]; + v3_cross( a, b, x ); + f32 d = v3_dot( x, c ); + if( (d < epsilon) && (d > -epsilon) ) + return 0; + + f32 v0[3], v1[3], v2[3]; + v3_cross( b, c, v0 ); + v3_cross( c, a, v1 ); + v3_cross( a, b, v2 ); + + v3_muls( v0, a[3], p ); + v3_muladds( p, v1, b[3], p ); + v3_muladds( p, v2, c[3], p ); + v3_divs( p, d, p ); + return 1; +} +static inline bool plane_intersect2( f32 a[4], f32 b[4], f32 p[3], f32 n[3] ) +{ + f32 const epsilon = 1e-6f; + f32 c[3]; + v3_cross( a, b, c ); + f32 d = v3_length2( c ); + if( (d < epsilon) && (d > -epsilon) ) + return 0; + + f32 v0[3], v1[3], vx[3]; + v3_cross( c, b, v0 ); + v3_cross( a, c, v1 ); + + v3_muls( v0, a[3], vx ); + v3_muladds( vx, v1, b[3], vx ); + v3_divs( vx, d, p ); + v3_copy( c, n ); + return 1; +} +static inline bool plane_segment( f32 plane[4], f32 a[3], f32 b[3], f32 co[3] ) +{ + f32 d0 = v3_dot( a, plane ) - plane[3], + d1 = v3_dot( b, plane ) - plane[3]; + if( d0*d1 < 0.0f ) + { + f32 tot = 1.0f/( fabsf(d0)+fabsf(d1) ); + v3_muls( a, fabsf(d1) * tot, co ); + v3_muladds( co, b, fabsf(d0) * tot, co ); + return 1; + } + else return 0; +} +static inline f64 plane_polarity( f64 p[4], f64 a[3] ) +{ + return (a[0] * p[0] + a[1] * p[1] + a[2] * p[2]) + -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2]); +} +static inline f32 ray_plane( f32 plane[4], f32 co[3], f32 dir[3] ) +{ + f32 d = v3_dot( plane, dir ); + if( fabsf(d) > 1e-6f ) + { + f32 v0[3]; + v3_muls( plane, plane[3], v0 ); + v3_sub( v0, co, v0 ); + return v3_dot( v0, plane ) / d; + } + else return INFINITY; +} diff --git a/include/maths/rigidbody.h b/include/maths/rigidbody.h new file mode 100644 index 0000000..10bc05a --- /dev/null +++ b/include/maths/rigidbody.h @@ -0,0 +1,188 @@ +/* + * Copyright (C) 2021-2025 Mt.ZERO Software - All Rights Reserved + */ + +#define k_friction 0.4f +#define k_damp_linear 0.1f /* scale velocity 1/(1+x) */ +#define k_damp_angular 0.1f /* scale angular 1/(1+x) */ +#define k_penetration_slop 0.01f +#define k_rb_inertia_scale 4.0f +#define k_phys_baumgarte 0.2f +//#define k_gravity 9.6f +#define k_rb_density 8.0f + +extern f32 k_gravity; + +enum rb_shape { + k_rb_shape_none = 0, + k_rb_shape_box = 1, + k_rb_shape_sphere = 2, + k_rb_shape_capsule = 3, +}; + +typedef struct rigidbody rigidbody; +typedef struct rb_capsule rb_capsule; + +struct rb_capsule +{ + f32 h, r; +}; + +struct rigidbody +{ + v3f co, v, w; + v4f q; + + f32 inv_mass; + + m3x3f iI, iIw; /* inertia model and inverse world tensor */ + m4x3f to_world, to_local; +}; + +VG_API void _vg_rigidbody_register(void); + +/* + * Initialize rigidbody inverse mass and inertia tensor with some common shapes + */ +void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h, f32 density, f32 inertia_scale ); +void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale ); +void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale ); + +/* + * Update ALL matrices and tensors on rigidbody + */ +void rb_update_matrices( rigidbody *rb ); + +/* + * Extrapolate rigidbody into a transform based on vg accumulator. + * Useful for rendering + */ +void rb_extrapolate( rigidbody *rb, v3f co, v4f q ); +void rb_iter( rigidbody *rb ); +void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ); + +/* + * Creates relative contact velocity vector + */ +void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ); + +/* + * Apply impulse to object + */ +void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ); + +/* + * Effectors + */ +void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag ); +void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, f32 spring, f32 dampening, f32 timestep ); + + + +/* TODO: Get rid of this! */ +#define VG_MAX_CONTACTS 256 + +typedef struct rb_ct rb_ct; +struct rb_ct +{ + rigidbody *rba, *rbb; + v3f co, n; + v3f t[2]; + float p, bias, norm_impulse, tangent_impulse[2], + normal_mass, tangent_mass[2]; + + u32 element_id; + + enum contact_type type; +} +extern rb_contact_buffer[VG_MAX_CONTACTS]; +extern int rb_contact_count; + +int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, v3f coB, f32 rb, rb_ct *buf ); +int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, m4x3f mtxB, rb_capsule *cb, rb_ct *buf ); +int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, m4x3f mtxB, m4x3f mtxB_inverse, boxf box, rb_ct *buf ); +int rb_sphere__box( v3f coA, f32 ra, m4x3f mtxB, m4x3f mtxB_inverse, boxf box, rb_ct *buf ); +int rb_sphere__sphere( v3f coA, f32 ra, v3f coB, f32 rb, rb_ct *buf ); +int rb_sphere__triangle( m4x3f mtxA, f32 r, v3f tri[3], rb_ct *buf ); +int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ); +int rb_global_has_space( void ); +rb_ct *rb_global_buffer( void ); +int rb_manifold_apply_filtered( rb_ct *man, int len ); +int rb_box_triangle_sat( v3f extent, v3f center, m4x3f to_local, v3f tri_src[3] ); + +/* + * Merge two contacts if they are within radius(r) of eachother + */ +void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ); +void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ); + +/* + * Resolve overlapping pairs + */ +void rb_manifold_filter_pairs( rb_ct *man, int len, float r ); + +/* + * Remove contacts that are facing away from A + */ +void rb_manifold_filter_backface( rb_ct *man, int len ); + +/* + * Filter out duplicate coplanar results. Good for spheres. + */ +void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ); + +void rb_debug_contact( rb_ct *ct ); +void rb_solver_reset(void); +rb_ct *rb_global_ct(void); +void rb_prepare_contact( rb_ct *ct, f32 dt ); +void rb_depenetrate( rb_ct *manifold, int len, v3f dt ); +void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len ); +void rb_contact_restitution( rb_ct *ct, float cr ); +void rb_solve_contacts( rb_ct *buf, int len ); + + + +typedef struct rb_constr_pos rb_constr_pos; +typedef struct rb_constr_swingtwist rb_constr_swingtwist; + +struct rb_constr_pos +{ + rigidbody *rba, *rbb; + v3f lca, lcb; +}; + +struct rb_constr_swingtwist +{ + rigidbody *rba, *rbb; + + v4f conevx, conevy; /* relative to rba */ + v3f view_offset, /* relative to rba */ + coneva, conevxb;/* relative to rbb */ + + int tangent_violation, axis_violation; + v3f axis, tangent_axis, tangent_target, axis_target; + + float conet; + float tangent_mass, axis_mass; + + f32 conv_tangent, conv_axis; +}; + +void rb_debug_position_constraints( rb_constr_pos *buffer, int len ); +void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); +void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); + +/* + * Solve a list of positional constraints + */ +void rb_solve_position_constraints( rb_constr_pos *buf, int len ); +void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); +void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len ); +void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb ); + +/* + * Correct position constraint drift errors + * [ 0.0 <= amt <= 1.0 ]: the correction amount + */ +void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt ); +void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, int len, float amt ); diff --git a/metacompiler/build.sh b/metacompiler/build.sh new file mode 100755 index 0000000..2c322ee --- /dev/null +++ b/metacompiler/build.sh @@ -0,0 +1,3 @@ +gcc -I. -fsanitize=address -lasan -Wall -Wno-unused-function -O0 -ggdb -std=c99 \ + -include ../foundation/common.h \ + main.c -o vgc diff --git a/metacompiler/main.c b/metacompiler/main.c new file mode 100644 index 0000000..28ae17a --- /dev/null +++ b/metacompiler/main.c @@ -0,0 +1,8 @@ +i32 main( i32 argc, const c8 *argv[] ) +{ + struct stream *log = _log_event( k_log_ok, "I am alive!\n", __LINE_STRING__ ); + string_append( log, "Hello, more... " ); + string_append_i64( log, argc, 10 ); + string_append( log, "\n" ); + return 0; +} diff --git a/vg_audio.c b/source/engine/audio_mixer.c similarity index 100% rename from vg_audio.c rename to source/engine/audio_mixer.c diff --git a/vg_profiler.c b/source/engine/profiler.c similarity index 100% rename from vg_profiler.c rename to source/engine/profiler.c diff --git a/vg_shader.c b/source/engine/shader.c similarity index 100% rename from vg_shader.c rename to source/engine/shader.c diff --git a/vg_steam2.c b/source/engine/steamworks.c similarity index 100% rename from vg_steam2.c rename to source/engine/steamworks.c diff --git a/vg_tex.c b/source/engine/texture.c similarity index 100% rename from vg_tex.c rename to source/engine/texture.c diff --git a/source/foundation/allocator_heap.c b/source/foundation/allocator_heap.c new file mode 100644 index 0000000..f969ce0 --- /dev/null +++ b/source/foundation/allocator_heap.c @@ -0,0 +1,21 @@ +#include "common_api.h" +#include + +void *_heap_allocate( u64 size ) +{ + void *buffer = malloc( size ); + if( !buffer ) _fatal_exit( "Out of heap memory (malloc)" ); + return buffer; +} + +void *_heap_reallocate( void *buf, u64 size ) +{ + void *new_buffer = realloc( buf, size ); + if( !new_buffer ) _fatal_exit( "Out of heap memory (realloc)" ); + return new_buffer; +} + +void _heap_free( void *buf ) +{ + free( buf ); +} diff --git a/source/foundation/allocator_pool.c b/source/foundation/allocator_pool.c new file mode 100644 index 0000000..6301acf --- /dev/null +++ b/source/foundation/allocator_pool.c @@ -0,0 +1,88 @@ +#include "common_api.h" + +void pool_init( struct pool_allocator *pool, struct pool_node *nodes, u16 node_count, struct pool_chain *full_chain ) +{ + pool->nodes = nodes; + pool->count = node_count; + for( u16 i=0; inodes[ i ].l = id-1; + pool->nodes[ i ].r = id==node_count? 0: id+1; + pool->nodes[ i ].refcount = 0; + pool->nodes[ i ].unused0 = 0; + } + full_chain->head = 1; + full_chain->tail = node_count; + full_chain->count = node_count; + full_chain->unused0 = 0; +} + +u32 pool_index( struct pool_allocator *pool, u16 pool_id ) +{ + ASSERT_CRITICAL( pool_id ); + ASSERT_CRITICAL( pool_id <= pool->count ); + return pool_id-1; +} + +u16 pool_reference( struct pool_allocator *pool, u16 pool_id, bool increment ) +{ + ASSERT_CRITICAL( pool_id ); + ASSERT_CRITICAL( pool ); + struct pool_node *pnode = &pool->nodes[ pool_id -1 ]; + if( increment ) + { + ASSERT_CRITICAL( pnode->refcount < 100 ); // 100 is one of the largest numbers known to man + pnode->refcount ++; + } + else + { + ASSERT_CRITICAL( pnode->refcount > 0 ); + pnode->refcount --; + } + return pnode->refcount; +} + +u16 pool_next( struct pool_allocator *pool, u16 pool_id, bool right ) +{ + ASSERT_CRITICAL( pool_id ); + if( right ) return pool->nodes[ pool_id -1 ].r; + else return pool->nodes[ pool_id -1 ].l; +} + +void pool_switch( struct pool_allocator *pool, struct pool_chain *source, struct pool_chain *dest, u16 which ) +{ + ASSERT_CRITICAL( which ); + struct pool_node *pnode = &pool->nodes[ which -1 ]; + if( source ) + { + /* unlink from source list pointers */ + if( source->tail == which ) + source->tail = pnode->l; + if( source->head == which ) + source->head = pnode->r; + ASSERT_CRITICAL( source->count ); + source->count --; + } + + /* unlink self from chain */ + if( pnode->l ) + pool->nodes[ pnode->l -1 ].r = pnode->r; + if( pnode->r ) + pool->nodes[ pnode->r -1 ].l = pnode->l; + pnode->r = 0; + pnode->l = 0; + + /* update destination list head/tail pointers */ + if( dest ) + { + pnode->r = dest->head; + if( dest->head ) + pool->nodes[ dest->head -1 ].l = which; + dest->head = which; + if( dest->tail == 0 ) + dest->tail = which; + dest->count ++; + ASSERT_CRITICAL( dest->count ); // Overflow? in some mad scenario... + } +} diff --git a/source/foundation/allocator_queue.c b/source/foundation/allocator_queue.c new file mode 100644 index 0000000..9fa8228 --- /dev/null +++ b/source/foundation/allocator_queue.c @@ -0,0 +1 @@ +#include "common_api.h" diff --git a/source/foundation/allocator_stack.c b/source/foundation/allocator_stack.c new file mode 100644 index 0000000..c1bfa12 --- /dev/null +++ b/source/foundation/allocator_stack.c @@ -0,0 +1,92 @@ +#include "common_api.h" + +void stack_init( struct stack_allocator *stack, void *buffer, u32 capacity, const c8 *debug_name ) +{ + zero_buffer( stack, sizeof(struct stack_allocator) ); + stack->data = buffer? buffer: _heap_allocate( capacity ); + stack->capacity = capacity; +} + +void *stack_allocate( struct stack_allocator *stack, u32 size, u32 alignment, const c8 *debug_name ) +{ + if( !stack ) + return _heap_allocate( size ); + ASSERT_CRITICAL( (alignment >= 1) && (alignment <= 64) ); + ASSERT_CRITICAL( ~stack->offset & 0x10000000 ); + ASSERT_CRITICAL( ~size & 0x10000000 ); + + u32 new_usage = stack->offset + size + alignment; + if( new_usage > stack->capacity ) + { + struct stream *log = _log_event( k_log_error, "Stack @", __LINE_STRING__ ); + string_append_u64( log, (u64)stack, 16 ); + string_append( log, ", capacity: " ); + string_append_u64( log, stack->capacity, 10 ); + string_append( log, ", used: " ); + string_append_u64( log, stack->offset, 10 ); + string_append( log, ", new_usage: " ); + string_append_u64( log, new_usage, 10 ); + string_append( log, "\n" ); + _fatal_exit( "Stack allocator overflow" ); + // log( F"Stack @{here}, capacity: {stack->capacity}, used: {stack->used}, new_usage: {new_usage}" ); + } + while( ((u64)stack->data + stack->offset) & (alignment-1) ) + stack->offset ++; + void *block = stack->data + stack->offset; + stack->offset += size; + return block; +} + +void stack_clear( struct stack_allocator *stack ) +{ + stack->offset = 0; +} + +u32 stack_offset( struct stack_allocator *stack, void *pointer ) +{ + return pointer - stack->data; +} + +void *stack_pointer( struct stack_allocator *stack, u32 offset ) +{ + return stack->data + offset; +} + +void stack_extend_last( struct stack_allocator *stack, i32 extra_bytes ) +{ + stack->offset += extra_bytes; +} + +#define TEMP_STACK_MAX 8 +struct stack_allocator _temp_stack; +u32 _temp_offsets[ TEMP_STACK_MAX ]; +u32 _temp_stack_depth = 0; + +u32 _start_temporary_frame(void) +{ + if( !_temp_stack.data ) + stack_init( &_temp_stack, NULL, 1024*1024*20, "Temp allocator" ); + ASSERT_CRITICAL( _temp_stack_depth < TEMP_STACK_MAX ); + u32 offset = _temp_stack.offset; + _temp_offsets[ _temp_stack_depth ++ ] = offset; + return offset; +} + +void _end_temporary_frame( u32 whence ) +{ + ASSERT_CRITICAL( _temp_stack_depth ); + ASSERT_CRITICAL( _temp_offsets[ _temp_stack_depth-1 ] == whence ); + _temp_stack_depth --; + _temp_stack.offset = whence; +} + +void *_temporary_allocate( u32 bytes, u32 alignment ) +{ + ASSERT_CRITICAL( _temp_stack_depth ); + return stack_allocate( &_temp_stack, bytes, alignment, NULL ); +} + +struct stack_allocator *_temporary_stack_allocator(void) +{ + return &_temp_stack; +} diff --git a/source/foundation/allocator_stretchy.c b/source/foundation/allocator_stretchy.c new file mode 100644 index 0000000..d6857a6 --- /dev/null +++ b/source/foundation/allocator_stretchy.c @@ -0,0 +1,52 @@ +#include "common_api.h" +#define SMALL_SEGMENTS 1 + +void stretchy_init( struct stretchy_allocator *stretchy, u32 element_size ) +{ + stretchy->count = 0; + stretchy->element_size = element_size; +} + +static void stretchy_core( struct stretchy_allocator *stretchy, u32 index, i32 *out_i, i32 *out_offset ) +{ + *out_i = 31 - __builtin_clz( (index>>SMALL_SEGMENTS)+1 ), + *out_offset = index - ((1 << ((*out_i)+SMALL_SEGMENTS)) - (1 << SMALL_SEGMENTS)); +} + +void *stretchy_get( struct stretchy_allocator *stretchy, u32 index ) +{ + i32 i, offset; + stretchy_core( stretchy, index, &i, &offset ); + return stretchy->segments[ i ] + stretchy->element_size*offset; +} + +void *stretchy_append( struct stretchy_allocator *stretchy ) +{ + i32 i, offset; + stretchy_core( stretchy, stretchy->count ++, &i, &offset ); + if( offset == 0 ) + stretchy->segments[ i ] = _heap_allocate( (1<<(i+SMALL_SEGMENTS)) * stretchy->element_size ); + return stretchy->segments[ i ] + stretchy->element_size*offset; +} + +u32 stretchy_count( struct stretchy_allocator *stretchy ) +{ + return stretchy->count; +} + +void stretchy_free( struct stretchy_allocator *stretchy ) +{ + u32 c = 1; + for( u32 i=0; stretchy->count >= c; i ++ ) + { + struct stream *log = _log( k_log_info, "" ); + string_append_u64( log, i, 10 ); + string_append( log, " freed: " ); + string_append_u64( log, (u64)stretchy->segments[i], 16 ); + string_append( log, "\n" ); + + _heap_free( stretchy->segments[i] ); + c += 1 << (SMALL_SEGMENTS + i); + stretchy->segments[i] = NULL; + } +} diff --git a/source/foundation/buffer_operations.c b/source/foundation/buffer_operations.c new file mode 100644 index 0000000..99bd8cd --- /dev/null +++ b/source/foundation/buffer_operations.c @@ -0,0 +1,75 @@ +#include "common_api.h" + +void zero_buffer( void *buffer, u32 length ) +{ + for( u32 i=0; i +#include +#include +#include +#include +#include + +static void sync_signal_handler( i32 signum ) +{ + raise( SIGTRAP ); + if( signum == SIGSEGV ) _fatal_exit( "SIGSEGV" ); + if( signum == SIGBUS ) _fatal_exit( "SIGBUS" ); + if( signum == SIGFPE ) _fatal_exit( "SIGFPE" ); + if( signum == SIGILL ) _fatal_exit( "SIGILL" ); + _fatal_exit( "UNKNOWN SIGNAL" ); +} + +void _exit_init(void) +{ + signal( SIGSEGV, sync_signal_handler ); + signal( SIGBUS, sync_signal_handler ); + signal( SIGFPE, sync_signal_handler ); + signal( SIGILL, sync_signal_handler ); +} + +void _fatal_exit( const c8 *reason ) +{ + fflush( stdout ); + write( STDOUT_FILENO, reason, strlen(reason) ); + + void *functions[20]; + int count = backtrace( functions, 20 ); + backtrace_symbols_fd( functions, count, STDOUT_FILENO ); + + exit(-1); +} + +void _normal_exit( const c8 *reason ) +{ + exit(0); +} diff --git a/source/foundation/io.c b/source/foundation/io.c new file mode 100644 index 0000000..27ae615 --- /dev/null +++ b/source/foundation/io.c @@ -0,0 +1,93 @@ +#include "common_api.h" +#define _DEFAULT_SOURCE +#include +#include +#include +#include + +struct directory +{ + DIR *handle; + struct dirent *data; + u32 index; + enum directory_status status; +}; + +struct directory *directory_open( const c8 *path, struct stack_allocator *stack ) +{ + struct directory *directory = stack_allocate( stack, sizeof(struct directory), 8, "Directory" ); + zero_buffer( directory, sizeof(struct directory) ); + + directory->handle = opendir( path ); + if( !directory->handle ) + { + if( errno == ENOTDIR ) + { + directory->status = k_directory_status_is_file; + return directory; + } + else + { + directory->status = k_directory_status_invalid_path; + return directory; + } + } + directory->status = k_directory_status_ok; + directory->index = 1; + return directory; +} + +enum directory_status directory_status( struct directory *directory ) +{ + return directory->status; +} + +const c8 *directory_entry_name( struct directory *directory ) +{ + return directory->data->d_name; +} + +static bool directory_skip( struct directory *directory ) +{ + const c8 *s = directory_entry_name( directory ); + if( s[0] == '.' ) + { + if( s[1] == '\0' ) + return 1; + else if( s[1] == '.' ) + { + if( s[2] == '\0' ) + return 1; + } + } + return 0; +} + +bool directory_next_entry( struct directory *directory ) +{ + while( (directory->data = readdir(directory->handle)) ) + { + directory->index ++; + if( !directory_skip( directory ) ) + break; + } + if( directory->data ) + return 1; + else return 0; +} + +enum directory_entry_type directory_entry_type( struct directory *directory ) +{ + if( directory->data->d_type == DT_DIR ) return k_directory_entry_type_dir; + if( directory->data->d_type == DT_REG ) return k_directory_entry_type_file; + return k_directory_entry_type_unknown; +} + +void directory_close( struct directory *directory ) +{ + if( directory->handle ) + closedir( directory->handle ); + directory->handle = NULL; + directory->data = NULL; + directory->index = 0; +} diff --git a/source/foundation/keyvalues.c b/source/foundation/keyvalues.c new file mode 100644 index 0000000..c4af334 --- /dev/null +++ b/source/foundation/keyvalues.c @@ -0,0 +1,575 @@ +#include "common_api.h" +#include + +#define KV_PAGE_COUNT 32 + +struct keyvalue +{ + u32 key_info; /* 20 bit hash, 10 bit key length, 2 bit type */ + u32 key_offset; /* 32 bit, indexes kvs->stack.data. same for any other _offset field */ + u32 brother_offset; + + union + { + struct + { + u16 offset_from_key, length; + } + value; + u32 first_child_offset; + }; +}; + +static struct keyvalue *keyvalues_new( struct keyvalues *kvs ) +{ + void *kv_page; + if( (kvs->kv_page_count == KV_PAGE_COUNT) || (kvs->kv_page_offset == 0) ) + { + u32 page_size = sizeof(struct keyvalue)*KV_PAGE_COUNT; + kv_page = stack_allocate( kvs->stack, page_size, 64, "KV Page" ); + zero_buffer( kv_page, page_size ); + kvs->kv_page_offset = stack_offset( kvs->stack, kv_page ); + kvs->kv_page_count = 0; + } + else + kv_page = stack_pointer( kvs->stack, kvs->kv_page_offset ); + struct keyvalue *kv = kv_page + kvs->kv_page_count * sizeof(struct keyvalue); + kvs->kv_page_count ++; + return kv; +} + +void keyvalues_init( struct keyvalues *kvs, struct stack_allocator *stack ) +{ + zero_buffer( kvs, sizeof(struct keyvalues) ); + kvs->stack = stack; + + struct keyvalue *root_kv = keyvalues_new( kvs ); + kvs->root_offset = stack_offset( kvs->stack, root_kv ); +} + +static u32 keyvalues_string_append( struct keyvalues *kvs, const c8 *string ) +{ + if( string == NULL ) + return 0; + + i32 length = buffer_first_index(string,0,0)+1; + char *buf = stack_allocate( kvs->stack, length, 1, "KV string (appended)" ); + buffer_copy( string, buf, length ); + return stack_offset( kvs->stack, buf ); +} + +u32 keyvalues_append_string( struct keyvalues *kvs, u32 parent_offset, const c8 *key, const c8 *value ) +{ + if( parent_offset == 0 ) + parent_offset = kvs->root_offset; + + struct keyvalue *kv = keyvalues_new( kvs ); + u32 key_offset = keyvalues_string_append( kvs, key ), + value_offset = keyvalues_string_append( kvs, value ); + + kv->key_info = (buffer_djb2(key,0) & 0xFFFFF) | + (key?(buffer_first_index(key,0,0)<<20):0) | + (value?(0x1<<30):0); + kv->key_offset = key_offset; + + if( value ) + { + ASSERT_CRITICAL( key ); + kv->value.offset_from_key = value_offset-key_offset; + kv->value.length = buffer_first_index(value,0,0); + } + + u32 kv_offset = stack_offset( kvs->stack, kv ); + struct keyvalue *parent = stack_pointer( kvs->stack, parent_offset ); + if( parent->first_child_offset ) + { + u32 brother_offset = parent->first_child_offset; + while( 1 ) + { + struct keyvalue *brother = stack_pointer( kvs->stack, brother_offset ); + if( brother->brother_offset ) + brother_offset = brother->brother_offset; + else + { + brother->brother_offset = kv_offset; + break; + } + } + } + else parent->first_child_offset = kv_offset; + return kv_offset; +} + +u32 keyvalues_type( struct keyvalues *kvs, u32 kv_offset ) +{ + struct keyvalue *kv = stack_pointer( kvs->stack, kv_offset ); + return (kv->key_info >> 30) & 0x3; +} + +c8 *keyvalues_key( struct keyvalues *kvs, u32 kv_offset, u32 *out_length ) +{ + if( kv_offset == 0 ) + return NULL; + + struct keyvalue *kv = stack_pointer( kvs->stack, kv_offset ); + u32 length = (kv->key_info >> 20) & 0x3FF; + if( out_length ) + *out_length = length; + return length? stack_pointer( kvs->stack, kv->key_offset ): NULL; +} + +c8 *keyvalues_value( struct keyvalues *kvs, u32 kv_offset, u32 *out_length ) +{ + if( kv_offset == 0 ) + return NULL; + if( keyvalues_type( kvs, kv_offset ) == k_keyvalue_type_frame ) + return NULL; + else + { + struct keyvalue *kv = stack_pointer( kvs->stack, kv_offset ); + if( out_length ) + *out_length = kv->value.length; + return stack_pointer( kvs->stack, kv->key_offset + kv->value.offset_from_key ); + } +} + +u32 keyvalues_get_next( struct keyvalues *kvs, u32 kv_offset ) +{ + struct keyvalue *kv = stack_pointer( kvs->stack, kv_offset ); + return kv->brother_offset; +} + +u32 keyvalues_get_child( struct keyvalues *kvs, u32 root_offset, u32 index ) +{ + if( keyvalues_type( kvs, root_offset ) == k_keyvalue_type_frame ) + { + struct keyvalue *parent = stack_pointer( kvs->stack, root_offset ); + u32 offset = parent->first_child_offset; + + for( u32 i=0; (istack, offset ); + offset = kv->brother_offset; + } + return offset; + } + else return 0; +} + +u32 keyvalues_get( struct keyvalues *kvs, u32 root_offset, const c8 *key ) +{ + if( root_offset == 0 ) + root_offset = kvs->root_offset; + + u32 hash = buffer_djb2( key, 0 ); + u32 child_offset = keyvalues_get_child( kvs, root_offset, 0 ); + while( child_offset ) + { + struct keyvalue *kv = stack_pointer( kvs->stack, child_offset ); + if( ((kv->key_info ^ hash) & 0xFFFFF) == 0 ) + { + u32 key_length; + const c8 *child_key = keyvalues_key( kvs, child_offset, &key_length ); + if( child_key ) + { + for( u32 i=0; istack ); + ASSERT_CRITICAL( out_kvs->kv_page_count ); + + zero_buffer( parser, sizeof(struct keyvalue_parser) ); + parser->kvs = out_kvs; + parser->frame_stack[0].frame_offset = root_offset; +} + +static void keyvalue_parser_link( struct keyvalue_parser *parser, u32 offset, u32 depth ) +{ + u32 parent_offset = parser->frame_stack[ depth ].frame_offset; + struct keyvalue *parent = stack_pointer( parser->kvs->stack, parent_offset ); + if( parent->first_child_offset == 0 ) + parent->first_child_offset = offset; + + u32 brother_offset = parser->frame_stack[ depth ].latest_child_offset; + if( brother_offset ) + { + struct keyvalue *brother = stack_pointer( parser->kvs->stack, brother_offset ); + brother->brother_offset = offset; + } + parser->frame_stack[ depth ].latest_child_offset = offset; +} + +void keyvalues_parse_stream( struct keyvalues *kvs, u32 root_offset, struct stream *in_stream ) +{ + struct keyvalue_parser parser; + keyvalue_parser_init( &parser, kvs, root_offset ); + + c8 c; + while( stream_read( in_stream, &c, 1 ) ) + { + parser.stat_source_characters ++; + if( c == '\0' ) + break; + + bool is_control_character = 0; + if( parser.token0_deliminator ) + { + if( c == parser.token0_deliminator ) + is_control_character = 1; + } + else + { + if( c==' '||c=='\t'||c=='\r'||c=='\n'||c=='{'||c=='}' ) + is_control_character = 1; + } + + if( is_control_character ) + { + if( parser.token0_length ) + { + parser.token0_length --; + stack_extend_last( parser.kvs->stack, +1 ); + parser.token0_buffer[ parser.token0_length ] = '\0'; + + if( parser.token1_length ) + { + /* KV pair */ + struct keyvalue *kv = keyvalues_new( parser.kvs ); + kv->key_info = (0xFFFFF & parser.token1_hash) | + (0x3FF & parser.token1_length)<<20 | + (0x1) << 30; + kv->key_offset = parser.token1_start_offset; + kv->value.offset_from_key = parser.token0_start_offset - parser.token1_start_offset; + kv->value.length = parser.token0_length; + parser.token1_length = 0; + keyvalue_parser_link( &parser, stack_offset( parser.kvs->stack, kv ), parser.depth ); + } + else + { + /* shift */ + parser.token1_start_offset = parser.token0_start_offset; + parser.token1_length = parser.token0_length; + parser.token1_hash = parser.token0_hash; + } + parser.token0_length = 0; + } + + if( c=='{'||c=='}'||c=='\n' ) + { + if( c == '{' ) + { + struct keyvalue *kv = keyvalues_new( parser.kvs ); + if( parser.token1_length ) + { + kv->key_info = (0xFFFFF & parser.token1_hash) | (0x3FF & parser.token1_length) << 20; + kv->key_offset = parser.token1_start_offset; + } + else + kv->key_info = 5381; + + u32 id = stack_offset( parser.kvs->stack, kv ), + depth = parser.depth; + + parser.depth ++; + parser.frame_stack[ parser.depth ].latest_child_offset = 0; + parser.frame_stack[ parser.depth ].frame_offset = id; + keyvalue_parser_link( &parser, id, depth ); + parser.token1_length = 0; + } + else if( c == '}' ) + { + if( parser.depth ) + parser.depth --; + parser.token1_length = 0; + } + } + parser.token0_deliminator = 0; + } + else + { + if( parser.token0_length ) + { + stack_extend_last( parser.kvs->stack, +1 ); + parser.token0_buffer[ parser.token0_length-1 ] = c; + parser.token0_length ++; + parser.token0_hash = ((parser.token0_hash << 5) + parser.token0_hash) + (u32)c; + parser.stat_memory_strings ++; + } + else + { + if( c =='"' || c=='\'' ) + { + parser.token0_buffer = stack_allocate( parser.kvs->stack, 0, 1, "KV string" ); + parser.token0_start_offset = stack_offset( parser.kvs->stack, parser.token0_buffer ); + parser.token0_deliminator = c; + parser.token0_hash = 5381; + parser.token0_length = 1; + } + else + { + parser.token0_buffer = stack_allocate( parser.kvs->stack, 1, 1, "KV string" ); + parser.token0_start_offset = stack_offset( parser.kvs->stack, parser.token0_buffer ); + parser.token0_buffer[0] = c; + parser.token0_length = 2; + parser.token0_hash = ((5381<<5)+5381) + (u32)c; + parser.stat_memory_strings ++; + } + } + } + } +} + +#if 0 +void vg_kv_write_init( vg_kv_write *w, vg_stream *stream ) +{ + vg_zero_mem( w, sizeof(vg_kv_write) ); + w->stream = stream; +} + +static void vg_kv_write_indent( vg_kv_write *w ) +{ + for( u32 i=0; idepth; i ++ ) + vg_stream_write( w->stream, (c8[]){ ' ' }, 1 ); +} + +static void vg_kv_write_string( vg_kv_write *w, const c8 *string, u32 length ) +{ + if( length == 0 ) + length = 0xffffffff; + + u32 i=0; + char delim=0; + for( ;istream, (c8[]){ delim }, 1 ); + vg_stream_write( w->stream, string, i ); + if( delim ) vg_stream_write( w->stream, (c8[]){ delim }, 1 ); +} + +void vg_kv_write_block( vg_kv_write *w, const c8 *name, u32 name_length ) +{ + vg_kv_write_indent( w ); + if( name ) + { + vg_kv_write_string( w, name, name_length ); + vg_stream_write( w->stream, (c8[]){ '\n' }, 1 ); + vg_kv_write_indent( w ); + } + + vg_stream_write( w->stream, (c8[]){ '{', '\n' }, 2 ); + w->depth ++; +} + +void vg_kv_end_block( vg_kv_write *w ) +{ + w->depth --; + vg_kv_write_indent( w ); + vg_stream_write( w->stream, (c8[]){ '}', '\n' }, 2 ); +} + +void vg_kv_write_kv( vg_kv_write *w, const c8 *key, u32 key_len, const c8 *value, u32 value_len ) +{ + vg_kv_write_indent( w ); + vg_kv_write_string( w, key, key_len ); + vg_stream_write( w->stream, (c8[]){ ' ' }, 1 ); + vg_kv_write_string( w, value, value_len ); + vg_stream_write( w->stream, (c8[]){ '\n' }, 1 ); +} + +void vg_kv_write_tree( vg_kv_write *w, vg_kvs *kvs, u32 root_offset ) +{ + if( root_offset == 0 ) + root_offset = kvs->root_offset; + + VG_ASSERT( vg_kv_type( kvs, root_offset ) == k_vg_kv_type_frame ); + + u32 root_len; + const c8 *root_str = vg_kv_key( kvs, root_offset, &root_len ); + vg_kv_write_block( w, root_str, root_len ); + + u32 child_offset = vg_kv_child( kvs, root_offset, 0 ); + while( child_offset ) + { + if( vg_kv_type( kvs, child_offset ) == k_vg_kv_type_frame ) + vg_kv_write_tree( w, kvs, child_offset ); + else + { + u32 key_len; + const c8 *key_str = vg_kv_key( kvs, child_offset, &key_len ); + + u32 value_len; + const c8 *value_str = vg_kv_value( kvs, child_offset, &value_len ); + + if( key_str && value_str ) + vg_kv_write_kv( w, key_str, key_len, value_str, value_len ); + } + + child_offset = vg_kv_next( kvs, child_offset ); + } + + vg_kv_end_block( w ); +} +#endif + +bool keyvalues_read_file( struct keyvalues *kvs, const c8 *path, struct stack_allocator *stack ) +{ + struct stream stream; + if( stream_open_file( &stream, path, k_stream_read ) ) + { + keyvalues_init( kvs, stack ); + keyvalues_parse_stream( kvs, 0, &stream ); + stream_close( &stream ); + return 1; + } + else return 0; +} + +#if 0 +bool vg_kv_write_file( vg_kvs *kvs, const c8 *path ) +{ + vg_stream stream; + if( vg_file_stream_open( &stream, path, VG_STREAM_WRITE ) ) + { + vg_kv_write writer; + vg_kv_write_init( &writer, &stream ); + vg_kv_write_tree( &writer, kvs, 0 ); + vg_file_stream_close( &stream ); + return 1; + } + else return 0; +} +#endif diff --git a/source/foundation/logging.c b/source/foundation/logging.c new file mode 100644 index 0000000..f1fddc7 --- /dev/null +++ b/source/foundation/logging.c @@ -0,0 +1,37 @@ +#include "common_api.h" +#include +#include +#include + +static struct stream _stdout_stream; +struct stream *_get_console_stream() +{ + static bool init = 0; + if( !init ) + { + init = 1; + _stdout_stream.posix_stream = stdout; + _stdout_stream.offset = 0; + _stdout_stream.buffer_length = 0; + _stdout_stream.flags = k_stream_posix | k_stream_write; + } + return &_stdout_stream; +} + +struct stream *_log_event( enum log_event type, const c8 *text, const c8 *code_location ) +{ + struct stream *output = _get_console_stream(); + string_append( output, code_location ); + string_append( output, "|LOGGY| " ); + string_append( output, text ); + return output; +} + +void _log_append_errno( struct stream *stream ) +{ + string_append( stream, " Errno: " ); + string_append_u64( stream, errno, 10 ); + string_append( stream, " (" ); + string_append( stream, strerror(errno) ); + string_append( stream, ")\n" ); +} diff --git a/source/foundation/options.c b/source/foundation/options.c new file mode 100644 index 0000000..c4aace3 --- /dev/null +++ b/source/foundation/options.c @@ -0,0 +1,267 @@ +#include "common_api.h" +#define MAX_OPTIONS 32 +#define MAX_ARGUMENTS 32 + +struct +{ + struct option + { + const c8 *alias, *desc; + c8 alias_c; + enum option_type + { + k_option_type_flag, + k_option_type_option, + k_option_type_long_flag, + k_option_type_long_option + } + type; + } + options[ MAX_OPTIONS ]; + u32 option_count; + + struct argument + { + enum argument_type + { + k_argument_singles, + k_argument_long, + k_argument_assign, + k_argument_regular + } + type; + + u32 name_length; + const c8 *name, *value; + u32 used; + } + arguments[ MAX_ARGUMENTS ]; + u32 argument_count; +} +static _options; + +static void _option_register( struct option option ) +{ + ASSERT_CRITICAL( _options.option_count < MAX_OPTIONS ); + _options.options[ _options.option_count ++ ] = option; +} + +void _options_init( i32 argc, const c8 *argv[] ) +{ + ASSERT_CRITICAL( argc <= MAX_ARGUMENTS ); + _options.argument_count = 0; + for( i32 i=1; iname = NULL; + argument->name_length = 0; + argument->value = NULL; + argument->used = 0; + if( v[0] == '-' ) + { + if( v[1] == '-' ) + { + argument->name = v+2; + argument->type = k_argument_long; + u32 k=2; + while( v[ k ] ) + { + if( v[ k ] == '=' ) + { + argument->name_length = k-2; + argument->value = v + k + 1; + argument->type = k_argument_assign; + goto next; + } + k ++; + } + argument->name_length = k-2; + } + else + { + argument->name = v+1; + argument->type = k_argument_singles; + u32 k=1; + while( v[k] ) + k ++; + argument->name_length = k-1; + argument->used = ~((0x1<name_length)-1); + } + } + else + { + argument->type = k_argument_regular; + argument->value = v; + argument->name = v; + } +next:; + } +} + +void _options_check_end(void) +{ + struct stream *console = _get_console_stream(); + if( _option_long( "help", "Helps you" ) ) + { + for( u32 i=0; i<_options.option_count; i ++ ) + { + struct option *option = &_options.options[i]; + const c8 *desc = option->desc? option->desc: ""; + + u32 base_offset = console->offset; + if( option->type == k_option_type_flag || option->type == k_option_type_option ) + { + string_append( console, "-" ); + string_append_c8( console, option->alias_c ); + if( option->type == k_option_type_option ) + string_append( console, " " ); + } + + if( option->type == k_option_type_long_flag || option->type == k_option_type_long_option ) + { + string_append( console, "--" ); + string_append( console, option->alias ); + if( option->type == k_option_type_long_option ) + string_append( console, "=" ); + } + + while( console->offset < base_offset + 60 ) + string_append_c8( console, ' ' ); + + string_append( console, desc ); + string_append_c8( console, '\n' ); + } + _normal_exit( "Help page" ); + } + + bool errors = 0; + for( u32 i=0; i<_options.argument_count; i ++ ) + { + struct argument *argument = &_options.arguments[ i ]; + if( argument->used != 0xffffffff ) + { + if( argument->type == k_argument_singles ) + { + for( u32 j=0; j<32; j ++ ) + { + if( !(argument->used & (0x1<name[j] ); + string_append( console, "'\n" ); + } + } + } + else + { + string_append( console, "Unknown option '" ); + string_append( console, argument->name ); + string_append( console, "'\n" ); + } + errors = 1; + } + } + + if( errors ) + _normal_exit( "Invalid argument" ); +} + +bool _option_flag( c8 c, const c8 *desc ) +{ + _option_register( (struct option){ .alias=NULL, .alias_c=c, .desc=desc, .type=k_option_type_flag } ); + for( u32 i=0; i<_options.argument_count; i ++ ) + { + struct argument *arg = &_options.arguments[ i ]; + if( arg->type == k_argument_singles ) + { + for( u32 j=0; jname_length; j ++ ) + { + if( arg->name[j] == c ) + { + arg->used |= 0x1 << j; + return 1; + } + } + } + } + return 0; +} + +const c8 *_option_argument( c8 c, const c8 *desc ) +{ + _option_register( (struct option){ .alias=NULL, .alias_c=c, .desc=desc, .type=k_option_type_option } ); + for( u32 i=0; i+1<_options.argument_count; i ++ ) + { + struct argument *arg = &_options.arguments[ i ]; + if( arg->type == k_argument_singles ) + { + for( u32 j=0; jname_length; j ++ ) + { + if( arg->name[j] == c ) + { + arg->used |= 0x1 << j; + return _options.arguments[ i + 1 ].value; + } + } + } + } + return NULL; +} + +static struct argument *_argument_get_named( enum argument_type type, const c8 *name ) +{ + for( u32 i=0; i<_options.argument_count; i ++ ) + { + struct argument *arg = &_options.arguments[ i ]; + if( arg->type == type ) + { + for( u32 j=0; arg->name_length; j ++ ) + { + c8 ca = name[j], cb = arg->name[j]; + if( ca == cb ) + { + if( ca == 0 ) + { + arg->used = 0xffffffff; + return arg; + } + } + else break; + } + } + } + return NULL; +} + +bool _option_long( c8 *name, const c8 *desc ) +{ + _option_register( (struct option){ .alias=name, .alias_c=0, .desc=desc, .type=k_option_type_long_flag } ); + return _argument_get_named( k_argument_long, name ) != NULL; +} + +const c8 *_option_long_argument( c8 *name, const c8 *desc ) +{ + _option_register( (struct option){ .alias=name, .alias_c=0, .desc=desc, .type=k_option_type_long_option } ); + struct argument *arg = _argument_get_named( k_argument_assign, name ); + return arg? arg->value: NULL; +} + +const c8 *_option( u32 index ) +{ + u32 count = 0; + for( u32 i=0; i<_options.argument_count; i ++ ) + { + struct argument *arg = &_options.arguments[ i ]; + if( arg->type == k_argument_regular ) + { + count ++; + if( index+1 == count ) + { + arg->used = 0xffffffff; + return arg->value; + } + } + } + return NULL; +} diff --git a/source/foundation/stream.c b/source/foundation/stream.c new file mode 100644 index 0000000..8d3b743 --- /dev/null +++ b/source/foundation/stream.c @@ -0,0 +1,163 @@ +#include "common_api.h" +#include +#include +#include + +void stream_open_buffer( struct stream *stream, void *buffer, u32 buffer_length, u32 flags ) +{ + stream->flags = flags; + stream->buffer_length = buffer_length; + stream->offset = 0; + stream->buffer = buffer; +} + +bool stream_open_file( struct stream *stream, const c8 *path, u32 flags ) +{ +#if defined( VG_ENGINE ) + if( !_thread_has_flags( ENGINE_THREAD_BACKGROUND ) ) + { + _log_event( k_log_warning, + "Performance: I/O file stream opened in main thread. This will cause frame stalls!\n", + __LINE_STRING__ ); + } +#endif + stream->posix_stream = fopen( path, (flags & k_stream_write)? "wb": "rb" ); + stream->offset = 0; + stream->buffer_length = 0; + stream->flags = flags | k_stream_posix; + if( stream->posix_stream ) + return 1; + else + { + struct stream *log = _log_event( k_log_error, "Failure to open file stream ( ", __LINE_STRING__ ); + string_append_u64( log, errno, 10 ); + string_append( log, ": " ); + string_append( log, strerror(errno) ); + string_append( log, " )\n" ); + return 0; + } +} + +void stream_close( struct stream *stream ) +{ + if( stream->flags & k_stream_posix ) + { + fclose( stream->posix_stream ); + stream->posix_stream = NULL; + } +} + +static u32 stream_usable_length( struct stream *stream, u32 length ) +{ + if( stream->buffer_length && ((stream->offset + length) > stream->buffer_length) ) + return stream->buffer_length - stream->offset; + else return length; +} + +u32 stream_read( struct stream *stream, void *buffer, u32 length ) +{ + ASSERT_CRITICAL( stream->flags & k_stream_read ); + u32 read_length = stream_usable_length( stream, length ); + if( read_length != length ) + stream->flags |= k_stream_overflow_error; + + if( stream->flags & k_stream_posix ) + { + u64 l = (u32)fread( buffer, 1, read_length, stream->posix_stream ); + if( l != read_length ) + { + if( !feof( stream->posix_stream ) ) + { + if( ferror( stream->posix_stream ) ) + { + struct stream *log = _log_event( k_log_error, "FILE read error ( ", __LINE_STRING__ ); + string_append_u64( log, errno, 10 ); + string_append( log, ": " ); + string_append( log, strerror(errno) ); + string_append( log, " )\n" ); + + fclose( stream->posix_stream ); + _fatal_exit( "FILE stream read error" ); + } + else + { + fclose( stream->posix_stream ); + _fatal_exit( "FILE stream read error (no reason was given)" ); + } + } + } + read_length = l; + } + else + for( u32 i=0; ibuffer)[stream->offset + i]; + + for( u32 i=read_length; ioffset += read_length; + return read_length; +} + +u32 stream_write( struct stream *stream, const void *buffer, u32 length ) +{ + ASSERT_CRITICAL( stream->flags & k_stream_write ); + u32 write_length = stream_usable_length( stream, length ); + + if( stream->flags & k_stream_posix ) + { + u64 l = fwrite( buffer, 1, write_length, stream->posix_stream ); + if( l != write_length ) + { + if( ferror( stream->posix_stream ) ) + { + struct stream *log = _log_event( k_log_error, "FILE write error ( ", __LINE_STRING__ ); + string_append_u64( log, errno, 10 ); + string_append( log, ": " ); + string_append( log, strerror(errno) ); + string_append( log, " )\n" ); + fclose( stream->posix_stream ); + _fatal_exit( "FILE stream write error" ); + } + else + { + fclose( stream->posix_stream ); + _fatal_exit( "FILE stream write error (no reason was given)" ); + } + } + } + else + for( u32 i=0; ibuffer)[stream->offset + i] = ((u8 *)buffer)[i]; + + stream->offset += write_length; + return write_length; +} + +u32 stream_offset( struct stream *stream ) +{ + return stream->offset; +} + +void stream_seek( struct stream *stream, u32 offset ) +{ + if( stream->flags & k_stream_posix ) + { + if( fseek( stream->posix_stream, offset, SEEK_SET ) == -1 ) + { + struct stream *log = _log_event( k_log_error, "FILE seek error ( ", __LINE_STRING__ ); + string_append_u64( log, errno, 10 ); + string_append( log, ": " ); + string_append( log, strerror(errno) ); + string_append( log, " )\n" ); + fclose( stream->posix_stream ); + _fatal_exit( "FILE stream seek error" ); + } + } + stream->offset = offset; +} + +bool stream_error( struct stream *stream ) +{ + return stream->flags & k_stream_overflow_error? 1: 0; +} diff --git a/source/foundation/string.c b/source/foundation/string.c new file mode 100644 index 0000000..820a350 --- /dev/null +++ b/source/foundation/string.c @@ -0,0 +1,261 @@ +#include "common_api.h" + +void string_init( struct stream *string, c8 *buffer, u32 buffer_length ) +{ + ASSERT_CRITICAL( buffer ); + ASSERT_CRITICAL( buffer_length >= 2 ); + buffer[ 0 ] = 0; + stream_open_buffer( string, buffer, buffer_length-1, k_stream_write|k_stream_read|k_stream_text ); +} + +void string_clip( struct stream *string, i32 length ) +{ + ASSERT_CRITICAL( string ); + ASSERT_CRITICAL( length < string->buffer_length ); + ASSERT_CRITICAL( !(string->flags & k_stream_posix) ); + stream_seek( string, length ); + string->buffer[ length ] = 0; +} + +void string_append( struct stream *string, const c8 *substring ) +{ + for( u32 i=0; substring[i]; i ++ ) + stream_write( string, substring+i, 1 ); + if( !(string->flags & k_stream_posix) ) + string->buffer[ stream_offset( string ) ] = 0; +} + +void string_append_c8( struct stream *string, c8 c ) +{ + stream_write( string, &c, 1 ); + if( !(string->flags & k_stream_posix) ) + string->buffer[ stream_offset( string ) ] = 0; +} + +static u32 string_u64_core( c8 reverse_buffer[64], u64 value, u64 base, u32 max_characters ) +{ + const c8 *digits = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ!!!!!!!!"; + ASSERT_CRITICAL( base >= 2 ); + if( max_characters == 0 ) + max_characters = 64; + if( value ) + { + u32 i = 0; + while( value && (i= 2 ); + c8 temp[65]; + u32 digits = string_u64_core( temp, value<0? -value: value, base, 0 ); + if( value < 0 ) + temp[ digits ++ ] = '-'; + u32 padding = (digits>width)? 0: width - digits; + + for( u32 i=0; i= 2 ); + c8 temp[64]; + u32 digits = string_u64_core( temp, value, base, 0 ); + for( u32 i=0; i= 2 ); + if( value < 0 ) + { + string_append_c8( string, '-' ); + value = -value; + } + string_append_u64( string, value, base ); +} + +void string_append_f64( struct stream *string, f64 value, u64 base, u32 decimal_places ) +{ + ASSERT_CRITICAL( decimal_places ); + ASSERT_CRITICAL( base >= 2 ); + if( value < 0.0 ) + { + string_append_c8( string, '-' ); + value = -value; + } + u64 m = 10; + for( u32 i=0; i<(decimal_places-1); i ++ ) + m *= 10; + /* decimal part gets +1.0f because anything less than 1 does not have leading 0s + * when printing it in the u64 print */ + u64 decimal_part = (u64)((f64)m * (value+1.0f) + 0.5), + normal_part = (u64)value; + string_append_u64( string, normal_part, base ); + string_append_c8( string, '.' ); + c8 temp[64]; + u32 digits = string_u64_core( temp, decimal_part, base, decimal_places ); + for( u32 i=0; i= '0' && c <= '9' ) + { + result = result*10 + ((u64)c - (u64)'0'); + got = 1; + } + else + goto err; + info = string_parse_c8( string, &c ); + } + info = k_string_parse_ok; + goto ok; + + err: while( info == k_string_parse_ok ); + info = string_parse_c8( string, &c ); + info = k_string_parse_error; + + ok: *value = result; + return got? info: k_string_parse_eof; +} + +enum string_parse_result string_parse_i64( struct stream *string, i64 *value ) +{ + c8 c; + enum string_parse_result info = k_string_parse_whitespace; + while( info == k_string_parse_whitespace ) + info = string_parse_c8( string, &c ); + + i64 result = 0, + sign = 1; + if( c == '+' || c == '-' ) + { + if( c == '-' ) + sign = -1; + info = string_parse_c8( string, &c ); + } + + bool got = 0; + while( info == k_string_parse_ok ) + { + if( c >= '0' && c <= '9' ) + { + result = result*10 + ((i64)c - (i64)'0'); + got = 1; + } + else + goto err; + info = string_parse_c8( string, &c ); + } + info = k_string_parse_ok; + goto ok; + + err: while( info == k_string_parse_ok ) + info = string_parse_c8( string, &c ); + info = k_string_parse_error; + + ok: *value = result*sign; + return got? info: k_string_parse_eof; +} + +enum string_parse_result string_parse_f64( struct stream *string, f64 *value ) +{ + c8 c; + enum string_parse_result info = k_string_parse_whitespace; + while( info == k_string_parse_whitespace ) + info = string_parse_c8( string, &c ); + + i64 result = 0, + resultm= 0; + u32 dp = 0; + f64 sign = 1.0; + + if( c == '+' || c == '-' ) + { + if( c == '-' ) + sign = -1.0; + info = string_parse_c8( string, &c ); + } + + bool got = 0, got_decimal = 0; + while( info == k_string_parse_ok ) + { + if( c == '.' ) + { + if( got_decimal ) + goto err; + resultm = result; + result = 0; + got_decimal = 1; + dp = 0; + } + else if( c >= '0' && c <= '9' ) + { + result = result*10 + ((i64)c - (i64)'0'); + got = 1; + dp ++; + } + else + goto err; + info = string_parse_c8( string, &c ); + } + info = k_string_parse_ok; + goto ok; + + err: while( info == k_string_parse_ok ) + info = string_parse_c8( string, &c ); + info = k_string_parse_error; + + ok:; + f64 int_part = 0.0, decimal_part = 0.0; + if( got_decimal ) + { + decimal_part = (f64)result; + for( u32 i=0; i 1.0f ) + { + *t = 1.0f; + *s = vg_clampf((b-c)/a,0.0f,1.0f); + } + } + } + + v3_muladds( p1, d1, *s, c1 ); + v3_muladds( p2, d2, *t, c2 ); + + v3f v0; + v3_sub( c1, c2, v0 ); + return v3_length2( v0 ); +} + +static int point_inside_aabb( boxf box, v3f point ) +{ + if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) && + (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) ) + return 1; + else + return 0; +} + +static void closest_point_aabb( v3f p, boxf box, v3f dest ) +{ + v3_maxv( p, box[0], dest ); + v3_minv( dest, box[1], dest ); +} + +static void closest_point_obb( v3f p, boxf box, + m4x3f mtx, m4x3f inv_mtx, v3f dest ) +{ + v3f local; + m4x3_mulv( inv_mtx, p, local ); + closest_point_aabb( local, box, local ); + m4x3_mulv( mtx, local, dest ); +} + +static f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) +{ + v3f v0, v1; + v3_sub( b, a, v0 ); + v3_sub( point, a, v1 ); + + f32 t = v3_dot( v1, v0 ) / v3_length2(v0); + t = vg_clampf(t,0.0f,1.0f); + v3_muladds( a, v0, t, dest ); + return t; +} + +static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) +{ + v3f ab, ac, ap; + f32 d1, d2; + + /* Region outside A */ + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_sub( p, tri[0], ap ); + + d1 = v3_dot(ab,ap); + d2 = v3_dot(ac,ap); + if( d1 <= 0.0f && d2 <= 0.0f ) + { + v3_copy( tri[0], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region outside B */ + v3f bp; + f32 d3, d4; + + v3_sub( p, tri[1], bp ); + d3 = v3_dot( ab, bp ); + d4 = v3_dot( ac, bp ); + + if( d3 >= 0.0f && d4 <= d3 ) + { + v3_copy( tri[1], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Edge region of AB */ + f32 vc = d1*d4 - d3*d2; + if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + f32 v = d1 / (d1-d3); + v3_muladds( tri[0], ab, v, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region outside C */ + v3f cp; + f32 d5, d6; + v3_sub( p, tri[2], cp ); + d5 = v3_dot(ab, cp); + d6 = v3_dot(ac, cp); + + if( d6 >= 0.0f && d5 <= d6 ) + { + v3_copy( tri[2], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region of AC */ + f32 vb = d5*d2 - d1*d6; + if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + f32 w = d2 / (d2-d6); + v3_muladds( tri[0], ac, w, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region of BC */ + f32 va = d3*d6 - d5*d4; + if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) + { + f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); + v3f bc; + v3_sub( tri[2], tri[1], bc ); + v3_muladds( tri[1], bc, w, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* P inside region, Q via barycentric coordinates uvw */ + f32 d = 1.0f/(va+vb+vc), + v = vb*d, + w = vc*d; + + v3_muladds( tri[0], ab, v, dest ); + v3_muladds( dest, ac, w, dest ); +} + +enum contact_type +{ + k_contact_type_default, + k_contact_type_disabled, + k_contact_type_edge +}; + +static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) +{ + v3f ab, ac, ap; + f32 d1, d2; + + /* Region outside A */ + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_sub( p, tri[0], ap ); + + d1 = v3_dot(ab,ap); + d2 = v3_dot(ac,ap); + if( d1 <= 0.0f && d2 <= 0.0f ) + { + v3_copy( tri[0], dest ); + return k_contact_type_default; + } + + /* Region outside B */ + v3f bp; + f32 d3, d4; + + v3_sub( p, tri[1], bp ); + d3 = v3_dot( ab, bp ); + d4 = v3_dot( ac, bp ); + + if( d3 >= 0.0f && d4 <= d3 ) + { + v3_copy( tri[1], dest ); + return k_contact_type_edge; + } + + /* Edge region of AB */ + f32 vc = d1*d4 - d3*d2; + if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + f32 v = d1 / (d1-d3); + v3_muladds( tri[0], ab, v, dest ); + return k_contact_type_edge; + } + + /* Region outside C */ + v3f cp; + f32 d5, d6; + v3_sub( p, tri[2], cp ); + d5 = v3_dot(ab, cp); + d6 = v3_dot(ac, cp); + + if( d6 >= 0.0f && d5 <= d6 ) + { + v3_copy( tri[2], dest ); + return k_contact_type_edge; + } + + /* Region of AC */ + f32 vb = d5*d2 - d1*d6; + if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + f32 w = d2 / (d2-d6); + v3_muladds( tri[0], ac, w, dest ); + return k_contact_type_edge; + } + + /* Region of BC */ + f32 va = d3*d6 - d5*d4; + if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) + { + f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); + v3f bc; + v3_sub( tri[2], tri[1], bc ); + v3_muladds( tri[1], bc, w, dest ); + return k_contact_type_edge; + } + + /* P inside region, Q via barycentric coordinates uvw */ + f32 d = 1.0f/(va+vb+vc), + v = vb*d, + w = vc*d; + + v3_muladds( tri[0], ab, v, dest ); + v3_muladds( dest, ac, w, dest ); + + return k_contact_type_default; +} + +static void closest_point_elipse( v2f p, v2f e, v2f o ) +{ + v2f pabs, ei, e2, ve, t; + + v2_abs( p, pabs ); + v2_div( (v2f){ 1.0f, 1.0f }, e, ei ); + v2_mul( e, e, e2 ); + v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve ); + + v2_fill( t, 0.70710678118654752f ); + + for( int i=0; i<3; i++ ){ + v2f v, u, ud, w; + + v2_mul( ve, t, v ); /* ve*t*t*t */ + v2_mul( v, t, v ); + v2_mul( v, t, v ); + + v2_sub( pabs, v, u ); + v2_normalize( u ); + + v2_mul( t, e, ud ); + v2_sub( ud, v, ud ); + + v2_muls( u, v2_length( ud ), u ); + + v2_add( v, u, w ); + v2_mul( w, ei, w ); + + v2_maxv( (v2f){0.0f,0.0f}, w, t ); + v2_normalize( t ); + } + + v2_mul( t, e, o ); + v2_copysign( o, p ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 5.d Raycasts & Spherecasts + * ----------------------------------------------------------------------------- + */ + +static int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist ) +{ + v3f v0, v1; + f32 tmin, tmax; + + v3_sub( box[0], co, v0 ); + v3_sub( box[1], co, v1 ); + + v3_mul( v0, dir_inv, v0 ); + v3_mul( v1, dir_inv, v1 ); + + tmin = vg_minf( v0[0], v1[0] ); + tmax = vg_maxf( v0[0], v1[0] ); + tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] )); + tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] )); + tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] )); + tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] )); + + return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f); +} + +/* Time of intersection with ray vs triangle */ +static int ray_tri( v3f tri[3], v3f co, + v3f dir, f32 *dist, int backfaces ) +{ + f32 const kEpsilon = 0.00001f; + + v3f v0, v1, h, s, q, n; + f32 a,f,u,v,t; + + f32 *pa = tri[0], + *pb = tri[1], + *pc = tri[2]; + + v3_sub( pb, pa, v0 ); + v3_sub( pc, pa, v1 ); + v3_cross( dir, v1, h ); + v3_cross( v0, v1, n ); + + if( (v3_dot( n, dir ) > 0.0f) && !backfaces ) /* Backface culling */ + return 0; + + /* Parralel */ + a = v3_dot( v0, h ); + + if( a > -kEpsilon && a < kEpsilon ) + return 0; + + f = 1.0f/a; + v3_sub( co, pa, s ); + + u = f * v3_dot(s, h); + if( u < 0.0f || u > 1.0f ) + return 0; + + v3_cross( s, v0, q ); + v = f * v3_dot( dir, q ); + if( v < 0.0f || u+v > 1.0f ) + return 0; + + t = f * v3_dot(v1, q); + if( t > kEpsilon ) + { + *dist = t; + return 1; + } + else return 0; +} + +/* time of intersection with ray vs sphere */ +static int ray_sphere( v3f c, f32 r, + v3f co, v3f dir, f32 *t ) +{ + v3f m; + v3_sub( co, c, m ); + + f32 b = v3_dot( m, dir ), + c1 = v3_dot( m, m ) - r*r; + + /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */ + if( c1 > 0.0f && b > 0.0f ) + return 0; + + f32 discr = b*b - c1; + + /* A negative discriminant corresponds to ray missing sphere */ + if( discr < 0.0f ) + return 0; + + /* + * Ray now found to intersect sphere, compute smallest t value of + * intersection + */ + *t = -b - sqrtf( discr ); + + /* If t is negative, ray started inside sphere so clamp t to zero */ + if( *t < 0.0f ) + *t = 0.0f; + + return 1; +} + +/* + * time of intersection of ray vs cylinder + * The cylinder does not have caps but is finite + * + * Heavily adapted from regular segment vs cylinder from: + * Real-Time Collision Detection + */ +static int ray_uncapped_finite_cylinder( v3f q, v3f p, f32 r, + v3f co, v3f dir, f32 *t ) +{ + v3f d, m, n, sb; + v3_muladds( co, dir, 1.0f, sb ); + + v3_sub( q, p, d ); + v3_sub( co, p, m ); + v3_sub( sb, co, n ); + + f32 md = v3_dot( m, d ), + nd = v3_dot( n, d ), + dd = v3_dot( d, d ), + nn = v3_dot( n, n ), + mn = v3_dot( m, n ), + a = dd*nn - nd*nd, + k = v3_dot( m, m ) - r*r, + c = dd*k - md*md; + + if( fabsf(a) < 0.00001f ) + { + /* Segment runs parallel to cylinder axis */ + return 0; + } + + f32 b = dd*mn - nd*md, + discr = b*b - a*c; + + if( discr < 0.0f ) + return 0; /* No real roots; no intersection */ + + *t = (-b - sqrtf(discr)) / a; + if( *t < 0.0f ) + return 0; /* Intersection behind ray */ + + /* Check within cylinder segment */ + if( md + (*t)*nd < 0.0f ) + return 0; + + if( md + (*t)*nd > dd ) + return 0; + + /* Segment intersects cylinder between the endcaps; t is correct */ + return 1; +} + +/* + * Time of intersection of sphere and triangle. Origin must be outside the + * colliding area. This is a fairly long procedure. + */ +static int spherecast_triangle( v3f tri[3], + v3f co, v3f dir, f32 r, f32 *t, v3f n ) +{ + v3f sum[3]; + v3f v0, v1; + + v3_sub( tri[1], tri[0], v0 ); + v3_sub( tri[2], tri[0], v1 ); + v3_cross( v0, v1, n ); + v3_normalize( n ); + v3_muladds( tri[0], n, r, sum[0] ); + v3_muladds( tri[1], n, r, sum[1] ); + v3_muladds( tri[2], n, r, sum[2] ); + + int hit = 0; + f32 t_min = INFINITY, + t1; + + if( ray_tri( sum, co, dir, &t1, 0 ) ){ + t_min = vg_minf( t_min, t1 ); + hit = 1; + } + + /* + * Currently disabled; ray_sphere requires |d| = 1. it is not very important. + */ +#if 0 + for( int i=0; i<3; i++ ){ + if( ray_sphere( tri[i], r, co, dir, &t1 ) ){ + t_min = vg_minf( t_min, t1 ); + hit = 1; + } + } +#endif + + for( int i=0; i<3; i++ ){ + int i0 = i, + i1 = (i+1)%3; + + if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ){ + if( t1 < t_min ){ + t_min = t1; + + v3f co1, ct, cx; + v3_add( dir, co, co1 ); + v3_lerp( co, co1, t_min, ct ); + + closest_point_segment( tri[i0], tri[i1], ct, cx ); + v3_sub( ct, cx, n ); + v3_normalize( n ); + } + + hit = 1; + } + } + + *t = t_min; + return hit; +} + +/* + * ----------------------------------------------------------------------------- + * Section 5.e Curves + * ----------------------------------------------------------------------------- + */ + +static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, f32 t, v3f p ) +{ + f32 tt = t*t, + ttt = tt*t; + + v3_muls( p1, ttt, p ); + v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p ); + v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p ); + v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p ); +} + +static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p ) +{ + f32 u = 1.0f-t; + + v3_muls( p0, u*u, p ); + v3_muladds( p, p1, 2.0f*u*t, p ); + v3_muladds( p, p2, t*t, p ); +} + +static f32 explicit_bezier( f32 A[2], f32 B[2], f32 C[2], f32 D[2], f32 x ) +{ + f32 dAxDx = D[0]-A[0], + unitBx = (B[0] - A[0]) / dAxDx, + unitCx = (C[0] - A[0]) / dAxDx, + + /* cubic coefficients */ + a = 3.0f*unitBx - 3.0f*unitCx + 1.0f, + b = -6.0f*unitBx + 3.0f*unitCx, + c = 3.0f*unitBx, + d = -(x - A[0]) / dAxDx, + + t0 = 0.0f, + Ft0 = d, + t1 = 1.0f, + Ft1 = a+b+c+d, + tc, Ftcx; + + /* Illinois method to find root */ + for( u32 j=0; j<8; j ++ ) + { + tc = t1 - Ft1*(t1-t0)/(Ft1-Ft0); + Ftcx = tc*tc*tc*a + tc*tc*b + tc*c + d; + + if( fabsf(Ftcx) < 0.00001f ) + break; + + if( Ft1*Ftcx < 0.0f ) + { + t0 = t1; + Ft0 = Ft1; + } + else + Ft0 *= 0.5f; + + t1 = tc; + Ft1 = Ftcx; + } + + /* Evaluate parametric bezier */ + f32 t2 = tc*tc, + t3 = tc*tc*tc; + + return D[1] * t3 + + C[1] * (-3.0f*t3 + 3.0f*t2) + + B[1] * ( 3.0f*t3 - 6.0f*t2 + 3.0f*tc) + + A[1] * (-1.0f*t3 + 3.0f*t2 - 3.0f*tc + 1.0f); +} + + +/* + * ----------------------------------------------------------------------------- + * Section 5.f Volumes + * ----------------------------------------------------------------------------- + */ + +static f32 vg_sphere_volume( f32 r ) +{ + return (4.0f/3.0f) * VG_PIf * r*r*r; +} + +static f32 vg_box_volume( boxf box ) +{ + v3f e; + v3_sub( box[1], box[0], e ); + return e[0]*e[1]*e[2]; +} + +static f32 vg_cylinder_volume( f32 r, f32 h ) +{ + return VG_PIf * r*r * h; +} + +static f32 vg_capsule_volume( f32 r, f32 h ) +{ + return vg_sphere_volume( r ) + vg_cylinder_volume( r, h-r*2.0f ); +} + +static void vg_sphere_bound( f32 r, boxf out_box ) +{ + v3_fill( out_box[0], -r ); + v3_fill( out_box[1], r ); +} + +static void vg_capsule_bound( f32 r, f32 h, boxf out_box ) +{ + v3_copy( (v3f){-r,-h*0.5f,r}, out_box[0] ); + v3_copy( (v3f){-r, h*0.5f,r}, out_box[1] ); +} + + +/* + * ----------------------------------------------------------------------------- + * Section 5.g Inertia Tensors + * ----------------------------------------------------------------------------- + */ + +/* + * Translate existing inertia tensor + */ +static void vg_translate_inertia( m3x3f inout_inertia, f32 mass, v3f d ) +{ + /* + * I = I_0 + m*[(d.d)E_3 - d(X)d] + * + * I: updated tensor + * I_0: original tensor + * m: scalar mass + * d: translation vector + * (X): outer product + * E_3: identity matrix + */ + m3x3f t, outer, scale; + m3x3_diagonal( t, v3_dot(d,d) ); + m3x3_outer_product( outer, d, d ); + m3x3_sub( t, outer, t ); + m3x3_diagonal( scale, mass ); + m3x3_mul( scale, t, t ); + m3x3_add( inout_inertia, t, inout_inertia ); +} + +/* + * Rotate existing inertia tensor + */ +static void vg_rotate_inertia( m3x3f inout_inertia, m3x3f rotation ) +{ + /* + * I = R I_0 R^T + * + * I: updated tensor + * I_0: original tensor + * R: rotation matrix + * R^T: tranposed rotation matrix + */ + + m3x3f Rt; + m3x3_transpose( rotation, Rt ); + m3x3_mul( rotation, inout_inertia, inout_inertia ); + m3x3_mul( inout_inertia, Rt, inout_inertia ); +} +/* + * Create inertia tensor for box + */ +static void vg_box_inertia( boxf box, f32 mass, m3x3f out_inertia ) +{ + v3f e, com; + v3_sub( box[1], box[0], e ); + v3_muladds( box[0], e, 0.5f, com ); + + f32 ex2 = e[0]*e[0], + ey2 = e[1]*e[1], + ez2 = e[2]*e[2], + ix = (ey2+ez2) * mass * (1.0f/12.0f), + iy = (ex2+ez2) * mass * (1.0f/12.0f), + iz = (ex2+ey2) * mass * (1.0f/12.0f); + + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ix, iy, iz } ); + vg_translate_inertia( out_inertia, mass, com ); +} + +/* + * Create inertia tensor for sphere + */ +static void vg_sphere_inertia( f32 r, f32 mass, m3x3f out_inertia ) +{ + f32 ixyz = r*r * mass * (2.0f/5.0f); + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ixyz, ixyz, ixyz } ); +} + +/* + * Create inertia tensor for capsule + */ +static void vg_capsule_inertia( f32 r, f32 h, f32 mass, m3x3f out_inertia ) +{ + f32 density = mass / vg_capsule_volume( r, h ), + ch = h-r*2.0f, /* cylinder height */ + cm = VG_PIf * ch*r*r * density, /* cylinder mass */ + hm = VG_TAUf * (1.0f/3.0f) * r*r*r * density, /* hemisphere mass */ + + iy = r*r*cm * 0.5f, + ixz = iy * 0.5f + cm*ch*ch*(1.0f/12.0f), + + aux0= (hm*2.0f*r*r)/5.0f; + + iy += aux0 * 2.0f; + + f32 aux1= ch*0.5f, + aux2= aux0 + hm*(aux1*aux1 + 3.0f*(1.0f/8.0f)*ch*r); + + ixz += aux2*2.0f; + + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ixz, iy, ixz } ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 6.a PSRNG and some distributions + * ----------------------------------------------------------------------------- + */ + +/* An implementation of the MT19937 Algorithm for the Mersenne Twister + * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and + * T. Nishimura, "Mersenne Twister: A 623-dimensionally + * equidistributed uniform pseudorandom number generator," ACM + * Transactions on Modeling and Computer Simulation Vol. 8, No. 1, + * January pp.3-30 1998. + * + * http://www.sultanik.com/Mersenne_twister + * https://github.com/ESultanik/mtwister/blob/master/mtwister.c + */ + +#define MT_UPPER_MASK 0x80000000 +#define MT_LOWER_MASK 0x7fffffff +#define MT_TEMPERING_MASK_B 0x9d2c5680 +#define MT_TEMPERING_MASK_C 0xefc60000 + +#define MT_STATE_VECTOR_LENGTH 624 + +/* changes to STATE_VECTOR_LENGTH also require changes to this */ +#define MT_STATE_VECTOR_M 397 + +typedef struct vg_rand vg_rand; +struct vg_rand { + u32 mt[MT_STATE_VECTOR_LENGTH]; + i32 index; +}; + +static void vg_rand_seed( vg_rand *rand, unsigned long seed ) +{ + /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator + * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer + * Programming," Vol. 2 (2nd Ed.) pp.102. + */ + rand->mt[0] = seed & 0xffffffff; + for( rand->index=1; rand->indexindex++) + rand->mt[rand->index] = (6069 * rand->mt[rand->index-1]) & 0xffffffff; +} + +/* + * Generates a pseudo-randomly generated long. + */ +static u32 vg_randu32( vg_rand *rand ) +{ + u32 y; + /* mag[x] = x * 0x9908b0df for x = 0,1 */ + static u32 mag[2] = {0x0, 0x9908b0df}; + if( rand->index >= MT_STATE_VECTOR_LENGTH || rand->index < 0 ) + { + /* generate STATE_VECTOR_LENGTH words at a time */ + int kk; + if( rand->index >= MT_STATE_VECTOR_LENGTH+1 || rand->index < 0 ) + vg_rand_seed( rand, 4357 ); + for( kk=0; kkmt[kk] & MT_UPPER_MASK) | (rand->mt[kk+1] & MT_LOWER_MASK); + rand->mt[kk] = rand->mt[kk+MT_STATE_VECTOR_M] ^ (y>>1) ^ mag[y & 0x1]; + } + for( ; kkmt[kk] & MT_UPPER_MASK) | (rand->mt[kk+1] & MT_LOWER_MASK); + rand->mt[kk] = rand->mt[ kk+(MT_STATE_VECTOR_M-MT_STATE_VECTOR_LENGTH)] ^ (y >> 1) ^ mag[y & 0x1]; + } + y = (rand->mt[MT_STATE_VECTOR_LENGTH-1] & MT_UPPER_MASK) | (rand->mt[0] & MT_LOWER_MASK); + rand->mt[MT_STATE_VECTOR_LENGTH-1] = rand->mt[MT_STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1]; + rand->index = 0; + } + y = rand->mt[rand->index++]; + y ^= (y >> 11); + y ^= (y << 7) & MT_TEMPERING_MASK_B; + y ^= (y << 15) & MT_TEMPERING_MASK_C; + y ^= (y >> 18); + return y; +} + +/* + * Generates a pseudo-randomly generated f64 in the range [0..1]. + */ +static inline f64 vg_randf64( vg_rand *rand ) +{ + return (f64)vg_randu32(rand)/(f64)0xffffffff; +} + +static inline f64 vg_randf64_range( vg_rand *rand, f64 min, f64 max ) +{ + return vg_lerp( min, max, (f64)vg_randf64(rand) ); +} + +static inline void vg_rand_dir( vg_rand *rand, v3f dir ) +{ + dir[0] = vg_randf64(rand); + dir[1] = vg_randf64(rand); + dir[2] = vg_randf64(rand); + + /* warning: *could* be 0 length. + * very unlikely.. 1 in (2^32)^3. but its mathematically wrong. */ + + v3_muls( dir, 2.0f, dir ); + v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir ); + v3_normalize( dir ); +} + +static inline void vg_rand_sphere( vg_rand *rand, v3f co ) +{ + vg_rand_dir(rand,co); + v3_muls( co, cbrtf( vg_randf64(rand) ), co ); +} + +static void vg_rand_disc( vg_rand *rand, v2f co ) +{ + f32 a = vg_randf64(rand) * VG_TAUf; + co[0] = sinf(a); + co[1] = cosf(a); + v2_muls( co, sqrtf( vg_randf64(rand) ), co ); +} + +static void vg_rand_cone( vg_rand *rand, v3f out_dir, f32 angle ) +{ + f32 r = sqrtf(vg_randf64(rand)) * angle * 0.5f, + a = vg_randf64(rand) * VG_TAUf; + + out_dir[0] = sinf(a) * sinf(r); + out_dir[1] = cosf(a) * sinf(r); + out_dir[2] = cosf(r); +} + +static void vg_hsv_rgb( v3f hsv, v3f rgb ) +{ + i32 i = floorf( hsv[0]*6.0f ); + f32 v = hsv[2], + f = hsv[0] * 6.0f - (f32)i, + p = v * (1.0f-hsv[1]), + q = v * (1.0f-f*hsv[1]), + t = v * (1.0f-(1.0f-f)*hsv[1]); + + switch( i % 6 ) + { + case 0: rgb[0] = v; rgb[1] = t; rgb[2] = p; break; + case 1: rgb[0] = q; rgb[1] = v; rgb[2] = p; break; + case 2: rgb[0] = p; rgb[1] = v; rgb[2] = t; break; + case 3: rgb[0] = p; rgb[1] = q; rgb[2] = v; break; + case 4: rgb[0] = t; rgb[1] = p; rgb[2] = v; break; + case 5: rgb[0] = v; rgb[1] = p; rgb[2] = q; break; + } +} + +static void vg_rgb_hsv( v3f rgb, v3f hsv ) +{ + f32 min = v3_minf( rgb ), + max = v3_maxf( rgb ), + range = max-min, + k_epsilon = 0.00001f; + + hsv[2] = max; + if( range < k_epsilon ) + { + hsv[0] = 0.0f; + hsv[1] = 0.0f; + return; + } + + if( max > k_epsilon ) + hsv[1] = range/max; + else + { + hsv[0] = 0.0f; + hsv[1] = 0.0f; + return; + } + + if( rgb[0] >= max ) + hsv[0] = (rgb[1]-rgb[2])/range; + else if( max == rgb[1] ) + hsv[0] = 2.0f+(rgb[2]-rgb[0])/range; + else + hsv[0] = 4.0f+(rgb[0]-rgb[1])/range; + hsv[0] = vg_fractf( hsv[0] * (60.0f/360.0f) ); +} diff --git a/vg_perlin.c b/source/maths/perlin.c similarity index 100% rename from vg_perlin.c rename to source/maths/perlin.c diff --git a/vg_rigidbody.c b/source/maths/rigidbody.c similarity index 100% rename from vg_rigidbody.c rename to source/maths/rigidbody.c diff --git a/vg_rigidbody_collision.c b/source/maths/rigidbody_collision.c similarity index 100% rename from vg_rigidbody_collision.c rename to source/maths/rigidbody_collision.c diff --git a/vg_rigidbody_constraints.c b/source/maths/rigidbody_constraints.c similarity index 100% rename from vg_rigidbody_constraints.c rename to source/maths/rigidbody_constraints.c diff --git a/source/tools/metacompiler.c b/source/tools/metacompiler.c new file mode 100644 index 0000000..5135544 --- /dev/null +++ b/source/tools/metacompiler.c @@ -0,0 +1,115 @@ +#include "common_api.h" + +struct +{ + bool address_sanitizer, + thread_sanitizer, + no_pdb; +} +static _metacompiler; + +struct source_file +{ + const c8 *path; +}; +struct stretchy_allocator _source_list; + +void _append_kv_list( const c8 *path ) +{ + struct keyvalues list; + ASSERT_CRITICAL( keyvalues_read_file( &list, path, _temporary_stack_allocator() ) ); + + u32 block = keyvalues_get_child( &list, 0, 0 ); + while( block ) + { + if( compare_buffers( keyvalues_key( &list, block, NULL ), 0, "binary", 0 ) ) + { + u32 command = keyvalues_get_child( &list, block, 0 ); + const c8 *binary_name = NULL; + + while( command ) + { + if( compare_buffers( keyvalues_key( &list, command, NULL ), 0, "name", 0 ) ) + binary_name = keyvalues_value( &list, command, NULL ); + + if( compare_buffers( keyvalues_key( &list, command, NULL ), 0, "source", 0 ) ) + { + struct source_file *source = stretchy_append( &_source_list ); + source->path = keyvalues_value( &list, command, NULL ); + } + + if( compare_buffers( keyvalues_key( &list, command, NULL ), 0, "append", 0 ) ) + { + struct stream stream; + if( stream_open_file( &stream, path, k_stream_read ) ) + { + keyvalues_parse_stream( &list, block, &stream ); + stream_close( &stream ); + } + } + + command = keyvalues_get_next( &list, command ); + } + + ASSERT_CRITICAL( binary_name ); + struct stream *log = _log( k_log_info, "Working on binary: " ); + string_append( log, binary_name ); + string_append( log, "\n" ); + } + + block = keyvalues_get_next( &list, block ); + } +} + +i32 main( i32 argc, const c8 *argv[] ) +{ + _exit_init(); + _options_init( argc, argv ); + + const c8 *list_file = NULL; + + const c8 *arg; + if( _option_long( "tsan", "Build using thread sanitizer" ) ) + _metacompiler.thread_sanitizer = 1; + + if( _option_long( "asan", "Build using address sanitizer" ) ) + _metacompiler.address_sanitizer = 1; + + if( _option_long( "no-pdb", "Build using address sanitizer" ) ) + _metacompiler.no_pdb = 1; + + if( _option_long( "game", "Build using the game engine system" ) ) + { + } + + if( (arg = _option_long_argument( "compiler", "zig, clang, gcc" )) ) + { + } + + if( (arg = _option_long_argument( "arch", "any, i386, x86_64" )) ) + { + } + + list_file = _option( 0 ); + + //if( (arg = _option_long_argument( "platform", + + _options_check_end(); + + ASSERT_CRITICAL( list_file ); + + stretchy_init( &_source_list, sizeof(struct source_file) ); + + u32 temp_frame = _start_temporary_frame(); + _append_kv_list( list_file ); + _end_temporary_frame( temp_frame ); + + for( u32 i=0; ipath ); + string_append( log, "\n" ); + } + + stretchy_free( &_source_list ); +} diff --git a/src/metacompiler.c b/src/metacompiler.c new file mode 100644 index 0000000..494534d --- /dev/null +++ b/src/metacompiler.c @@ -0,0 +1,4 @@ +i32 main( i32 argc, const c8 *argv[] ) +{ + return 0; +} diff --git a/vg.hconf b/vg.hconf index e1e8a47..e1b78dc 100644 --- a/vg.hconf +++ b/vg.hconf @@ -16,12 +16,12 @@ # include # include -# include +# include /* remove */ # include -# include -# include -# include -# include +# include /* remove? eventually? */ +# include /* remove? */ +# include /* remove */ +# include /* remove? eventually? */ # include # include # include diff --git a/vg_io.c b/vg_io.c index b2fc61c..b927e41 100644 --- a/vg_io.c +++ b/vg_io.c @@ -1,3 +1,6 @@ +#include +#include + const char *dir_open_result_str[] = { [k_dir_open_none] = "None", diff --git a/vg_io.h b/vg_io.h index 032e4d5..ea2f5ee 100644 --- a/vg_io.h +++ b/vg_io.h @@ -1,6 +1,6 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_io.c" -#else +#include +#include +#include typedef struct vg_dir vg_dir; @@ -79,5 +79,3 @@ void *vg_file_read( vg_stack_allocator *stack, const char *path, u32 *size, bool bool vg_asset_write( const char *path, void *data, i64 size ); const char *vg_path_filename( const char *path ); - -#endif diff --git a/vg_log.h b/vg_log.h index 138efb8..c05a1f5 100644 --- a/vg_log.h +++ b/vg_log.h @@ -1,7 +1,3 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_log.c" -#else - #define VG_LOG_MCSTR(S) VG_LOG_MCSTR2(S) #define VG_LOG_MCSTR2(S) #S #define VG_LOG_WHERE __FILE__ ":" VG_LOG_MCSTR(__LINE__)\ @@ -67,5 +63,3 @@ void _vg_logx_va( FILE *file, const char *location, const char *prefix, const char *colour, const char *fmt, va_list args ); - -#endif diff --git a/vg_m.h b/vg_m.h deleted file mode 100644 index 5a92c4c..0000000 --- a/vg_m.h +++ /dev/null @@ -1,2683 +0,0 @@ -#if !defined( VG_MATH_HEADER ) -# define VG_MATH_HEADER - -/* Copyright (C) 2021-2025 Harry Godden (hgn) - All Rights Reserved - * - * 0. Misc - * 1. Scalar operations - * 2. Vectors - * 2.a 2D Vectors - * 2.b 3D Vectors - * 2.c 4D Vectors - * 3. Quaternions - * 4. Matrices - * 4.a 2x2 matrices - * 4.b 3x3 matrices - * 4.c 4x3 matrices - * 4.d 4x4 matrices - * 5. Geometry - * 5.a Boxes - * 5.b Planes - * 5.c Closest points - * 5.d Raycast & Spherecasts - * 5.e Curves - * 5.f Volumes - * 5.g Inertia tensors - * 6. Statistics - * 6.a Random numbers - */ - -#define VG_PIf 3.14159265358979323846264338327950288f -#define VG_TAUf 6.28318530717958647692528676655900576f - -/* - * ----------------------------------------------------------------------------- - * Section 0. Misc Operations - * ----------------------------------------------------------------------------- - */ - -/* get the f32 as the raw bits in a u32 without converting */ -static u32 vg_ftu32( f32 a ) -{ - u32 *ptr = (u32 *)(&a); - return *ptr; -} - -/* check if f32 is infinite */ -static int vg_isinff( f32 a ) -{ - return ((vg_ftu32(a)) & 0x7FFFFFFFU) == 0x7F800000U; -} - -/* check if f32 is not a number */ -static int vg_isnanf( f32 a ) -{ - return !vg_isinff(a) && ((vg_ftu32(a)) & 0x7F800000U) == 0x7F800000U; -} - -/* check if f32 is a number and is not infinite */ -static int vg_validf( f32 a ) -{ - return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U; -} - -static int v3_valid( v3f a ){ - for( u32 i=0; i<3; i++ ) - if( !vg_validf(a[i]) ) return 0; - return 1; -} - -/* - * ----------------------------------------------------------------------------- - * Section 1. Scalar Operations - * ----------------------------------------------------------------------------- - */ - -static inline f32 vg_minf( f32 a, f32 b ){ return a < b? a: b; } -static inline f32 vg_maxf( f32 a, f32 b ){ return a > b? a: b; } - -static inline int vg_min( int a, int b ){ return a < b? a: b; } -static inline int vg_max( int a, int b ){ return a > b? a: b; } - -static inline f32 vg_clampf( f32 a, f32 min, f32 max ) -{ - return vg_minf( max, vg_maxf( a, min ) ); -} - -static inline f32 vg_signf( f32 a ) -{ - return a < 0.0f? -1.0f: 1.0f; -} - -static inline f32 vg_fractf( f32 a ) -{ - return a - floorf( a ); -} - -static inline f64 vg_fractf64( f64 a ){ - return a - floor( a ); -} - -static f32 vg_cfrictf( f32 velocity, f32 F ) -{ - return -vg_signf(velocity) * vg_minf( F, fabsf(velocity) ); -} - -static inline f32 vg_rad( f32 deg ) -{ - return deg * VG_PIf / 180.0f; -} - -/* angle to reach b from a */ -static f32 vg_angle_diff( f32 a, f32 b ) -{ - f32 d = fmod(b,VG_TAUf)-fmodf(a,VG_TAUf); - if( fabsf(d) > VG_PIf ) - d = -vg_signf(d) * (VG_TAUf - fabsf(d)); - return d; -} - -/* - * quantize float to bit count - */ -static u32 vg_quantf( f32 a, u32 bits, f32 min, f32 max ){ - u32 mask = (0x1 << bits) - 1; - return vg_clampf((a - min) * ((f32)mask/(max-min)), 0.0f, mask ); -} - -/* - * un-quantize discreet to float - */ -static f32 vg_dequantf( u32 q, u32 bits, f32 min, f32 max ){ - u32 mask = (0x1 << bits) - 1; - return min + (f32)q * ((max-min) / (f32)mask); -} - -/* https://iquilezles.org/articles/functions/ - * - * Use k to control the stretching of the function. Its maximum, which is 1, - * happens at exactly x = 1/k. - */ -static f32 vg_exp_impulse( f32 x, f32 k ){ - f32 h = k*x; - return h*expf(1.0f-h); -} - -/* - * ----------------------------------------------------------------------------- - * Section 2.a 2D Vectors - * ----------------------------------------------------------------------------- - */ - -static inline void v2_copy( v2f a, v2f d ) -{ - d[0] = a[0]; d[1] = a[1]; -} - -static inline void v2_zero( v2f a ) -{ - a[0] = 0.f; a[1] = 0.f; -} - -static inline void v2_add( v2f a, v2f b, v2f d ) -{ - d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; -} - -static inline void v2_sub( v2f a, v2f b, v2f d ) -{ - d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; -} - -static inline void v2_minv( v2f a, v2f b, v2f dest ) -{ - dest[0] = vg_minf(a[0], b[0]); - dest[1] = vg_minf(a[1], b[1]); -} - -static inline void v2_maxv( v2f a, v2f b, v2f dest ) -{ - dest[0] = vg_maxf(a[0], b[0]); - dest[1] = vg_maxf(a[1], b[1]); -} - -static inline f32 v2_dot( v2f a, v2f b ) -{ - return a[0] * b[0] + a[1] * b[1]; -} - -static inline f32 v2_cross( v2f a, v2f b ) -{ - return a[0]*b[1] - a[1]*b[0]; -} - -static inline void v2_abs( v2f a, v2f d ) -{ - d[0] = fabsf( a[0] ); - d[1] = fabsf( a[1] ); -} - -static inline void v2_muls( v2f a, f32 s, v2f d ) -{ - d[0] = a[0]*s; d[1] = a[1]*s; -} - -static inline void v2_divs( v2f a, f32 s, v2f d ) -{ - d[0] = a[0]/s; d[1] = a[1]/s; -} - -static inline void v2_mul( v2f a, v2f b, v2f d ) -{ - d[0] = a[0]*b[0]; - d[1] = a[1]*b[1]; -} - -static inline void v2_div( v2f a, v2f b, v2f d ) -{ - d[0] = a[0]/b[0]; d[1] = a[1]/b[1]; -} - -static inline void v2_muladd( v2f a, v2f b, v2f s, v2f d ) -{ - d[0] = a[0]+b[0]*s[0]; - d[1] = a[1]+b[1]*s[1]; -} - -static inline void v2_muladds( v2f a, v2f b, f32 s, v2f d ) -{ - d[0] = a[0]+b[0]*s; - d[1] = a[1]+b[1]*s; -} - -static inline f32 v2_length2( v2f a ) -{ - return a[0]*a[0] + a[1]*a[1]; -} - -static inline f32 v2_length( v2f a ) -{ - return sqrtf( v2_length2( a ) ); -} - -static inline f32 v2_dist2( v2f a, v2f b ) -{ - v2f delta; - v2_sub( a, b, delta ); - return v2_length2( delta ); -} - -static inline f32 v2_dist( v2f a, v2f b ) -{ - return sqrtf( v2_dist2( a, b ) ); -} - -static inline void v2_lerp( v2f a, v2f b, f32 t, v2f d ) -{ - d[0] = a[0] + t*(b[0]-a[0]); - d[1] = a[1] + t*(b[1]-a[1]); -} - -static inline void v2_normalize( v2f a ) -{ - v2_muls( a, 1.0f / v2_length( a ), a ); -} - -static void v2_normalize_clamp( v2f a ) -{ - f32 l2 = v2_length2( a ); - if( l2 > 1.0f ) - v2_muls( a, 1.0f/sqrtf(l2), a ); -} - -static inline void v2_floor( v2f a, v2f b ) -{ - b[0] = floorf( a[0] ); - b[1] = floorf( a[1] ); -} - -static inline void v2_fill( v2f a, f32 v ) -{ - a[0] = v; - a[1] = v; -} - -static inline void v2_copysign( v2f a, v2f b ) -{ - a[0] = copysignf( a[0], b[0] ); - a[1] = copysignf( a[1], b[1] ); -} - -/* integer variants - * ---------------- */ - -static inline void v2i_copy( v2i a, v2i b ) -{ - b[0] = a[0]; b[1] = a[1]; -} - -static inline int v2i_eq( v2i a, v2i b ) -{ - return ((a[0] == b[0]) && (a[1] == b[1])); -} - -static inline void v2i_add( v2i a, v2i b, v2i d ) -{ - d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; -} - -static inline void v2i_sub( v2i a, v2i b, v2i d ) -{ - d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; -} - -/* - * ----------------------------------------------------------------------------- - * Section 2.b 3D Vectors - * ----------------------------------------------------------------------------- - */ - -static inline void v3_copy( v3f a, v3f b ) -{ - b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; -} - -static inline void v3_zero( v3f a ) -{ - a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; -} - -static inline void v3_add( v3f a, v3f b, v3f d ) -{ - d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; -} - -static inline void v3i_add( v3i a, v3i b, v3i d ) -{ - d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; -} - -static inline void v3_sub( v3f a, v3f b, v3f d ) -{ - d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2]; -} - -static inline void v3i_sub( v3i a, v3i b, v3i d ) -{ - d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2]; -} - -static inline void v3_mul( v3f a, v3f b, v3f d ) -{ - d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2]; -} - -static inline void v3_div( v3f a, v3f b, v3f d ) -{ - d[0] = b[0]!=0.0f? a[0]/b[0]: INFINITY; - d[1] = b[1]!=0.0f? a[1]/b[1]: INFINITY; - d[2] = b[2]!=0.0f? a[2]/b[2]: INFINITY; -} - -static inline void v3_muls( v3f a, f32 s, v3f d ) -{ - d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s; -} - -static inline void v3_fill( v3f a, f32 v ) -{ - a[0] = v; - a[1] = v; - a[2] = v; -} - -static inline void v4_fill( v4f a, f32 v ) -{ - a[0] = v; - a[1] = v; - a[2] = v; - a[3] = v; -} - -static inline void v3_divs( v3f a, f32 s, v3f d ) -{ - if( s == 0.0f ) - v3_fill( d, INFINITY ); - else - { - d[0] = a[0]/s; - d[1] = a[1]/s; - d[2] = a[2]/s; - } -} - -static inline void v3_muladds( v3f a, v3f b, f32 s, v3f d ) -{ - d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s; -} - -static inline void v3_muladd( v2f a, v2f b, v2f s, v2f d ) -{ - d[0] = a[0]+b[0]*s[0]; - d[1] = a[1]+b[1]*s[1]; - d[2] = a[2]+b[2]*s[2]; -} - -static inline f32 v3_dot( v3f a, v3f b ) -{ - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; -} - -static inline void v3_cross( v3f a, v3f b, v3f dest ) -{ - v3f d; - d[0] = a[1]*b[2] - a[2]*b[1]; - d[1] = a[2]*b[0] - a[0]*b[2]; - d[2] = a[0]*b[1] - a[1]*b[0]; - v3_copy( d, dest ); -} - -static inline f32 v3_length2( v3f a ) -{ - return v3_dot( a, a ); -} - -static inline f32 v3_length( v3f a ) -{ - return sqrtf( v3_length2( a ) ); -} - -static inline f32 v3_dist2( v3f a, v3f b ) -{ - v3f delta; - v3_sub( a, b, delta ); - return v3_length2( delta ); -} - -static inline f32 v3_dist( v3f a, v3f b ) -{ - return sqrtf( v3_dist2( a, b ) ); -} - -static inline void v3_normalize( v3f a ) -{ - v3_muls( a, 1.f / v3_length( a ), a ); -} - -static inline f32 vg_lerpf( f32 a, f32 b, f32 t ){ - return a + t*(b-a); -} - -static inline f64 vg_lerp( f64 a, f64 b, f64 t ) -{ - return a + t*(b-a); -} - -static inline void vg_slewf( f32 *a, f32 b, f32 speed ){ - f32 d = vg_signf( b-*a ), - c = *a + d*speed; - *a = vg_minf( b*d, c*d ) * d; -} - -static inline f32 vg_smoothstepf( f32 x ){ - return x*x*(3.0f - 2.0f*x); -} - - -/* correctly lerp around circular period -pi -> pi */ -static f32 vg_alerpf( f32 a, f32 b, f32 t ) -{ - f32 d = fmodf( b-a, VG_TAUf ), - s = fmodf( 2.0f*d, VG_TAUf ) - d; - return a + s*t; -} - -static inline void v3_lerp( v3f a, v3f b, f32 t, v3f d ) -{ - d[0] = a[0] + t*(b[0]-a[0]); - d[1] = a[1] + t*(b[1]-a[1]); - d[2] = a[2] + t*(b[2]-a[2]); -} - -static inline void v3_minv( v3f a, v3f b, v3f dest ) -{ - dest[0] = vg_minf(a[0], b[0]); - dest[1] = vg_minf(a[1], b[1]); - dest[2] = vg_minf(a[2], b[2]); -} - -static inline void v3_maxv( v3f a, v3f b, v3f dest ) -{ - dest[0] = vg_maxf(a[0], b[0]); - dest[1] = vg_maxf(a[1], b[1]); - dest[2] = vg_maxf(a[2], b[2]); -} - -static inline f32 v3_minf( v3f a ) -{ - return vg_minf( vg_minf( a[0], a[1] ), a[2] ); -} - -static inline f32 v3_maxf( v3f a ) -{ - return vg_maxf( vg_maxf( a[0], a[1] ), a[2] ); -} - -static inline void v3_floor( v3f a, v3f b ) -{ - b[0] = floorf( a[0] ); - b[1] = floorf( a[1] ); - b[2] = floorf( a[2] ); -} - -static inline void v3_ceil( v3f a, v3f b ) -{ - b[0] = ceilf( a[0] ); - b[1] = ceilf( a[1] ); - b[2] = ceilf( a[2] ); -} - -static inline void v3_negate( v3f a, v3f b ) -{ - b[0] = -a[0]; - b[1] = -a[1]; - b[2] = -a[2]; -} - -static inline void v3_rotate( v3f v, f32 angle, v3f axis, v3f d ) -{ - v3f v1, v2, k; - f32 c, s; - - c = cosf( angle ); - s = sinf( angle ); - - v3_copy( axis, k ); - v3_normalize( k ); - v3_muls( v, c, v1 ); - v3_cross( k, v, v2 ); - v3_muls( v2, s, v2 ); - v3_add( v1, v2, v1 ); - v3_muls( k, v3_dot(k, v) * (1.0f - c), v2); - v3_add( v1, v2, d ); -} - -static void v3_tangent_basis( v3f n, v3f tx, v3f ty ){ - /* Compute tangent basis (box2d) */ - if( fabsf( n[0] ) >= 0.57735027f ){ - tx[0] = n[1]; - tx[1] = -n[0]; - tx[2] = 0.0f; - } - else{ - tx[0] = 0.0f; - tx[1] = n[2]; - tx[2] = -n[1]; - } - - v3_normalize( tx ); - v3_cross( n, tx, ty ); -} - -/* - * Compute yaw and pitch based of a normalized vector representing forward - * forward: -z - * result -> (YAW,PITCH,0.0) - */ -static void v3_angles( v3f v, v3f out_angles ){ - float yaw = atan2f( v[0], -v[2] ), - pitch = atan2f( - -v[1], - sqrtf( - v[0]*v[0] + v[2]*v[2] - ) - ); - - out_angles[0] = yaw; - out_angles[1] = pitch; - out_angles[2] = 0.0f; -} - -/* - * Compute the forward vector from (YAW,PITCH,ROLL) - * forward: -z - */ -static void v3_angles_vector( v3f angles, v3f out_v ){ - out_v[0] = sinf( angles[0] ) * cosf( angles[1] ); - out_v[1] = -sinf( angles[1] ); - out_v[2] = -cosf( angles[0] ) * cosf( angles[1] ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 2.c 4D Vectors - * ----------------------------------------------------------------------------- - */ - -static inline void v4_copy( v4f a, v4f b ) -{ - b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3]; -} - -static inline void v4_add( v4f a, v4f b, v4f d ) -{ - d[0] = a[0]+b[0]; - d[1] = a[1]+b[1]; - d[2] = a[2]+b[2]; - d[3] = a[3]+b[3]; -} - -static inline void v4_zero( v4f a ) -{ - a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f; -} - -static inline void v4_muls( v4f a, f32 s, v4f d ) -{ - d[0] = a[0]*s; - d[1] = a[1]*s; - d[2] = a[2]*s; - d[3] = a[3]*s; -} - -static inline void v4_muladds( v4f a, v4f b, f32 s, v4f d ) -{ - d[0] = a[0]+b[0]*s; - d[1] = a[1]+b[1]*s; - d[2] = a[2]+b[2]*s; - d[3] = a[3]+b[3]*s; -} - -static inline void v4_lerp( v4f a, v4f b, f32 t, v4f d ) -{ - d[0] = a[0] + t*(b[0]-a[0]); - d[1] = a[1] + t*(b[1]-a[1]); - d[2] = a[2] + t*(b[2]-a[2]); - d[3] = a[3] + t*(b[3]-a[3]); -} - -static inline f32 v4_dot( v4f a, v4f b ) -{ - return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; -} - -static inline f32 v4_length( v4f a ) -{ - return sqrtf( v4_dot(a,a) ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 3 Quaternions - * ----------------------------------------------------------------------------- - */ - -static inline void q_identity( v4f q ) -{ - q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f; -} - -static inline void q_axis_angle( v4f q, v3f axis, f32 angle ) -{ - f32 a = angle*0.5f, - c = cosf(a), - s = sinf(a); - - q[0] = s*axis[0]; - q[1] = s*axis[1]; - q[2] = s*axis[2]; - q[3] = c; -} - -static inline void q_mul( v4f q, v4f q1, v4f d ) -{ - v4f t; - t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1]; - t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0]; - t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3]; - t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2]; - v4_copy( t, d ); -} - -static inline void q_normalize( v4f q ) -{ - f32 l2 = v4_dot(q,q); - if( l2 < 0.00001f ) q_identity( q ); - else { - f32 s = 1.0f/sqrtf(l2); - q[0] *= s; - q[1] *= s; - q[2] *= s; - q[3] *= s; - } -} - -static inline void q_inv( v4f q, v4f d ) -{ - f32 s = 1.0f / v4_dot(q,q); - d[0] = -q[0]*s; - d[1] = -q[1]*s; - d[2] = -q[2]*s; - d[3] = q[3]*s; -} - -static inline void q_nlerp( v4f a, v4f b, f32 t, v4f d ){ - if( v4_dot(a,b) < 0.0f ){ - v4f c; - v4_muls( b, -1.0f, c ); - v4_lerp( a, c, t, d ); - } - else - v4_lerp( a, b, t, d ); - - q_normalize( d ); -} - -static inline void q_m3x3( v4f q, m3x3f d ) -{ - f32 - l = v4_length(q), - s = l > 0.0f? 2.0f/l: 0.0f, - - xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0], - yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1], - zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2]; - - d[0][0] = 1.0f - yy - zz; - d[1][1] = 1.0f - xx - zz; - d[2][2] = 1.0f - xx - yy; - d[0][1] = xy + wz; - d[1][2] = yz + wx; - d[2][0] = xz + wy; - d[1][0] = xy - wz; - d[2][1] = yz - wx; - d[0][2] = xz - wy; -} - -static void q_mulv( v4f q, v3f v, v3f d ) -{ - v3f v1, v2; - - v3_muls( q, 2.0f*v3_dot(q,v), v1 ); - v3_muls( v, q[3]*q[3] - v3_dot(q,q), v2 ); - v3_add( v1, v2, v1 ); - v3_cross( q, v, v2 ); - v3_muls( v2, 2.0f*q[3], v2 ); - v3_add( v1, v2, d ); -} - -static f32 q_dist( v4f q0, v4f q1 ){ - return acosf( 2.0f * v4_dot(q0,q1) -1.0f ); -} - -static inline void q_copy( v4f a, v4f b ) -{ - b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3]; -} - -/* - * ----------------------------------------------------------------------------- - * Section 4.a 2x2 matrices - * ----------------------------------------------------------------------------- - */ - -#define M2X2_INDENTIY {{1.0f, 0.0f, }, \ - {0.0f, 1.0f, }} - -#define M2X2_ZERO {{0.0f, 0.0f, }, \ - {0.0f, 0.0f, }} - -static inline void m2x2_copy( m2x2f a, m2x2f b ) -{ - v2_copy( a[0], b[0] ); - v2_copy( a[1], b[1] ); -} - -static inline void m2x2_identity( m2x2f a ) -{ - m2x2f id = M2X2_INDENTIY; - m2x2_copy( id, a ); -} - -static inline void m2x2_create_rotation( m2x2f a, f32 theta ) -{ - f32 s, c; - - s = sinf( theta ); - c = cosf( theta ); - - a[0][0] = c; - a[0][1] = -s; - a[1][0] = s; - a[1][1] = c; -} - -static inline void m2x2_mulv( m2x2f m, v2f v, v2f d ) -{ - v2f res; - - res[0] = m[0][0]*v[0] + m[1][0]*v[1]; - res[1] = m[0][1]*v[0] + m[1][1]*v[1]; - - v2_copy( res, d ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 4.b 3x3 matrices - * ----------------------------------------------------------------------------- - */ - -#define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ - { 0.0f, 1.0f, 0.0f, },\ - { 0.0f, 0.0f, 1.0f, }} - -#define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\ - { 0.0f, 0.0f, 0.0f, },\ - { 0.0f, 0.0f, 0.0f, }} - - -static void euler_m3x3( v3f angles, m3x3f d ) -{ - f32 cosY = cosf( angles[0] ), - sinY = sinf( angles[0] ), - cosP = cosf( angles[1] ), - sinP = sinf( angles[1] ), - cosR = cosf( angles[2] ), - sinR = sinf( angles[2] ); - - d[2][0] = -sinY * cosP; - d[2][1] = sinP; - d[2][2] = cosY * cosP; - - d[0][0] = cosY * cosR; - d[0][1] = sinR; - d[0][2] = sinY * cosR; - - v3_cross( d[0], d[2], d[1] ); -} - -static void m3x3_q( m3x3f m, v4f q ) -{ - f32 diag, r, rinv; - - diag = m[0][0] + m[1][1] + m[2][2]; - if( diag >= 0.0f ) - { - r = sqrtf( 1.0f + diag ); - rinv = 0.5f / r; - q[0] = rinv * (m[1][2] - m[2][1]); - q[1] = rinv * (m[2][0] - m[0][2]); - q[2] = rinv * (m[0][1] - m[1][0]); - q[3] = r * 0.5f; - } - else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] ) - { - r = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] ); - rinv = 0.5f / r; - q[0] = r * 0.5f; - q[1] = rinv * (m[0][1] + m[1][0]); - q[2] = rinv * (m[0][2] + m[2][0]); - q[3] = rinv * (m[1][2] - m[2][1]); - } - else if( m[1][1] >= m[2][2] ) - { - r = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] ); - rinv = 0.5f / r; - q[0] = rinv * (m[0][1] + m[1][0]); - q[1] = r * 0.5f; - q[2] = rinv * (m[1][2] + m[2][1]); - q[3] = rinv * (m[2][0] - m[0][2]); - } - else - { - r = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] ); - rinv = 0.5f / r; - q[0] = rinv * (m[0][2] + m[2][0]); - q[1] = rinv * (m[1][2] + m[2][1]); - q[2] = r * 0.5f; - q[3] = rinv * (m[0][1] - m[1][0]); - } -} - -/* a X b == [b]T a == ...*/ -static void m3x3_skew_symetric( m3x3f a, v3f v ) -{ - a[0][0] = 0.0f; - a[0][1] = v[2]; - a[0][2] = -v[1]; - a[1][0] = -v[2]; - a[1][1] = 0.0f; - a[1][2] = v[0]; - a[2][0] = v[1]; - a[2][1] = -v[0]; - a[2][2] = 0.0f; -} - -/* aka kronecker product */ -static void m3x3_outer_product( m3x3f out_m, v3f a, v3f b ) -{ - out_m[0][0] = a[0]*b[0]; - out_m[0][1] = a[0]*b[1]; - out_m[0][2] = a[0]*b[2]; - out_m[1][0] = a[1]*b[0]; - out_m[1][1] = a[1]*b[1]; - out_m[1][2] = a[1]*b[2]; - out_m[2][0] = a[2]*b[0]; - out_m[2][1] = a[2]*b[1]; - out_m[2][2] = a[2]*b[2]; -} - -static void m3x3_add( m3x3f a, m3x3f b, m3x3f d ) -{ - v3_add( a[0], b[0], d[0] ); - v3_add( a[1], b[1], d[1] ); - v3_add( a[2], b[2], d[2] ); -} - -static void m3x3_sub( m3x3f a, m3x3f b, m3x3f d ) -{ - v3_sub( a[0], b[0], d[0] ); - v3_sub( a[1], b[1], d[1] ); - v3_sub( a[2], b[2], d[2] ); -} - -static inline void m3x3_copy( m3x3f a, m3x3f b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); - v3_copy( a[2], b[2] ); -} - -static inline void m3x3_identity( m3x3f a ) -{ - m3x3f id = M3X3_IDENTITY; - m3x3_copy( id, a ); -} - -static void m3x3_diagonal( m3x3f out_a, f32 v ) -{ - m3x3_identity( out_a ); - out_a[0][0] = v; - out_a[1][1] = v; - out_a[2][2] = v; -} - -static void m3x3_setdiagonalv3( m3x3f a, v3f v ) -{ - a[0][0] = v[0]; - a[1][1] = v[1]; - a[2][2] = v[2]; -} - -static inline void m3x3_zero( m3x3f a ) -{ - m3x3f z = M3X3_ZERO; - m3x3_copy( z, a ); -} - -static inline void m3x3_inv( m3x3f src, m3x3f dest ) -{ - f32 a = src[0][0], b = src[0][1], c = src[0][2], - d = src[1][0], e = src[1][1], f = src[1][2], - g = src[2][0], h = src[2][1], i = src[2][2]; - - f32 det = 1.f / - (+a*(e*i-h*f) - -b*(d*i-f*g) - +c*(d*h-e*g)); - - dest[0][0] = (e*i-h*f)*det; - dest[0][1] = -(b*i-c*h)*det; - dest[0][2] = (b*f-c*e)*det; - dest[1][0] = -(d*i-f*g)*det; - dest[1][1] = (a*i-c*g)*det; - dest[1][2] = -(a*f-d*c)*det; - dest[2][0] = (d*h-g*e)*det; - dest[2][1] = -(a*h-g*b)*det; - dest[2][2] = (a*e-d*b)*det; -} - -static f32 m3x3_det( m3x3f m ) -{ - return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) - - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) - + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); -} - -static inline void m3x3_transpose( m3x3f src, m3x3f dest ) -{ - f32 a = src[0][0], b = src[0][1], c = src[0][2], - d = src[1][0], e = src[1][1], f = src[1][2], - g = src[2][0], h = src[2][1], i = src[2][2]; - - dest[0][0] = a; - dest[0][1] = d; - dest[0][2] = g; - dest[1][0] = b; - dest[1][1] = e; - dest[1][2] = h; - dest[2][0] = c; - dest[2][1] = f; - dest[2][2] = i; -} - -static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d ) -{ - f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], - a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], - a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], - - b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], - b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], - b20 = b[2][0], b21 = b[2][1], b22 = b[2][2]; - - d[0][0] = a00*b00 + a10*b01 + a20*b02; - d[0][1] = a01*b00 + a11*b01 + a21*b02; - d[0][2] = a02*b00 + a12*b01 + a22*b02; - d[1][0] = a00*b10 + a10*b11 + a20*b12; - d[1][1] = a01*b10 + a11*b11 + a21*b12; - d[1][2] = a02*b10 + a12*b11 + a22*b12; - d[2][0] = a00*b20 + a10*b21 + a20*b22; - d[2][1] = a01*b20 + a11*b21 + a21*b22; - d[2][2] = a02*b20 + a12*b21 + a22*b22; -} - -static inline void m3x3_mulv( m3x3f m, v3f v, v3f d ) -{ - v3f res; - - res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2]; - res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2]; - res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2]; - - v3_copy( res, d ); -} - -static inline void m3x3_projection( m3x3f dst, - f32 const left, f32 const right, f32 const bottom, f32 const top ) -{ - f32 rl, tb; - - m3x3_zero( dst ); - - rl = 1.0f / (right - left); - tb = 1.0f / (top - bottom); - - dst[0][0] = 2.0f * rl; - dst[1][1] = 2.0f * tb; - dst[2][2] = 1.0f; -} - -static inline void m3x3_translate( m3x3f m, v3f v ) -{ - m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0]; - m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1]; - m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2]; -} - -static inline void m3x3_scale( m3x3f m, v3f v ) -{ - v3_muls( m[0], v[0], m[0] ); - v3_muls( m[1], v[1], m[1] ); - v3_muls( m[2], v[2], m[2] ); -} - -static inline void m3x3_scalef( m3x3f m, f32 f ) -{ - v3f v; - v3_fill( v, f ); - m3x3_scale( m, v ); -} - -static inline void m3x3_rotate( m3x3f m, f32 angle ) -{ - f32 m00 = m[0][0], m10 = m[1][0], - m01 = m[0][1], m11 = m[1][1], - m02 = m[0][2], m12 = m[1][2]; - f32 c, s; - - s = sinf( angle ); - c = cosf( angle ); - - m[0][0] = m00 * c + m10 * s; - m[0][1] = m01 * c + m11 * s; - m[0][2] = m02 * c + m12 * s; - - m[1][0] = m00 * -s + m10 * c; - m[1][1] = m01 * -s + m11 * c; - m[1][2] = m02 * -s + m12 * c; -} - -/* - * ----------------------------------------------------------------------------- - * Section 4.c 4x3 matrices - * ----------------------------------------------------------------------------- - */ - -#define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ - { 0.0f, 1.0f, 0.0f, },\ - { 0.0f, 0.0f, 1.0f, },\ - { 0.0f, 0.0f, 0.0f }} - -static inline void m4x3_to_3x3( m4x3f a, m3x3f b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); - v3_copy( a[2], b[2] ); -} - -static inline void m4x3_invert_affine( m4x3f a, m4x3f b ) -{ - m3x3_transpose( a, b ); - m3x3_mulv( b, a[3], b[3] ); - v3_negate( b[3], b[3] ); -} - -static void m4x3_invert_full( m4x3f src, m4x3f dst ) -{ - f32 t2, t4, t5, - det, - a = src[0][0], b = src[0][1], c = src[0][2], - e = src[1][0], f = src[1][1], g = src[1][2], - i = src[2][0], j = src[2][1], k = src[2][2], - m = src[3][0], n = src[3][1], o = src[3][2]; - - t2 = j*o - n*k; - t4 = i*o - m*k; - t5 = i*n - m*j; - - dst[0][0] = f*k - g*j; - dst[1][0] =-(e*k - g*i); - dst[2][0] = e*j - f*i; - dst[3][0] =-(e*t2 - f*t4 + g*t5); - - dst[0][1] =-(b*k - c*j); - dst[1][1] = a*k - c*i; - dst[2][1] =-(a*j - b*i); - dst[3][1] = a*t2 - b*t4 + c*t5; - - t2 = f*o - n*g; - t4 = e*o - m*g; - t5 = e*n - m*f; - - dst[0][2] = b*g - c*f ; - dst[1][2] =-(a*g - c*e ); - dst[2][2] = a*f - b*e ; - dst[3][2] =-(a*t2 - b*t4 + c * t5); - - det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]); - v3_muls( dst[0], det, dst[0] ); - v3_muls( dst[1], det, dst[1] ); - v3_muls( dst[2], det, dst[2] ); - v3_muls( dst[3], det, dst[3] ); -} - -static inline void m4x3_copy( m4x3f a, m4x3f b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); - v3_copy( a[2], b[2] ); - v3_copy( a[3], b[3] ); -} - -static inline void m4x3_identity( m4x3f a ) -{ - m4x3f id = M4X3_IDENTITY; - m4x3_copy( id, a ); -} - -static void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) -{ - f32 - a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], - a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], - a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], - a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], - b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], - b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], - b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], - b30 = b[3][0], b31 = b[3][1], b32 = b[3][2]; - - d[0][0] = a00*b00 + a10*b01 + a20*b02; - d[0][1] = a01*b00 + a11*b01 + a21*b02; - d[0][2] = a02*b00 + a12*b01 + a22*b02; - d[1][0] = a00*b10 + a10*b11 + a20*b12; - d[1][1] = a01*b10 + a11*b11 + a21*b12; - d[1][2] = a02*b10 + a12*b11 + a22*b12; - d[2][0] = a00*b20 + a10*b21 + a20*b22; - d[2][1] = a01*b20 + a11*b21 + a21*b22; - d[2][2] = a02*b20 + a12*b21 + a22*b22; - d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30; - d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31; - d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32; -} - -#if 0 /* shat appf mingw wstringop-overflow */ -inline -#endif -static void m4x3_mulv( m4x3f m, v3f v, v3f d ) -{ - v3f res; - - res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]; - res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]; - res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]; - - v3_copy( res, d ); -} - -/* - * Transform plane ( xyz, distance ) - */ -static void m4x3_mulp( m4x3f m, v4f p, v4f d ) -{ - v3f o; - - v3_muls( p, p[3], o ); - m4x3_mulv( m, o, o ); - m3x3_mulv( m, p, d ); - - d[3] = v3_dot( o, d ); -} - -/* - * Affine transforms - */ - -static void m4x3_translate( m4x3f m, v3f v ) -{ - v3_muladds( m[3], m[0], v[0], m[3] ); - v3_muladds( m[3], m[1], v[1], m[3] ); - v3_muladds( m[3], m[2], v[2], m[3] ); -} - -static void m4x3_rotate_x( m4x3f m, f32 angle ) -{ - m4x3f t = M4X3_IDENTITY; - f32 c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[1][1] = c; - t[1][2] = s; - t[2][1] = -s; - t[2][2] = c; - - m4x3_mul( m, t, m ); -} - -static void m4x3_rotate_y( m4x3f m, f32 angle ) -{ - m4x3f t = M4X3_IDENTITY; - f32 c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[0][0] = c; - t[0][2] = -s; - t[2][0] = s; - t[2][2] = c; - - m4x3_mul( m, t, m ); -} - -static void m4x3_rotate_z( m4x3f m, f32 angle ) -{ - m4x3f t = M4X3_IDENTITY; - f32 c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[0][0] = c; - t[0][1] = s; - t[1][0] = -s; - t[1][1] = c; - - m4x3_mul( m, t, m ); -} - -static void m4x3_expand( m4x3f m, m4x4f d ) -{ - v3_copy( m[0], d[0] ); - v3_copy( m[1], d[1] ); - v3_copy( m[2], d[2] ); - v3_copy( m[3], d[3] ); - d[0][3] = 0.0f; - d[1][3] = 0.0f; - d[2][3] = 0.0f; - d[3][3] = 1.0f; -} - -static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s ) -{ - v3_copy( m[3], co ); - s[0] = v3_length(m[0]); - s[1] = v3_length(m[1]); - s[2] = v3_length(m[2]); - - m3x3f rot; - v3_divs( m[0], s[0], rot[0] ); - v3_divs( m[1], s[1], rot[1] ); - v3_divs( m[2], s[2], rot[2] ); - - m3x3_q( rot, q ); -} - -static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ){ - v3f v; - m4x3_mulv( m, point, v ); - - v3_minv( box[0], v, box[0] ); - v3_maxv( box[1], v, box[1] ); -} - -static void m4x3_expand_aabb_aabb( m4x3f m, boxf boxa, boxf boxb ){ - v3f a; v3f b; - v3_copy( boxb[0], a ); - v3_copy( boxb[1], b ); - m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], a[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], a[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], a[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], a[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], b[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], b[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], b[2] } ); - m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], b[2] } ); -} -static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) -{ - v3f dir; - v3_sub( target, pos, dir ); - v3_normalize( dir ); - - v3_copy( dir, m[2] ); - - v3_cross( up, m[2], m[0] ); - v3_normalize( m[0] ); - - v3_cross( m[2], m[0], m[1] ); - v3_copy( pos, m[3] ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 4.d 4x4 matrices - * ----------------------------------------------------------------------------- - */ - -#define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\ - { 0.0f, 1.0f, 0.0f, 0.0f },\ - { 0.0f, 0.0f, 1.0f, 0.0f },\ - { 0.0f, 0.0f, 0.0f, 1.0f }} -#define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\ - { 0.0f, 0.0f, 0.0f, 0.0f },\ - { 0.0f, 0.0f, 0.0f, 0.0f },\ - { 0.0f, 0.0f, 0.0f, 0.0f }} - -static void m4x4_projection( m4x4f m, f32 angle, - f32 ratio, f32 fnear, f32 ffar ) -{ - f32 scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear, - r = ratio * scale, - l = -r, - t = scale, - b = -t; - - m[0][0] = 2.0f * fnear / (r - l); - m[0][1] = 0.0f; - m[0][2] = 0.0f; - m[0][3] = 0.0f; - - m[1][0] = 0.0f; - m[1][1] = 2.0f * fnear / (t - b); - m[1][2] = 0.0f; - m[1][3] = 0.0f; - - m[2][0] = (r + l) / (r - l); - m[2][1] = (t + b) / (t - b); - m[2][2] = -(ffar + fnear) / (ffar - fnear); - m[2][3] = -1.0f; - - m[3][0] = 0.0f; - m[3][1] = 0.0f; - m[3][2] = -2.0f * ffar * fnear / (ffar - fnear); - m[3][3] = 0.0f; -} - -static void m4x4_translate( m4x4f m, v3f v ) -{ - v4_muladds( m[3], m[0], v[0], m[3] ); - v4_muladds( m[3], m[1], v[1], m[3] ); - v4_muladds( m[3], m[2], v[2], m[3] ); -} - -static inline void m4x4_copy( m4x4f a, m4x4f b ) -{ - v4_copy( a[0], b[0] ); - v4_copy( a[1], b[1] ); - v4_copy( a[2], b[2] ); - v4_copy( a[3], b[3] ); -} - -static inline void m4x4_identity( m4x4f a ) -{ - m4x4f id = M4X4_IDENTITY; - m4x4_copy( id, a ); -} - -static inline void m4x4_zero( m4x4f a ) -{ - m4x4f zero = M4X4_ZERO; - m4x4_copy( zero, a ); -} - -static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d ) -{ - f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], - a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3], - a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3], - a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3], - - b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], b03 = b[0][3], - b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], b13 = b[1][3], - b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], b23 = b[2][3], - b30 = b[3][0], b31 = b[3][1], b32 = b[3][2], b33 = b[3][3]; - - d[0][0] = a00*b00 + a10*b01 + a20*b02 + a30*b03; - d[0][1] = a01*b00 + a11*b01 + a21*b02 + a31*b03; - d[0][2] = a02*b00 + a12*b01 + a22*b02 + a32*b03; - d[0][3] = a03*b00 + a13*b01 + a23*b02 + a33*b03; - d[1][0] = a00*b10 + a10*b11 + a20*b12 + a30*b13; - d[1][1] = a01*b10 + a11*b11 + a21*b12 + a31*b13; - d[1][2] = a02*b10 + a12*b11 + a22*b12 + a32*b13; - d[1][3] = a03*b10 + a13*b11 + a23*b12 + a33*b13; - d[2][0] = a00*b20 + a10*b21 + a20*b22 + a30*b23; - d[2][1] = a01*b20 + a11*b21 + a21*b22 + a31*b23; - d[2][2] = a02*b20 + a12*b21 + a22*b22 + a32*b23; - d[2][3] = a03*b20 + a13*b21 + a23*b22 + a33*b23; - d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30*b33; - d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31*b33; - d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32*b33; - d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33; -} - -static inline void m4x4_mulv( m4x4f m, v4f v, v4f d ) -{ - v4f res; - - res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3]; - res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3]; - res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3]; - res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3]; - - v4_copy( res, d ); -} - -static inline void m4x4_inv( m4x4f a, m4x4f d ) -{ - f32 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], - a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3], - a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3], - a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3], - det, - t[6]; - - t[0] = a22*a33 - a32*a23; - t[1] = a21*a33 - a31*a23; - t[2] = a21*a32 - a31*a22; - t[3] = a20*a33 - a30*a23; - t[4] = a20*a32 - a30*a22; - t[5] = a20*a31 - a30*a21; - - d[0][0] = a11*t[0] - a12*t[1] + a13*t[2]; - d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]); - d[2][0] = a10*t[1] - a11*t[3] + a13*t[5]; - d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]); - - d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]); - d[1][1] = a00*t[0] - a02*t[3] + a03*t[4]; - d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]); - d[3][1] = a00*t[2] - a01*t[4] + a02*t[5]; - - t[0] = a12*a33 - a32*a13; - t[1] = a11*a33 - a31*a13; - t[2] = a11*a32 - a31*a12; - t[3] = a10*a33 - a30*a13; - t[4] = a10*a32 - a30*a12; - t[5] = a10*a31 - a30*a11; - - d[0][2] = a01*t[0] - a02*t[1] + a03*t[2]; - d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]); - d[2][2] = a00*t[1] - a01*t[3] + a03*t[5]; - d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]); - - t[0] = a12*a23 - a22*a13; - t[1] = a11*a23 - a21*a13; - t[2] = a11*a22 - a21*a12; - t[3] = a10*a23 - a20*a13; - t[4] = a10*a22 - a20*a12; - t[5] = a10*a21 - a20*a11; - - d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]); - d[1][3] = a00*t[0] - a02*t[3] + a03*t[4]; - d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]); - d[3][3] = a00*t[2] - a01*t[4] + a02*t[5]; - - det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]); - v4_muls( d[0], det, d[0] ); - v4_muls( d[1], det, d[1] ); - v4_muls( d[2], det, d[2] ); - v4_muls( d[3], det, d[3] ); -} - -/* - * http://www.terathon.com/lengyel/Lengyel-Oblique.pdf - */ -static void m4x4_clip_projection( m4x4f mat, v4f plane ){ - v4f c = - { - (vg_signf(plane[0]) + mat[2][0]) / mat[0][0], - (vg_signf(plane[1]) + mat[2][1]) / mat[1][1], - -1.0f, - (1.0f + mat[2][2]) / mat[3][2] - }; - - v4_muls( plane, 2.0f / v4_dot(plane,c), c ); - - mat[0][2] = c[0]; - mat[1][2] = c[1]; - mat[2][2] = c[2] + 1.0f; - mat[3][2] = c[3]; -} - -/* - * Undoes the above operation - */ -static void m4x4_reset_clipping( m4x4f mat, float ffar, float fnear ){ - mat[0][2] = 0.0f; - mat[1][2] = 0.0f; - mat[2][2] = -(ffar + fnear) / (ffar - fnear); - mat[3][2] = -2.0f * ffar * fnear / (ffar - fnear); -} - -/* - * ----------------------------------------------------------------------------- - * Section 5.a Boxes - * ----------------------------------------------------------------------------- - */ - -static inline void box_addpt( boxf a, v3f pt ) -{ - v3_minv( a[0], pt, a[0] ); - v3_maxv( a[1], pt, a[1] ); -} - -static inline void box_concat( boxf a, boxf b ) -{ - v3_minv( a[0], b[0], a[0] ); - v3_maxv( a[1], b[1], a[1] ); -} - -static inline void box_copy( boxf a, boxf b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); -} - -static inline int box_overlap( boxf a, boxf b ) -{ - return - ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) && - ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) && - ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] ) - ; -} - -static int box_within_pt( boxf box, v3f pt ) -{ - if( (pt[0] >= box[0][0]) && (pt[1] >= box[0][1]) && (pt[2] >= box[0][2]) && - (pt[0] <= box[1][0]) && (pt[1] <= box[1][1]) && (pt[2] <= box[1][2]) ) - { - return 1; - } - else return 0; -} - -static int box_within( boxf greater, boxf lesser ) -{ - v3f a, b; - v3_sub( lesser[0], greater[0], a ); - v3_sub( lesser[1], greater[1], b ); - - if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) && - (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) ) - { - return 1; - } - else return 0; -} - -static inline void box_init_inf( boxf box ){ - v3_fill( box[0], INFINITY ); - v3_fill( box[1], -INFINITY ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 5.b Planes - * ----------------------------------------------------------------------------- - */ - -static inline void tri_to_plane( f64 a[3], f64 b[3], - f64 c[3], f64 p[4] ) -{ - f64 edge0[3]; - f64 edge1[3]; - f64 l; - - edge0[0] = b[0] - a[0]; - edge0[1] = b[1] - a[1]; - edge0[2] = b[2] - a[2]; - - edge1[0] = c[0] - a[0]; - edge1[1] = c[1] - a[1]; - edge1[2] = c[2] - a[2]; - - p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1]; - p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2]; - p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0]; - - l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); - p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l; - - p[0] = p[0] / l; - p[1] = p[1] / l; - p[2] = p[2] / l; -} - -static int plane_intersect3( v4f a, v4f b, v4f c, v3f p ) -{ - f32 const epsilon = 1e-6f; - - v3f x; - v3_cross( a, b, x ); - f32 d = v3_dot( x, c ); - - if( (d < epsilon) && (d > -epsilon) ) return 0; - - v3f v0, v1, v2; - v3_cross( b, c, v0 ); - v3_cross( c, a, v1 ); - v3_cross( a, b, v2 ); - - v3_muls( v0, a[3], p ); - v3_muladds( p, v1, b[3], p ); - v3_muladds( p, v2, c[3], p ); - v3_divs( p, d, p ); - - return 1; -} - -static int plane_intersect2( v4f a, v4f b, v3f p, v3f n ) -{ - f32 const epsilon = 1e-6f; - - v4f c; - v3_cross( a, b, c ); - f32 d = v3_length2( c ); - - if( (d < epsilon) && (d > -epsilon) ) - return 0; - - v3f v0, v1, vx; - v3_cross( c, b, v0 ); - v3_cross( a, c, v1 ); - - v3_muls( v0, a[3], vx ); - v3_muladds( vx, v1, b[3], vx ); - v3_divs( vx, d, p ); - v3_copy( c, n ); - - return 1; -} - -static int plane_segment( v4f plane, v3f a, v3f b, v3f co ) -{ - f32 d0 = v3_dot( a, plane ) - plane[3], - d1 = v3_dot( b, plane ) - plane[3]; - - if( d0*d1 < 0.0f ) - { - f32 tot = 1.0f/( fabsf(d0)+fabsf(d1) ); - - v3_muls( a, fabsf(d1) * tot, co ); - v3_muladds( co, b, fabsf(d0) * tot, co ); - return 1; - } - - return 0; -} - -static inline f64 plane_polarity( f64 p[4], f64 a[3] ) -{ - return - (a[0] * p[0] + a[1] * p[1] + a[2] * p[2]) - -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2]) - ; -} - -static f32 ray_plane( v4f plane, v3f co, v3f dir ){ - f32 d = v3_dot( plane, dir ); - if( fabsf(d) > 1e-6f ){ - v3f v0; - v3_muls( plane, plane[3], v0 ); - v3_sub( v0, co, v0 ); - return v3_dot( v0, plane ) / d; - } - else return INFINITY; -} - -/* - * ----------------------------------------------------------------------------- - * Section 5.c Closest point functions - * ----------------------------------------------------------------------------- - */ - -/* - * These closest point tests were learned from Real-Time Collision Detection by - * Christer Ericson - */ -static f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, - f32 *s, f32 *t, v3f c1, v3f c2) -{ - v3f d1,d2,r; - v3_sub( q1, p1, d1 ); - v3_sub( q2, p2, d2 ); - v3_sub( p1, p2, r ); - - f32 a = v3_length2( d1 ), - e = v3_length2( d2 ), - f = v3_dot( d2, r ); - - const f32 kEpsilon = 0.0001f; - - if( a <= kEpsilon && e <= kEpsilon ) - { - *s = 0.0f; - *t = 0.0f; - v3_copy( p1, c1 ); - v3_copy( p2, c2 ); - - v3f v0; - v3_sub( c1, c2, v0 ); - - return v3_length2( v0 ); - } - - if( a<= kEpsilon ) - { - *s = 0.0f; - *t = vg_clampf( f / e, 0.0f, 1.0f ); - } - else - { - f32 c = v3_dot( d1, r ); - if( e <= kEpsilon ) - { - *t = 0.0f; - *s = vg_clampf( -c / a, 0.0f, 1.0f ); - } - else - { - f32 b = v3_dot(d1,d2), - d = a*e-b*b; - - if( d != 0.0f ) - { - *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f); - } - else - { - *s = 0.0f; - } - - *t = (b*(*s)+f) / e; - - if( *t < 0.0f ) - { - *t = 0.0f; - *s = vg_clampf( -c / a, 0.0f, 1.0f ); - } - else if( *t > 1.0f ) - { - *t = 1.0f; - *s = vg_clampf((b-c)/a,0.0f,1.0f); - } - } - } - - v3_muladds( p1, d1, *s, c1 ); - v3_muladds( p2, d2, *t, c2 ); - - v3f v0; - v3_sub( c1, c2, v0 ); - return v3_length2( v0 ); -} - -static int point_inside_aabb( boxf box, v3f point ) -{ - if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) && - (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) ) - return 1; - else - return 0; -} - -static void closest_point_aabb( v3f p, boxf box, v3f dest ) -{ - v3_maxv( p, box[0], dest ); - v3_minv( dest, box[1], dest ); -} - -static void closest_point_obb( v3f p, boxf box, - m4x3f mtx, m4x3f inv_mtx, v3f dest ) -{ - v3f local; - m4x3_mulv( inv_mtx, p, local ); - closest_point_aabb( local, box, local ); - m4x3_mulv( mtx, local, dest ); -} - -static f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) -{ - v3f v0, v1; - v3_sub( b, a, v0 ); - v3_sub( point, a, v1 ); - - f32 t = v3_dot( v1, v0 ) / v3_length2(v0); - t = vg_clampf(t,0.0f,1.0f); - v3_muladds( a, v0, t, dest ); - return t; -} - -static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) -{ - v3f ab, ac, ap; - f32 d1, d2; - - /* Region outside A */ - v3_sub( tri[1], tri[0], ab ); - v3_sub( tri[2], tri[0], ac ); - v3_sub( p, tri[0], ap ); - - d1 = v3_dot(ab,ap); - d2 = v3_dot(ac,ap); - if( d1 <= 0.0f && d2 <= 0.0f ) - { - v3_copy( tri[0], dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* Region outside B */ - v3f bp; - f32 d3, d4; - - v3_sub( p, tri[1], bp ); - d3 = v3_dot( ab, bp ); - d4 = v3_dot( ac, bp ); - - if( d3 >= 0.0f && d4 <= d3 ) - { - v3_copy( tri[1], dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* Edge region of AB */ - f32 vc = d1*d4 - d3*d2; - if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) - { - f32 v = d1 / (d1-d3); - v3_muladds( tri[0], ab, v, dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* Region outside C */ - v3f cp; - f32 d5, d6; - v3_sub( p, tri[2], cp ); - d5 = v3_dot(ab, cp); - d6 = v3_dot(ac, cp); - - if( d6 >= 0.0f && d5 <= d6 ) - { - v3_copy( tri[2], dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* Region of AC */ - f32 vb = d5*d2 - d1*d6; - if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) - { - f32 w = d2 / (d2-d6); - v3_muladds( tri[0], ac, w, dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* Region of BC */ - f32 va = d3*d6 - d5*d4; - if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) - { - f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); - v3f bc; - v3_sub( tri[2], tri[1], bc ); - v3_muladds( tri[1], bc, w, dest ); - v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); - return; - } - - /* P inside region, Q via barycentric coordinates uvw */ - f32 d = 1.0f/(va+vb+vc), - v = vb*d, - w = vc*d; - - v3_muladds( tri[0], ab, v, dest ); - v3_muladds( dest, ac, w, dest ); -} - -enum contact_type -{ - k_contact_type_default, - k_contact_type_disabled, - k_contact_type_edge -}; - -static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) -{ - v3f ab, ac, ap; - f32 d1, d2; - - /* Region outside A */ - v3_sub( tri[1], tri[0], ab ); - v3_sub( tri[2], tri[0], ac ); - v3_sub( p, tri[0], ap ); - - d1 = v3_dot(ab,ap); - d2 = v3_dot(ac,ap); - if( d1 <= 0.0f && d2 <= 0.0f ) - { - v3_copy( tri[0], dest ); - return k_contact_type_default; - } - - /* Region outside B */ - v3f bp; - f32 d3, d4; - - v3_sub( p, tri[1], bp ); - d3 = v3_dot( ab, bp ); - d4 = v3_dot( ac, bp ); - - if( d3 >= 0.0f && d4 <= d3 ) - { - v3_copy( tri[1], dest ); - return k_contact_type_edge; - } - - /* Edge region of AB */ - f32 vc = d1*d4 - d3*d2; - if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) - { - f32 v = d1 / (d1-d3); - v3_muladds( tri[0], ab, v, dest ); - return k_contact_type_edge; - } - - /* Region outside C */ - v3f cp; - f32 d5, d6; - v3_sub( p, tri[2], cp ); - d5 = v3_dot(ab, cp); - d6 = v3_dot(ac, cp); - - if( d6 >= 0.0f && d5 <= d6 ) - { - v3_copy( tri[2], dest ); - return k_contact_type_edge; - } - - /* Region of AC */ - f32 vb = d5*d2 - d1*d6; - if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) - { - f32 w = d2 / (d2-d6); - v3_muladds( tri[0], ac, w, dest ); - return k_contact_type_edge; - } - - /* Region of BC */ - f32 va = d3*d6 - d5*d4; - if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) - { - f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); - v3f bc; - v3_sub( tri[2], tri[1], bc ); - v3_muladds( tri[1], bc, w, dest ); - return k_contact_type_edge; - } - - /* P inside region, Q via barycentric coordinates uvw */ - f32 d = 1.0f/(va+vb+vc), - v = vb*d, - w = vc*d; - - v3_muladds( tri[0], ab, v, dest ); - v3_muladds( dest, ac, w, dest ); - - return k_contact_type_default; -} - -static void closest_point_elipse( v2f p, v2f e, v2f o ) -{ - v2f pabs, ei, e2, ve, t; - - v2_abs( p, pabs ); - v2_div( (v2f){ 1.0f, 1.0f }, e, ei ); - v2_mul( e, e, e2 ); - v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve ); - - v2_fill( t, 0.70710678118654752f ); - - for( int i=0; i<3; i++ ){ - v2f v, u, ud, w; - - v2_mul( ve, t, v ); /* ve*t*t*t */ - v2_mul( v, t, v ); - v2_mul( v, t, v ); - - v2_sub( pabs, v, u ); - v2_normalize( u ); - - v2_mul( t, e, ud ); - v2_sub( ud, v, ud ); - - v2_muls( u, v2_length( ud ), u ); - - v2_add( v, u, w ); - v2_mul( w, ei, w ); - - v2_maxv( (v2f){0.0f,0.0f}, w, t ); - v2_normalize( t ); - } - - v2_mul( t, e, o ); - v2_copysign( o, p ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 5.d Raycasts & Spherecasts - * ----------------------------------------------------------------------------- - */ - -static int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist ) -{ - v3f v0, v1; - f32 tmin, tmax; - - v3_sub( box[0], co, v0 ); - v3_sub( box[1], co, v1 ); - - v3_mul( v0, dir_inv, v0 ); - v3_mul( v1, dir_inv, v1 ); - - tmin = vg_minf( v0[0], v1[0] ); - tmax = vg_maxf( v0[0], v1[0] ); - tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] )); - tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] )); - tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] )); - tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] )); - - return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f); -} - -/* Time of intersection with ray vs triangle */ -static int ray_tri( v3f tri[3], v3f co, - v3f dir, f32 *dist, int backfaces ) -{ - f32 const kEpsilon = 0.00001f; - - v3f v0, v1, h, s, q, n; - f32 a,f,u,v,t; - - f32 *pa = tri[0], - *pb = tri[1], - *pc = tri[2]; - - v3_sub( pb, pa, v0 ); - v3_sub( pc, pa, v1 ); - v3_cross( dir, v1, h ); - v3_cross( v0, v1, n ); - - if( (v3_dot( n, dir ) > 0.0f) && !backfaces ) /* Backface culling */ - return 0; - - /* Parralel */ - a = v3_dot( v0, h ); - - if( a > -kEpsilon && a < kEpsilon ) - return 0; - - f = 1.0f/a; - v3_sub( co, pa, s ); - - u = f * v3_dot(s, h); - if( u < 0.0f || u > 1.0f ) - return 0; - - v3_cross( s, v0, q ); - v = f * v3_dot( dir, q ); - if( v < 0.0f || u+v > 1.0f ) - return 0; - - t = f * v3_dot(v1, q); - if( t > kEpsilon ) - { - *dist = t; - return 1; - } - else return 0; -} - -/* time of intersection with ray vs sphere */ -static int ray_sphere( v3f c, f32 r, - v3f co, v3f dir, f32 *t ) -{ - v3f m; - v3_sub( co, c, m ); - - f32 b = v3_dot( m, dir ), - c1 = v3_dot( m, m ) - r*r; - - /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */ - if( c1 > 0.0f && b > 0.0f ) - return 0; - - f32 discr = b*b - c1; - - /* A negative discriminant corresponds to ray missing sphere */ - if( discr < 0.0f ) - return 0; - - /* - * Ray now found to intersect sphere, compute smallest t value of - * intersection - */ - *t = -b - sqrtf( discr ); - - /* If t is negative, ray started inside sphere so clamp t to zero */ - if( *t < 0.0f ) - *t = 0.0f; - - return 1; -} - -/* - * time of intersection of ray vs cylinder - * The cylinder does not have caps but is finite - * - * Heavily adapted from regular segment vs cylinder from: - * Real-Time Collision Detection - */ -static int ray_uncapped_finite_cylinder( v3f q, v3f p, f32 r, - v3f co, v3f dir, f32 *t ) -{ - v3f d, m, n, sb; - v3_muladds( co, dir, 1.0f, sb ); - - v3_sub( q, p, d ); - v3_sub( co, p, m ); - v3_sub( sb, co, n ); - - f32 md = v3_dot( m, d ), - nd = v3_dot( n, d ), - dd = v3_dot( d, d ), - nn = v3_dot( n, n ), - mn = v3_dot( m, n ), - a = dd*nn - nd*nd, - k = v3_dot( m, m ) - r*r, - c = dd*k - md*md; - - if( fabsf(a) < 0.00001f ) - { - /* Segment runs parallel to cylinder axis */ - return 0; - } - - f32 b = dd*mn - nd*md, - discr = b*b - a*c; - - if( discr < 0.0f ) - return 0; /* No real roots; no intersection */ - - *t = (-b - sqrtf(discr)) / a; - if( *t < 0.0f ) - return 0; /* Intersection behind ray */ - - /* Check within cylinder segment */ - if( md + (*t)*nd < 0.0f ) - return 0; - - if( md + (*t)*nd > dd ) - return 0; - - /* Segment intersects cylinder between the endcaps; t is correct */ - return 1; -} - -/* - * Time of intersection of sphere and triangle. Origin must be outside the - * colliding area. This is a fairly long procedure. - */ -static int spherecast_triangle( v3f tri[3], - v3f co, v3f dir, f32 r, f32 *t, v3f n ) -{ - v3f sum[3]; - v3f v0, v1; - - v3_sub( tri[1], tri[0], v0 ); - v3_sub( tri[2], tri[0], v1 ); - v3_cross( v0, v1, n ); - v3_normalize( n ); - v3_muladds( tri[0], n, r, sum[0] ); - v3_muladds( tri[1], n, r, sum[1] ); - v3_muladds( tri[2], n, r, sum[2] ); - - int hit = 0; - f32 t_min = INFINITY, - t1; - - if( ray_tri( sum, co, dir, &t1, 0 ) ){ - t_min = vg_minf( t_min, t1 ); - hit = 1; - } - - /* - * Currently disabled; ray_sphere requires |d| = 1. it is not very important. - */ -#if 0 - for( int i=0; i<3; i++ ){ - if( ray_sphere( tri[i], r, co, dir, &t1 ) ){ - t_min = vg_minf( t_min, t1 ); - hit = 1; - } - } -#endif - - for( int i=0; i<3; i++ ){ - int i0 = i, - i1 = (i+1)%3; - - if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ){ - if( t1 < t_min ){ - t_min = t1; - - v3f co1, ct, cx; - v3_add( dir, co, co1 ); - v3_lerp( co, co1, t_min, ct ); - - closest_point_segment( tri[i0], tri[i1], ct, cx ); - v3_sub( ct, cx, n ); - v3_normalize( n ); - } - - hit = 1; - } - } - - *t = t_min; - return hit; -} - -/* - * ----------------------------------------------------------------------------- - * Section 5.e Curves - * ----------------------------------------------------------------------------- - */ - -static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, f32 t, v3f p ) -{ - f32 tt = t*t, - ttt = tt*t; - - v3_muls( p1, ttt, p ); - v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p ); - v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p ); - v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p ); -} - -static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p ) -{ - f32 u = 1.0f-t; - - v3_muls( p0, u*u, p ); - v3_muladds( p, p1, 2.0f*u*t, p ); - v3_muladds( p, p2, t*t, p ); -} - -static f32 explicit_bezier( f32 A[2], f32 B[2], f32 C[2], f32 D[2], f32 x ) -{ - f32 dAxDx = D[0]-A[0], - unitBx = (B[0] - A[0]) / dAxDx, - unitCx = (C[0] - A[0]) / dAxDx, - - /* cubic coefficients */ - a = 3.0f*unitBx - 3.0f*unitCx + 1.0f, - b = -6.0f*unitBx + 3.0f*unitCx, - c = 3.0f*unitBx, - d = -(x - A[0]) / dAxDx, - - t0 = 0.0f, - Ft0 = d, - t1 = 1.0f, - Ft1 = a+b+c+d, - tc, Ftcx; - - /* Illinois method to find root */ - for( u32 j=0; j<8; j ++ ) - { - tc = t1 - Ft1*(t1-t0)/(Ft1-Ft0); - Ftcx = tc*tc*tc*a + tc*tc*b + tc*c + d; - - if( fabsf(Ftcx) < 0.00001f ) - break; - - if( Ft1*Ftcx < 0.0f ) - { - t0 = t1; - Ft0 = Ft1; - } - else - Ft0 *= 0.5f; - - t1 = tc; - Ft1 = Ftcx; - } - - /* Evaluate parametric bezier */ - f32 t2 = tc*tc, - t3 = tc*tc*tc; - - return D[1] * t3 - + C[1] * (-3.0f*t3 + 3.0f*t2) - + B[1] * ( 3.0f*t3 - 6.0f*t2 + 3.0f*tc) - + A[1] * (-1.0f*t3 + 3.0f*t2 - 3.0f*tc + 1.0f); -} - - -/* - * ----------------------------------------------------------------------------- - * Section 5.f Volumes - * ----------------------------------------------------------------------------- - */ - -static f32 vg_sphere_volume( f32 r ){ - return (4.0f/3.0f) * VG_PIf * r*r*r; -} - -static f32 vg_box_volume( boxf box ){ - v3f e; - v3_sub( box[1], box[0], e ); - return e[0]*e[1]*e[2]; -} - -static f32 vg_cylinder_volume( f32 r, f32 h ){ - return VG_PIf * r*r * h; -} - -static f32 vg_capsule_volume( f32 r, f32 h ){ - return vg_sphere_volume( r ) + vg_cylinder_volume( r, h-r*2.0f ); -} - -static void vg_sphere_bound( f32 r, boxf out_box ){ - v3_fill( out_box[0], -r ); - v3_fill( out_box[1], r ); -} - -static void vg_capsule_bound( f32 r, f32 h, boxf out_box ){ - v3_copy( (v3f){-r,-h*0.5f,r}, out_box[0] ); - v3_copy( (v3f){-r, h*0.5f,r}, out_box[1] ); -} - - -/* - * ----------------------------------------------------------------------------- - * Section 5.g Inertia Tensors - * ----------------------------------------------------------------------------- - */ - -/* - * Translate existing inertia tensor - */ -static void vg_translate_inertia( m3x3f inout_inertia, f32 mass, v3f d ){ - /* - * I = I_0 + m*[(d.d)E_3 - d(X)d] - * - * I: updated tensor - * I_0: original tensor - * m: scalar mass - * d: translation vector - * (X): outer product - * E_3: identity matrix - */ - m3x3f t, outer, scale; - m3x3_diagonal( t, v3_dot(d,d) ); - m3x3_outer_product( outer, d, d ); - m3x3_sub( t, outer, t ); - m3x3_diagonal( scale, mass ); - m3x3_mul( scale, t, t ); - m3x3_add( inout_inertia, t, inout_inertia ); -} - -/* - * Rotate existing inertia tensor - */ -static void vg_rotate_inertia( m3x3f inout_inertia, m3x3f rotation ){ - /* - * I = R I_0 R^T - * - * I: updated tensor - * I_0: original tensor - * R: rotation matrix - * R^T: tranposed rotation matrix - */ - - m3x3f Rt; - m3x3_transpose( rotation, Rt ); - m3x3_mul( rotation, inout_inertia, inout_inertia ); - m3x3_mul( inout_inertia, Rt, inout_inertia ); -} -/* - * Create inertia tensor for box - */ -static void vg_box_inertia( boxf box, f32 mass, m3x3f out_inertia ){ - v3f e, com; - v3_sub( box[1], box[0], e ); - v3_muladds( box[0], e, 0.5f, com ); - - f32 ex2 = e[0]*e[0], - ey2 = e[1]*e[1], - ez2 = e[2]*e[2], - ix = (ey2+ez2) * mass * (1.0f/12.0f), - iy = (ex2+ez2) * mass * (1.0f/12.0f), - iz = (ex2+ey2) * mass * (1.0f/12.0f); - - m3x3_identity( out_inertia ); - m3x3_setdiagonalv3( out_inertia, (v3f){ ix, iy, iz } ); - vg_translate_inertia( out_inertia, mass, com ); -} - -/* - * Create inertia tensor for sphere - */ -static void vg_sphere_inertia( f32 r, f32 mass, m3x3f out_inertia ){ - f32 ixyz = r*r * mass * (2.0f/5.0f); - - m3x3_identity( out_inertia ); - m3x3_setdiagonalv3( out_inertia, (v3f){ ixyz, ixyz, ixyz } ); -} - -/* - * Create inertia tensor for capsule - */ -static void vg_capsule_inertia( f32 r, f32 h, f32 mass, m3x3f out_inertia ){ - f32 density = mass / vg_capsule_volume( r, h ), - ch = h-r*2.0f, /* cylinder height */ - cm = VG_PIf * ch*r*r * density, /* cylinder mass */ - hm = VG_TAUf * (1.0f/3.0f) * r*r*r * density, /* hemisphere mass */ - - iy = r*r*cm * 0.5f, - ixz = iy * 0.5f + cm*ch*ch*(1.0f/12.0f), - - aux0= (hm*2.0f*r*r)/5.0f; - - iy += aux0 * 2.0f; - - f32 aux1= ch*0.5f, - aux2= aux0 + hm*(aux1*aux1 + 3.0f*(1.0f/8.0f)*ch*r); - - ixz += aux2*2.0f; - - m3x3_identity( out_inertia ); - m3x3_setdiagonalv3( out_inertia, (v3f){ ixz, iy, ixz } ); -} - -/* - * ----------------------------------------------------------------------------- - * Section 6.a PSRNG and some distributions - * ----------------------------------------------------------------------------- - */ - -/* An implementation of the MT19937 Algorithm for the Mersenne Twister - * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and - * T. Nishimura, "Mersenne Twister: A 623-dimensionally - * equidistributed uniform pseudorandom number generator," ACM - * Transactions on Modeling and Computer Simulation Vol. 8, No. 1, - * January pp.3-30 1998. - * - * http://www.sultanik.com/Mersenne_twister - * https://github.com/ESultanik/mtwister/blob/master/mtwister.c - */ - -#define MT_UPPER_MASK 0x80000000 -#define MT_LOWER_MASK 0x7fffffff -#define MT_TEMPERING_MASK_B 0x9d2c5680 -#define MT_TEMPERING_MASK_C 0xefc60000 - -#define MT_STATE_VECTOR_LENGTH 624 - -/* changes to STATE_VECTOR_LENGTH also require changes to this */ -#define MT_STATE_VECTOR_M 397 - -typedef struct vg_rand vg_rand; -struct vg_rand { - u32 mt[MT_STATE_VECTOR_LENGTH]; - i32 index; -}; - -static void vg_rand_seed( vg_rand *rand, unsigned long seed ) { - /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator - * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer - * Programming," Vol. 2 (2nd Ed.) pp.102. - */ - rand->mt[0] = seed & 0xffffffff; - for( rand->index=1; rand->indexindex++){ - rand->mt[rand->index] = (6069 * rand->mt[rand->index-1]) & 0xffffffff; - } -} - -/* - * Generates a pseudo-randomly generated long. - */ -static u32 vg_randu32( vg_rand *rand ) { - u32 y; - /* mag[x] = x * 0x9908b0df for x = 0,1 */ - static u32 mag[2] = {0x0, 0x9908b0df}; - if( rand->index >= MT_STATE_VECTOR_LENGTH || rand->index < 0 ){ - /* generate STATE_VECTOR_LENGTH words at a time */ - int kk; - if( rand->index >= MT_STATE_VECTOR_LENGTH+1 || rand->index < 0 ){ - vg_rand_seed( rand, 4357 ); - } - for( kk=0; kkmt[kk] & MT_UPPER_MASK) | - (rand->mt[kk+1] & MT_LOWER_MASK); - rand->mt[kk] = rand->mt[kk+MT_STATE_VECTOR_M] ^ (y>>1) ^ mag[y & 0x1]; - } - for( ; kkmt[kk] & MT_UPPER_MASK) | - (rand->mt[kk+1] & MT_LOWER_MASK); - rand->mt[kk] = - rand->mt[ kk+(MT_STATE_VECTOR_M-MT_STATE_VECTOR_LENGTH)] ^ - (y >> 1) ^ mag[y & 0x1]; - } - y = (rand->mt[MT_STATE_VECTOR_LENGTH-1] & MT_UPPER_MASK) | - (rand->mt[0] & MT_LOWER_MASK); - rand->mt[MT_STATE_VECTOR_LENGTH-1] = - rand->mt[MT_STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1]; - rand->index = 0; - } - y = rand->mt[rand->index++]; - y ^= (y >> 11); - y ^= (y << 7) & MT_TEMPERING_MASK_B; - y ^= (y << 15) & MT_TEMPERING_MASK_C; - y ^= (y >> 18); - return y; -} - -/* - * Generates a pseudo-randomly generated f64 in the range [0..1]. - */ -static inline f64 vg_randf64( vg_rand *rand ){ - return (f64)vg_randu32(rand)/(f64)0xffffffff; -} - -static inline f64 vg_randf64_range( vg_rand *rand, f64 min, f64 max ){ - return vg_lerp( min, max, (f64)vg_randf64(rand) ); -} - -static inline void vg_rand_dir( vg_rand *rand, v3f dir ){ - dir[0] = vg_randf64(rand); - dir[1] = vg_randf64(rand); - dir[2] = vg_randf64(rand); - - /* warning: *could* be 0 length. - * very unlikely.. 1 in (2^32)^3. but its mathematically wrong. */ - - v3_muls( dir, 2.0f, dir ); - v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir ); - - v3_normalize( dir ); -} - -static inline void vg_rand_sphere( vg_rand *rand, v3f co ){ - vg_rand_dir(rand,co); - v3_muls( co, cbrtf( vg_randf64(rand) ), co ); -} - -static void vg_rand_disc( vg_rand *rand, v2f co ){ - f32 a = vg_randf64(rand) * VG_TAUf; - co[0] = sinf(a); - co[1] = cosf(a); - v2_muls( co, sqrtf( vg_randf64(rand) ), co ); -} - -static void vg_rand_cone( vg_rand *rand, v3f out_dir, f32 angle ) -{ - f32 r = sqrtf(vg_randf64(rand)) * angle * 0.5f, - a = vg_randf64(rand) * VG_TAUf; - - out_dir[0] = sinf(a) * sinf(r); - out_dir[1] = cosf(a) * sinf(r); - out_dir[2] = cosf(r); -} - -static void vg_hsv_rgb( v3f hsv, v3f rgb ){ - i32 i = floorf( hsv[0]*6.0f ); - f32 v = hsv[2], - f = hsv[0] * 6.0f - (f32)i, - p = v * (1.0f-hsv[1]), - q = v * (1.0f-f*hsv[1]), - t = v * (1.0f-(1.0f-f)*hsv[1]); - - switch( i % 6 ){ - case 0: rgb[0] = v; rgb[1] = t; rgb[2] = p; break; - case 1: rgb[0] = q; rgb[1] = v; rgb[2] = p; break; - case 2: rgb[0] = p; rgb[1] = v; rgb[2] = t; break; - case 3: rgb[0] = p; rgb[1] = q; rgb[2] = v; break; - case 4: rgb[0] = t; rgb[1] = p; rgb[2] = v; break; - case 5: rgb[0] = v; rgb[1] = p; rgb[2] = q; break; - } -} - -static void vg_rgb_hsv( v3f rgb, v3f hsv ){ - f32 min = v3_minf( rgb ), - max = v3_maxf( rgb ), - range = max-min, - k_epsilon = 0.00001f; - - hsv[2] = max; - if( range < k_epsilon ){ - hsv[0] = 0.0f; - hsv[1] = 0.0f; - return; - } - - if( max > k_epsilon ){ - hsv[1] = range/max; - } - else { - hsv[0] = 0.0f; - hsv[1] = 0.0f; - return; - } - - if( rgb[0] >= max ) - hsv[0] = (rgb[1]-rgb[2])/range; - else if( max == rgb[1] ) - hsv[0] = 2.0f+(rgb[2]-rgb[0])/range; - else - hsv[0] = 4.0f+(rgb[0]-rgb[1])/range; - - hsv[0] = vg_fractf( hsv[0] * (60.0f/360.0f) ); -} - -#endif diff --git a/vg_mem.c b/vg_mem.c index f5acab6..821213b 100644 --- a/vg_mem.c +++ b/vg_mem.c @@ -112,6 +112,7 @@ static void vg_mem_print_size( u32 bytes, char buf[32] ) snprintf( buf, 32, "%ub", bytes ); } +#if 0 void vg_mem_dumphex( FILE *fp, void *buffer, u32 offset, u32 bytes ) { fprintf( fp, "buffer at %p, offset +%u, length %u\n", buffer, offset, bytes ); @@ -148,6 +149,7 @@ void vg_mem_dumphex( FILE *fp, void *buffer, u32 offset, u32 bytes ) fprintf( fp, "----------------------------------------------------------------------------\n" ); } +#endif #if !defined( VG_ENGINE ) diff --git a/vg_mem.h b/vg_mem.h index fd91e66..a2d7810 100644 --- a/vg_mem.h +++ b/vg_mem.h @@ -1,6 +1,4 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_mem.c" -#else +#include #define VG_KB( X ) (X*1024) #define VG_MB( X ) (X*1024*1024) @@ -27,7 +25,9 @@ struct vg_stack_allocator void *data; }; +#if 0 void vg_mem_dumphex( FILE *fp, void *buffer, u32 offset, u32 bytes ); +#endif /* NOT USED IN THIS FILE */ u32 vg_align16( u32 s ); @@ -56,5 +56,3 @@ VG_API void _vg_end_temp_frame( u32 whence ); VG_API void *_vg_temp_alloc( u32 bytes, u32 alignment ); VG_API vg_stack_allocator *_vg_temp_stack(void); #endif - -#endif diff --git a/vg_platform.h b/vg_platform.h index e6dfd2b..f8db5bd 100644 --- a/vg_platform.h +++ b/vg_platform.h @@ -64,19 +64,20 @@ #define VG_ENGINE_HOOK( X ) #define VG_STACK_INLINE -typedef uint8_t u8; -typedef char c8; -typedef uint16_t u16; -typedef uint32_t u32; -typedef uint64_t u64; -typedef int8_t i8; -typedef int16_t i16; -typedef int32_t i32; -typedef int64_t i64; -typedef float f32; -typedef double f64; -typedef uint8_t bool; +typedef unsigned char u8; +typedef char c8; +typedef unsigned short int u16; +typedef unsigned int u32; +typedef unsigned long int u64; +typedef char i8; +typedef signed short int i16; +typedef signed int i32; +typedef signed long int i64; +typedef float f32; +typedef double f64; +typedef unsigned char bool; +/* TODO: delete these!!!!!!! */ typedef i32 v2i[2]; typedef i32 v3i[3]; typedef i32 v4i[4]; diff --git a/vg_rigidbody.h b/vg_rigidbody.h deleted file mode 100644 index 59c1524..0000000 --- a/vg_rigidbody.h +++ /dev/null @@ -1,88 +0,0 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_rigidbody.c" -#else - -/* - * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved - * - * Rigidbody main file, related: - * vg_rigidbody_collision.h - * vg_rigidbody_constraints.h - */ - -#define k_friction 0.4f -#define k_damp_linear 0.1f /* scale velocity 1/(1+x) */ -#define k_damp_angular 0.1f /* scale angular 1/(1+x) */ -#define k_penetration_slop 0.01f -#define k_rb_inertia_scale 4.0f -#define k_phys_baumgarte 0.2f -//#define k_gravity 9.6f -#define k_rb_density 8.0f - -extern f32 k_gravity; - -enum rb_shape { - k_rb_shape_none = 0, - k_rb_shape_box = 1, - k_rb_shape_sphere = 2, - k_rb_shape_capsule = 3, -}; - -typedef struct rigidbody rigidbody; -typedef struct rb_capsule rb_capsule; - -struct rb_capsule -{ - f32 h, r; -}; - -struct rigidbody -{ - v3f co, v, w; - v4f q; - - f32 inv_mass; - - m3x3f iI, iIw; /* inertia model and inverse world tensor */ - m4x3f to_world, to_local; -}; - -VG_API void _vg_rigidbody_register(void); - -/* - * Initialize rigidbody inverse mass and inertia tensor with some common shapes - */ -void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h, f32 density, f32 inertia_scale ); -void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale ); -void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale ); - -/* - * Update ALL matrices and tensors on rigidbody - */ -void rb_update_matrices( rigidbody *rb ); - -/* - * Extrapolate rigidbody into a transform based on vg accumulator. - * Useful for rendering - */ -void rb_extrapolate( rigidbody *rb, v3f co, v4f q ); -void rb_iter( rigidbody *rb ); -void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ); - -/* - * Creates relative contact velocity vector - */ -void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ); - -/* - * Apply impulse to object - */ -void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ); - -/* - * Effectors - */ -void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag ); -void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, f32 spring, f32 dampening, f32 timestep ); - -#endif diff --git a/vg_rigidbody_collision.h b/vg_rigidbody_collision.h deleted file mode 100644 index 024a35a..0000000 --- a/vg_rigidbody_collision.h +++ /dev/null @@ -1,74 +0,0 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_rigidbody_collision.c" -#else - -/* TODO: Get rid of this! */ -#define VG_MAX_CONTACTS 256 - -typedef struct rb_ct rb_ct; -struct rb_ct -{ - rigidbody *rba, *rbb; - v3f co, n; - v3f t[2]; - float p, bias, norm_impulse, tangent_impulse[2], - normal_mass, tangent_mass[2]; - - u32 element_id; - - enum contact_type type; -} -extern rb_contact_buffer[VG_MAX_CONTACTS]; -extern int rb_contact_count; - -int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, - v3f coB, f32 rb, rb_ct *buf ); -int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, rb_capsule *cb, rb_ct *buf ); -int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ); -int rb_sphere__box( v3f coA, f32 ra, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ); -int rb_sphere__sphere( v3f coA, f32 ra, v3f coB, f32 rb, rb_ct *buf ); -int rb_sphere__triangle( m4x3f mtxA, f32 r, v3f tri[3], rb_ct *buf ); -int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ); -int rb_global_has_space( void ); -rb_ct *rb_global_buffer( void ); -int rb_manifold_apply_filtered( rb_ct *man, int len ); - -int rb_box_triangle_sat( v3f extent, v3f center, - m4x3f to_local, v3f tri_src[3] ); - -/* - * Merge two contacts if they are within radius(r) of eachother - */ -void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ); -void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ); - -/* - * Resolve overlapping pairs - */ -void rb_manifold_filter_pairs( rb_ct *man, int len, float r ); - -/* - * Remove contacts that are facing away from A - */ -void rb_manifold_filter_backface( rb_ct *man, int len ); - -/* - * Filter out duplicate coplanar results. Good for spheres. - */ -void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ); - -void rb_debug_contact( rb_ct *ct ); -void rb_solver_reset(void); -rb_ct *rb_global_ct(void); -void rb_prepare_contact( rb_ct *ct, f32 dt ); -void rb_depenetrate( rb_ct *manifold, int len, v3f dt ); -void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len ); -void rb_contact_restitution( rb_ct *ct, float cr ); -void rb_solve_contacts( rb_ct *buf, int len ); - -#endif diff --git a/vg_rigidbody_constraints.h b/vg_rigidbody_constraints.h deleted file mode 100644 index fd15053..0000000 --- a/vg_rigidbody_constraints.h +++ /dev/null @@ -1,50 +0,0 @@ -#if defined( VG_IMPLEMENTATION ) -# include "vg/vg_rigidbody_constraints.c" -#else - -typedef struct rb_constr_pos rb_constr_pos; -typedef struct rb_constr_swingtwist rb_constr_swingtwist; - -struct rb_constr_pos -{ - rigidbody *rba, *rbb; - v3f lca, lcb; -}; - -struct rb_constr_swingtwist -{ - rigidbody *rba, *rbb; - - v4f conevx, conevy; /* relative to rba */ - v3f view_offset, /* relative to rba */ - coneva, conevxb;/* relative to rbb */ - - int tangent_violation, axis_violation; - v3f axis, tangent_axis, tangent_target, axis_target; - - float conet; - float tangent_mass, axis_mass; - - f32 conv_tangent, conv_axis; -}; - -void rb_debug_position_constraints( rb_constr_pos *buffer, int len ); -void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); -void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); - -/* - * Solve a list of positional constraints - */ -void rb_solve_position_constraints( rb_constr_pos *buf, int len ); -void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ); -void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len ); -void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb ); - -/* - * Correct position constraint drift errors - * [ 0.0 <= amt <= 1.0 ]: the correction amount - */ -void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt ); -void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, int len, float amt ); - -#endif -- 2.25.1