From: hgn Date: Mon, 15 Sep 2025 22:33:45 +0000 (+0000) Subject: idiot X-Git-Url: https://skaterift.com/git/?a=commitdiff_plain;h=90a71ff4e3d421c73d39a2a48ad289fa467f3422;p=vg.git idiot --- 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/source/engine/audio_mixer.c b/source/engine/audio_mixer.c new file mode 100644 index 0000000..56aee24 --- /dev/null +++ b/source/engine/audio_mixer.c @@ -0,0 +1,1455 @@ +struct vg_audio _vg_audio = +{ + .master_volume_ui = 1.0f, + .dsp_enabled_ui = 1 +}; + +_Thread_local static bool _vg_audio_thread_has_lock = 0; + +void vg_audio_lock(void) +{ + SDL_LockMutex( _vg_audio.mutex ); + _vg_audio_thread_has_lock = 1; +} + +void vg_audio_unlock(void) +{ + _vg_audio_thread_has_lock = 0; + SDL_UnlockMutex( _vg_audio.mutex ); +} + +static void vg_audio_assert_lock(void) +{ + if( _vg_audio_thread_has_lock == 0 ) + { + vg_error( "vg_audio function requires locking\n" ); + abort(); + } +} + +/* clip loading from disk + * ------------------------------------------------------------------------------- + */ +VG_TIER_2 bool vg_audio_clip_load( audio_clip *clip, vg_stack_allocator *stack ) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); + + /* load in directly */ + u32 format = clip->flags & AUDIO_FLAG_FORMAT; + if( format == k_audio_format_vorbis ) + { + if( clip->path ) + { + clip->any_data = vg_file_read( stack, clip->path, &clip->size, 0 ); + if( clip->any_data ) + { + float mb = (float)(clip->size) / (1024.0f*1024.0f); + vg_info( "Loaded audio clip '%s' (%.1fmb)\n", clip->path, mb ); + return 1; + } + else + { + vg_error( "Audio failed to load '%s'\n", clip->path ); + return 0; + } + } + else + { + vg_error( "No path specified, embeded vorbis unsupported\n" ); + return 0; + } + } + else if( format == k_audio_format_stereo ) + { + vg_error( "Unsupported format (Stereo uncompressed)\n" ); + return 0; + } + else if( format == k_audio_format_bird ) + { + if( !clip->any_data ) + { + vg_error( "No data, external birdsynth unsupported\n" ); + return 0; + } + + struct synth_bird *bird = vg_stack_allocate( stack, sizeof(struct synth_bird), 8, NULL ); + bird->settings = clip->any_data; + clip->any_data = bird; + vg_info( "Loaded bird synthesis pattern (%u bytes)\n", clip->size ); + return 1; + } + else + { + if( !clip->path ) + { + vg_error( "No path specified, embeded mono unsupported\n" ); + return 0; + } + + u32 temp_frame = _vg_start_temp_frame(); + stb_vorbis_alloc alloc = + { + .alloc_buffer = vg_stack_allocate( _vg_temp_stack(), AUDIO_DECODE_SIZE, 8, "Vorbis alloc buffer (temp)" ), + .alloc_buffer_length_in_bytes = AUDIO_DECODE_SIZE + }; + + u32 fsize; + void *filedata = vg_file_read( _vg_temp_stack(), clip->path, &fsize, 0 ); + int err; + stb_vorbis *decoder = stb_vorbis_open_memory( filedata, fsize, &err, &alloc ); + if( !decoder ) + { + vg_error( "stb_vorbis_open_memory failed on '%s' (%d)\n", clip->path, err ); + _vg_end_temp_frame( temp_frame ); + return 0; + } + + /* only mono is supported in uncompressed */ + u32 length_samples = stb_vorbis_stream_length_in_samples( decoder ), + data_size = length_samples * sizeof(i16); + + clip->any_data = vg_stack_allocate( stack, data_size, 8, NULL ); + clip->size = length_samples; + int read_samples = stb_vorbis_get_samples_i16_downmixed( decoder, clip->any_data, length_samples ); + _vg_end_temp_frame( temp_frame ); + + if( read_samples == length_samples ) + return 1; + else + { + vg_error( "Decode error, read_samples did not match length_samples\n" ); + return 0; + } + } +} + +VG_TIER_2 u32 vg_audio_clip_loadn( audio_clip *arr, u32 count, vg_stack_allocator *stack ) +{ + u32 succeeded = 0; + for( u32 i=0; i 0) && (id <= AUDIO_CHANNELS) ); + return &_vg_audio.channels[ id-1 ]; +} + +static struct audio_channel_controls *get_audio_channel_controls( audio_channel_id id ) +{ + vg_audio_assert_lock(); + VG_ASSERT( (id > 0) && (id <= AUDIO_CHANNELS) ); + return &_vg_audio.channels[ id-1 ].controls; +} + +static struct audio_channel_state *get_audio_channel_state( audio_channel_id id ) +{ + VG_ASSERT( (id > 0) && (id <= AUDIO_CHANNELS) ); + return &_vg_audio.channels[ id-1 ].state; +} + +static audio_lfo *get_audio_lfo( audio_channel_id lfo_id ) +{ + VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); + return &_vg_audio.lfos[ lfo_id-1 ]; +} + +static struct audio_lfo_controls *get_audio_lfo_controls( audio_channel_id lfo_id ) +{ + vg_audio_assert_lock(); + VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); + return &_vg_audio.lfos[ lfo_id-1 ].controls; +} + +static struct audio_lfo_state *get_audio_lfo_state( audio_channel_id lfo_id ) +{ + VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); + return &_vg_audio.lfos[ lfo_id-1 ].state; +} + +static void audio_decode_uncompressed_mono( i16 *src, u32 count, f32 *dst ) +{ + for( u32 i=0; istage == k_channel_stage_none ) + { + channel->stage = k_channel_stage_allocation; + channel->ui_name[0] = 0; + channel->ui_colour = 0x00333333; + channel->group = 0; + channel->clip = NULL; + channel->ui_volume = 0; + channel->ui_pan = 0; + channel->ui_spacial_volume = 0; + channel->ui_spacial_pan = 0; + channel->ui_activity = k_channel_activity_wake; + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->flags = 0x00; + controls->volume_target = AUDIO_VOLUME_100; + controls->volume_slew_rate_per_sample = (f64)AUDIO_VOLUME_100 / (0.1*44100.0); + controls->pan_target = 0; + controls->pan_slew_rate_per_sample = (f64)AUDIO_PAN_RIGHT_100 / (0.1*44100.0); + controls->sampling_rate_multiplier = 1.0f; + controls->lfo_id = 0; + controls->lfo_attenuation_amount = 0.0f; + controls->pause = 0; + v4_copy( (v4f){0,0,0,1}, controls->spacial_falloff ); + + struct audio_channel_state *state = get_audio_channel_state( id ); + state->activity = k_channel_activity_wake; + state->loaded_clip_length = 0; + state->decoder_handle.bird = NULL; + state->decoder_handle.vorbis = NULL; + state->cursor = 0; + state->volume = AUDIO_VOLUME_100; + state->pan = 0; + state->spacial_volume = 0; + state->spacial_pan = 0; + state->spacial_warm = 0; + return id; + } + } + + return 0; +} + +void vg_audio_set_channel_clip( audio_channel_id id, audio_clip *clip ) +{ + vg_audio_assert_lock(); + + audio_channel *channel = get_audio_channel( id ); + VG_ASSERT( channel->stage == k_channel_stage_allocation ); + VG_ASSERT( channel->clip == NULL ); + + channel->clip = clip; + + u32 audio_format = channel->clip->flags & AUDIO_FLAG_FORMAT; + if( audio_format == k_audio_format_bird ) + strcpy( channel->ui_name, "[array]" ); + else if( audio_format == k_audio_format_gen ) + strcpy( channel->ui_name, "[program]" ); + else + vg_strncpy( clip->path, channel->ui_name, 32, k_strncpy_always_add_null ); +} + +void vg_audio_set_channel_group( audio_channel_id id, u16 group ) +{ + vg_audio_assert_lock(); + + audio_channel *channel = get_audio_channel( id ); + VG_ASSERT( channel->stage == k_channel_stage_allocation ); + VG_ASSERT( channel->group == 0 ); + channel->group = group; + if( group ) + channel->ui_colour = (((u32)group * 29986577) & 0x00ffffff) | 0xff000000; +} + +u32 vg_audio_count_channels_in_group( u16 group ) +{ + vg_audio_assert_lock(); + + u32 count = 0; + for( int id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + if( channel->stage != k_channel_stage_none ) + { + if( channel->group == group ) + count ++; + } + } + + return count; +} + +audio_channel_id vg_audio_get_first_active_channel_in_group( u16 group ) +{ + vg_audio_assert_lock(); + for( int id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + if( (channel->stage != k_channel_stage_none) && (channel->group == group) ) + return id; + } + return 0; +} + +void vg_audio_sidechain_lfo_to_channel( audio_channel_id id, audio_channel_id lfo_id, f32 amount ) +{ + vg_audio_assert_lock(); + + audio_lfo *lfo = get_audio_lfo( lfo_id ); + VG_ASSERT( lfo->stage == k_channel_stage_active ); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->lfo_id = lfo_id; + controls->lfo_attenuation_amount = amount; +} + +void vg_audio_add_channel_flags( audio_channel_id id, u32 flags ) +{ + vg_audio_assert_lock(); + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->flags |= flags; +} + +void vg_audio_set_channel_spacial_falloff( audio_channel_id id, v3f co, f32 range ) +{ + vg_audio_assert_lock(); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + v3_copy( co, controls->spacial_falloff ); + controls->spacial_falloff[3] = range == 0.0f? 1.0f: 1.0f/range; + + vg_audio_add_channel_flags( id, AUDIO_FLAG_SPACIAL_3D ); +} + +void vg_audio_sync_ui_master_controls(void) +{ + vg_audio_assert_lock(); + _vg_audio.controls.volume_target = ((f64)AUDIO_VOLUME_100) * _vg_audio.master_volume_ui; + _vg_audio.controls.dsp_enabled = _vg_audio.dsp_enabled_ui; +} + +void vg_audio_set_channel_volume( audio_channel_id id, f64 volume, bool instant ) +{ + vg_audio_assert_lock(); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->volume_target = ((f64)AUDIO_VOLUME_100) * volume; + + if( instant ) + { + audio_channel *channel = get_audio_channel( id ); + VG_ASSERT( channel->stage == k_channel_stage_allocation ); + + struct audio_channel_state *state = get_audio_channel_state( id ); + state->volume = controls->volume_target; + } +} + +void vg_audio_set_channel_volume_slew_duration( audio_channel_id id, f64 length_seconds ) +{ + vg_audio_assert_lock(); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->volume_slew_rate_per_sample = (f64)AUDIO_VOLUME_100 / (length_seconds * 44100.0); +} + +void vg_audio_set_channel_pan( audio_channel_id id, f64 pan, bool instant ) +{ + vg_audio_assert_lock(); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->pan_target = ((f64)AUDIO_PAN_RIGHT_100) * pan; + + if( instant ) + { + audio_channel *channel = get_audio_channel( id ); + VG_ASSERT( channel->stage == k_channel_stage_allocation ); + + struct audio_channel_state *state = get_audio_channel_state( id ); + state->pan = controls->pan_target; + } +} + +void vg_audio_set_channel_pan_slew_duration( audio_channel_id id, f64 length_seconds ) +{ + vg_audio_assert_lock(); + + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->pan_slew_rate_per_sample = (f64)AUDIO_PAN_RIGHT_100 / (length_seconds * 44100.0); +} + +void vg_audio_set_channel_sampling_rate( audio_channel_id id, f32 rate ) +{ + vg_audio_assert_lock(); + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->sampling_rate_multiplier = rate; +} + +void vg_audio_start_channel( audio_channel_id id ) +{ + vg_audio_assert_lock(); + + audio_channel *channel = get_audio_channel( id ); + VG_ASSERT( channel->stage == k_channel_stage_allocation ); + VG_ASSERT( channel->clip ); + channel->stage = k_channel_stage_active; +} + +audio_channel_id vg_audio_crossfade( audio_channel_id id, audio_clip *new_clip, f32 transition_seconds ) +{ + vg_audio_assert_lock(); + + audio_channel *channel = get_audio_channel( id ); + audio_channel_id new_id = 0; + if( new_clip ) + { + new_id = vg_audio_get_first_idle_channel(); + if( new_id ) + { + vg_audio_set_channel_clip( new_id, new_clip ); + vg_audio_set_channel_volume_slew_duration( new_id, transition_seconds ); + vg_audio_set_channel_volume( new_id, 1.0, 0 ); + vg_audio_set_channel_group( new_id, channel->group ); + + struct audio_channel_controls *existing_controls = get_audio_channel_controls( id ), + *new_controls = get_audio_channel_controls( new_id ); + + memcpy( new_controls, existing_controls, sizeof( struct audio_channel_controls ) ); + vg_audio_start_channel( new_id ); + } + } + + vg_audio_set_channel_volume_slew_duration( id, transition_seconds ); + vg_audio_set_channel_volume( id, 0.0, 0 ); + vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED ); + + return new_id; +} + +bool vg_audio_is_channel_using_clip( audio_channel_id id, audio_clip *clip ) +{ + vg_audio_assert_lock(); + audio_channel *channel = get_audio_channel( id ); + + if( channel->clip == clip ) return 1; + else return 0; +} + +void vg_audio_fadeout_flagged_audio( u32 flag, f32 length ) +{ + vg_audio_assert_lock(); + for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + if( channel->stage != k_channel_stage_none ) + { + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + if( controls->flags & flag ) + { + if( _vg_audio.working ) + vg_audio_crossfade( id, NULL, 1.0f ); + else + channel->stage = k_channel_stage_none; + } + } + } +} + +bool vg_audio_flagged_stopped( u32 flag ) +{ + vg_audio_lock(); + for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + if( channel->stage != k_channel_stage_none ) + { + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + if( controls->flags & flag ) + { + vg_audio_unlock(); + return 0; + } + } + } + vg_audio_unlock(); + return 1; +} + +void vg_audio_set_channel_pause( audio_channel_id id, bool pause ) +{ + vg_audio_assert_lock(); + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + controls->pause = pause; +} + +void vg_audio_set_flagged_pause( u32 flag, bool pause ) +{ + vg_audio_assert_lock(); + for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + if( channel->stage != k_channel_stage_none ) + { + struct audio_channel_controls *controls = get_audio_channel_controls( id ); + if( controls->flags & flag ) + vg_audio_set_channel_pause( id, pause ); + } + } +} + +void vg_audio_oneshot_3d( audio_clip *clip, v3f co, f32 range, f32 volume, u16 group, u32 flags ) +{ + vg_audio_assert_lock(); + audio_channel_id id = vg_audio_get_first_idle_channel(); + + if( id ) + { + vg_audio_set_channel_clip( id, clip ); + vg_audio_set_channel_spacial_falloff( id, co, range ); + vg_audio_set_channel_group( id, group ); + vg_audio_set_channel_volume( id, volume, 1 ); + vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED | flags ); + vg_audio_start_channel( id ); + } +} + +void vg_audio_oneshot( audio_clip *clip, f32 volume, f32 pan, u16 group, u32 flags ) +{ + vg_audio_assert_lock(); + audio_channel_id id = vg_audio_get_first_idle_channel(); + + if( id ) + { + vg_audio_set_channel_clip( id, clip ); + vg_audio_set_channel_group( id, group ); + vg_audio_set_channel_volume( id, volume, 1 ); + vg_audio_set_channel_pan( id, pan, 1 ); + vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED | flags ); + vg_audio_start_channel( id ); + } +} + + + +/* lfos + * ---------------------------------------------------------------------------------------- */ + +audio_channel_id vg_audio_get_first_idle_lfo(void) +{ + vg_audio_assert_lock(); + + for( int id=1; id<=AUDIO_LFOS; id ++ ) + { + audio_lfo *lfo = get_audio_lfo( id ); + + if( lfo->stage == k_channel_stage_none ) + { + lfo->stage = k_channel_stage_allocation; + + const u32 default_lfo_period = 44100; + + struct audio_lfo_controls *controls = get_audio_lfo_controls( id ); + controls->period_in_samples = default_lfo_period; + controls->wave_type = k_lfo_triangle; + controls->polynomial_coefficient = 0.0f; + controls->flags = 0x00; + + struct audio_lfo_state *state = get_audio_lfo_state( id ); + state->time = 0; + state->last_period_in_samples = default_lfo_period; + state->frame_reference_count = 0; + state->time_at_frame_start = 0; + return id; + } + } + + return 0; +} + +void vg_audio_set_lfo_polynomial_bipolar( audio_channel_id lfo_id, f32 coefficient ) +{ + vg_audio_assert_lock(); + + struct audio_lfo_controls *controls = get_audio_lfo_controls( lfo_id ); + controls->polynomial_coefficient = coefficient; + controls->sqrt_polynomial_coefficient = sqrtf(coefficient); + controls->wave_type = k_lfo_polynomial_bipolar; +} + +void vg_audio_set_lfo_frequency( audio_channel_id lfo_id, f32 freq ) +{ + vg_audio_assert_lock(); + + struct audio_lfo_controls *controls = get_audio_lfo_controls( lfo_id ); + u32 length = 44100.0f / freq; + controls->period_in_samples = length; + + audio_lfo *lfo = get_audio_lfo( lfo_id ); + if( lfo->stage == k_channel_stage_allocation ) + { + struct audio_lfo_state *state = get_audio_lfo_state( lfo_id ); + state->last_period_in_samples = length; + } +} + +void vg_audio_start_lfo( audio_channel_id lfo_id ) +{ + vg_audio_assert_lock(); + audio_lfo *lfo = get_audio_lfo( lfo_id ); + lfo->stage = k_channel_stage_active; +} + +static void audio_channel_get_samples( audio_channel_id id, struct audio_channel_controls *controls, + u32 count, f32 *out_stereo ) +{ + _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:decode" ); + + u32 remaining = count; + u32 buffer_pos = 0; + + audio_channel *channel = get_audio_channel( id ); + struct audio_channel_state *state = get_audio_channel_state( id ); + u32 format = channel->clip->flags & AUDIO_FLAG_FORMAT; + + while( remaining ) + { + u32 samples_this_run = VG_MIN( remaining, state->loaded_clip_length - state->cursor ); + remaining -= samples_this_run; + + f32 *dst = &out_stereo[ buffer_pos * 2 ]; + + if( format == k_audio_format_stereo ) + { + for( u32 i=0; idecoder_handle.vorbis, + dst, samples_this_run ); + if( read_samples != samples_this_run ) + { + vg_warn( "Invalid samples read (%s)\n", channel->clip->path ); + + for( u32 i=0; idecoder_handle.bird, dst, samples_this_run ); + } + else if( format == k_audio_format_gen ) + { + void (*fn)( void *data, f32 *buf, u32 count ) = channel->clip->generative_function; + fn( channel->clip->any_data, dst, samples_this_run ); + } + else + { + i16 *src_buffer = channel->clip->any_data, + *src = &src_buffer[ state->cursor ]; + audio_decode_uncompressed_mono( src, samples_this_run, dst ); + } + + state->cursor += samples_this_run; + buffer_pos += samples_this_run; + + if( (controls->flags & AUDIO_FLAG_LOOP) && remaining ) + { + if( format == k_audio_format_vorbis ) + stb_vorbis_seek_start( state->decoder_handle.vorbis ); + else if( format == k_audio_format_bird ) + synth_bird_reset( state->decoder_handle.bird ); + + state->cursor = 0; + continue; + } + else + break; + } + + while( remaining ) + { + out_stereo[ buffer_pos*2 + 0 ] = 0.0f; + out_stereo[ buffer_pos*2 + 1 ] = 0.0f; + buffer_pos ++; + remaining --; + } + + _vg_profiler_exit_block( _vg_audio.profiler ); +} + +static f32 audio_lfo_get_sample( audio_channel_id lfo_id, struct audio_lfo_controls *controls ) +{ + struct audio_lfo_state *state = get_audio_lfo_state( lfo_id ); + state->time ++; + + if( state->time >= controls->period_in_samples ) + state->time = 0; + + f32 t = state->time; + t /= (f32)controls->period_in_samples; + + if( controls->wave_type == k_lfo_polynomial_bipolar ) + { + /* + * # + * # # + * # # + * # # + * ### # ### + * ## # + * # # + * # # + * ## + */ + + t *= 2.0f; + t -= 1.0f; + + return (( 2.0f * controls->sqrt_polynomial_coefficient * t ) / + /* --------------------------------------- */ + ( 1.0f + controls->polynomial_coefficient * t*t )) * (1.0f-fabsf(t)); + } + else + return 0.0f; +} + +static void audio_slew_i32( i32 *value, i32 target, i32 rate ) +{ + i32 sign = target - *value; + if( sign == 0 ) + return; + + sign = sign>0? 1: -1; + i32 c = *value + sign*rate; + + if( target*sign < c*sign ) *value = target; + else *value = c; +} + +static void audio_channel_mix( audio_channel_id id, + struct audio_channel_controls *controls, + struct audio_master_controls *master_controls, f32 *inout_buffer ) +{ + struct audio_channel_state *state = get_audio_channel_state( id ); + + bool is_3d = controls->flags & AUDIO_FLAG_SPACIAL_3D? 1: 0; + bool use_doppler = controls->flags & AUDIO_FLAG_NO_DOPPLER? 0: 1; + + f32 frame_sample_rate = controls->sampling_rate_multiplier; + + i32 spacial_volume_target = 0, + spacial_pan_target = 0; + + if( is_3d ) + { + v3f delta; + v3_sub( controls->spacial_falloff, master_controls->listener_position, delta ); + + f32 dist = v3_length( delta ); + + if( dist <= 0.01f ) + { + spacial_pan_target = 0; + spacial_volume_target = AUDIO_VOLUME_100; + } + else if( dist > 20000.0f || !vg_validf( dist ) ) + { + spacial_pan_target = 0; + spacial_volume_target = 0; + } + else + { + f32 vol = vg_maxf( 0.0f, 1.0f - controls->spacial_falloff[3]*dist ); + vol = powf( vol, 5.0f ); + spacial_volume_target = (f64)vg_clampf( vol, 0.0f, 1.0f ) * (f64)AUDIO_VOLUME_100 * 0.5; + + v3_muls( delta, 1.0f/dist, delta ); + f32 pan = v3_dot( master_controls->listener_right_ear_direction, delta ); + spacial_pan_target = (f64)vg_clampf( pan, -1.0f, 1.0f ) * (f64)AUDIO_PAN_RIGHT_100; + + if( use_doppler ) + { + const float vs = 323.0f; + + f32 dv = v3_dot( delta, master_controls->listener_velocity ); + f32 doppler = (vs+dv)/vs; + doppler = vg_clampf( doppler, 0.6f, 1.4f ); + + if( fabsf(doppler-1.0f) > 0.01f ) + frame_sample_rate *= doppler; + } + } + + if( !state->spacial_warm ) + { + state->spacial_volume = spacial_volume_target; + state->spacial_pan = spacial_pan_target; + state->spacial_warm = 1; + } + } + + u32 buffer_length = AUDIO_MIX_FRAME_SIZE; + if( frame_sample_rate != 1.0f ) + { + float l = ceilf( (float)(AUDIO_MIX_FRAME_SIZE) * frame_sample_rate ); + buffer_length = l+1; + } + + f32 samples[ AUDIO_MIX_FRAME_SIZE*2 * 2 ]; + audio_channel_get_samples( id, controls, buffer_length, samples ); + + _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:mixing" ); + + /* TODO: Slew this */ + f64 master_volume = (f64)_vg_audio.state.volume / (f64)AUDIO_VOLUME_100; + + for( u32 j=0; jvolume, controls->volume_target, controls->volume_slew_rate_per_sample ); + audio_slew_i32( &state->pan, controls->pan_target, controls->pan_slew_rate_per_sample ); + + f64 v_c = ((f64)state->volume / (f64)AUDIO_VOLUME_100) * master_volume; + + if( controls->lfo_id ) + { + struct audio_lfo_state *state = get_audio_lfo_state( controls->lfo_id ); + f32 lfo_value = audio_lfo_get_sample( controls->lfo_id, state->controls ); + v_c *= 1.0 + lfo_value * controls->lfo_attenuation_amount; + } + + f64 v_l = v_c*v_c, + v_r = v_c*v_c; + + if( is_3d ) + { + const i32 vol_rate = (f64)AUDIO_VOLUME_100 / (0.05 * 44100.0), + pan_rate = (f64)AUDIO_PAN_RIGHT_100 / (0.05 * 44100.0); + + audio_slew_i32( &state->spacial_volume, spacial_volume_target, vol_rate ); + audio_slew_i32( &state->spacial_pan, spacial_pan_target, pan_rate ); + + f64 v_s = (f64)state->spacial_volume / (f64)AUDIO_VOLUME_100, + v_p = (f64)state->spacial_pan / (f64)AUDIO_PAN_RIGHT_100; + + v_l *= v_s * (1.0-v_p); + v_r *= v_s * (1.0+v_p); + } + + f32 s_l, s_r; + if( frame_sample_rate != 1.0f ) + { + /* absolutely garbage resampling, but it will do + */ + f32 sample_index = frame_sample_rate * (f32)j; + f32 t = vg_fractf( sample_index ); + + u32 i0 = floorf( sample_index ), + i1 = i0+1; + + s_l = samples[ i0*2+0 ]*(1.0f-t) + samples[ i1*2+0 ]*t; + s_r = samples[ i0*2+1 ]*(1.0f-t) + samples[ i1*2+1 ]*t; + } + else + { + s_l = samples[ j*2+0 ]; + s_r = samples[ j*2+1 ]; + } + + inout_buffer[ j*2+0 ] += s_l * v_l; + inout_buffer[ j*2+1 ] += s_r * v_r; + } + + _vg_profiler_exit_block( _vg_audio.profiler ); +} + + +static void _vg_audio_mixer( void *user, u8 *stream, int byte_count ) +{ + _vg_profiler_tick( _vg_audio.profiler ); + + int sample_count = byte_count/(2*sizeof(f32)); + + f32 *output_stereo = (f32 *)stream; + for( int i=0; istage == k_channel_stage_active ) + { + struct audio_channel_controls *controls = get_audio_channel_controls(id); + if( controls->pause ) + { + channel->ui_activity = k_channel_activity_paused; + continue; + } + + active_channel_list[ active_channel_count ] = id; + memcpy( &channel_controls[ active_channel_count ], controls, sizeof( struct audio_channel_controls ) ); + active_channel_count ++; + } + } + for( u32 id=1; id<=AUDIO_LFOS; id ++ ) + { + audio_lfo *lfo = get_audio_lfo( id ); + if( lfo->stage == k_channel_stage_active ) + { + struct audio_lfo_controls *local_controls = &lfo_controls[ active_lfo_count ]; + active_lfo_list[ active_lfo_count ] = id; + memcpy( local_controls, get_audio_lfo_controls(id), sizeof(struct audio_lfo_controls) ); + active_lfo_count ++; + + struct audio_lfo_state *state = get_audio_lfo_state(id); + state->controls = local_controls; + } + } + dsp_update_tunings(); + vg_audio_unlock(); + + /* init step */ + master_state->volume = master_controls.volume_target; + master_state->volume_at_frame_start = master_controls.volume_target; + + for( u32 i=0; iactivity == k_channel_activity_wake ) + { + audio_channel *channel = get_audio_channel( id ); + + u32 format = channel->clip->flags & AUDIO_FLAG_FORMAT; + if( format == k_audio_format_vorbis ) + { + /* Setup vorbis decoder */ + u8 *buf = (u8*)_vg_audio.decoding_buffer, + *loc = &buf[AUDIO_DECODE_SIZE*id]; + + stb_vorbis_alloc alloc = { + .alloc_buffer = (char *)loc, + .alloc_buffer_length_in_bytes = AUDIO_DECODE_SIZE + }; + + int err; + stb_vorbis *decoder = stb_vorbis_open_memory( channel->clip->any_data, channel->clip->size, &err, &alloc ); + + if( !decoder ) + { + vg_error( "stb_vorbis_open_memory failed on '%s' (%d)\n", channel->clip->path, err ); + channel_state->activity = k_channel_activity_error; + } + else + { + channel_state->loaded_clip_length = stb_vorbis_stream_length_in_samples( decoder ); + channel_state->decoder_handle.vorbis = decoder; + channel_state->activity = k_channel_activity_playing; + } + } + else if( format == k_audio_format_bird ) + { + u8 *buf = (u8*)_vg_audio.decoding_buffer; + struct synth_bird *loc = (void *)&buf[AUDIO_DECODE_SIZE*id]; + + memcpy( loc, channel->clip->any_data, channel->clip->size ); + synth_bird_reset( loc ); + + channel_state->decoder_handle.bird = loc; + channel_state->loaded_clip_length = synth_bird_get_length_in_samples( loc ); + channel_state->activity = k_channel_activity_playing; + } + else if( format == k_audio_format_stereo ) + { + channel_state->loaded_clip_length = channel->clip->size / 2; + channel_state->activity = k_channel_activity_playing; + } + else if( format == k_audio_format_gen ) + { + channel_state->loaded_clip_length = 0xffffffff; + channel_state->activity = k_channel_activity_playing; + } + else + { + channel_state->loaded_clip_length = channel->clip->size; + channel_state->activity = k_channel_activity_playing; + } + } + } + + for( u32 i=0; iperiod_in_samples != state->last_period_in_samples ) + { + state->last_period_in_samples = controls->period_in_samples; + f64 t = state->time; + t/= (f64)controls->period_in_samples; + state->time = (f64)controls->period_in_samples * t; + } + + state->time_at_frame_start = state->time; + state->frame_reference_count = 0; + } + + /* mix step */ + for( u32 dry_layer=0; dry_layer<=1; dry_layer ++ ) + { + for( u32 i=0; iactivity == k_channel_activity_playing ) + { + if( master_controls.dsp_enabled ) + { + if( controls->flags & AUDIO_FLAG_NO_DSP ) + { + if( !dry_layer ) + continue; + } + else + { + if( dry_layer ) + continue; + } + } + + if( controls->lfo_id ) + { + struct audio_lfo_state *lfo_state = get_audio_lfo_state( controls->lfo_id ); + lfo_state->time = lfo_state->time_at_frame_start; + lfo_state->frame_reference_count ++; + } + + u32 remaining = sample_count, + subpos = 0; + + while( remaining ) + { + audio_channel_mix( id, controls, &master_controls, output_stereo+subpos ); + remaining -= AUDIO_MIX_FRAME_SIZE; + subpos += AUDIO_MIX_FRAME_SIZE*2; + } + + if( (state->cursor >= state->loaded_clip_length) && !(controls->flags & AUDIO_FLAG_LOOP) ) + state->activity = k_channel_activity_end; + } + } + + if( master_controls.dsp_enabled ) + { + if( !dry_layer ) + { + _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:dsp/effects" ); + for( int i=0; iui_activity = state->activity; + channel->ui_volume = state->volume; + channel->ui_pan = state->pan; + channel->ui_spacial_volume = state->spacial_volume; + channel->ui_spacial_pan = state->spacial_pan; + + if( controls->flags & AUDIO_FLAG_RELINQUISHED ) + { + bool die = 0; + if( state->activity == k_channel_activity_end ) die = 1; + if( state->activity == k_channel_activity_error ) die = 1; + if( state->volume == 0 ) die = 1; + + if( die ) + { + channel->stage = k_channel_stage_none; + } + } + } + + _vg_audio.samples_written_last_audio_frame = sample_count; + vg_audio_unlock(); +} + +/* + * Debugging + */ +struct vg_audio_view_data +{ + i32 view_3d; +}; + +static f32 audio_volume_integer_to_float( i32 volume ) +{ + return (f32)volume / (f32)AUDIO_VOLUME_100; +} + +static void cb_vg_audio_view( ui_context *ctx, ui_rect rect, struct vg_magi_panel *magi ) +{ + struct vg_audio_view_data *vd = magi->data; + + ui_rect left, panel; + ui_split( rect, k_ui_axis_v, 256, 2, left, panel ); + ui_checkbox( ctx, left, "3D labels", &vd->view_3d ); + + vg_audio_lock(); + char perf[128]; + ui_rect overlap_buffer[ AUDIO_CHANNELS ]; + u32 overlap_length = 0; + + /* Draw audio stack */ + for( int id=1; id<=AUDIO_CHANNELS; id ++ ) + { + audio_channel *channel = get_audio_channel( id ); + + ui_rect row; + ui_split( panel, k_ui_axis_h, 18, 1, row, panel ); + + bool show_row = ui_clip( rect, row, row ); + + if( channel->stage == k_channel_stage_none ) + { + if( show_row ) + ui_fill( ctx, row, 0x50333333 ); + } + else if( channel->stage == k_channel_stage_allocation ) + { + if( show_row ) + ui_fill( ctx, row, 0x50ff3333 ); + } + else if( channel->stage == k_channel_stage_active ) + { + if( show_row ) + { + char buf[256]; + vg_str str; + vg_strnull( &str, buf, sizeof(buf) ); + vg_strcati64r( &str, id, 10, 2, ' ' ); + + if( channel->group ) + { + vg_strcat( &str, " grp" ); + vg_strcati64r( &str, channel->group, 10, 6, ' ' ); + } + else + vg_strcat( &str, " " ); + + vg_strcat( &str, " flags:" ); + u32 flags = get_audio_channel_controls( id )->flags; + vg_strcatch( &str, (flags & AUDIO_FLAG_RELINQUISHED)? 'R': '_' ); + vg_strcatch( &str, (flags & AUDIO_FLAG_SPACIAL_3D)? 'S': '_' ); + vg_strcatch( &str, (flags & AUDIO_FLAG_WORLD)? 'W': '_' ); + vg_strcatch( &str, (flags & AUDIO_FLAG_NO_DOPPLER)? '_':'D' ); + vg_strcatch( &str, (flags & AUDIO_FLAG_NO_DSP)? '_':'E' ); + vg_strcatch( &str, (flags & AUDIO_FLAG_LOOP)? 'L':'_' ); + + const char *formats[] = + { + " mono ", + " stereo ", + " vorbis ", + " none0 ", + " none1 ", + " none2 ", + " none3 ", + " none4 ", + "synth:bird", + " none5 ", + " none6 ", + " none7 ", + " none8 ", + " none9 ", + " none10 ", + " none11 ", + }; + u32 format_index = (channel->clip->flags & AUDIO_FLAG_FORMAT)>>9; + vg_strcat( &str, " format:" ); + vg_strcat( &str, formats[format_index] ); + + const char *activties[] = + { + "wake ", + "play ", + "pause", + "end ", + "error" + }; + vg_strcat( &str, " " ); + vg_strcat( &str, activties[channel->ui_activity] ); + + vg_strcat( &str, " " ); + f32 volume = (f32)channel->ui_volume / (f32)AUDIO_VOLUME_100; + vg_strcati64r( &str, volume * 100.0f, 10, 3, ' ' ); + vg_strcatch( &str, '%' ); + + vg_strcat( &str, " " ); + vg_strcat( &str, channel->ui_name ); + + ui_rect row_l, row_r; + ui_split( row, k_ui_axis_v, 32, 2, row_l, row_r ); + + ui_rect indicator_l, indicator_r; + ui_split_ratio( row_l, k_ui_axis_v, 0.5f, 1, indicator_l, indicator_r ); + + ui_fill( ctx, indicator_l, 0xff000000 ); + if( volume > 0.01f ) + { + f32 h = volume * (f32)indicator_l[3]; + ui_rect vol_bar = { indicator_l[0], indicator_l[1] + indicator_l[3] - h, indicator_l[2], h }; + ui_fill( ctx, vol_bar, 0xff00ff00 ); + + if( flags & AUDIO_FLAG_SPACIAL_3D ) + { + f32 h = ((f32)channel->ui_spacial_volume / (f32)AUDIO_VOLUME_100) * (f32)indicator_l[3]; + ui_rect space_bar = { indicator_l[0], indicator_l[1] + indicator_l[3] - h, indicator_l[2], 1 }; + ui_fill( ctx, space_bar, 0xffffffff ); + } + } + + f32 pan = (f32)channel->ui_pan / (f32)AUDIO_PAN_RIGHT_100; + ui_fill( ctx, indicator_r, 0xff111111 ); + f32 midpoint = (f32)indicator_r[0] + (f32)indicator_r[2] * 0.5f; + + f32 pan_abs = fabsf(pan); + if( pan_abs > 0.01f ) + { + ui_rect bar = { midpoint,indicator_r[1], pan_abs * (f32)indicator_r[2] * 0.5f, indicator_r[3] }; + + if( pan < 0.0f ) + bar[0] -= (f32)bar[2]; + + ui_fill( ctx, bar, 0xff00aaff ); + } + + if( flags & AUDIO_FLAG_SPACIAL_3D ) + { + f32 w = ((f32)channel->ui_spacial_pan / (f32)AUDIO_PAN_RIGHT_100) * (f32)indicator_r[2] * 0.5f; + ui_rect space_bar = { midpoint+w, indicator_r[1], 1, indicator_r[3] }; + ui_fill( ctx, space_bar, 0xffffffff ); + } + + ui_fill( ctx, row_r, 0xa0000000 | channel->ui_colour ); + ui_text( ctx, row_r, buf, 1, k_ui_align_middle_left, 0 ); + } + } + +#if 0 +#ifdef VG_3D + if( vd->view_3d && (ch->flags & AUDIO_FLAG_SPACIAL_3D) ) + { + v4f wpos; + v3_copy( ch->spacial_falloff, wpos ); + + wpos[3] = 1.0f; + m4x4_mulv( vg.pv, wpos, wpos ); + + if( wpos[3] > 0.0f ) + { + v2_muls( wpos, (1.0f/wpos[3]) * 0.5f, wpos ); + v2_add( wpos, (v2f){ 0.5f, 0.5f }, wpos ); + + ui_rect wr; + wr[0] = vg_clampf(wpos[0] * vg.window_x, -32000.0f,32000.0f); + wr[1] = vg_clampf((1.0f-wpos[1]) * vg.window_y,-32000.0f,32000.0f); + wr[2] = 1000; + wr[3] = 17; + + for( int j=0; j<12; j++ ) + { + int collide = 0; + for( int k=0; k= wk[0])) && + ((wr[1] <= wk[1]+wk[3]) && (wr[1]+wr[3] >= wk[1])) ) + { + collide = 1; + break; + } + } + + if( !collide ) + break; + else + wr[1] += 18; + } + + ui_text( ctx, wr, perf, 1, k_ui_align_middle_left, 0 ); + rect_copy( wr, overlap_buffer[ overlap_length ++ ] ); + } + } +#endif +#endif + } + + vg_audio_unlock(); +} + +static void cb_vg_audio_close( struct vg_magi_panel *me ) +{ + vg_audio_lock(); + _vg_audio.inspector_open = 0; + vg_audio_unlock(); + free( me->data ); +} + +static int cmd_vg_audio( int argc, const char *argv[] ) +{ + vg_audio_lock(); + if( _vg_audio.inspector_open ) + { + vg_error( "Only 1 audio inspector at a time.\n" ); + vg_audio_unlock(); + return 0; + } + + _vg_audio.inspector_open = 1; + vg_audio_unlock(); + ui_px w = 800, h=8*18; + struct vg_magi_panel *magi = _vg_magi_open( w,h, VG_MAGI_ALL ); + magi->title = "Audio inspector"; + + struct vg_audio_view_data *vd = malloc(sizeof(struct vg_audio_view_data)); + vd->view_3d = 0; + + magi->data = vd; + magi->ui_cb = cb_vg_audio_view; + magi->close_cb = cb_vg_audio_close; + return 1; +} + +VG_API void _vg_audio_register(void) +{ + vg_console_reg_cmd( "vg_audio", cmd_vg_audio, NULL ); + vg_console_reg_var( "volume", &_vg_audio.master_volume_ui, k_var_dtype_f32, VG_VAR_PERSISTENT ); + vg_console_reg_var( "vg_dsp", &_vg_audio.dsp_enabled_ui, k_var_dtype_i32, VG_VAR_PERSISTENT ); + vg_console_reg_var( "vg_audio_device", &_vg_audio.device_choice, k_var_dtype_str, VG_VAR_PERSISTENT ); +} + +void vg_audio_device_init(void) +{ + SDL_AudioSpec spec_desired, spec_got; + spec_desired.callback = _vg_audio_mixer; + spec_desired.channels = 2; + spec_desired.format = AUDIO_F32; + spec_desired.freq = 44100; + spec_desired.padding = 0; + spec_desired.samples = AUDIO_FRAME_SIZE; + spec_desired.silence = 0; + spec_desired.size = 0; + spec_desired.userdata = NULL; + + _vg_audio.sdl_output_device = SDL_OpenAudioDevice( _vg_audio.device_choice.buffer, 0, + &spec_desired, &spec_got, 0 ); + + vg_info( "Start audio device (%u, F32, %u) @%s\n", spec_desired.freq, AUDIO_FRAME_SIZE, + _vg_audio.device_choice.buffer ); + + if( _vg_audio.sdl_output_device ) + { + SDL_PauseAudioDevice( _vg_audio.sdl_output_device, 0 ); + vg_success( "Unpaused device %d.\n", _vg_audio.sdl_output_device ); + _vg_audio.working = 1; + } + else + { + _vg_audio.working = 0; + vg_error( + "SDL_OpenAudioDevice failed. Your default audio device must support (or transcode from):\n" + " Sample rate: 44100 hz\n" + " Buffer size: 512 samples\n" + " Channels: 2\n" + " Format: s16 or f32\n" ); + } +} + +static void vg_audio_free(void) +{ + SDL_CloseAudioDevice( _vg_audio.sdl_output_device ); + _vg_audio.sdl_output_device = 0; +} + +VG_API void _vg_audio_init(void) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_SDL ) ); + + _vg_audio.profiler = _vg_profiler_create( "vg.audio", 1000.0f/20.0f ); + + /* fixed */ + u32 decode_size = AUDIO_DECODE_SIZE * AUDIO_CHANNELS; + _vg_audio.decoding_buffer = vg_stack_allocate( NULL, decode_size, 8, "Decoding buffers" ); + + struct audio_master_controls *master_controls = &_vg_audio.controls; + master_controls->dsp_enabled = _vg_audio.dsp_enabled_ui; + master_controls->volume_target = (f64)_vg_audio.master_volume_ui * (f64)AUDIO_VOLUME_100; + v3_copy( (v3f){1,0,0}, master_controls->listener_right_ear_direction ); + v3_zero( master_controls->listener_velocity ); + v3_zero( master_controls->listener_position ); + _vg_dsp_init(); + + _vg_audio.mutex = SDL_CreateMutex(); + if( _vg_audio.mutex == NULL ) + vg_fatal_error( "SDL2: %s\n", SDL_GetError() ); + vg_audio_device_init(); + + _vg_add_exit_function( vg_audio_free ); +} + +void vg_audio_preupdate(void) +{ + bool before_working = _vg_audio.working; + _vg_audio.working = 1; + if( _vg_audio.sdl_output_device == 0 ) + _vg_audio.working = 0; + else + if( SDL_GetAudioDeviceStatus( _vg_audio.sdl_output_device ) == SDL_AUDIO_STOPPED ) + _vg_audio.working = 0; +} diff --git a/source/engine/profiler.c b/source/engine/profiler.c new file mode 100644 index 0000000..f4bd4fc --- /dev/null +++ b/source/engine/profiler.c @@ -0,0 +1,223 @@ +#define PROFILER_ROW_MAX 16 +#define PROFILER_STACK_MAX 8 +#define PROFILER_HISTORY_LENGTH 64 + +struct vg_profiler +{ + const c8 *name; + f32 history_ms[ PROFILER_HISTORY_LENGTH ][ PROFILER_ROW_MAX ]; + u32 buffer_current; + + u64 row_accumulated[ PROFILER_ROW_MAX ]; + const c8 *row_names[ PROFILER_ROW_MAX ]; + u32 row_length; + + u64 sample_stack[ PROFILER_STACK_MAX ]; + u32 block_stack[ PROFILER_STACK_MAX ]; + u32 stack_height; + + u64 tick_last, segment_last; + + f32 default_budget_ms; +}; + +struct +{ + vg_stack_allocator stack; + u32 count; + vg_mutex tick_lock; +} +_vg_profiler; + +VG_API void _vg_profiler_init(void) +{ + vg_stack_init( &_vg_profiler.stack, NULL, VG_MB(8), "Profilers" ); + VG_ASSERT( VG_MUTEX_INIT( _vg_profiler.tick_lock ) ); +} + +VG_API u32 _vg_profiler_create( const c8 *name, f32 default_budget_ms ) +{ + struct vg_profiler *profiler = vg_stack_allocate( &_vg_profiler.stack, sizeof(struct vg_profiler), 1, "Profiler" ); + vg_zero_mem( profiler, sizeof(struct vg_profiler) ); + profiler->name = name; + profiler->default_budget_ms = default_budget_ms; + _vg_profiler.count ++; + return vg_stack_offset( &_vg_profiler.stack, profiler ); +} + +VG_API_INTERNAL static struct vg_profiler *_vg_profiler_get( u32 id ) +{ + return vg_stack_pointer( &_vg_profiler.stack, id ); +} + +VG_API void _vg_profiler_tick( u32 profiler_id ) +{ + struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); + + u64 new_tick = SDL_GetPerformanceCounter(), + duration = new_tick - profiler->tick_last; + + f64 to_ms = 1000.0 / (f64)SDL_GetPerformanceFrequency(); + + VG_MUTEX_LOCK( _vg_profiler.tick_lock ); + for( u32 i=0; ihistory_ms[ profiler->buffer_current ][ i ] = (f32)((f64)profiler->row_accumulated[i] * to_ms); + profiler->row_accumulated[i] = 0; + } + profiler->buffer_current ++; + if( profiler->buffer_current >= PROFILER_HISTORY_LENGTH ) + profiler->buffer_current = 0; + VG_MUTEX_UNLOCK( _vg_profiler.tick_lock ); + + profiler->tick_last = new_tick; +} + +static u32 vg_profiler_block_id( struct vg_profiler *profiler, const c8 *block_name ) +{ + for( u32 i=0; irow_length; i ++ ) + if( profiler->row_names[i] == block_name ) + return i + 1; + if( profiler->row_length < PROFILER_ROW_MAX ) + { + profiler->row_names[ profiler->row_length ++ ] = block_name; + return profiler->row_length; + } + else return 0; +} + +VG_API void _vg_profiler_enter_block( u32 profiler_id, const c8 *block_name ) +{ + u64 seg_end = SDL_GetPerformanceCounter(); + + struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); + VG_ASSERT( profiler->stack_height < PROFILER_STACK_MAX ); + + if( profiler->stack_height ) + { + u32 lower_block = profiler->block_stack[ profiler->stack_height ]; + if( lower_block ) + profiler->row_accumulated[ lower_block-1 ] += (seg_end - profiler->segment_last); + } + + u32 block_id = vg_profiler_block_id( profiler, block_name ); + profiler->block_stack[ profiler->stack_height ] = block_id; + profiler->stack_height ++; + profiler->segment_last = seg_end; +} + +VG_API void _vg_profiler_exit_block( u32 profiler_id ) +{ + u64 seg_end = SDL_GetPerformanceCounter(); + + struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); + VG_ASSERT( profiler->stack_height ); + + profiler->stack_height --; + u32 block_id = profiler->block_stack[ profiler->stack_height ]; + + if( block_id ) + profiler->row_accumulated[ block_id-1 ] += (seg_end - profiler->segment_last ); + + profiler->segment_last = seg_end; +} + +VG_API void _vg_profiler_draw( ui_context *ctx, u32 profiler_id, f32 budget_ms, ui_rect panel, i32 dir, bool normalize ) +{ + struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); + if( panel[2] == 0 ) + panel[2] = 256; + if( panel[3] == 0 ) + panel[3] = PROFILER_HISTORY_LENGTH * 2; + + f32 sh = (f32)panel[3^dir] / (f32)PROFILER_HISTORY_LENGTH, + sw = (f32)panel[2^dir]; + + ui_fill( ctx, panel, 0xa0000000 ); + + u32 colours[ PROFILER_ROW_MAX ]; + for( u32 i=0; irow_length; i ++ ) + colours[i] = vg_strdjb2( profiler->row_names[i] ) | 0xff000000; + + for( i32 i=0; irow_length; j ++ ) + { + f32 sample_ms = profiler->history_ms[i][j], + pos_x = (total_ms / budget_ms) * sw, + width = (sample_ms / budget_ms) * sw; + + ui_rect block; + block[0^dir] = panel[0^dir] + pos_x; + block[1^dir] = panel[1^dir] + (f32)i*sh; + block[2^dir] = VG_MAX( 1, width-1 ); + block[3^dir] = ceilf(sh)-1; + ui_fill( ctx, block, colours[j] ); + total_ms += sample_ms; + } + } + +#if 0 + c8 infbuf[64]; + snprintf( infbuf, 64, "accuracy: %.7fms", rate_mul ); + ui_text( ctx, (ui_rect){ panel[0], panel[1], panel[2]-4, 24 }, infbuf, 1, k_ui_align_right, 0 ); + + for( int i=0; iname; + snprintf( infbuf, 64, "%.4fms %s", avgs[i] * (1.0f/(VG_PROFILE_SAMPLE_COUNT-1)), name ); + ui_text( ctx, (ui_rect){ panel[0], panel[1] + 24 + i*14, panel[2]-4, 14 }, infbuf, 1, k_ui_align_right, 0 ); + } +#endif +} + +static void cb_vg_profiler( ui_context *ctx, ui_rect rect, struct vg_magi_panel *magi ) +{ + struct vg_profiler *profiler = magi->data; + _vg_profiler_draw( ctx, vg_stack_offset( &_vg_profiler.stack, profiler ), profiler->default_budget_ms, rect, 0, 0 ); +} + +static int cmd_vg_profile( int argc, const char *argv[] ) +{ + if( argc == 1 ) + { + for( u32 i=0; i<_vg_profiler.count; i ++ ) + { + struct vg_profiler *profiler = vg_stack_pointer( &_vg_profiler.stack, i*sizeof(struct vg_profiler) ); + if( !strcmp( argv[0], profiler->name ) ) + { + ui_px w = 256, h = PROFILER_HISTORY_LENGTH * 2; + struct vg_magi_panel *magi = _vg_magi_open( w,h, VG_MAGI_MOVEABLE|VG_MAGI_PERSISTENT ); + magi->title = "Profiler"; + magi->data = profiler; + magi->ui_cb = cb_vg_profiler; + magi->close_cb = NULL; + return 1; + } + } + } + else + vg_error( "Usage: vg_profile \n" ); + + return 0; +} + +static void cmd_vg_profile_poll( int argc, const c8 *argv[] ) +{ + const c8 *term = argv[ argc-1 ]; + if( argc == 1 ) + { + for( u32 i=0; i<_vg_profiler.count; i ++ ) + { + struct vg_profiler *profiler = vg_stack_pointer( &_vg_profiler.stack, i*sizeof(struct vg_profiler) ); + console_suggest_score_text( profiler->name, term, 0 ); + } + } +} + +VG_API void _vg_profiler_register(void) +{ + vg_console_reg_cmd( "vg_profile", cmd_vg_profile, cmd_vg_profile_poll ); +} diff --git a/source/engine/shader.c b/source/engine/shader.c new file mode 100644 index 0000000..e530014 --- /dev/null +++ b/source/engine/shader.c @@ -0,0 +1,212 @@ +const char *vg_shader_gl_ver = "#version 330 core\n"; + +/* + * Compile OpenGL subshader from GLSL source. Type is subshader type. + * If critical is set to 1, the program will fatal exit on compile failure. + */ +static GLuint vg_compile_opengl_subshader( GLint type, const c8 *src, bool critical, const c8 *debug_path ) +{ + GLuint shader = glCreateShader( type ); + + if( shader == 0 ) + { + vg_fatal_error( "glCreateShader returned 0.\n" ); + return 0; + } + + glShaderSource( shader, 2, (const char*[2]){ vg_shader_gl_ver, src }, NULL ); + glCompileShader( shader ); + + GLint status; + glGetShaderiv( shader, GL_COMPILE_STATUS, &status ); + + if( status == GL_TRUE ) + { + return shader; + } + else + { + GLchar info[1024]; + GLsizei len; + glGetShaderInfoLog( shader, sizeof(info), &len, info ); + + const char *type_str = "?"; + + if( type == GL_VERTEX_SHADER ) type_str = "GL_VERTEX_SHADER"; + else if( type == GL_FRAGMENT_SHADER ) type_str = "GL_FRAGMENT_SHADER"; + + if( critical ) + vg_fatal_error( "shader source path: %s\n %s subshader compile error:\n\n%s\n", debug_path, type_str, info ); + return 0; + } +} + +/* + * Final compilation by linking, if critical is 1, a fatal exit will occur on + * link failure + */ +static int vg_link_opengl_program( GLuint program, bool critical ) +{ + glLinkProgram( program ); + + GLint success; + glGetProgramiv( program, GL_LINK_STATUS, &success ); + + if( success ) return 1; + else + { + char info[ 512 ]; + glGetProgramInfoLog( program, sizeof(info), NULL, info ); + if( critical ) + vg_fatal_error( "Shader program link error:\n\n%s\n", info ); + return 0; + } +} + +/* + * Compile vg_shader from static source code. Will fatal exit if there is a + * compile error + */ +void vg_compile_shader( struct vg_shader *shader ) +{ + VG_ASSERT( shader->compiled == 0 ); + + const c8 *vs = _vg_shaders_glsl + shader->vs.glsl, + *fs = _vg_shaders_glsl + shader->fs.glsl; + GLuint vert = vg_compile_opengl_subshader( GL_VERTEX_SHADER, vs, 1, _vg_shaders_infos + shader->vs.src ), + frag = vg_compile_opengl_subshader( GL_FRAGMENT_SHADER, fs, 1, _vg_shaders_infos + shader->fs.src ), + program = glCreateProgram(); + + glAttachShader( program, vert ); + glAttachShader( program, frag ); + + vg_link_opengl_program( program, 1 ); + + glDeleteShader( vert ); + glDeleteShader( frag ); + + _vg_shader_names[ shader->names_start ] = program; + shader->compiled = 1; +} + +/* + * Recompile vg_shader from its original source files. This won't work in the + * shipped version of the engine. + */ +void vg_recompile_shader( struct vg_shader *shader ) +{ + VG_ASSERT( shader->compiled == 1 ); + + char error[260]; + char path_buf[260]; + vg_str path; + + vg_strnull( &path, path_buf, sizeof(path_buf) ); + vg_strcat( &path, "../../" ); + vg_strcat( &path, _vg_shaders_infos + shader->vs.src ); + VG_ASSERT( vg_strgood( &path ) ); + + c8 *vs = stb_include_file( path_buf, "", "../../shaders", error ); + + vg_strnull( &path, path_buf, sizeof(path_buf) ); + vg_strcat( &path, "../../" ); + vg_strcat( &path, _vg_shaders_infos + shader->fs.src ); + VG_ASSERT( vg_strgood( &path ) ); + + c8 *fs = stb_include_file( path_buf, "", "../../shaders", error ); + + if( !vs || !fs ) + { + vg_warn( "Could not recompile shader due to missing source files:\n" ); + + if( !vs ) vg_info( " Vertex: %s\n", _vg_shaders_infos + shader->vs.src ); + if( !fs ) vg_info( " Fragment: %s\n", _vg_shaders_infos + shader->fs.src ); + free( vs ); + free( fs ); + return; + } + + GLuint vert = vg_compile_opengl_subshader( GL_VERTEX_SHADER, vs, 0, _vg_shaders_infos + shader->vs.src ), + frag = vg_compile_opengl_subshader( GL_FRAGMENT_SHADER, fs, 0, _vg_shaders_infos + shader->fs.src ); + + free( vs ); + free( fs ); + + if( !vert || !frag ) return; + + GLuint program = glCreateProgram(); + + glAttachShader( program, vert ); + glAttachShader( program, frag ); + + if( vg_link_opengl_program( program, 0 ) ) + { + /* replace existing */ + glDeleteProgram( _vg_shader_names[ shader->names_start ] ); + _vg_shader_names[ shader->names_start ] = program; + } + else + { + /* womp womp */ + glDeleteProgram( program ); + } + + glDeleteShader( vert ); + glDeleteShader( frag ); +} + +void vg_free_shader( struct vg_shader *shader ) +{ + if( shader->compiled ) + { + shader->compiled = 0; + glDeleteProgram( _vg_shader_names[ shader->names_start ] ); + _vg_shader_names[ shader->names_start ] = 0; + for( u32 i=0; iuniform_count; i ++ ) + _vg_shader_names[ shader->names_start + 1 + i ] = 0; + } +} + +static void vg_shader_link( struct vg_shader *shader ) +{ + GLuint program = _vg_shader_names[ shader->names_start ]; + const c8 *uniform_alias = _vg_shaders_uniform_names + shader->uniform_aliases_offset; + + for( u32 i=0; iuniform_count; i ++ ) + { + u32 index = shader->names_start + 1 + i; + if( uniform_alias[0] == '$' ) _vg_shader_names[ index ] = glGetUniformBlockIndex( program, uniform_alias+1 ); + else _vg_shader_names[ index ] = glGetUniformLocation( program, uniform_alias ); + while( *uniform_alias ) + uniform_alias ++; + uniform_alias ++; + } +} + +VG_API void _vg_shaders_init(void) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_MAIN ) ); + + vg_info( "Compiling shaders\n" ); + for( int i=0; im_eResult == k_EResultOK ) + vg_logsteam( " New app ticket ready\n" ); + else + vg_logsteam( KYEL " Could not request new encrypted app ticket (%u)\n", response->m_eResult ); + + if( SteamAPI_ISteamUser_GetEncryptedAppTicket( _steam_api.pSteamUser, _steam_api.app_symmetric_key, + VG_ARRAY_LEN(_steam_api.app_symmetric_key), &_steam_api.app_key_length )) + { + vg_logsteam( KGRN " Loaded app ticket\n" ); + } + else + { + vg_logsteam( KRED " No ticket availible\n" ); + _steam_api.app_key_length = 0; + } +} +#endif + +#if defined( VG_SERVER ) +static u8 vg_char_base16( c8 c ) +{ + if( c >= '0' && c <= '9' ) + return c-'0'; + if( c >= 'a' && c <= 'f' ) + return (c-'a') + 10; + return 0; +} +#endif + +#if defined( VG_SERVER ) +VG_API bool _vg_steam_init( u32 unIP, u16 usGamePort, u16 usQueryPort, EServerMode eServerMode, + const c8 *pchVersionString, const c8 *appid_str ) +#else +VG_API bool _vg_steam_init(void) +#endif +{ + if( _steam_api.disabled ) + return 0; + + SteamErrMsg err; + + /* Steamworks init step + * ---------------------------------------------------------------------------- */ +#if defined( VG_ENGINE ) + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_STEAM ) ); + + const char *pszInternalCheckInterfaceVersions = + STEAMUTILS_INTERFACE_VERSION "\0" + STEAMNETWORKINGUTILS_INTERFACE_VERSION "\0" + STEAMAPPS_INTERFACE_VERSION "\0" + STEAMCONTROLLER_INTERFACE_VERSION "\0" + STEAMFRIENDS_INTERFACE_VERSION "\0" + STEAMGAMESEARCH_INTERFACE_VERSION "\0" + STEAMHTMLSURFACE_INTERFACE_VERSION "\0" + STEAMHTTP_INTERFACE_VERSION "\0" + STEAMINPUT_INTERFACE_VERSION "\0" + STEAMINVENTORY_INTERFACE_VERSION "\0" + STEAMMATCHMAKINGSERVERS_INTERFACE_VERSION "\0" + STEAMMATCHMAKING_INTERFACE_VERSION "\0" + STEAMMUSICREMOTE_INTERFACE_VERSION "\0" + STEAMMUSIC_INTERFACE_VERSION "\0" + STEAMNETWORKINGMESSAGES_INTERFACE_VERSION "\0" + STEAMNETWORKINGSOCKETS_INTERFACE_VERSION "\0" + STEAMNETWORKING_INTERFACE_VERSION "\0" + STEAMPARENTALSETTINGS_INTERFACE_VERSION "\0" + STEAMPARTIES_INTERFACE_VERSION "\0" + STEAMREMOTEPLAY_INTERFACE_VERSION "\0" + STEAMREMOTESTORAGE_INTERFACE_VERSION "\0" + STEAMSCREENSHOTS_INTERFACE_VERSION "\0" + STEAMUGC_INTERFACE_VERSION "\0" + STEAMUSERSTATS_INTERFACE_VERSION "\0" + STEAMUSER_INTERFACE_VERSION "\0" + STEAMVIDEO_INTERFACE_VERSION "\0" + + "\0"; + + if( SteamInternal_SteamAPI_Init( pszInternalCheckInterfaceVersions, &err ) != k_ESteamAPIInitResult_OK ) + { + _steam_api.disabled = 1; + vg_logsteam( KRED "SteamInternal_SteamAPI_Init() failed: '%s'\nAll steam interactions disabled for this session\n", err ); + return 0; + } +#endif + +#if defined( VG_SERVER ) + FILE *txt = fopen( "steam_appid.txt", "w" ); + fputs( appid_str, txt ); + fclose( txt ); + + // FIXME: Add no-auth launch option (hoist from skaterift, as we have it there, to vg steam module?) + vg_stack_allocator stack; + vg_stack_init( &stack, NULL, VG_KB(256), NULL ); + u32 size; + c8 *src = vg_file_read( &stack, "application_key", &size, 0 ); + if( src ) + { + if( size < k_nSteamEncryptedAppTicketSymmetricKeyLen ) + { + vg_error( "Application key was invalid size\n" ); + return 0; + } + + for( int i=0; iuserdata = NULL; + call->cb = cb_auth_ticket_recieved; + call->id = SteamAPI_ISteamUser_RequestEncryptedAppTicket( _steam_api.pSteamUser, NULL, 0 ); + } +# endif +#endif + + return 1; +} + +static const c8 *string_ESteamNetworkingConnectionState( ESteamNetworkingConnectionState s ) +{ + if( s == k_ESteamNetworkingConnectionState_None ) return "None"; + if( s == k_ESteamNetworkingConnectionState_Connecting) return "Connecting"; + if( s == k_ESteamNetworkingConnectionState_FindingRoute) return "Finding route"; + if( s == k_ESteamNetworkingConnectionState_Connected) return "Connected"; + if( s == k_ESteamNetworkingConnectionState_ClosedByPeer) return "Closed By Peer"; + if( s == k_ESteamNetworkingConnectionState_ProblemDetectedLocally) return "Problem detected locally"; + if( s == k_ESteamNetworkingConnectionState_FinWait) return "Finwait"; + if( s == k_ESteamNetworkingConnectionState_Linger) return "Linger"; + if( s == k_ESteamNetworkingConnectionState_Dead) return "Dead"; + return "enum-out-of-range"; +} + +static const c8 *string_ESteamAPICallFailure( ESteamAPICallFailure e ) +{ + if( e == k_ESteamAPICallFailureNone ) return "None"; + if( e == k_ESteamAPICallFailureSteamGone ) return "Steam Gone"; + if( e == k_ESteamAPICallFailureNetworkFailure ) return "Network Failure"; + if( e == k_ESteamAPICallFailureInvalidHandle ) return KBLK "Invalid Handle"; + if( e == k_ESteamAPICallFailureMismatchedCallback ) return "Mismatched Callback"; + return "enum-out-of-range"; +} + +void vg_steam_frame(void) +{ + if( _steam_api.disabled ) + return; + + SteamAPI_ManualDispatch_RunFrame( _steam_api.hPipe ); + + CallbackMsg_t callback; + while( SteamAPI_ManualDispatch_GetNextCallback( _steam_api.hPipe, &callback ) ) + { + /* Check for dispatching API call results */ + i32 type = callback.m_iCallback; + if( type == k_iSteamUtils_SteamAPICallCompleted ) + { + SteamAPICallCompleted_t *inf = callback.m_pubParam; + + bool bFailed; + void *call_data = alloca( inf->m_cubParam ); + + if( SteamAPI_ManualDispatch_GetAPICallResult( _steam_api.hPipe, inf->m_hAsyncCall, + call_data, inf->m_cubParam, + inf->m_iCallback, &bFailed ) ) + { + vg_logsteam( "api_call_completed %lu\n", inf->m_hAsyncCall ); + + int j=0; + for( int i=0; i<_steam_api.api_call_count; i++ ) + { + if( _steam_api.api_calls[i].id != inf->m_hAsyncCall ) + _steam_api.api_calls[j ++] = _steam_api.api_calls[i]; + else + _steam_api.api_calls[i].cb( call_data, _steam_api.api_calls[i].userdata ); + } + + if( _steam_api.api_call_count == j ) + vg_error( "No tracker was register for API call\n" ); + + _steam_api.api_call_count = j; + } + else + { + enum ESteamAPICallFailure e = + SteamAPI_ISteamUtils_GetAPICallFailureReason( _steam_api.pSteamUtils, inf->m_hAsyncCall ); + const c8 *fail_str = string_ESteamAPICallFailure( e ); + vg_logsteam( KRED "Error getting call result on %lu (code %d)\n", inf->m_hAsyncCall, fail_str ); + } + } + else + { + /* + * Look at callback.m_iCallback to see what kind of callback it is, + * and dispatch to appropriate handler(s) + * void *data = callback.m_pubParam; + */ + if( type == k_iSteamUser_SteamServersConnected ) + vg_success( "Steam servers connected" ); + else if( type == k_iSteamUser_SteamConnectFailure ) + { + SteamServerConnectFailure_t *inf = callback.m_pubParam; + vg_logsteam( KRED "Steam server connect failure, code %d, retrying: %d\n", inf->m_eResult, inf->m_bStillRetrying ); + } + else if( type == k_iSteamUser_SteamServersDisconnected ) + { + SteamServersDisconnected_t *inf = callback.m_pubParam; + vg_logsteam( "Steam servers disconnect, code %d\n", inf->m_eResult ); + } + else if( type == k_iSteamNetworkingSockets_SteamNetConnectionStatusChangedCallback ) + { + SteamNetConnectionStatusChangedCallback_t *inf = callback.m_pubParam; + const c8 *status_string = string_ESteamNetworkingConnectionState( inf->m_info.m_eState ); + vg_logsteam( "Connection status changed: %x -> %s\n Debug: '%s'\n EndDebug: '%s'\n", + inf->m_hConn, status_string, inf->m_info.m_szConnectionDescription, + inf->m_info.m_szEndDebug ); + + if( _steam_api.cb_connection_changed ) + _steam_api.cb_connection_changed( inf ); + } + else if( type == k_iSteamNetworkingSockets_SteamNetAuthenticationStatus ) + { + SteamNetAuthenticationStatus_t *inf = callback.m_pubParam; + vg_logsteam( "Steam Authentication Status: %d\n Debug: '%s'\n", inf->m_eAvail, inf->m_debugMsg ); + } + } + + SteamAPI_ManualDispatch_FreeLastCallback( _steam_api.hPipe ); + } +} + +VG_API void _vg_steam_shutdown(void) +{ +#if defined( VG_SERVER ) + if( _steam_api.is_connected ) + { + SteamAPI_ISteamGameServer_LogOff( _steam_api.pSteamGameServer ); + _steam_api.is_connected = 0; + } + SteamGameServer_Shutdown(); +#else + SteamAPI_Shutdown(); +#endif +} + +vg_steam_api_call *vg_alloc_async_steam_api_call(void) +{ + if( _steam_api.api_call_count == VG_ARRAY_LEN(_steam_api.api_calls) ) + return NULL; + return &_steam_api.api_calls[ _steam_api.api_call_count ++ ]; +} + +#if defined( VG_ENGINE ) +void vg_steam_set_achievement( const c8 *name, bool yes ) +{ + if( _steam_api.disabled || _steam_api.demo_mode ) + return; + + if( yes ) + { + if( SteamAPI_ISteamUserStats_SetAchievement( _steam_api.pSteamUserStats, name ) ) + { + vg_logsteam( KGRN "Set achievement '%s'\n", name ); + SteamAPI_ISteamUserStats_StoreStats( _steam_api.pSteamUserStats ); + } + } + else + { + if( SteamAPI_ISteamUserStats_ClearAchievement( _steam_api.pSteamUserStats, name ) ) + { + vg_logsteam( KBLK "Clear achievement '%s'\n", name ); + SteamAPI_ISteamUserStats_StoreStats( _steam_api.pSteamUserStats ); + } + } +} +#endif diff --git a/source/engine/texture.c b/source/engine/texture.c new file mode 100644 index 0000000..7acd81c --- /dev/null +++ b/source/engine/texture.c @@ -0,0 +1,430 @@ +#define QOI_OP_INDEX 0x00 /* 00xxxxxx */ +#define QOI_OP_DIFF 0x40 /* 01xxxxxx */ +#define QOI_OP_LUMA 0x80 /* 10xxxxxx */ +#define QOI_OP_RUN 0xc0 /* 11xxxxxx */ +#define QOI_OP_RGB 0xfe /* 11111110 */ +#define QOI_OP_RGBA 0xff /* 11111111 */ +#define QOI_MASK_2 0xc0 /* 11000000 */ + +#define QOI_COLOR_HASH(C) (C.rgba[0]*3 + C.rgba[1]*5 + C.rgba[2]*7 + C.rgba[3]*11) +#define QOI_MAGIC \ + (((u32)'q') | ((u32)'o') << 8 | \ + ((u32)'i') << 16 | ((u32)'f') << 24) + +static const u8 qoi_padding[8] = {0,0,0,0,0,0,0,1}; + +#define cpu_to_big32 big32_to_cpu +VG_TIER_0 static inline u32 big32_to_cpu( u32 x ) +{ + return ((x & 0xFF) << 24) | ((x & 0xFF00) << 8) | ((x & 0xFF0000) >> 8) | (x >> 24); +} + +VG_TIER_0 bool vg_qoi_validate( const qoi_desc *desc ) +{ + if( (desc->width == 0) || (desc->height == 0) || + (desc->width >= 2048) || (desc->height >= 2048) ) + { + vg_error( "QOI file is invalid; Unpermitted size (%ux%u)\n", desc->width, desc->height ); + return 0; + } + + if( !(desc->channels == 3 || desc->channels == 4) ) + { + vg_error( "QOI file is invalid; Only 3 or 4 channels permitted. File has %u\n", desc->channels ); + return 0; + } + + return 1; +} + +/* Initalize stream context and return the number of bytes required to store the final RGB/A image data. */ +VG_TIER_0 u32 vg_qoi_stream_init( qoi_desc *desc, vg_stream *stream ) +{ + vg_stream_read( stream, desc, sizeof(qoi_desc) ); + if( desc->magic != QOI_MAGIC ) + { + vg_error( "QOI file is invalid; Magic Number incorrect.\n" ); + return 0; + } + + desc->width = big32_to_cpu( desc->width ); + desc->height = big32_to_cpu( desc->height ); + + if( vg_qoi_validate( desc ) ) + return desc->width * desc->height * desc->channels; + else return 0; +} + +VG_TIER_0 void vg_qoi_stream_decode( qoi_desc *desc, vg_stream *stream, u8 *pixels, bool v_flip ) +{ + qoi_rgba_t index[64], px; + vg_zero_mem( index, sizeof(qoi_rgba_t)*64 ); + vg_zero_mem( &px, sizeof(qoi_rgba_t) ); + px.rgba[3] = 255; + + u32 run=0; + + for( u32 y=0; yheight; y ++ ) + { + for( u32 x=0; xwidth; x ++ ) + { + if( run > 0 ) + run --; + else + { + u8 b1; + vg_stream_read( stream, &b1, 1 ); + + if( b1 == QOI_OP_RGB ) + vg_stream_read( stream, px.rgba, 3 ); + else if( b1 == QOI_OP_RGBA ) + vg_stream_read( stream, px.rgba, 4 ); + else if( (b1 & QOI_MASK_2) == QOI_OP_INDEX ) + px = index[b1]; + else if( (b1 & QOI_MASK_2) == QOI_OP_DIFF ) + { + px.rgba[0] += (i32)((b1 >> 4) & 0x03) - 2; + px.rgba[1] += (i32)((b1 >> 2) & 0x03) - 2; + px.rgba[2] += (i32)( b1 & 0x03) - 2; + } + else if( (b1 & QOI_MASK_2) == QOI_OP_LUMA ) + { + u8 b2; + vg_stream_read( stream, &b2, 1 ); + i32 vg = (i32)(b1 & 0x3f) - 32; + px.rgba[0] += vg - 8 + (i32)((b2 >> 4) & 0x0f); + px.rgba[1] += vg; + px.rgba[2] += vg - 8 + (i32)(b2 & 0x0f); + } + else if( (b1 & QOI_MASK_2) == QOI_OP_RUN ) + run = (b1 & 0x3f); + index[ QOI_COLOR_HASH(px) % 64 ] = px; + } + + u32 row = v_flip? desc->height-(y+1): y; + for( u32 i=0; i < desc->channels; i ++ ) + pixels[ ((row*desc->width) + x)*desc->channels + i ] = px.rgba[i]; + } + } +} + +VG_TIER_0 u32 vg_query_qoi_max_compressed_size( const qoi_desc *desc ) +{ + return desc->width * desc->height * (desc->channels + 1) + sizeof(qoi_desc) + sizeof(qoi_padding); +} + +VG_TIER_0 u32 vg_qoi_stream_encode( const qoi_desc *desc, const u8 *pixels, vg_stream *stream, bool v_flip ) +{ + if( !vg_qoi_validate( desc ) ) + return 0; + + qoi_desc file_header = *desc; + file_header.magic = QOI_MAGIC; + file_header.width = cpu_to_big32( file_header.width ); + file_header.height = cpu_to_big32( file_header.height ); + vg_stream_write( stream, &file_header, sizeof(qoi_desc) ); + + qoi_rgba_t index[64]; + vg_zero_mem( index, sizeof(qoi_rgba_t)*64 ); + qoi_rgba_t px_prev = { .rgba = { 0, 0, 0, 255 } }; + qoi_rgba_t px = px_prev; + + u32 run = 0; + for( u32 y=0; yheight; y ++ ) + { + for( u32 x=0; xwidth; x ++ ) + { + u32 row = v_flip? desc->height-(y+1): y; + for( u32 i=0; i < desc->channels; i ++ ) + px.rgba[i] = pixels[ ((row*desc->width) + x)*desc->channels + i ]; + + if( px.v == px_prev.v ) + { + run ++; + if( run == 62 || ((y+1 == desc->height) && (x+1 == desc->width)) ) + { + u8 b1 = QOI_OP_RUN | (run - 1); + vg_stream_write( stream, &b1, 1 ); + run = 0; + } + } + else + { + if( run > 0 ) + { + u8 b1 = QOI_OP_RUN | (run - 1); + vg_stream_write( stream, &b1, 1 ); + run = 0; + } + + u32 index_pos = QOI_COLOR_HASH( px ) % 64; + if( index[ index_pos ].v == px.v ) + { + u8 b1 = QOI_OP_INDEX | index_pos; + vg_stream_write( stream, &b1, 1 ); + } + else + { + index[ index_pos ] = px; + if( px.rgba[3] == px_prev.rgba[3] ) + { + i8 vr = px.rgba[0] - px_prev.rgba[0], + vg = px.rgba[1] - px_prev.rgba[1], + vb = px.rgba[2] - px_prev.rgba[2], + vg_r = vr - vg, + vg_b = vb - vg; + + if( vr > -3 && vr < 2 && + vg > -3 && vg < 2 && + vb > -3 && vb < 2 ) + { + vg_stream_write( stream, (u8[]){ QOI_OP_DIFF | (vr + 2) << 4 | (vg + 2) << 2 | (vb + 2) }, 1 ); + } + else if( vg_r > -9 && vg_r < 8 && + vg > -33 && vg < 32 && + vg_b > -9 && vg_b < 8 ) + { + vg_stream_write( stream, (u8[]){ QOI_OP_LUMA | (vg + 32), + (vg_r + 8) << 4 | (vg_b + 8) }, 2 ); + } + else + { + vg_stream_write( stream, (u8[]){ QOI_OP_RGB }, 1 ); + vg_stream_write( stream, px.rgba, 3 ); + } + } + else + { + vg_stream_write( stream, (u8 []){ QOI_OP_RGBA }, 1 ); + vg_stream_write( stream, px.rgba, 4 ); + } + } + } + px_prev = px; + } + } + vg_stream_write( stream, qoi_padding, sizeof(qoi_padding) ); + return 1; +} + +/* VG_PART + * ------------------------------------------------------------------------------------------------------------------ */ + +struct +{ + GLuint error2d, errorcube; +} +_vg_tex; + +VG_API void _vg_tex_init(void) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_OPENGL ) ); + + static u8 const_vg_tex2d_err[] = + { + 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, + 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, + 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, + 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, + }; + + glGenTextures( 1, &_vg_tex.error2d ); + glBindTexture( GL_TEXTURE_2D, _vg_tex.error2d ); + glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 4, 4, 0, GL_RGBA, GL_UNSIGNED_BYTE, const_vg_tex2d_err ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT ); + + glGenTextures( 1, &_vg_tex.errorcube ); + glBindTexture( GL_TEXTURE_CUBE_MAP, _vg_tex.errorcube ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, GL_LINEAR ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE ); + + for( u32 j=0; j<6; j ++ ) + { + glTexImage2D( GL_TEXTURE_CUBE_MAP_POSITIVE_X + j, 0, GL_RGBA, 4, 4, + 0, GL_RGBA, GL_UNSIGNED_BYTE, const_vg_tex2d_err ); + } +} + +struct tex_upload_task +{ + vg_tex *tex; + u32 width, height, channels, flags; + u8 image_buffer[]; +}; + +VG_API_INTERNAL static void _vg_tex_upload( struct tex_upload_task *in_args, vg_async_info *async ) +{ + VG_ASSERT( _vg_tex.errorcube && _vg_tex.error2d ); + + u32 flags = in_args->flags; + vg_tex *tex = in_args->tex; + + if( flags & VG_TEX_ERROR ) + { + tex->name = (flags & VG_TEX_CUBEMAP)? _vg_tex.errorcube: _vg_tex.error2d; + tex->flags = (flags & VG_TEX_CUBEMAP) | VG_TEX_ERROR | VG_TEX_COMPLETE; + } + else + { + u32 pixel_format = 0; + if( in_args->channels == 3 ) pixel_format = GL_RGB; + else if( in_args->channels == 4 ) pixel_format = GL_RGBA; + else vg_fatal_error( "Can't upload texture with '%u' channels.\n", in_args->channels ); + + glGenTextures( 1, &tex->name ); + + u32 filter_min = 0, + filter_mag = 0; + if( flags & VG_TEX_LINEAR ) + { + if( flags & VG_TEX_NOMIP ) filter_min = GL_LINEAR; + else filter_min = GL_LINEAR_MIPMAP_LINEAR; + filter_mag = GL_LINEAR; + } + else + { + VG_ASSERT( flags & VG_TEX_NEAREST ); + filter_min = GL_NEAREST; + filter_mag = GL_NEAREST; + } + + if( flags & VG_TEX_CUBEMAP ) + { + u32 w = in_args->width, + h = in_args->height/6; + + glBindTexture( GL_TEXTURE_CUBE_MAP, tex->name ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, filter_min ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, filter_mag ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE ); /* can this be anything else? */ + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE ); + glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE ); + + for( u32 j=0; j<6; j ++ ) + { + u32 offset = w*h*j*in_args->channels; + glTexImage2D( GL_TEXTURE_CUBE_MAP_POSITIVE_X + j, 0, pixel_format, + w, h, + 0, pixel_format, GL_UNSIGNED_BYTE, in_args->image_buffer + offset ); + } + + if( !(flags & VG_TEX_NOMIP) ) + glGenerateMipmap( GL_TEXTURE_CUBE_MAP ); + } + else + { + glBindTexture( GL_TEXTURE_2D, tex->name ); + glTexImage2D( GL_TEXTURE_2D, 0, pixel_format, in_args->width, in_args->height, + 0, pixel_format, GL_UNSIGNED_BYTE, in_args->image_buffer ); + + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, filter_min ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, filter_mag ); + + u32 wrap_s = 0, + wrap_t = 0; + + if( flags & VG_TEX_CLAMP ) + { + wrap_s = GL_CLAMP_TO_EDGE; + wrap_t = GL_CLAMP_TO_EDGE; + } + else + { + VG_ASSERT( flags & VG_TEX_REPEAT ); + wrap_s = GL_REPEAT; + wrap_t = GL_REPEAT; + } + + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrap_s ); + glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrap_t ); + + if( !(flags & VG_TEX_NOMIP) ) + glGenerateMipmap( GL_TEXTURE_2D ); + } + tex->flags = flags | VG_TEX_COMPLETE; + } +} + +VG_API bool _vg_tex_load_stream( vg_tex *out_tex, vg_stream *in_stream, u32 flags ) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); + + qoi_desc qoi; + u32 size = vg_qoi_stream_init( &qoi, in_stream ); + if( size ) + { + _vg_async_context_push_groups( VG_ASYNC_GROUP_OPENGL, 0 ); + struct tex_upload_task *out_args = _vg_async_alloc( VG_THREAD_MAIN_ID, sizeof( struct tex_upload_task ) + size ); + vg_qoi_stream_decode( &qoi, in_stream, out_args->image_buffer, flags & VG_TEX_FLIP_V? 1: 0 ); + out_args->tex = out_tex; + out_args->width = qoi.width; + out_args->height = qoi.height; + out_args->channels = qoi.channels; + out_args->flags = flags; + _vg_async_send( out_args, (vg_async_fn)_vg_tex_upload ); + _vg_async_context_pop_groups(); + return 1; + } + else + return 0; +} + +VG_API void _vg_tex_load( vg_tex *out_tex, const c8 *path, u32 flags ) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); + + bool error = 0; + vg_stream file; + if( vg_file_stream_open( &file, path, VG_STREAM_READ ) ) + { + if( !_vg_tex_load_stream( out_tex, &file, flags ) ) + error = 1; + vg_file_stream_close( &file ); + } + else + error = 1; + + if( error ) + { + _vg_async_context_push_groups( VG_ASYNC_GROUP_OPENGL, 0 ); + struct tex_upload_task *out_args = _vg_async_alloc( VG_THREAD_MAIN_ID, sizeof( struct tex_upload_task ) ); + out_args->tex = out_tex; + out_args->width = 0; + out_args->height = 0; + out_args->channels = 0; + out_args->flags = VG_TEX_ERROR; + _vg_async_send( out_args, (vg_async_fn)_vg_tex_upload ); + _vg_async_context_pop_groups(); + } +} + +VG_API u32 vg_tex_name( GLuint target, vg_tex *tex ) +{ + if( !tex ) + { + return (target == GL_TEXTURE_2D)? _vg_tex.error2d: _vg_tex.errorcube; + } + if( tex->flags & VG_TEX_COMPLETE ) return tex->name; + else return (target == GL_TEXTURE_2D)? _vg_tex.error2d: _vg_tex.errorcube; +} + +VG_API void vg_tex_bind( GLuint target, vg_tex *tex, u32 slot ) +{ + glActiveTexture( GL_TEXTURE0 + slot ); + glBindTexture( target, vg_tex_name( target, tex ) ); +} + +VG_API void vg_tex_delete( vg_tex *tex ) +{ + VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_OPENGL ) ); + VG_ASSERT( tex->flags & VG_TEX_COMPLETE ); + if( !(tex->flags & VG_TEX_ERROR) ) + glDeleteTextures( 1, &tex->name ); + vg_zero_mem( tex, sizeof(vg_tex) ); +} 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; inodes[ inode ]; + box_init_inf( node->bbx ); + for( u32 i=0; icount; i++ ) + { + u32 idx = node->start+i; + bh->system->expand_bound( bh->user, node->bbx, idx ); + } +} + +static void bh_subdivide( bh_tree *bh, u32 inode ) +{ + bh_node *node = &bh->nodes[ inode ]; + if( node->count <= bh->max_per_leaf ) + return; + + v3f extent; + v3_sub( node->bbx[1], node->bbx[0], extent ); + + int axis = 0; + if( extent[1] > extent[0] ) axis = 1; + if( extent[2] > extent[axis] ) axis = 2; + + float split = node->bbx[0][axis] + extent[axis]*0.5f; + float avg = 0.0; + for( u32 t=0; tcount; t++ ){ + u32 idx = node->start+t; + avg += bh->system->item_centroid( bh->user, idx, axis ); + } + avg /= (float)node->count; + split = avg; + + + i32 i = node->start, + j = i + node->count-1; + + while( i <= j ) + { + f32 centroid = bh->system->item_centroid( bh->user, i, axis ); + if( centroid < split ) + i ++; + else + { + bh->system->item_swap( bh->user, i, j ); + j --; + } + } + + u32 left_count = i - node->start; + if( left_count == 0 || left_count == node->count ) + return; + + u32 il = bh->node_count ++, + ir = bh->node_count ++; + bh_node *lnode = &bh->nodes[il], + *rnode = &bh->nodes[ir]; + lnode->start = node->start; + lnode->count = left_count; + rnode->start = i; + rnode->count = node->count - left_count; + + node->il = il; + node->ir = ir; + node->count = 0; + + bh_update_bounds( bh, il ); + bh_update_bounds( bh, ir ); + bh_subdivide( bh, il ); + bh_subdivide( bh, ir ); +} + +static void bh_rebuild( bh_tree *bh, u32 item_count ) +{ + bh_node *root = &bh->nodes[0]; + bh->node_count = 1; + + root->il = 0; + root->ir = 0; + root->count = item_count; + root->start = 0; + + bh_update_bounds( bh, 0 ); + + if( item_count > 2 ) + bh_subdivide( bh, 0 ); +} + +VG_TIER_1 void bh_create( bh_tree *bh, bh_system *system, void *user, u32 item_count, + u32 max_per_leaf, vg_stack_allocator *stack ) +{ + vg_zero_mem( bh, sizeof(bh_tree) ); + + u32 alloc_count = VG_MAX( 1, item_count ); + i32 max_size = sizeof(bh_node)*(alloc_count*2-1); + bh->nodes = vg_stack_allocate( stack, max_size, 8, "BVH Tree" ); + bh->system = system; + bh->user = user; + bh->max_per_leaf = max_per_leaf; + bh_rebuild( bh, item_count ); + + i32 used_size = sizeof(bh_node) * bh->node_count; + vg_stack_extend_last( stack, used_size - max_size ); + vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) ); +} + +VG_TIER_0 void bh_debug_leaf( bh_tree *bh, bh_node *node ) +{ + vg_line_boxf( node->bbx, 0xff00ff00 ); + + if( bh->system->item_debug ) + { + for( u32 i=0; icount; i++ ) + { + u32 idx = node->start+i; + bh->system->item_debug( bh->user, idx ); + } + } +} + +/* + * Trace the bh tree all the way down to the leaf nodes where pos is inside + */ +VG_TIER_0 void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour ) +{ + bh_node *node = &bh->nodes[ inode ]; + if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) && + (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) ) + { + if( !node->count ) + { + vg_line_boxf( node->bbx, colour ); + bh_debug_trace( bh, node->il, pos, colour ); + bh_debug_trace( bh, node->ir, pos, colour ); + } + else + if( bh->system->item_debug ) + bh_debug_leaf( bh, node ); + } +} + +VG_TIER_0 void bh_iter_init_generic( i32 root, bh_iter *it ) +{ + it->stack[0].id = root; + it->stack[0].depth = 0; + it->depth = 0; + it->i = 0; +} + +VG_TIER_0 void bh_iter_init_box( i32 root, bh_iter *it, boxf box ) +{ + bh_iter_init_generic( root, it ); + it->query = k_bh_query_box; + box_copy( box, it->box.box ); +} + +VG_TIER_0 void bh_iter_init_ray( i32 root, bh_iter *it, v3f co, v3f dir, f32 max_dist ) +{ + bh_iter_init_generic( root, it ); + it->query = k_bh_query_ray; + v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir ); + v3_copy( co, it->ray.co ); + it->ray.max_dist = max_dist; +} + +VG_TIER_0 void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range ) +{ + bh_iter_init_generic( root, it ); + it->query = k_bh_query_range; + + v3_copy( co, it->range.co ); + it->range.dist_sqr = range*range; +} + +VG_TIER_0 i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em ) +{ + while( it->depth >= 0 ) + { + bh_node *inode = &bh->nodes[ it->stack[it->depth].id ]; + + /* Only process overlapping nodes */ + i32 q = 0; + + if( it->i ) /* already checked */ + q = 1; + else + { + if( it->query == k_bh_query_box ) + q = box_overlap( inode->bbx, it->box.box ); + else if( it->query == k_bh_query_ray ) + q = ray_aabb1( inode->bbx, it->ray.co, it->ray.inv_dir, it->ray.max_dist ); + else + { + v3f nearest; + closest_point_aabb( it->range.co, inode->bbx, nearest ); + if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr ) + q = 1; + } + } + + if( !q ) + { + it->depth --; + continue; + } + + if( inode->count ) + { + if( it->i < inode->count ) + { + *em = inode->start+it->i; + it->i ++; + return 1; + } + else + { + it->depth --; + it->i = 0; + } + } + else + { + if( it->depth+1 >= VG_ARRAY_LEN(it->stack) ) + { + vg_error( "Maximum stack reached!\n" ); + return 0; + } + + it->stack[it->depth ].id = inode->il; + it->stack[it->depth+1].id = inode->ir; + it->depth ++; + it->i = 0; + } + } + return 0; +} + +VG_TIER_0 i32 bh_closest_point( bh_tree *bh, v3f pos, v3f closest, f32 max_dist ) +{ + if( bh->node_count < 2 ) + return -1; + + max_dist = max_dist*max_dist; + + i32 queue[ 128 ], + depth = 0, + best_item = -1; + + queue[0] = 0; + + while( depth >= 0 ) + { + bh_node *inode = &bh->nodes[ queue[depth] ]; + + v3f p1; + closest_point_aabb( pos, inode->bbx, p1 ); + + /* branch into node if its closer than current best */ + f32 node_dist = v3_dist2( pos, p1 ); + if( node_dist < max_dist ) + { + if( inode->count ) + { + for( i32 i=0; icount; i++ ) + { + v3f p2; + bh->system->item_closest( bh->user, inode->start+i, pos, p2 ); + + f32 item_dist = v3_dist2( pos, p2 ); + if( item_dist < max_dist ) + { + max_dist = item_dist; + v3_copy( p2, closest ); + best_item = inode->start+i; + } + } + + depth --; + } + else + { + queue[depth] = inode->il; + queue[depth+1] = inode->ir; + depth ++; + } + } + else + depth --; + } + + return best_item; +} diff --git a/source/maths/common_maths.c b/source/maths/common_maths.c new file mode 100644 index 0000000..c1dcab5 --- /dev/null +++ b/source/maths/common_maths.c @@ -0,0 +1,998 @@ +/* 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 + */ + +/* + * ----------------------------------------------------------------------------- + * 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) ); +} diff --git a/source/maths/perlin.c b/source/maths/perlin.c new file mode 100644 index 0000000..6647a91 --- /dev/null +++ b/source/maths/perlin.c @@ -0,0 +1,149 @@ +static int perlin_hash[] = { +0x46,0xD5,0xB8,0xD3,0xF2,0xE5,0xCC,0x07,0xD0,0xB3,0x7A,0xA2,0xC3,0xDA,0xDC,0x7F, +0xE0,0xB7,0x42,0xA0,0xBF,0x41,0x92,0x32,0x6F,0x0D,0x45,0xC7,0x54,0xDB,0x30,0xC2, +0xD5,0xDA,0x55,0x09,0xDE,0x74,0x48,0x20,0xE1,0x24,0x5C,0x4D,0x6F,0x36,0xD8,0xE9, +0x8D,0x8F,0x54,0x99,0x98,0x51,0xFE,0xDB,0x26,0x04,0x65,0x57,0x56,0xF3,0x53,0x30, +0x3D,0x16,0xC0,0xB6,0xF2,0x47,0xCF,0x62,0xB0,0x6C,0x8F,0x4F,0x8C,0x4C,0x17,0xF0, +0x19,0x7E,0x2D,0x81,0x8D,0xFB,0x10,0xD3,0x49,0x50,0x60,0xFD,0x38,0x15,0x3B,0xEE, +0x05,0xC1,0xCF,0x62,0x97,0x75,0xDF,0x4E,0x4D,0x89,0x5E,0x88,0x5C,0x30,0x8C,0x54, +0x1D,0x39,0x41,0xEA,0xA2,0x63,0x12,0x1B,0x8E,0x35,0x22,0x9B,0x98,0xA3,0x7F,0x80, +0xD6,0x27,0x94,0x66,0xB5,0x1D,0x7E,0xDF,0x96,0x28,0x38,0x3A,0xA0,0xE8,0x71,0x09, +0x62,0x5E,0x9D,0x53,0x58,0x1B,0x7D,0x0D,0x2D,0x99,0x77,0x83,0xC3,0x89,0xC2,0xA2, +0xA7,0x1D,0x78,0x80,0x37,0xC1,0x87,0xFF,0x65,0xBF,0x2C,0xF1,0xE5,0xB3,0x09,0xE0, +0x25,0x92,0x83,0x0F,0x8A,0x57,0x3C,0x0B,0xC6,0xBC,0x44,0x16,0xE3,0xCE,0xC3,0x0D, +0x69,0xD3,0xC6,0x99,0xB8,0x46,0x44,0xC4,0xF3,0x1E,0xBF,0xF5,0xB4,0xDB,0xFB,0x93, +0xA1,0x7B,0xC9,0x08,0x77,0x22,0xE5,0x02,0xEF,0x9E,0x90,0x94,0x8A,0xA6,0x3D,0x7E, +0xA2,0xA0,0x10,0x82,0x47,0x5C,0xAA,0xF8,0x2F,0x0D,0x9F,0x76,0xDA,0x99,0x0F,0xCB, +0xE2,0x02,0x0C,0x75,0xCA,0x35,0x29,0xA6,0x49,0x83,0x6D,0x91,0xB4,0xEC,0x31,0x69, +0xBA,0x13,0xF3,0xC7,0x21,0x06,0xC8,0x79,0xEF,0xB1,0x9C,0x6A,0xEE,0x64,0x9A,0xDC, +0x1E,0xC6,0x18,0x93,0xA9,0x7E,0x89,0x7D,0x96,0xE5,0x44,0xB8,0x00,0x15,0xAF,0x8C, +0x78,0x8F,0xA8,0x05,0xA7,0x07,0x25,0x9A,0xC8,0x5D,0x90,0x1A,0x41,0x53,0x30,0xD3, +0x24,0x33,0x71,0xB4,0x50,0x6E,0xE4,0xEA,0x0D,0x2B,0x6D,0xF5,0x17,0x08,0x74,0x49, +0x71,0xC2,0xAC,0xF7,0xDC,0xB2,0x7E,0xCC,0xB6,0x1B,0xB8,0xA9,0x52,0xCF,0x6B,0x51, +0xD2,0x4E,0xC9,0x43,0xEE,0x2E,0x92,0x24,0xBB,0x47,0x4D,0x0C,0x3E,0x21,0x53,0x19, +0xD4,0x82,0xE2,0xC6,0x93,0x85,0x0A,0xF8,0xFA,0x04,0x07,0xD3,0x1D,0xEC,0x03,0x66, +0xFD,0xB1,0xFB,0x8F,0xC5,0xDE,0xE8,0x29,0xDF,0x23,0x09,0x9D,0x7C,0x43,0x3D,0x4D, +0x89,0xB9,0x6F,0xB4,0x6B,0x4A,0x51,0xC3,0x94,0xF4,0x7C,0x5E,0x19,0x87,0x79,0xC1, +0x80,0x0C,0x45,0x12,0xEC,0x95,0xF3,0x31,0x68,0x42,0xE1,0x06,0x57,0x0E,0xA7,0xFB, +0x78,0x96,0x87,0x23,0xA5,0x20,0x7A,0x09,0x3A,0x45,0xE6,0xD9,0x5E,0x6A,0xD6,0xAA, +0x29,0x50,0x92,0x4E,0xD0,0xB5,0x91,0xC2,0x9A,0xCF,0x07,0xFE,0xB2,0x15,0xEB,0xE4, +0x84,0x40,0x14,0x47,0xFA,0x93,0xB9,0x06,0x69,0xDB,0xBD,0x4E,0xEA,0x52,0x9B,0xDE, +0x5B,0x50,0x36,0xAB,0xB3,0x1F,0xD2,0xCD,0x9C,0x13,0x07,0x7E,0x8B,0xED,0x72,0x62, +0x74,0x77,0x3B,0x88,0xAC,0x5B,0x6A,0xBC,0xDA,0x99,0xE8,0x24,0x90,0x5A,0xCA,0x8D, +0x5C,0x2B,0xF8,0xF1,0xE1,0x1D,0x94,0x11,0xEA,0xCC,0x02,0x09,0x1E,0xA2,0x48,0x67, +0x87,0x5A,0x7E,0xC6,0xCC,0xA3,0xFB,0xC5,0x36,0xEB,0x5C,0xE1,0xAF,0x1E,0xBE,0xE7, +0xD8,0x8F,0x70,0xAE,0x42,0x05,0xF5,0xCD,0x2D,0xA2,0xB0,0xFD,0xEF,0x65,0x2C,0x22, +0xCB,0x8C,0x8B,0xAA,0x3D,0x86,0xE2,0xCD,0xBE,0xC3,0x42,0x38,0xE3,0x9C,0x08,0xB5, +0xAE,0xBD,0x54,0x73,0x83,0x70,0x24,0x47,0xCA,0x4C,0x04,0xC4,0xE0,0x1D,0x40,0xED, +0xF4,0x2B,0x50,0x8E,0x97,0xB3,0xF0,0xA6,0x76,0xDB,0x49,0x30,0xE5,0xD9,0x71,0x07, +0xB2,0xF1,0x0F,0xD6,0x77,0xAA,0x72,0xC0,0xAF,0x66,0xD8,0x40,0xC6,0x08,0x19,0x8C, +0xD9,0x8F,0x5A,0x75,0xAC,0xBE,0xC2,0x40,0x5B,0xBD,0x0D,0x1D,0x00,0xAF,0x26,0x5E, +0x78,0x43,0xAA,0xC6,0x4F,0xF3,0xD8,0xE2,0x7F,0x0C,0x1E,0x77,0x4D,0x35,0x96,0x23, +0x32,0x44,0x03,0x8D,0x92,0xE7,0xFD,0x48,0x07,0xD0,0x58,0xFC,0x6D,0xC9,0x91,0x33, +0xF0,0x23,0x45,0xA4,0x29,0xB9,0xF5,0xB0,0x68,0x8F,0x7B,0x59,0x15,0x8E,0xA6,0x66, +0x15,0xA0,0x76,0x9B,0x69,0xCB,0x38,0xA5,0xF4,0xB4,0x6B,0xDC,0x1F,0xAB,0xAE,0x12, +0x77,0xC0,0x8C,0x4A,0x03,0xB9,0x73,0xD3,0x6D,0x52,0xC5,0xF5,0x6E,0x4E,0x4B,0xA3, +0x24,0x02,0x58,0xEE,0x5F,0xF9,0xD6,0xD0,0x1D,0xBC,0xF4,0xB8,0x4F,0xFD,0x4B,0x2D, +0x34,0x77,0x46,0xE5,0xD4,0x33,0x7B,0x9C,0x35,0xCD,0xB0,0x5D,0x06,0x39,0x99,0xEB, +0x0C,0xD0,0x0F,0xF7,0x92,0xB5,0x58,0x5B,0x5E,0x79,0x12,0xF4,0x05,0xF6,0x21,0x07, +0x0B,0x49,0x1A,0xFB,0xD4,0x98,0xC4,0xEF,0x7A,0xD6,0xCA,0xA1,0xDA,0xB3,0x51,0x00, +0x76,0xEC,0x08,0x48,0x40,0x35,0xD7,0x94,0xBE,0xF5,0x7B,0xA4,0x20,0x81,0x5F,0x82, +0xF3,0x6F,0x96,0x24,0x98,0xB6,0x49,0x18,0xC8,0xC5,0x8C,0xD2,0x38,0x7F,0xC4,0xF6, +0xAA,0x87,0xDC,0x73,0x5B,0xA1,0xAF,0xE5,0x3D,0x37,0x6B,0x85,0xED,0x38,0x62,0x7D, +0x57,0xBD,0xCF,0xB5,0x1B,0xA8,0xBB,0x32,0x33,0xD3,0x34,0x5A,0xC1,0x5D,0xFB,0x28, +0x6E,0xE1,0x67,0x51,0xBB,0x31,0x92,0x83,0xAC,0xAA,0x72,0x52,0xFD,0x13,0x4F,0x73, +0xD3,0xF0,0x5E,0xFC,0xBA,0xB1,0x3C,0x7B,0x08,0x76,0x03,0x38,0x1E,0xD1,0xCC,0x33, +0xA3,0x1E,0xFC,0xE0,0x82,0x30,0x27,0x93,0x71,0x35,0x75,0x77,0xBA,0x78,0x10,0x33, +0xCD,0xAB,0xCF,0x8E,0xAD,0xF9,0x32,0xC9,0x15,0x9F,0xD6,0x6D,0xA8,0xAE,0xB1,0x3F, +0x90,0xEB,0xD4,0xF9,0x31,0x81,0xA3,0x53,0x99,0x4B,0x3C,0x93,0x3B,0xFE,0x55,0xFF, +0x25,0x9F,0xCC,0x07,0xC5,0x2C,0x14,0xA7,0xA4,0x1E,0x6C,0xB6,0x91,0x2A,0xE0,0x3E, +0x7F,0x39,0x0A,0xD9,0x24,0x3C,0x01,0xA0,0x30,0x99,0x8E,0xB8,0x1D,0xF9,0xA7,0x78, +0x86,0x95,0x35,0x0E,0x21,0xDA,0x7A,0x7B,0xAD,0x9F,0x4E,0xF6,0x63,0x5B,0x96,0xBB, +0x87,0x36,0x3F,0xA7,0x1A,0x66,0x91,0xCD,0xB0,0x3B,0xC0,0x4F,0x54,0xD2,0x5F,0xBB, +0x38,0x89,0x1C,0x79,0x7E,0xA2,0x02,0xE4,0x80,0x84,0x1E,0x33,0xAB,0x74,0xFA,0xBE, +0x31,0x46,0x2E,0xC5,0x15,0xB9,0x12,0xE9,0xD3,0x73,0x43,0xEA,0x74,0x11,0xA7,0xC0, +0xD5,0xD8,0x39,0x08,0x9F,0x4F,0xC7,0x71,0x25,0x09,0x51,0x65,0xD6,0xA8,0x02,0x1F +}; + +// Note: Hash must be power of 2! +#define PERLIN_HASH_LENGTH 1024 +#define PERLIN_HASH_MASK (PERLIN_HASH_LENGTH-1) + +static int perlin_noise2( int x, int y, int seed ) +{ + return perlin_hash[ (perlin_hash[(y+seed) & PERLIN_HASH_MASK] + x) + & PERLIN_HASH_MASK ]; +} + +static int perlin_noise1( int i, int seed ) +{ + return perlin_hash[ (seed + i) & PERLIN_HASH_MASK ]; +} + +static f32 perlin_smooth( f32 x, f32 y, f32 t ) +{ + return vg_lerpf( x, y, t*t*(3.0f-2.0f*t) ); +} + +f32 vg_perlin_noise_2d( f32 x, f32 y, int seed ) +{ + int ix = x, iy = y; + f32 x_frac = x - ix, + y_frac = y - iy; + + int s = perlin_noise2( ix, iy, seed ), + t = perlin_noise2( ix+1, iy, seed ), + u = perlin_noise2( ix, iy+1, seed ), + v = perlin_noise2( ix+1, iy+1, seed ); + + f32 low = perlin_smooth( s,t,x_frac ), + high = perlin_smooth( u,v,x_frac ); + + return perlin_smooth( low, high, y_frac ); +} + +f32 vg_perlin_noise_1d( f32 v, int seed ) +{ + int iv = v; + f32 frac = v-iv; + int s = perlin_noise1( iv, seed ), + t = perlin_noise1( iv+1, seed ); + + return perlin_smooth( s, t, frac ); +} + +f32 vg_perlin_fract_1d( f32 v, f32 freq, int octaves, int seed ) +{ + f32 xa = v*freq, + amp = 1.0f, + fin = 0.f, + div = 0.f; + + for( int i=0; iinv_mass = 1.0f/mass; + + m3x3f I; + vg_capsule_inertia( r, h, mass * inertia_scale, I ); + m3x3_inv( I, rb->iI ); +} + +void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale ) +{ + f32 vol = vg_box_volume( box ), + mass = vol*density; + + rb->inv_mass = 1.0f/mass; + + m3x3f I; + vg_box_inertia( box, mass * inertia_scale, I ); + m3x3_inv( I, rb->iI ); +} + +void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale ) +{ + f32 vol = vg_sphere_volume( r ), + mass = vol*density; + + rb->inv_mass = 1.0f/mass; + m3x3f I; + vg_sphere_inertia( r, mass * inertia_scale, I ); + m3x3_inv( I, rb->iI ); +} + +void rb_update_matrices( rigidbody *rb ) +{ + //q_normalize( rb->q ); + q_m3x3( rb->q, rb->to_world ); + v3_copy( rb->co, rb->to_world[3] ); + m4x3_invert_affine( rb->to_world, rb->to_local ); + + /* I = R I_0 R^T */ + m3x3_mul( rb->to_world, rb->iI, rb->iIw ); + m3x3_mul( rb->iIw, rb->to_local, rb->iIw ); +} + +void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) +{ + float substep = vg.time_fixed_extrapolate; + v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co ); + + if( v3_length2( rb->w ) > 0.0f ){ + v4f rotation; + v3f axis; + v3_copy( rb->w, axis ); + + float mag = v3_length( axis ); + v3_divs( axis, mag, axis ); + q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep ); + q_mul( rotation, rb->q, q ); + q_normalize( q ); + } + else{ + v4_copy( rb->q, q ); + } +} + +void rb_iter( rigidbody *rb ) +{ + if( !vg_validf(rb->v[0]) || !vg_validf(rb->v[1]) || !vg_validf(rb->v[2]) ) + { + vg_fatal_error( + "Aborting the program because velocity has invalid value in one " + "or more components: %f %f %f\n", rb->v[0],rb->v[1],rb->v[2] ); + } + + v3f gravity = { 0.0f, -k_gravity, 0.0f }; + v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v ); + + /* intergrate velocity */ + v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co ); +#if 0 + v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w ); +#endif + + /* inegrate inertia */ + if( v3_length2( rb->w ) > 0.0f ){ + v4f rotation; + v3f axis; + v3_copy( rb->w, axis ); + + float mag = v3_length( axis ); + v3_divs( axis, mag, axis ); + q_axis_angle( rotation, axis, mag*vg.time_fixed_delta ); + q_mul( rotation, rb->q, rb->q ); + q_normalize( rb->q ); + } +} + +/* + * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf, + * page 76. + */ +void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ) +{ + /* convert to body coordinates */ + v3f w_local; + m3x3_mulv( rb->to_local, rb->w, w_local ); + + /* Residual vector */ + v3f f, v0; + m3x3_mulv( I, w_local, v0 ); + v3_cross( w_local, v0, f ); + v3_muls( f, h, f ); + + /* Jacobian */ + m3x3f iJ, J, skew_w_local, skew_v0, m0; + m3x3_skew_symetric( skew_w_local, w_local ); + m3x3_mul( skew_w_local, I, m0 ); + + m3x3_skew_symetric( skew_v0, v0 ); + m3x3_sub( m0, skew_v0, J ); + m3x3_scalef( J, h ); + m3x3_add( I, J, J ); + + /* Single Newton-Raphson update */ + v3f w1; + m3x3_inv( J, iJ ); + m3x3_mulv( iJ, f, w1 ); + v3_sub( w_local, w1, rb->w ); + + m3x3_mulv( rb->to_world, rb->w, rb->w ); +} + +void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ) +{ + v3f rva, rvb; + v3_cross( rba->w, ra, rva ); + v3_add( rba->v, rva, rva ); + v3_cross( rbb->w, rb, rvb ); + v3_add( rbb->v, rvb, rvb ); + + v3_sub( rva, rvb, rv ); +} + +void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ) +{ + /* linear */ + v3_muladds( rb->v, impulse, rb->inv_mass, rb->v ); + + /* Angular velocity */ + v3f wa; + v3_cross( delta, impulse, wa ); + + m3x3_mulv( rb->iIw, wa, wa ); + v3_add( rb->w, wa, rb->w ); +} + + +void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag ) +{ + /* float */ + float depth = v3_dot( plane, ra->co ) - plane[3], + lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt; + + v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v ); + + if( depth < 0.0f ) + v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v ); +} + +/* apply a spring&dampener force to match ra(worldspace) on rigidbody, to + * rt(worldspace) + */ +void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, + f32 spring, f32 dampening, f32 timestep ) +{ + float d = v3_dot( rt, ra ); + float a = acosf( vg_clampf( d, -1.0f, 1.0f ) ); + + v3f axis; + v3_cross( rt, ra, axis ); + + float Fs = -a * spring, + Fd = -v3_dot( rba->w, axis ) * dampening; + + v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w ); +} diff --git a/source/maths/rigidbody_collision.c b/source/maths/rigidbody_collision.c new file mode 100644 index 0000000..156416e --- /dev/null +++ b/source/maths/rigidbody_collision.c @@ -0,0 +1,880 @@ +int rb_contact_count = 0; +struct rb_ct rb_contact_buffer[VG_MAX_CONTACTS]; + +/* + * Contact generators + * + * These do not automatically allocate contacts, an appropriately sized + * buffer must be supplied. The function returns the size of the manifold + * which was generated. + * + * The values set on the contacts are: n, co, p, rba, rbb + */ + +/* + * By collecting the minimum(time) and maximum(time) pairs of points, we + * build a reduced and stable exact manifold. + * + * tx: time at point + * rx: minimum distance of these points + * dx: the delta between the two points + * + * pairs will only ammend these if they are creating a collision + */ +typedef struct capsule_manifold capsule_manifold; +struct capsule_manifold{ + f32 t0, t1; + f32 r0, r1; + v3f d0, d1; +}; + +/* + * Expand a line manifold with a new pair. t value is the time along segment + * on the oriented object which created this pair. + */ +static void rb_capsule_manifold( v3f pa, v3f pb, f32 t, f32 r, + capsule_manifold *manifold ){ + v3f delta; + v3_sub( pa, pb, delta ); + + if( v3_length2(delta) < r*r ){ + if( t < manifold->t0 ){ + v3_copy( delta, manifold->d0 ); + manifold->t0 = t; + manifold->r0 = r; + } + + if( t > manifold->t1 ){ + v3_copy( delta, manifold->d1 ); + manifold->t1 = t; + manifold->r1 = r; + } + } +} + +static void rb_capsule_manifold_init( capsule_manifold *manifold ){ + manifold->t0 = INFINITY; + manifold->t1 = -INFINITY; +} + +static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, + capsule_manifold *manifold, + rb_ct *buf ){ + v3f p0, p1; + v3_muladds( mtx[3], mtx[1], -c->h*0.5f+c->r, p0 ); + v3_muladds( mtx[3], mtx[1], c->h*0.5f-c->r, p1 ); + + int count = 0; + if( manifold->t0 <= 1.0f ){ + rb_ct *ct = buf; + + v3f pa; + v3_muls( p0, 1.0f-manifold->t0, pa ); + v3_muladds( pa, p1, manifold->t0, pa ); + + f32 d = v3_length( manifold->d0 ); + v3_muls( manifold->d0, 1.0f/d, ct->n ); + v3_muladds( pa, ct->n, -c->r, ct->co ); + + ct->p = manifold->r0 - d; + ct->type = k_contact_type_default; + count ++; + } + + if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) ){ + rb_ct *ct = buf+count; + + v3f pa; + v3_muls( p0, 1.0f-manifold->t1, pa ); + v3_muladds( pa, p1, manifold->t1, pa ); + + f32 d = v3_length( manifold->d1 ); + v3_muls( manifold->d1, 1.0f/d, ct->n ); + v3_muladds( pa, ct->n, -c->r, ct->co ); + + ct->p = manifold->r1 - d; + ct->type = k_contact_type_default; + + count ++; + } + + /* + * Debugging + */ + +#if 0 + if( count == 2 ) + vg_line( buf[0].co, buf[1].co, 0xff0000ff ); +#endif + + return count; +} + +int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, + v3f coB, f32 rb, rb_ct *buf ){ + f32 ha = ca->h, + ra = ca->r, + r = ra + rb; + + v3f p0, p1; + v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); + v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); + + v3f c, delta; + closest_point_segment( p0, p1, coB, c ); + v3_sub( c, coB, delta ); + f32 d2 = v3_length2(delta); + + if( d2 < r*r ){ + f32 d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + ct->p = r-d; + + v3f p0, p1; + v3_muladds( c, ct->n, -ra, p0 ); + v3_muladds( coB, ct->n, rb, p1 ); + v3_add( p0, p1, ct->co ); + v3_muls( ct->co, 0.5f, ct->co ); + ct->type = k_contact_type_default; + return 1; + } + else return 0; +} + +int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, + m4x3f mtxB, rb_capsule *cb, rb_ct *buf ) +{ + f32 ha = ca->h, + hb = cb->h, + ra = ca->r, + rb = cb->r, + r = ra+rb; + + v3f p0, p1, p2, p3; + v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); + v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); + v3_muladds( mtxB[3], mtxB[1], -hb*0.5f+rb, p2 ); + v3_muladds( mtxB[3], mtxB[1], hb*0.5f-rb, p3 ); + + capsule_manifold manifold; + rb_capsule_manifold_init( &manifold ); + + v3f pa, pb; + f32 ta, tb; + closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb ); + rb_capsule_manifold( pa, pb, ta, r, &manifold ); + + ta = closest_point_segment( p0, p1, p2, pa ); + tb = closest_point_segment( p0, p1, p3, pb ); + rb_capsule_manifold( pa, p2, ta, r, &manifold ); + rb_capsule_manifold( pb, p3, tb, r, &manifold ); + + closest_point_segment( p2, p3, p0, pa ); + closest_point_segment( p2, p3, p1, pb ); + rb_capsule_manifold( p0, pa, 0.0f, r, &manifold ); + rb_capsule_manifold( p1, pb, 1.0f, r, &manifold ); + + return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); +} + +/* + * Generates up to two contacts; optimised for the most stable manifold + */ +int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, + m4x3f mtxB, m4x3f mtxB_inverse, boxf box, + rb_ct *buf ) +{ + f32 h = ca->h, r = ca->r; + + /* + * Solving this in symetric local space of the cube saves us some time and a + * couple branches when it comes to the quad stage. + */ + v3f centroid; + v3_add( box[0], box[1], centroid ); + v3_muls( centroid, 0.5f, centroid ); + + boxf bbx; + v3_sub( box[0], centroid, bbx[0] ); + v3_sub( box[1], centroid, bbx[1] ); + + v3f pc, p0w, p1w, p0, p1; + v3_muladds( mtxA[3], mtxA[1], -h*0.5f+r, p0w ); + v3_muladds( mtxA[3], mtxA[1], h*0.5f-r, p1w ); + + m4x3_mulv( mtxB_inverse, p0w, p0 ); + m4x3_mulv( mtxB_inverse, p1w, p1 ); + v3_sub( p0, centroid, p0 ); + v3_sub( p1, centroid, p1 ); + v3_add( p0, p1, pc ); + v3_muls( pc, 0.5f, pc ); + + /* + * Finding an appropriate quad to collide lines with + */ + v3f region; + v3_div( pc, bbx[1], region ); + + v3f quad[4]; + if( (fabsf(region[0]) > fabsf(region[1])) && + (fabsf(region[0]) > fabsf(region[2])) ) + { + f32 px = vg_signf(region[0]) * bbx[1][0]; + v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] ); + v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] ); + v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] ); + v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] ); + } + else if( fabsf(region[1]) > fabsf(region[2]) ) + { + f32 py = vg_signf(region[1]) * bbx[1][1]; + v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] ); + v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] ); + v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] ); + v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] ); + } + else + { + f32 pz = vg_signf(region[2]) * bbx[1][2]; + v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] ); + v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] ); + v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] ); + v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] ); + } + + capsule_manifold manifold; + rb_capsule_manifold_init( &manifold ); + + v3f c0, c1; + closest_point_aabb( p0, bbx, c0 ); + closest_point_aabb( p1, bbx, c1 ); + + v3f d0, d1, da; + v3_sub( c0, p0, d0 ); + v3_sub( c1, p1, d1 ); + v3_sub( p1, p0, da ); + + v3_normalize(d0); + v3_normalize(d1); + v3_normalize(da); + + if( v3_dot( da, d0 ) <= 0.01f ) + rb_capsule_manifold( p0, c0, 0.0f, r, &manifold ); + + if( v3_dot( da, d1 ) >= -0.01f ) + rb_capsule_manifold( p1, c1, 1.0f, r, &manifold ); + + for( i32 i=0; i<4; i++ ){ + i32 i0 = i, + i1 = (i+1)%4; + + v3f ca, cb; + f32 ta, tb; + closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb ); + rb_capsule_manifold( ca, cb, ta, r, &manifold ); + } + + /* + * Create final contacts based on line manifold + */ + m3x3_mulv( mtxB, manifold.d0, manifold.d0 ); + m3x3_mulv( mtxB, manifold.d1, manifold.d1 ); + return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); +} + +int rb_sphere__box( v3f coA, f32 ra, + m4x3f mtxB, m4x3f mtxB_inverse, boxf box, + rb_ct *buf ) +{ + v3f co, delta; + closest_point_obb( coA, box, mtxB, mtxB_inverse, co ); + v3_sub( coA, co, delta ); + + f32 d2 = v3_length2(delta); + + if( d2 <= ra*ra ){ + f32 d; + + rb_ct *ct = buf; + if( d2 <= 0.0001f ){ + v3f e, coB; + v3_sub( box[1], box[0], e ); + v3_muls( e, 0.5f, e ); + v3_add( box[0], e, coB ); + v3_sub( coA, coB, delta ); + + /* + * some extra testing is required to find the best axis to push the + * object back outside the box. Since there isnt a clear seperating + * vector already, especially on really high aspect boxes. + */ + f32 lx = v3_dot( mtxB[0], delta ), + ly = v3_dot( mtxB[1], delta ), + lz = v3_dot( mtxB[2], delta ), + px = e[0] - fabsf(lx), + py = e[1] - fabsf(ly), + pz = e[2] - fabsf(lz); + + if( px < py && px < pz ) v3_muls( mtxB[0], vg_signf(lx), ct->n ); + else if( py < pz ) v3_muls( mtxB[1], vg_signf(ly), ct->n ); + else v3_muls( mtxB[2], vg_signf(lz), ct->n ); + + v3_muladds( coA, ct->n, -ra, ct->co ); + ct->p = ra; + } + else{ + d = sqrtf(d2); + v3_muls( delta, 1.0f/d, ct->n ); + ct->p = ra-d; + v3_copy( co, ct->co ); + } + + ct->type = k_contact_type_default; + return 1; + } + else return 0; +} + +int rb_sphere__sphere( v3f coA, f32 ra, v3f coB, f32 rb, rb_ct *buf ) +{ + v3f delta; + v3_sub( coA, coB, delta ); + + f32 d2 = v3_length2(delta), + r = ra+rb; + + if( d2 < r*r ){ + f32 d = sqrtf(d2); + + rb_ct *ct = buf; + v3_muls( delta, 1.0f/d, ct->n ); + + v3f p0, p1; + v3_muladds( coA, ct->n,-ra, p0 ); + v3_muladds( coB, ct->n, rb, p1 ); + v3_add( p0, p1, ct->co ); + v3_muls( ct->co, 0.5f, ct->co ); + ct->type = k_contact_type_default; + ct->p = r-d; + return 1; + } + else return 0; +} + +int rb_sphere__triangle( m4x3f mtxA, f32 r, v3f tri[3], rb_ct *buf ) +{ + v3f delta, co; + enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co ); + v3_sub( mtxA[3], co, delta ); + f32 d2 = v3_length2( delta ); + + if( d2 <= r*r ){ + rb_ct *ct = buf; + + v3f ab, ac, tn; + v3_sub( tri[2], tri[0], ab ); + v3_sub( tri[1], tri[0], ac ); + v3_cross( ac, ab, tn ); + v3_copy( tn, ct->n ); + + if( v3_length2( ct->n ) <= 0.00001f ){ +#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING + vg_error( "Zero area triangle!\n" ); +#endif + return 0; + } + + v3_normalize( ct->n ); + + f32 d = sqrtf(d2); + + v3_copy( co, ct->co ); + ct->type = type; + ct->p = r-d; + return 1; + } + + return 0; +} + +int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ) +{ + v3f pc, p0w, p1w; + v3_muladds( mtxA[3], mtxA[1], -c->h*0.5f+c->r, p0w ); + v3_muladds( mtxA[3], mtxA[1], c->h*0.5f-c->r, p1w ); + + capsule_manifold manifold; + rb_capsule_manifold_init( &manifold ); + + v3f v0, v1, n; + v3_sub( tri[1], tri[0], v0 ); + v3_sub( tri[2], tri[0], v1 ); + v3_cross( v0, v1, n ); + + if( v3_length2( n ) <= 0.00001f ){ +#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING + vg_error( "Zero area triangle!\n" ); +#endif + return 0; + } + + v3_normalize( n ); + +#if 1 + /* deep penetration recovery. for when we clip through the triangles. so its + * not very 'correct' */ + f32 dist; + if( ray_tri( tri, p0w, mtxA[1], &dist, 1 ) ){ + f32 l = c->h - c->r*2.0f; + if( (dist >= 0.0f) && (dist < l) ){ + v3f co; + v3_muladds( p0w, mtxA[1], dist, co ); + vg_line_point( co, 0.02f, 0xffffff00 ); + + v3f d0, d1; + v3_sub( p0w, co, d0 ); + v3_sub( p1w, co, d1 ); + + f32 p = vg_minf( v3_dot( n, d0 ), v3_dot( n, d1 ) ) - c->r; + + rb_ct *ct = buf; + ct->p = -p; + ct->type = k_contact_type_default; + v3_copy( n, ct->n ); + v3_muladds( co, n, p, ct->co ); + + return 1; + } + } +#endif + + v3f c0, c1; + closest_on_triangle_1( p0w, tri, c0 ); + closest_on_triangle_1( p1w, tri, c1 ); + + v3f d0, d1, da; + v3_sub( c0, p0w, d0 ); + v3_sub( c1, p1w, d1 ); + v3_sub( p1w, p0w, da ); + + v3_normalize(d0); + v3_normalize(d1); + v3_normalize(da); + + /* the two balls at the ends */ + if( v3_dot( da, d0 ) <= 0.01f ) + rb_capsule_manifold( p0w, c0, 0.0f, c->r, &manifold ); + if( v3_dot( da, d1 ) >= -0.01f ) + rb_capsule_manifold( p1w, c1, 1.0f, c->r, &manifold ); + + /* the edges to edges */ + for( int i=0; i<3; i++ ){ + int i0 = i, + i1 = (i+1)%3; + + v3f ca, cb; + f32 ta, tb; + closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb ); + rb_capsule_manifold( ca, cb, ta, c->r, &manifold ); + } + + int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf ); + for( int i=0; i VG_ARRAY_LEN(rb_contact_buffer) ) + return 0; + + return 1; +} + +rb_ct *rb_global_buffer( void ) +{ + return &rb_contact_buffer[ rb_contact_count ]; +} + +/* + * ----------------------------------------------------------------------------- + * Boolean shape overlap functions + * ----------------------------------------------------------------------------- + */ + +/* + * Project AABB, and triangle interval onto axis to check if they overlap + */ +static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ + float + + r = extent[0] * fabsf(axis[0]) + + extent[1] * fabsf(axis[1]) + + extent[2] * fabsf(axis[2]), + + p0 = v3_dot( axis, tri[0] ), + p1 = v3_dot( axis, tri[1] ), + p2 = v3_dot( axis, tri[2] ), + + e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2))); + + if( e > r ) return 0; + else return 1; +} + +/* + * Seperating axis test box vs triangle + */ +int rb_box_triangle_sat( v3f extent, v3f center, + m4x3f to_local, v3f tri_src[3] ) +{ + v3f tri[3]; + + for( int i=0; i<3; i++ ){ + m4x3_mulv( to_local, tri_src[i], tri[i] ); + v3_sub( tri[i], center, tri[i] ); + } + + v3f f0,f1,f2,n; + v3_sub( tri[1], tri[0], f0 ); + v3_sub( tri[2], tri[1], f1 ); + v3_sub( tri[0], tri[2], f2 ); + + + v3f axis[9]; + v3_cross( (v3f){1.0f,0.0f,0.0f}, f0, axis[0] ); + v3_cross( (v3f){1.0f,0.0f,0.0f}, f1, axis[1] ); + v3_cross( (v3f){1.0f,0.0f,0.0f}, f2, axis[2] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f0, axis[3] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f1, axis[4] ); + v3_cross( (v3f){0.0f,1.0f,0.0f}, f2, axis[5] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f0, axis[6] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f1, axis[7] ); + v3_cross( (v3f){0.0f,0.0f,1.0f}, f2, axis[8] ); + + for( int i=0; i<9; i++ ) + if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0; + + /* u0, u1, u2 */ + if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0; + if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0; + if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0; + + /* normal */ + v3_cross( f0, f1, n ); + if(!rb_box_triangle_interval( extent, n, tri )) return 0; + + return 1; +} + +/* + * ----------------------------------------------------------------------------- + * Manifold + * ----------------------------------------------------------------------------- + */ + +int rb_manifold_apply_filtered( rb_ct *man, int len ) +{ + int k = 0; + + for( int i=0; itype == k_contact_type_disabled ) + continue; + + man[k ++] = man[i]; + } + + return k; +} + +void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ) +{ + if( v3_dist2( ci->co, cj->co ) < r*r ){ + cj->type = k_contact_type_disabled; + ci->p = (ci->p + cj->p) * 0.5f; + + v3_add( ci->co, cj->co, ci->co ); + v3_muls( ci->co, 0.5f, ci->co ); + + v3f delta; + v3_sub( ci->rba->co, ci->co, delta ); + + float c0 = v3_dot( ci->n, delta ), + c1 = v3_dot( cj->n, delta ); + + if( c0 < 0.0f || c1 < 0.0f ){ + /* error */ + ci->type = k_contact_type_disabled; + } + else{ + v3f n; + v3_muls( ci->n, c0, n ); + v3_muladds( n, cj->n, c1, n ); + v3_normalize( n ); + v3_copy( n, ci->n ); + } + } +} + +/* + * + */ +void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ) +{ + for( int i=0; itype != k_contact_type_edge ) + continue; + + for( int j=i+1; jtype != k_contact_type_edge ) + continue; + + rb_manifold_contact_weld( ci, cj, r ); + } + } +} + +void rb_manifold_filter_pairs( rb_ct *man, int len, float r ) +{ + for( int i=0; itype == k_contact_type_disabled ) continue; + + for( int j=i+1; jtype == k_contact_type_disabled ) continue; + + if( v3_dist2( ci->co, cj->co ) < r*r ){ + cj->type = k_contact_type_disabled; + v3_add( cj->n, ci->n, ci->n ); + ci->p += cj->p; + similar ++; + } + } + + if( similar ){ + float n = 1.0f/((float)similar+1.0f); + v3_muls( ci->n, n, ci->n ); + ci->p *= n; + + if( v3_length2(ci->n) < 0.1f*0.1f ) + ci->type = k_contact_type_disabled; + else + v3_normalize( ci->n ); + } + } +} + +void rb_manifold_filter_backface( rb_ct *man, int len ) +{ + for( int i=0; itype == k_contact_type_disabled ) + continue; + + v3f delta; + v3_sub( ct->co, ct->rba->co, delta ); + + if( v3_dot( delta, ct->n ) > -0.001f ) + ct->type = k_contact_type_disabled; + } +} + +void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ) +{ + for( int i=0; itype == k_contact_type_disabled || + ci->type == k_contact_type_edge ) + continue; + + float d1 = v3_dot( ci->co, ci->n ); + + for( int j=0; jtype == k_contact_type_disabled ) + continue; + + float d2 = v3_dot( cj->co, ci->n ), + d = d2-d1; + + if( fabsf( d ) <= w ){ + cj->type = k_contact_type_disabled; + } + } + } +} + +void rb_debug_contact( rb_ct *ct ) +{ + v3f p1; + v3_muladds( ct->co, ct->n, 0.05f, p1 ); + + if( ct->type == k_contact_type_default ){ + vg_line_point( ct->co, 0.0125f, 0xff0000ff ); + vg_line( ct->co, p1, 0xffffffff ); + } + else if( ct->type == k_contact_type_edge ){ + vg_line_point( ct->co, 0.0125f, 0xff00ffc0 ); + vg_line( ct->co, p1, 0xffffffff ); + } +} + +void rb_solver_reset(void) +{ + rb_contact_count = 0; +} + +rb_ct *rb_global_ct(void) +{ + return rb_contact_buffer + rb_contact_count; +} + +void rb_prepare_contact( rb_ct *ct, f32 dt ) +{ + ct->bias = -k_phys_baumgarte * (dt*3600.0f) + * vg_minf( 0.0f, -ct->p+k_penetration_slop ); + + v3_tangent_basis( ct->n, ct->t[0], ct->t[1] ); + ct->norm_impulse = 0.0f; + ct->tangent_impulse[0] = 0.0f; + ct->tangent_impulse[1] = 0.0f; +} + +/* + * calculate total move to depenetrate object from contacts. + * manifold should belong to ONE object only + */ +void rb_depenetrate( rb_ct *manifold, int len, v3f dt ) +{ + v3_zero( dt ); + + for( int j=0; j<7; j++ ){ + for( int i=0; in, dt ), + remaining = (ct->p-k_penetration_slop) - resolved_amt, + apply = vg_maxf( remaining, 0.0f ) * 0.4f; + + v3_muladds( dt, ct->n, apply, dt ); + } + } +} + +/* + * Initializing things like tangent vectors + */ +void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len ) +{ + for( int i=0; ico, ct->rba->co, ra ); + v3_sub( ct->co, ct->rbb->co, rb ); + v3_cross( ra, ct->n, raCn ); + v3_cross( rb, ct->n, rbCn ); + + /* orient inverse inertia tensors */ + v3f raCnI, rbCnI; + m3x3_mulv( ct->rba->iIw, raCn, raCnI ); + m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI ); + + ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass; + ct->normal_mass += v3_dot( raCn, raCnI ); + ct->normal_mass += v3_dot( rbCn, rbCnI ); + ct->normal_mass = 1.0f/ct->normal_mass; + + for( int j=0; j<2; j++ ){ + v3f raCtI, rbCtI; + v3_cross( ct->t[j], ra, raCt ); + v3_cross( ct->t[j], rb, rbCt ); + m3x3_mulv( ct->rba->iIw, raCt, raCtI ); + m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI ); + + ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass; + ct->tangent_mass[j] += v3_dot( raCt, raCtI ); + ct->tangent_mass[j] += v3_dot( rbCt, rbCtI ); + ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j]; + } + } +} + +void rb_contact_restitution( rb_ct *ct, float cr ) +{ + v3f rv, ra, rb; + v3_sub( ct->co, ct->rba->co, ra ); + v3_sub( ct->co, ct->rbb->co, rb ); + rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); + + float v = v3_dot( rv, ct->n ); + + if( v < -1.0f ){ + ct->bias += -cr * v; + } +} + +/* + * One iteration to solve the contact constraint + */ +void rb_solve_contacts( rb_ct *buf, int len ) +{ + for( int i=0; ico, ct->rba->co, ra ); + v3_sub( ct->co, ct->rbb->co, rb ); + rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); + + /* Friction */ + for( int j=0; j<2; j++ ){ + float f = k_friction * ct->norm_impulse, + vt = v3_dot( rv, ct->t[j] ), + lambda = ct->tangent_mass[j] * -vt; + + float temp = ct->tangent_impulse[j]; + ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f ); + lambda = ct->tangent_impulse[j] - temp; + + v3f impulse; + v3_muls( ct->t[j], lambda, impulse ); + rb_linear_impulse( ct->rba, ra, impulse ); + + v3_muls( ct->t[j], -lambda, impulse ); + rb_linear_impulse( ct->rbb, rb, impulse ); + } + + /* Normal */ + rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); + float vn = v3_dot( rv, ct->n ), + lambda = ct->normal_mass * (-vn + ct->bias); + + float temp = ct->norm_impulse; + ct->norm_impulse = vg_maxf( temp + lambda, 0.0f ); + lambda = ct->norm_impulse - temp; + + v3f impulse; + v3_muls( ct->n, lambda, impulse ); + rb_linear_impulse( ct->rba, ra, impulse ); + + v3_muls( ct->n, -lambda, impulse ); + rb_linear_impulse( ct->rbb, rb, impulse ); + } +} diff --git a/source/maths/rigidbody_constraints.c b/source/maths/rigidbody_constraints.c new file mode 100644 index 0000000..ea26487 --- /dev/null +++ b/source/maths/rigidbody_constraints.c @@ -0,0 +1,490 @@ +/* + * ----------------------------------------------------------------------------- + * Constraints + * ----------------------------------------------------------------------------- + */ + +void rb_debug_position_constraints( rb_constr_pos *buffer, int len ) +{ + for( int i=0; irba, *rbb = constr->rbb; + + v3f wca, wcb; + m3x3_mulv( rba->to_world, constr->lca, wca ); + m3x3_mulv( rbb->to_world, constr->lcb, wcb ); + + v3f p0, p1; + v3_add( wca, rba->co, p0 ); + v3_add( wcb, rbb->co, p1 ); + vg_line_point( p0, 0.0025f, 0xff000000 ); + vg_line_point( p1, 0.0025f, 0xffffffff ); + vg_line2( p0, p1, 0xff000000, 0xffffffff ); + } +} + +void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) +{ + for( int i=0; irba->to_world, st->conevx, vx ); + m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); + m3x3_mulv( st->rba->to_world, st->conevy, vy ); + m3x3_mulv( st->rbb->to_world, st->coneva, va ); + m4x3_mulv( st->rba->to_world, st->view_offset, center ); + v3_cross( vy, vx, axis ); + + /* Constraint violated ? */ + float fx = v3_dot( vx, va ), /* projection world */ + fy = v3_dot( vy, va ), + fn = v3_dot( va, axis ), + + rx = st->conevx[3], /* elipse radii */ + ry = st->conevy[3], + + lx = fx/rx, /* projection local (fn==lz) */ + ly = fy/ry; + + st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f); + if( st->tangent_violation ){ + /* Calculate a good position and the axis to solve on */ + v2f closest, tangent, + p = { fx/fabsf(fn), fy/fabsf(fn) }; + + closest_point_elipse( p, (v2f){rx,ry}, closest ); + tangent[0] = -closest[1] / (ry*ry); + tangent[1] = closest[0] / (rx*rx); + v2_normalize( tangent ); + + v3f v0, v1; + v3_muladds( axis, vx, closest[0], v0 ); + v3_muladds( v0, vy, closest[1], v0 ); + v3_normalize( v0 ); + + v3_muls( vx, tangent[0], v1 ); + v3_muladds( v1, vy, tangent[1], v1 ); + + v3_copy( v0, st->tangent_target ); + v3_copy( v1, st->tangent_axis ); + + /* calculate mass */ + v3f aIw, bIw; + m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw ); + m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw ); + st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) + + v3_dot( st->tangent_axis, bIw )); + + float angle = v3_dot( va, st->tangent_target ); + } + + v3f refaxis; + v3_cross( vy, va, refaxis ); /* our default rotation */ + v3_normalize( refaxis ); + + float angle = v3_dot( refaxis, vxb ); + st->axis_violation = fabsf(angle) < st->conet; + + if( st->axis_violation ){ + v3f dir_test; + v3_cross( refaxis, vxb, dir_test ); + + if( v3_dot(dir_test, va) < 0.0f ) + st->axis_violation = -st->axis_violation; + + float newang = (float)st->axis_violation * acosf(st->conet-0.0001f); + + v3f refaxis_up; + v3_cross( va, refaxis, refaxis_up ); + v3_muls( refaxis_up, sinf(newang), st->axis_target ); + v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target ); + + /* calculate mass */ + v3_copy( va, st->axis ); + v3f aIw, bIw; + m3x3_mulv( st->rba->iIw, st->axis, aIw ); + m3x3_mulv( st->rbb->iIw, st->axis, bIw ); + st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) + + v3_dot( st->axis, bIw )); + } + } +} + +void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) +{ + float size = 0.12f; + + for( int i=0; irba->to_world, st->conevx, vx ); + m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); + m3x3_mulv( st->rba->to_world, st->conevy, vy ); + m3x3_mulv( st->rbb->to_world, st->coneva, va ); + m4x3_mulv( st->rba->to_world, st->view_offset, center ); + v3_cross( vy, vx, axis ); + + float rx = st->conevx[3], /* elipse radii */ + ry = st->conevy[3]; + + v3f p0, p1; + v3_muladds( center, va, size, p1 ); + vg_line( center, p1, 0xffffffff ); + vg_line_point( p1, 0.00025f, 0xffffffff ); + + if( st->tangent_violation ){ + v3_muladds( center, st->tangent_target, size, p0 ); + + vg_line( center, p0, 0xff00ff00 ); + vg_line_point( p0, 0.00025f, 0xff00ff00 ); + vg_line( p1, p0, 0xff000000 ); + } + + for( int x=0; x<32; x++ ){ + float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf, + t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf, + c0 = cosf( t0 ), + s0 = sinf( t0 ), + c1 = cosf( t1 ), + s1 = sinf( t1 ); + + v3f v0, v1; + v3_muladds( axis, vx, c0*rx, v0 ); + v3_muladds( v0, vy, s0*ry, v0 ); + v3_muladds( axis, vx, c1*rx, v1 ); + v3_muladds( v1, vy, s1*ry, v1 ); + + v3_normalize( v0 ); + v3_normalize( v1 ); + + v3_muladds( center, v0, size, p0 ); + v3_muladds( center, v1, size, p1 ); + + u32 col0r = fabsf(c0) * 255.0f, + col0g = fabsf(s0) * 255.0f, + col1r = fabsf(c1) * 255.0f, + col1g = fabsf(s1) * 255.0f, + col = st->tangent_violation? 0xff0000ff: 0xff000000, + col0 = col | (col0r<<16) | (col0g << 8), + col1 = col | (col1r<<16) | (col1g << 8); + + vg_line2( center, p0, VG__NONE, col0 ); + vg_line2( p0, p1, col0, col1 ); + } + + /* Draw twist */ + v3_muladds( center, va, size, p0 ); + v3_muladds( p0, vxb, size, p1 ); + + vg_line( p0, p1, 0xff0000ff ); + + if( st->axis_violation ){ + v3_muladds( p0, st->axis_target, size*1.25f, p1 ); + vg_line( p0, p1, 0xffffff00 ); + vg_line_point( p1, 0.0025f, 0xffffff80 ); + } + + v3f refaxis; + v3_cross( vy, va, refaxis ); /* our default rotation */ + v3_normalize( refaxis ); + v3f refaxis_up; + v3_cross( va, refaxis, refaxis_up ); + float newang = acosf(st->conet-0.0001f); + + v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 ); + v3_muladds( p1, refaxis, -cosf(newang)*size, p1 ); + vg_line( p0, p1, 0xff000000 ); + + v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 ); + v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 ); + vg_line( p0, p1, 0xff404040 ); + } +} + +void rb_solve_position_constraints( rb_constr_pos *buf, int len ) +{ + for( int i=0; irba, *rbb = constr->rbb; + + v3f wa, wb; + m3x3_mulv( rba->to_world, constr->lca, wa ); + m3x3_mulv( rbb->to_world, constr->lcb, wb ); + + m3x3f ssra, ssrat, ssrb, ssrbt; + + m3x3_skew_symetric( ssrat, wa ); + m3x3_skew_symetric( ssrbt, wb ); + m3x3_transpose( ssrat, ssra ); + m3x3_transpose( ssrbt, ssrb ); + + v3f b, b_wa, b_wb, b_a, b_b; + m3x3_mulv( ssra, rba->w, b_wa ); + m3x3_mulv( ssrb, rbb->w, b_wb ); + v3_add( rba->v, b_wa, b ); + v3_sub( b, rbb->v, b ); + v3_sub( b, b_wb, b ); + v3_muls( b, -1.0f, b ); + + m3x3f invMa, invMb; + m3x3_diagonal( invMa, rba->inv_mass ); + m3x3_diagonal( invMb, rbb->inv_mass ); + + m3x3f ia, ib; + m3x3_mul( ssra, rba->iIw, ia ); + m3x3_mul( ia, ssrat, ia ); + m3x3_mul( ssrb, rbb->iIw, ib ); + m3x3_mul( ib, ssrbt, ib ); + + m3x3f cma, cmb; + m3x3_add( invMa, ia, cma ); + m3x3_add( invMb, ib, cmb ); + + m3x3f A; + m3x3_add( cma, cmb, A ); + + /* Solve Ax = b ( A^-1*b = x ) */ + v3f impulse; + m3x3f invA; + m3x3_inv( A, invA ); + m3x3_mulv( invA, b, impulse ); + + v3f delta_va, delta_wa, delta_vb, delta_wb; + m3x3f iwa, iwb; + m3x3_mul( rba->iIw, ssrat, iwa ); + m3x3_mul( rbb->iIw, ssrbt, iwb ); + + m3x3_mulv( invMa, impulse, delta_va ); + m3x3_mulv( invMb, impulse, delta_vb ); + m3x3_mulv( iwa, impulse, delta_wa ); + m3x3_mulv( iwb, impulse, delta_wb ); + + v3_add( rba->v, delta_va, rba->v ); + v3_add( rba->w, delta_wa, rba->w ); + v3_sub( rbb->v, delta_vb, rbb->v ); + v3_sub( rbb->w, delta_wb, rbb->w ); + } +} + +void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) +{ + for( int i=0; iaxis_violation ) + continue; + + float rv = v3_dot( st->axis, st->rbb->w ) - + v3_dot( st->axis, st->rba->w ); + + if( rv * (float)st->axis_violation > 0.0f ) + continue; + + v3f impulse, wa, wb; + v3_muls( st->axis, rv*st->axis_mass, impulse ); + m3x3_mulv( st->rba->iIw, impulse, wa ); + v3_add( st->rba->w, wa, st->rba->w ); + + v3_muls( impulse, -1.0f, impulse ); + m3x3_mulv( st->rbb->iIw, impulse, wb ); + v3_add( st->rbb->w, wb, st->rbb->w ); + + float rv2 = v3_dot( st->axis, st->rbb->w ) - + v3_dot( st->axis, st->rba->w ); + } + + for( int i=0; itangent_violation ) + continue; + + float rv = v3_dot( st->tangent_axis, st->rbb->w ) - + v3_dot( st->tangent_axis, st->rba->w ); + + if( rv > 0.0f ) + continue; + + v3f impulse, wa, wb; + v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse ); + m3x3_mulv( st->rba->iIw, impulse, wa ); + v3_add( st->rba->w, wa, st->rba->w ); + + v3_muls( impulse, -1.0f, impulse ); + m3x3_mulv( st->rbb->iIw, impulse, wb ); + v3_add( st->rbb->w, wb, st->rbb->w ); + + float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) - + v3_dot( st->tangent_axis, st->rba->w ); + } +} + +/* debugging */ +void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len ) +{ + for( int i=0; iaxis_violation ){ + st->conv_axis = 0.0f; + continue; + } + + f32 rv = v3_dot( st->axis, st->rbb->w ) - + v3_dot( st->axis, st->rba->w ); + + if( rv * (f32)st->axis_violation > 0.0f ) + st->conv_axis = 0.0f; + else + st->conv_axis = rv; + } + + for( int i=0; itangent_violation ){ + st->conv_tangent = 0.0f; + continue; + } + + f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) - + v3_dot( st->tangent_axis, st->rba->w ); + + if( rv > 0.0f ) + st->conv_tangent = 0.0f; + else + st->conv_tangent = rv; + } +} + +void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb ) +{ + m3x3f ssra, ssrb, ssrat, ssrbt; + m3x3f cma, cmb; + + m3x3_skew_symetric( ssrat, ra ); + m3x3_skew_symetric( ssrbt, rb ); + m3x3_transpose( ssrat, ssra ); + m3x3_transpose( ssrbt, ssrb ); + + m3x3_mul( ssra, rba->iIw, cma ); + m3x3_mul( cma, ssrat, cma ); + m3x3_mul( ssrb, rbb->iIw, cmb ); + m3x3_mul( cmb, ssrbt, cmb ); + + m3x3f A, invA; + m3x3_add( cma, cmb, A ); + m3x3_inv( A, invA ); + + v3f b_wa, b_wb, b; + m3x3_mulv( ssra, rba->w, b_wa ); + m3x3_mulv( ssrb, rbb->w, b_wb ); + v3_add( b_wa, b_wb, b ); + v3_negate( b, b ); + + v3f impulse; + m3x3_mulv( invA, b, impulse ); + + v3f delta_wa, delta_wb; + m3x3f iwa, iwb; + m3x3_mul( rba->iIw, ssrat, iwa ); + m3x3_mul( rbb->iIw, ssrbt, iwb ); + m3x3_mulv( iwa, impulse, delta_wa ); + m3x3_mulv( iwb, impulse, delta_wb ); + v3_add( rba->w, delta_wa, rba->w ); + v3_sub( rbb->w, delta_wb, rbb->w ); +} + +void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt ) +{ + for( int i=0; irba, *rbb = constr->rbb; + + v3f p0, p1, d; + m3x3_mulv( rba->to_world, constr->lca, p0 ); + m3x3_mulv( rbb->to_world, constr->lcb, p1 ); + v3_add( rba->co, p0, p0 ); + v3_add( rbb->co, p1, p1 ); + v3_sub( p1, p0, d ); + +#if 1 + v3_muladds( rbb->co, d, -1.0f * amt, rbb->co ); + rb_update_matrices( rbb ); +#else + f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass), + a = mt * (k_phys_baumgarte/k_rb_delta); + + v3_muladds( rba->v, d, a* rba->inv_mass, rba->v ); + v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v ); +#endif + } +} + +void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, int len, float amt ) +{ + for( int i=0; itangent_violation ) + continue; + + v3f va; + m3x3_mulv( st->rbb->to_world, st->coneva, va ); + + f32 angle = v3_dot( va, st->tangent_target ); + + if( fabsf(angle) < 0.9999f ){ + v3f axis; + v3_cross( va, st->tangent_target, axis ); +#if 1 + angle = acosf(angle) * amt; + v4f correction; + q_axis_angle( correction, axis, angle ); + q_mul( correction, st->rbb->q, st->rbb->q ); + q_normalize( st->rbb->q ); + rb_update_matrices( st->rbb ); +#else + f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass), + wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta); + //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w ); + v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w ); +#endif + } + } + + for( int i=0; iaxis_violation ) + continue; + + v3f vxb; + m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); + + f32 angle = v3_dot( vxb, st->axis_target ); + + if( fabsf(angle) < 0.9999f ){ + v3f axis; + v3_cross( vxb, st->axis_target, axis ); + +#if 1 + angle = acosf(angle) * amt; + v4f correction; + q_axis_angle( correction, axis, angle ); + q_mul( correction, st->rbb->q, st->rbb->q ); + q_normalize( st->rbb->q ); + rb_update_matrices( st->rbb ); +#else + f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass), + wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta); + //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w ); + v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w ); +#endif + } + } +} 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_audio.c b/vg_audio.c deleted file mode 100644 index 56aee24..0000000 --- a/vg_audio.c +++ /dev/null @@ -1,1455 +0,0 @@ -struct vg_audio _vg_audio = -{ - .master_volume_ui = 1.0f, - .dsp_enabled_ui = 1 -}; - -_Thread_local static bool _vg_audio_thread_has_lock = 0; - -void vg_audio_lock(void) -{ - SDL_LockMutex( _vg_audio.mutex ); - _vg_audio_thread_has_lock = 1; -} - -void vg_audio_unlock(void) -{ - _vg_audio_thread_has_lock = 0; - SDL_UnlockMutex( _vg_audio.mutex ); -} - -static void vg_audio_assert_lock(void) -{ - if( _vg_audio_thread_has_lock == 0 ) - { - vg_error( "vg_audio function requires locking\n" ); - abort(); - } -} - -/* clip loading from disk - * ------------------------------------------------------------------------------- - */ -VG_TIER_2 bool vg_audio_clip_load( audio_clip *clip, vg_stack_allocator *stack ) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); - - /* load in directly */ - u32 format = clip->flags & AUDIO_FLAG_FORMAT; - if( format == k_audio_format_vorbis ) - { - if( clip->path ) - { - clip->any_data = vg_file_read( stack, clip->path, &clip->size, 0 ); - if( clip->any_data ) - { - float mb = (float)(clip->size) / (1024.0f*1024.0f); - vg_info( "Loaded audio clip '%s' (%.1fmb)\n", clip->path, mb ); - return 1; - } - else - { - vg_error( "Audio failed to load '%s'\n", clip->path ); - return 0; - } - } - else - { - vg_error( "No path specified, embeded vorbis unsupported\n" ); - return 0; - } - } - else if( format == k_audio_format_stereo ) - { - vg_error( "Unsupported format (Stereo uncompressed)\n" ); - return 0; - } - else if( format == k_audio_format_bird ) - { - if( !clip->any_data ) - { - vg_error( "No data, external birdsynth unsupported\n" ); - return 0; - } - - struct synth_bird *bird = vg_stack_allocate( stack, sizeof(struct synth_bird), 8, NULL ); - bird->settings = clip->any_data; - clip->any_data = bird; - vg_info( "Loaded bird synthesis pattern (%u bytes)\n", clip->size ); - return 1; - } - else - { - if( !clip->path ) - { - vg_error( "No path specified, embeded mono unsupported\n" ); - return 0; - } - - u32 temp_frame = _vg_start_temp_frame(); - stb_vorbis_alloc alloc = - { - .alloc_buffer = vg_stack_allocate( _vg_temp_stack(), AUDIO_DECODE_SIZE, 8, "Vorbis alloc buffer (temp)" ), - .alloc_buffer_length_in_bytes = AUDIO_DECODE_SIZE - }; - - u32 fsize; - void *filedata = vg_file_read( _vg_temp_stack(), clip->path, &fsize, 0 ); - int err; - stb_vorbis *decoder = stb_vorbis_open_memory( filedata, fsize, &err, &alloc ); - if( !decoder ) - { - vg_error( "stb_vorbis_open_memory failed on '%s' (%d)\n", clip->path, err ); - _vg_end_temp_frame( temp_frame ); - return 0; - } - - /* only mono is supported in uncompressed */ - u32 length_samples = stb_vorbis_stream_length_in_samples( decoder ), - data_size = length_samples * sizeof(i16); - - clip->any_data = vg_stack_allocate( stack, data_size, 8, NULL ); - clip->size = length_samples; - int read_samples = stb_vorbis_get_samples_i16_downmixed( decoder, clip->any_data, length_samples ); - _vg_end_temp_frame( temp_frame ); - - if( read_samples == length_samples ) - return 1; - else - { - vg_error( "Decode error, read_samples did not match length_samples\n" ); - return 0; - } - } -} - -VG_TIER_2 u32 vg_audio_clip_loadn( audio_clip *arr, u32 count, vg_stack_allocator *stack ) -{ - u32 succeeded = 0; - for( u32 i=0; i 0) && (id <= AUDIO_CHANNELS) ); - return &_vg_audio.channels[ id-1 ]; -} - -static struct audio_channel_controls *get_audio_channel_controls( audio_channel_id id ) -{ - vg_audio_assert_lock(); - VG_ASSERT( (id > 0) && (id <= AUDIO_CHANNELS) ); - return &_vg_audio.channels[ id-1 ].controls; -} - -static struct audio_channel_state *get_audio_channel_state( audio_channel_id id ) -{ - VG_ASSERT( (id > 0) && (id <= AUDIO_CHANNELS) ); - return &_vg_audio.channels[ id-1 ].state; -} - -static audio_lfo *get_audio_lfo( audio_channel_id lfo_id ) -{ - VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); - return &_vg_audio.lfos[ lfo_id-1 ]; -} - -static struct audio_lfo_controls *get_audio_lfo_controls( audio_channel_id lfo_id ) -{ - vg_audio_assert_lock(); - VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); - return &_vg_audio.lfos[ lfo_id-1 ].controls; -} - -static struct audio_lfo_state *get_audio_lfo_state( audio_channel_id lfo_id ) -{ - VG_ASSERT( (lfo_id > 0) && (lfo_id <= AUDIO_LFOS) ); - return &_vg_audio.lfos[ lfo_id-1 ].state; -} - -static void audio_decode_uncompressed_mono( i16 *src, u32 count, f32 *dst ) -{ - for( u32 i=0; istage == k_channel_stage_none ) - { - channel->stage = k_channel_stage_allocation; - channel->ui_name[0] = 0; - channel->ui_colour = 0x00333333; - channel->group = 0; - channel->clip = NULL; - channel->ui_volume = 0; - channel->ui_pan = 0; - channel->ui_spacial_volume = 0; - channel->ui_spacial_pan = 0; - channel->ui_activity = k_channel_activity_wake; - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->flags = 0x00; - controls->volume_target = AUDIO_VOLUME_100; - controls->volume_slew_rate_per_sample = (f64)AUDIO_VOLUME_100 / (0.1*44100.0); - controls->pan_target = 0; - controls->pan_slew_rate_per_sample = (f64)AUDIO_PAN_RIGHT_100 / (0.1*44100.0); - controls->sampling_rate_multiplier = 1.0f; - controls->lfo_id = 0; - controls->lfo_attenuation_amount = 0.0f; - controls->pause = 0; - v4_copy( (v4f){0,0,0,1}, controls->spacial_falloff ); - - struct audio_channel_state *state = get_audio_channel_state( id ); - state->activity = k_channel_activity_wake; - state->loaded_clip_length = 0; - state->decoder_handle.bird = NULL; - state->decoder_handle.vorbis = NULL; - state->cursor = 0; - state->volume = AUDIO_VOLUME_100; - state->pan = 0; - state->spacial_volume = 0; - state->spacial_pan = 0; - state->spacial_warm = 0; - return id; - } - } - - return 0; -} - -void vg_audio_set_channel_clip( audio_channel_id id, audio_clip *clip ) -{ - vg_audio_assert_lock(); - - audio_channel *channel = get_audio_channel( id ); - VG_ASSERT( channel->stage == k_channel_stage_allocation ); - VG_ASSERT( channel->clip == NULL ); - - channel->clip = clip; - - u32 audio_format = channel->clip->flags & AUDIO_FLAG_FORMAT; - if( audio_format == k_audio_format_bird ) - strcpy( channel->ui_name, "[array]" ); - else if( audio_format == k_audio_format_gen ) - strcpy( channel->ui_name, "[program]" ); - else - vg_strncpy( clip->path, channel->ui_name, 32, k_strncpy_always_add_null ); -} - -void vg_audio_set_channel_group( audio_channel_id id, u16 group ) -{ - vg_audio_assert_lock(); - - audio_channel *channel = get_audio_channel( id ); - VG_ASSERT( channel->stage == k_channel_stage_allocation ); - VG_ASSERT( channel->group == 0 ); - channel->group = group; - if( group ) - channel->ui_colour = (((u32)group * 29986577) & 0x00ffffff) | 0xff000000; -} - -u32 vg_audio_count_channels_in_group( u16 group ) -{ - vg_audio_assert_lock(); - - u32 count = 0; - for( int id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - if( channel->stage != k_channel_stage_none ) - { - if( channel->group == group ) - count ++; - } - } - - return count; -} - -audio_channel_id vg_audio_get_first_active_channel_in_group( u16 group ) -{ - vg_audio_assert_lock(); - for( int id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - if( (channel->stage != k_channel_stage_none) && (channel->group == group) ) - return id; - } - return 0; -} - -void vg_audio_sidechain_lfo_to_channel( audio_channel_id id, audio_channel_id lfo_id, f32 amount ) -{ - vg_audio_assert_lock(); - - audio_lfo *lfo = get_audio_lfo( lfo_id ); - VG_ASSERT( lfo->stage == k_channel_stage_active ); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->lfo_id = lfo_id; - controls->lfo_attenuation_amount = amount; -} - -void vg_audio_add_channel_flags( audio_channel_id id, u32 flags ) -{ - vg_audio_assert_lock(); - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->flags |= flags; -} - -void vg_audio_set_channel_spacial_falloff( audio_channel_id id, v3f co, f32 range ) -{ - vg_audio_assert_lock(); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - v3_copy( co, controls->spacial_falloff ); - controls->spacial_falloff[3] = range == 0.0f? 1.0f: 1.0f/range; - - vg_audio_add_channel_flags( id, AUDIO_FLAG_SPACIAL_3D ); -} - -void vg_audio_sync_ui_master_controls(void) -{ - vg_audio_assert_lock(); - _vg_audio.controls.volume_target = ((f64)AUDIO_VOLUME_100) * _vg_audio.master_volume_ui; - _vg_audio.controls.dsp_enabled = _vg_audio.dsp_enabled_ui; -} - -void vg_audio_set_channel_volume( audio_channel_id id, f64 volume, bool instant ) -{ - vg_audio_assert_lock(); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->volume_target = ((f64)AUDIO_VOLUME_100) * volume; - - if( instant ) - { - audio_channel *channel = get_audio_channel( id ); - VG_ASSERT( channel->stage == k_channel_stage_allocation ); - - struct audio_channel_state *state = get_audio_channel_state( id ); - state->volume = controls->volume_target; - } -} - -void vg_audio_set_channel_volume_slew_duration( audio_channel_id id, f64 length_seconds ) -{ - vg_audio_assert_lock(); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->volume_slew_rate_per_sample = (f64)AUDIO_VOLUME_100 / (length_seconds * 44100.0); -} - -void vg_audio_set_channel_pan( audio_channel_id id, f64 pan, bool instant ) -{ - vg_audio_assert_lock(); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->pan_target = ((f64)AUDIO_PAN_RIGHT_100) * pan; - - if( instant ) - { - audio_channel *channel = get_audio_channel( id ); - VG_ASSERT( channel->stage == k_channel_stage_allocation ); - - struct audio_channel_state *state = get_audio_channel_state( id ); - state->pan = controls->pan_target; - } -} - -void vg_audio_set_channel_pan_slew_duration( audio_channel_id id, f64 length_seconds ) -{ - vg_audio_assert_lock(); - - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->pan_slew_rate_per_sample = (f64)AUDIO_PAN_RIGHT_100 / (length_seconds * 44100.0); -} - -void vg_audio_set_channel_sampling_rate( audio_channel_id id, f32 rate ) -{ - vg_audio_assert_lock(); - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->sampling_rate_multiplier = rate; -} - -void vg_audio_start_channel( audio_channel_id id ) -{ - vg_audio_assert_lock(); - - audio_channel *channel = get_audio_channel( id ); - VG_ASSERT( channel->stage == k_channel_stage_allocation ); - VG_ASSERT( channel->clip ); - channel->stage = k_channel_stage_active; -} - -audio_channel_id vg_audio_crossfade( audio_channel_id id, audio_clip *new_clip, f32 transition_seconds ) -{ - vg_audio_assert_lock(); - - audio_channel *channel = get_audio_channel( id ); - audio_channel_id new_id = 0; - if( new_clip ) - { - new_id = vg_audio_get_first_idle_channel(); - if( new_id ) - { - vg_audio_set_channel_clip( new_id, new_clip ); - vg_audio_set_channel_volume_slew_duration( new_id, transition_seconds ); - vg_audio_set_channel_volume( new_id, 1.0, 0 ); - vg_audio_set_channel_group( new_id, channel->group ); - - struct audio_channel_controls *existing_controls = get_audio_channel_controls( id ), - *new_controls = get_audio_channel_controls( new_id ); - - memcpy( new_controls, existing_controls, sizeof( struct audio_channel_controls ) ); - vg_audio_start_channel( new_id ); - } - } - - vg_audio_set_channel_volume_slew_duration( id, transition_seconds ); - vg_audio_set_channel_volume( id, 0.0, 0 ); - vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED ); - - return new_id; -} - -bool vg_audio_is_channel_using_clip( audio_channel_id id, audio_clip *clip ) -{ - vg_audio_assert_lock(); - audio_channel *channel = get_audio_channel( id ); - - if( channel->clip == clip ) return 1; - else return 0; -} - -void vg_audio_fadeout_flagged_audio( u32 flag, f32 length ) -{ - vg_audio_assert_lock(); - for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - if( channel->stage != k_channel_stage_none ) - { - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - if( controls->flags & flag ) - { - if( _vg_audio.working ) - vg_audio_crossfade( id, NULL, 1.0f ); - else - channel->stage = k_channel_stage_none; - } - } - } -} - -bool vg_audio_flagged_stopped( u32 flag ) -{ - vg_audio_lock(); - for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - if( channel->stage != k_channel_stage_none ) - { - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - if( controls->flags & flag ) - { - vg_audio_unlock(); - return 0; - } - } - } - vg_audio_unlock(); - return 1; -} - -void vg_audio_set_channel_pause( audio_channel_id id, bool pause ) -{ - vg_audio_assert_lock(); - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - controls->pause = pause; -} - -void vg_audio_set_flagged_pause( u32 flag, bool pause ) -{ - vg_audio_assert_lock(); - for( u32 id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - if( channel->stage != k_channel_stage_none ) - { - struct audio_channel_controls *controls = get_audio_channel_controls( id ); - if( controls->flags & flag ) - vg_audio_set_channel_pause( id, pause ); - } - } -} - -void vg_audio_oneshot_3d( audio_clip *clip, v3f co, f32 range, f32 volume, u16 group, u32 flags ) -{ - vg_audio_assert_lock(); - audio_channel_id id = vg_audio_get_first_idle_channel(); - - if( id ) - { - vg_audio_set_channel_clip( id, clip ); - vg_audio_set_channel_spacial_falloff( id, co, range ); - vg_audio_set_channel_group( id, group ); - vg_audio_set_channel_volume( id, volume, 1 ); - vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED | flags ); - vg_audio_start_channel( id ); - } -} - -void vg_audio_oneshot( audio_clip *clip, f32 volume, f32 pan, u16 group, u32 flags ) -{ - vg_audio_assert_lock(); - audio_channel_id id = vg_audio_get_first_idle_channel(); - - if( id ) - { - vg_audio_set_channel_clip( id, clip ); - vg_audio_set_channel_group( id, group ); - vg_audio_set_channel_volume( id, volume, 1 ); - vg_audio_set_channel_pan( id, pan, 1 ); - vg_audio_add_channel_flags( id, AUDIO_FLAG_RELINQUISHED | flags ); - vg_audio_start_channel( id ); - } -} - - - -/* lfos - * ---------------------------------------------------------------------------------------- */ - -audio_channel_id vg_audio_get_first_idle_lfo(void) -{ - vg_audio_assert_lock(); - - for( int id=1; id<=AUDIO_LFOS; id ++ ) - { - audio_lfo *lfo = get_audio_lfo( id ); - - if( lfo->stage == k_channel_stage_none ) - { - lfo->stage = k_channel_stage_allocation; - - const u32 default_lfo_period = 44100; - - struct audio_lfo_controls *controls = get_audio_lfo_controls( id ); - controls->period_in_samples = default_lfo_period; - controls->wave_type = k_lfo_triangle; - controls->polynomial_coefficient = 0.0f; - controls->flags = 0x00; - - struct audio_lfo_state *state = get_audio_lfo_state( id ); - state->time = 0; - state->last_period_in_samples = default_lfo_period; - state->frame_reference_count = 0; - state->time_at_frame_start = 0; - return id; - } - } - - return 0; -} - -void vg_audio_set_lfo_polynomial_bipolar( audio_channel_id lfo_id, f32 coefficient ) -{ - vg_audio_assert_lock(); - - struct audio_lfo_controls *controls = get_audio_lfo_controls( lfo_id ); - controls->polynomial_coefficient = coefficient; - controls->sqrt_polynomial_coefficient = sqrtf(coefficient); - controls->wave_type = k_lfo_polynomial_bipolar; -} - -void vg_audio_set_lfo_frequency( audio_channel_id lfo_id, f32 freq ) -{ - vg_audio_assert_lock(); - - struct audio_lfo_controls *controls = get_audio_lfo_controls( lfo_id ); - u32 length = 44100.0f / freq; - controls->period_in_samples = length; - - audio_lfo *lfo = get_audio_lfo( lfo_id ); - if( lfo->stage == k_channel_stage_allocation ) - { - struct audio_lfo_state *state = get_audio_lfo_state( lfo_id ); - state->last_period_in_samples = length; - } -} - -void vg_audio_start_lfo( audio_channel_id lfo_id ) -{ - vg_audio_assert_lock(); - audio_lfo *lfo = get_audio_lfo( lfo_id ); - lfo->stage = k_channel_stage_active; -} - -static void audio_channel_get_samples( audio_channel_id id, struct audio_channel_controls *controls, - u32 count, f32 *out_stereo ) -{ - _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:decode" ); - - u32 remaining = count; - u32 buffer_pos = 0; - - audio_channel *channel = get_audio_channel( id ); - struct audio_channel_state *state = get_audio_channel_state( id ); - u32 format = channel->clip->flags & AUDIO_FLAG_FORMAT; - - while( remaining ) - { - u32 samples_this_run = VG_MIN( remaining, state->loaded_clip_length - state->cursor ); - remaining -= samples_this_run; - - f32 *dst = &out_stereo[ buffer_pos * 2 ]; - - if( format == k_audio_format_stereo ) - { - for( u32 i=0; idecoder_handle.vorbis, - dst, samples_this_run ); - if( read_samples != samples_this_run ) - { - vg_warn( "Invalid samples read (%s)\n", channel->clip->path ); - - for( u32 i=0; idecoder_handle.bird, dst, samples_this_run ); - } - else if( format == k_audio_format_gen ) - { - void (*fn)( void *data, f32 *buf, u32 count ) = channel->clip->generative_function; - fn( channel->clip->any_data, dst, samples_this_run ); - } - else - { - i16 *src_buffer = channel->clip->any_data, - *src = &src_buffer[ state->cursor ]; - audio_decode_uncompressed_mono( src, samples_this_run, dst ); - } - - state->cursor += samples_this_run; - buffer_pos += samples_this_run; - - if( (controls->flags & AUDIO_FLAG_LOOP) && remaining ) - { - if( format == k_audio_format_vorbis ) - stb_vorbis_seek_start( state->decoder_handle.vorbis ); - else if( format == k_audio_format_bird ) - synth_bird_reset( state->decoder_handle.bird ); - - state->cursor = 0; - continue; - } - else - break; - } - - while( remaining ) - { - out_stereo[ buffer_pos*2 + 0 ] = 0.0f; - out_stereo[ buffer_pos*2 + 1 ] = 0.0f; - buffer_pos ++; - remaining --; - } - - _vg_profiler_exit_block( _vg_audio.profiler ); -} - -static f32 audio_lfo_get_sample( audio_channel_id lfo_id, struct audio_lfo_controls *controls ) -{ - struct audio_lfo_state *state = get_audio_lfo_state( lfo_id ); - state->time ++; - - if( state->time >= controls->period_in_samples ) - state->time = 0; - - f32 t = state->time; - t /= (f32)controls->period_in_samples; - - if( controls->wave_type == k_lfo_polynomial_bipolar ) - { - /* - * # - * # # - * # # - * # # - * ### # ### - * ## # - * # # - * # # - * ## - */ - - t *= 2.0f; - t -= 1.0f; - - return (( 2.0f * controls->sqrt_polynomial_coefficient * t ) / - /* --------------------------------------- */ - ( 1.0f + controls->polynomial_coefficient * t*t )) * (1.0f-fabsf(t)); - } - else - return 0.0f; -} - -static void audio_slew_i32( i32 *value, i32 target, i32 rate ) -{ - i32 sign = target - *value; - if( sign == 0 ) - return; - - sign = sign>0? 1: -1; - i32 c = *value + sign*rate; - - if( target*sign < c*sign ) *value = target; - else *value = c; -} - -static void audio_channel_mix( audio_channel_id id, - struct audio_channel_controls *controls, - struct audio_master_controls *master_controls, f32 *inout_buffer ) -{ - struct audio_channel_state *state = get_audio_channel_state( id ); - - bool is_3d = controls->flags & AUDIO_FLAG_SPACIAL_3D? 1: 0; - bool use_doppler = controls->flags & AUDIO_FLAG_NO_DOPPLER? 0: 1; - - f32 frame_sample_rate = controls->sampling_rate_multiplier; - - i32 spacial_volume_target = 0, - spacial_pan_target = 0; - - if( is_3d ) - { - v3f delta; - v3_sub( controls->spacial_falloff, master_controls->listener_position, delta ); - - f32 dist = v3_length( delta ); - - if( dist <= 0.01f ) - { - spacial_pan_target = 0; - spacial_volume_target = AUDIO_VOLUME_100; - } - else if( dist > 20000.0f || !vg_validf( dist ) ) - { - spacial_pan_target = 0; - spacial_volume_target = 0; - } - else - { - f32 vol = vg_maxf( 0.0f, 1.0f - controls->spacial_falloff[3]*dist ); - vol = powf( vol, 5.0f ); - spacial_volume_target = (f64)vg_clampf( vol, 0.0f, 1.0f ) * (f64)AUDIO_VOLUME_100 * 0.5; - - v3_muls( delta, 1.0f/dist, delta ); - f32 pan = v3_dot( master_controls->listener_right_ear_direction, delta ); - spacial_pan_target = (f64)vg_clampf( pan, -1.0f, 1.0f ) * (f64)AUDIO_PAN_RIGHT_100; - - if( use_doppler ) - { - const float vs = 323.0f; - - f32 dv = v3_dot( delta, master_controls->listener_velocity ); - f32 doppler = (vs+dv)/vs; - doppler = vg_clampf( doppler, 0.6f, 1.4f ); - - if( fabsf(doppler-1.0f) > 0.01f ) - frame_sample_rate *= doppler; - } - } - - if( !state->spacial_warm ) - { - state->spacial_volume = spacial_volume_target; - state->spacial_pan = spacial_pan_target; - state->spacial_warm = 1; - } - } - - u32 buffer_length = AUDIO_MIX_FRAME_SIZE; - if( frame_sample_rate != 1.0f ) - { - float l = ceilf( (float)(AUDIO_MIX_FRAME_SIZE) * frame_sample_rate ); - buffer_length = l+1; - } - - f32 samples[ AUDIO_MIX_FRAME_SIZE*2 * 2 ]; - audio_channel_get_samples( id, controls, buffer_length, samples ); - - _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:mixing" ); - - /* TODO: Slew this */ - f64 master_volume = (f64)_vg_audio.state.volume / (f64)AUDIO_VOLUME_100; - - for( u32 j=0; jvolume, controls->volume_target, controls->volume_slew_rate_per_sample ); - audio_slew_i32( &state->pan, controls->pan_target, controls->pan_slew_rate_per_sample ); - - f64 v_c = ((f64)state->volume / (f64)AUDIO_VOLUME_100) * master_volume; - - if( controls->lfo_id ) - { - struct audio_lfo_state *state = get_audio_lfo_state( controls->lfo_id ); - f32 lfo_value = audio_lfo_get_sample( controls->lfo_id, state->controls ); - v_c *= 1.0 + lfo_value * controls->lfo_attenuation_amount; - } - - f64 v_l = v_c*v_c, - v_r = v_c*v_c; - - if( is_3d ) - { - const i32 vol_rate = (f64)AUDIO_VOLUME_100 / (0.05 * 44100.0), - pan_rate = (f64)AUDIO_PAN_RIGHT_100 / (0.05 * 44100.0); - - audio_slew_i32( &state->spacial_volume, spacial_volume_target, vol_rate ); - audio_slew_i32( &state->spacial_pan, spacial_pan_target, pan_rate ); - - f64 v_s = (f64)state->spacial_volume / (f64)AUDIO_VOLUME_100, - v_p = (f64)state->spacial_pan / (f64)AUDIO_PAN_RIGHT_100; - - v_l *= v_s * (1.0-v_p); - v_r *= v_s * (1.0+v_p); - } - - f32 s_l, s_r; - if( frame_sample_rate != 1.0f ) - { - /* absolutely garbage resampling, but it will do - */ - f32 sample_index = frame_sample_rate * (f32)j; - f32 t = vg_fractf( sample_index ); - - u32 i0 = floorf( sample_index ), - i1 = i0+1; - - s_l = samples[ i0*2+0 ]*(1.0f-t) + samples[ i1*2+0 ]*t; - s_r = samples[ i0*2+1 ]*(1.0f-t) + samples[ i1*2+1 ]*t; - } - else - { - s_l = samples[ j*2+0 ]; - s_r = samples[ j*2+1 ]; - } - - inout_buffer[ j*2+0 ] += s_l * v_l; - inout_buffer[ j*2+1 ] += s_r * v_r; - } - - _vg_profiler_exit_block( _vg_audio.profiler ); -} - - -static void _vg_audio_mixer( void *user, u8 *stream, int byte_count ) -{ - _vg_profiler_tick( _vg_audio.profiler ); - - int sample_count = byte_count/(2*sizeof(f32)); - - f32 *output_stereo = (f32 *)stream; - for( int i=0; istage == k_channel_stage_active ) - { - struct audio_channel_controls *controls = get_audio_channel_controls(id); - if( controls->pause ) - { - channel->ui_activity = k_channel_activity_paused; - continue; - } - - active_channel_list[ active_channel_count ] = id; - memcpy( &channel_controls[ active_channel_count ], controls, sizeof( struct audio_channel_controls ) ); - active_channel_count ++; - } - } - for( u32 id=1; id<=AUDIO_LFOS; id ++ ) - { - audio_lfo *lfo = get_audio_lfo( id ); - if( lfo->stage == k_channel_stage_active ) - { - struct audio_lfo_controls *local_controls = &lfo_controls[ active_lfo_count ]; - active_lfo_list[ active_lfo_count ] = id; - memcpy( local_controls, get_audio_lfo_controls(id), sizeof(struct audio_lfo_controls) ); - active_lfo_count ++; - - struct audio_lfo_state *state = get_audio_lfo_state(id); - state->controls = local_controls; - } - } - dsp_update_tunings(); - vg_audio_unlock(); - - /* init step */ - master_state->volume = master_controls.volume_target; - master_state->volume_at_frame_start = master_controls.volume_target; - - for( u32 i=0; iactivity == k_channel_activity_wake ) - { - audio_channel *channel = get_audio_channel( id ); - - u32 format = channel->clip->flags & AUDIO_FLAG_FORMAT; - if( format == k_audio_format_vorbis ) - { - /* Setup vorbis decoder */ - u8 *buf = (u8*)_vg_audio.decoding_buffer, - *loc = &buf[AUDIO_DECODE_SIZE*id]; - - stb_vorbis_alloc alloc = { - .alloc_buffer = (char *)loc, - .alloc_buffer_length_in_bytes = AUDIO_DECODE_SIZE - }; - - int err; - stb_vorbis *decoder = stb_vorbis_open_memory( channel->clip->any_data, channel->clip->size, &err, &alloc ); - - if( !decoder ) - { - vg_error( "stb_vorbis_open_memory failed on '%s' (%d)\n", channel->clip->path, err ); - channel_state->activity = k_channel_activity_error; - } - else - { - channel_state->loaded_clip_length = stb_vorbis_stream_length_in_samples( decoder ); - channel_state->decoder_handle.vorbis = decoder; - channel_state->activity = k_channel_activity_playing; - } - } - else if( format == k_audio_format_bird ) - { - u8 *buf = (u8*)_vg_audio.decoding_buffer; - struct synth_bird *loc = (void *)&buf[AUDIO_DECODE_SIZE*id]; - - memcpy( loc, channel->clip->any_data, channel->clip->size ); - synth_bird_reset( loc ); - - channel_state->decoder_handle.bird = loc; - channel_state->loaded_clip_length = synth_bird_get_length_in_samples( loc ); - channel_state->activity = k_channel_activity_playing; - } - else if( format == k_audio_format_stereo ) - { - channel_state->loaded_clip_length = channel->clip->size / 2; - channel_state->activity = k_channel_activity_playing; - } - else if( format == k_audio_format_gen ) - { - channel_state->loaded_clip_length = 0xffffffff; - channel_state->activity = k_channel_activity_playing; - } - else - { - channel_state->loaded_clip_length = channel->clip->size; - channel_state->activity = k_channel_activity_playing; - } - } - } - - for( u32 i=0; iperiod_in_samples != state->last_period_in_samples ) - { - state->last_period_in_samples = controls->period_in_samples; - f64 t = state->time; - t/= (f64)controls->period_in_samples; - state->time = (f64)controls->period_in_samples * t; - } - - state->time_at_frame_start = state->time; - state->frame_reference_count = 0; - } - - /* mix step */ - for( u32 dry_layer=0; dry_layer<=1; dry_layer ++ ) - { - for( u32 i=0; iactivity == k_channel_activity_playing ) - { - if( master_controls.dsp_enabled ) - { - if( controls->flags & AUDIO_FLAG_NO_DSP ) - { - if( !dry_layer ) - continue; - } - else - { - if( dry_layer ) - continue; - } - } - - if( controls->lfo_id ) - { - struct audio_lfo_state *lfo_state = get_audio_lfo_state( controls->lfo_id ); - lfo_state->time = lfo_state->time_at_frame_start; - lfo_state->frame_reference_count ++; - } - - u32 remaining = sample_count, - subpos = 0; - - while( remaining ) - { - audio_channel_mix( id, controls, &master_controls, output_stereo+subpos ); - remaining -= AUDIO_MIX_FRAME_SIZE; - subpos += AUDIO_MIX_FRAME_SIZE*2; - } - - if( (state->cursor >= state->loaded_clip_length) && !(controls->flags & AUDIO_FLAG_LOOP) ) - state->activity = k_channel_activity_end; - } - } - - if( master_controls.dsp_enabled ) - { - if( !dry_layer ) - { - _vg_profiler_enter_block( _vg_audio.profiler, "vg_audio:dsp/effects" ); - for( int i=0; iui_activity = state->activity; - channel->ui_volume = state->volume; - channel->ui_pan = state->pan; - channel->ui_spacial_volume = state->spacial_volume; - channel->ui_spacial_pan = state->spacial_pan; - - if( controls->flags & AUDIO_FLAG_RELINQUISHED ) - { - bool die = 0; - if( state->activity == k_channel_activity_end ) die = 1; - if( state->activity == k_channel_activity_error ) die = 1; - if( state->volume == 0 ) die = 1; - - if( die ) - { - channel->stage = k_channel_stage_none; - } - } - } - - _vg_audio.samples_written_last_audio_frame = sample_count; - vg_audio_unlock(); -} - -/* - * Debugging - */ -struct vg_audio_view_data -{ - i32 view_3d; -}; - -static f32 audio_volume_integer_to_float( i32 volume ) -{ - return (f32)volume / (f32)AUDIO_VOLUME_100; -} - -static void cb_vg_audio_view( ui_context *ctx, ui_rect rect, struct vg_magi_panel *magi ) -{ - struct vg_audio_view_data *vd = magi->data; - - ui_rect left, panel; - ui_split( rect, k_ui_axis_v, 256, 2, left, panel ); - ui_checkbox( ctx, left, "3D labels", &vd->view_3d ); - - vg_audio_lock(); - char perf[128]; - ui_rect overlap_buffer[ AUDIO_CHANNELS ]; - u32 overlap_length = 0; - - /* Draw audio stack */ - for( int id=1; id<=AUDIO_CHANNELS; id ++ ) - { - audio_channel *channel = get_audio_channel( id ); - - ui_rect row; - ui_split( panel, k_ui_axis_h, 18, 1, row, panel ); - - bool show_row = ui_clip( rect, row, row ); - - if( channel->stage == k_channel_stage_none ) - { - if( show_row ) - ui_fill( ctx, row, 0x50333333 ); - } - else if( channel->stage == k_channel_stage_allocation ) - { - if( show_row ) - ui_fill( ctx, row, 0x50ff3333 ); - } - else if( channel->stage == k_channel_stage_active ) - { - if( show_row ) - { - char buf[256]; - vg_str str; - vg_strnull( &str, buf, sizeof(buf) ); - vg_strcati64r( &str, id, 10, 2, ' ' ); - - if( channel->group ) - { - vg_strcat( &str, " grp" ); - vg_strcati64r( &str, channel->group, 10, 6, ' ' ); - } - else - vg_strcat( &str, " " ); - - vg_strcat( &str, " flags:" ); - u32 flags = get_audio_channel_controls( id )->flags; - vg_strcatch( &str, (flags & AUDIO_FLAG_RELINQUISHED)? 'R': '_' ); - vg_strcatch( &str, (flags & AUDIO_FLAG_SPACIAL_3D)? 'S': '_' ); - vg_strcatch( &str, (flags & AUDIO_FLAG_WORLD)? 'W': '_' ); - vg_strcatch( &str, (flags & AUDIO_FLAG_NO_DOPPLER)? '_':'D' ); - vg_strcatch( &str, (flags & AUDIO_FLAG_NO_DSP)? '_':'E' ); - vg_strcatch( &str, (flags & AUDIO_FLAG_LOOP)? 'L':'_' ); - - const char *formats[] = - { - " mono ", - " stereo ", - " vorbis ", - " none0 ", - " none1 ", - " none2 ", - " none3 ", - " none4 ", - "synth:bird", - " none5 ", - " none6 ", - " none7 ", - " none8 ", - " none9 ", - " none10 ", - " none11 ", - }; - u32 format_index = (channel->clip->flags & AUDIO_FLAG_FORMAT)>>9; - vg_strcat( &str, " format:" ); - vg_strcat( &str, formats[format_index] ); - - const char *activties[] = - { - "wake ", - "play ", - "pause", - "end ", - "error" - }; - vg_strcat( &str, " " ); - vg_strcat( &str, activties[channel->ui_activity] ); - - vg_strcat( &str, " " ); - f32 volume = (f32)channel->ui_volume / (f32)AUDIO_VOLUME_100; - vg_strcati64r( &str, volume * 100.0f, 10, 3, ' ' ); - vg_strcatch( &str, '%' ); - - vg_strcat( &str, " " ); - vg_strcat( &str, channel->ui_name ); - - ui_rect row_l, row_r; - ui_split( row, k_ui_axis_v, 32, 2, row_l, row_r ); - - ui_rect indicator_l, indicator_r; - ui_split_ratio( row_l, k_ui_axis_v, 0.5f, 1, indicator_l, indicator_r ); - - ui_fill( ctx, indicator_l, 0xff000000 ); - if( volume > 0.01f ) - { - f32 h = volume * (f32)indicator_l[3]; - ui_rect vol_bar = { indicator_l[0], indicator_l[1] + indicator_l[3] - h, indicator_l[2], h }; - ui_fill( ctx, vol_bar, 0xff00ff00 ); - - if( flags & AUDIO_FLAG_SPACIAL_3D ) - { - f32 h = ((f32)channel->ui_spacial_volume / (f32)AUDIO_VOLUME_100) * (f32)indicator_l[3]; - ui_rect space_bar = { indicator_l[0], indicator_l[1] + indicator_l[3] - h, indicator_l[2], 1 }; - ui_fill( ctx, space_bar, 0xffffffff ); - } - } - - f32 pan = (f32)channel->ui_pan / (f32)AUDIO_PAN_RIGHT_100; - ui_fill( ctx, indicator_r, 0xff111111 ); - f32 midpoint = (f32)indicator_r[0] + (f32)indicator_r[2] * 0.5f; - - f32 pan_abs = fabsf(pan); - if( pan_abs > 0.01f ) - { - ui_rect bar = { midpoint,indicator_r[1], pan_abs * (f32)indicator_r[2] * 0.5f, indicator_r[3] }; - - if( pan < 0.0f ) - bar[0] -= (f32)bar[2]; - - ui_fill( ctx, bar, 0xff00aaff ); - } - - if( flags & AUDIO_FLAG_SPACIAL_3D ) - { - f32 w = ((f32)channel->ui_spacial_pan / (f32)AUDIO_PAN_RIGHT_100) * (f32)indicator_r[2] * 0.5f; - ui_rect space_bar = { midpoint+w, indicator_r[1], 1, indicator_r[3] }; - ui_fill( ctx, space_bar, 0xffffffff ); - } - - ui_fill( ctx, row_r, 0xa0000000 | channel->ui_colour ); - ui_text( ctx, row_r, buf, 1, k_ui_align_middle_left, 0 ); - } - } - -#if 0 -#ifdef VG_3D - if( vd->view_3d && (ch->flags & AUDIO_FLAG_SPACIAL_3D) ) - { - v4f wpos; - v3_copy( ch->spacial_falloff, wpos ); - - wpos[3] = 1.0f; - m4x4_mulv( vg.pv, wpos, wpos ); - - if( wpos[3] > 0.0f ) - { - v2_muls( wpos, (1.0f/wpos[3]) * 0.5f, wpos ); - v2_add( wpos, (v2f){ 0.5f, 0.5f }, wpos ); - - ui_rect wr; - wr[0] = vg_clampf(wpos[0] * vg.window_x, -32000.0f,32000.0f); - wr[1] = vg_clampf((1.0f-wpos[1]) * vg.window_y,-32000.0f,32000.0f); - wr[2] = 1000; - wr[3] = 17; - - for( int j=0; j<12; j++ ) - { - int collide = 0; - for( int k=0; k= wk[0])) && - ((wr[1] <= wk[1]+wk[3]) && (wr[1]+wr[3] >= wk[1])) ) - { - collide = 1; - break; - } - } - - if( !collide ) - break; - else - wr[1] += 18; - } - - ui_text( ctx, wr, perf, 1, k_ui_align_middle_left, 0 ); - rect_copy( wr, overlap_buffer[ overlap_length ++ ] ); - } - } -#endif -#endif - } - - vg_audio_unlock(); -} - -static void cb_vg_audio_close( struct vg_magi_panel *me ) -{ - vg_audio_lock(); - _vg_audio.inspector_open = 0; - vg_audio_unlock(); - free( me->data ); -} - -static int cmd_vg_audio( int argc, const char *argv[] ) -{ - vg_audio_lock(); - if( _vg_audio.inspector_open ) - { - vg_error( "Only 1 audio inspector at a time.\n" ); - vg_audio_unlock(); - return 0; - } - - _vg_audio.inspector_open = 1; - vg_audio_unlock(); - ui_px w = 800, h=8*18; - struct vg_magi_panel *magi = _vg_magi_open( w,h, VG_MAGI_ALL ); - magi->title = "Audio inspector"; - - struct vg_audio_view_data *vd = malloc(sizeof(struct vg_audio_view_data)); - vd->view_3d = 0; - - magi->data = vd; - magi->ui_cb = cb_vg_audio_view; - magi->close_cb = cb_vg_audio_close; - return 1; -} - -VG_API void _vg_audio_register(void) -{ - vg_console_reg_cmd( "vg_audio", cmd_vg_audio, NULL ); - vg_console_reg_var( "volume", &_vg_audio.master_volume_ui, k_var_dtype_f32, VG_VAR_PERSISTENT ); - vg_console_reg_var( "vg_dsp", &_vg_audio.dsp_enabled_ui, k_var_dtype_i32, VG_VAR_PERSISTENT ); - vg_console_reg_var( "vg_audio_device", &_vg_audio.device_choice, k_var_dtype_str, VG_VAR_PERSISTENT ); -} - -void vg_audio_device_init(void) -{ - SDL_AudioSpec spec_desired, spec_got; - spec_desired.callback = _vg_audio_mixer; - spec_desired.channels = 2; - spec_desired.format = AUDIO_F32; - spec_desired.freq = 44100; - spec_desired.padding = 0; - spec_desired.samples = AUDIO_FRAME_SIZE; - spec_desired.silence = 0; - spec_desired.size = 0; - spec_desired.userdata = NULL; - - _vg_audio.sdl_output_device = SDL_OpenAudioDevice( _vg_audio.device_choice.buffer, 0, - &spec_desired, &spec_got, 0 ); - - vg_info( "Start audio device (%u, F32, %u) @%s\n", spec_desired.freq, AUDIO_FRAME_SIZE, - _vg_audio.device_choice.buffer ); - - if( _vg_audio.sdl_output_device ) - { - SDL_PauseAudioDevice( _vg_audio.sdl_output_device, 0 ); - vg_success( "Unpaused device %d.\n", _vg_audio.sdl_output_device ); - _vg_audio.working = 1; - } - else - { - _vg_audio.working = 0; - vg_error( - "SDL_OpenAudioDevice failed. Your default audio device must support (or transcode from):\n" - " Sample rate: 44100 hz\n" - " Buffer size: 512 samples\n" - " Channels: 2\n" - " Format: s16 or f32\n" ); - } -} - -static void vg_audio_free(void) -{ - SDL_CloseAudioDevice( _vg_audio.sdl_output_device ); - _vg_audio.sdl_output_device = 0; -} - -VG_API void _vg_audio_init(void) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_SDL ) ); - - _vg_audio.profiler = _vg_profiler_create( "vg.audio", 1000.0f/20.0f ); - - /* fixed */ - u32 decode_size = AUDIO_DECODE_SIZE * AUDIO_CHANNELS; - _vg_audio.decoding_buffer = vg_stack_allocate( NULL, decode_size, 8, "Decoding buffers" ); - - struct audio_master_controls *master_controls = &_vg_audio.controls; - master_controls->dsp_enabled = _vg_audio.dsp_enabled_ui; - master_controls->volume_target = (f64)_vg_audio.master_volume_ui * (f64)AUDIO_VOLUME_100; - v3_copy( (v3f){1,0,0}, master_controls->listener_right_ear_direction ); - v3_zero( master_controls->listener_velocity ); - v3_zero( master_controls->listener_position ); - _vg_dsp_init(); - - _vg_audio.mutex = SDL_CreateMutex(); - if( _vg_audio.mutex == NULL ) - vg_fatal_error( "SDL2: %s\n", SDL_GetError() ); - vg_audio_device_init(); - - _vg_add_exit_function( vg_audio_free ); -} - -void vg_audio_preupdate(void) -{ - bool before_working = _vg_audio.working; - _vg_audio.working = 1; - if( _vg_audio.sdl_output_device == 0 ) - _vg_audio.working = 0; - else - if( SDL_GetAudioDeviceStatus( _vg_audio.sdl_output_device ) == SDL_AUDIO_STOPPED ) - _vg_audio.working = 0; -} diff --git a/vg_bvh.c b/vg_bvh.c deleted file mode 100644 index 9da3fad..0000000 --- a/vg_bvh.c +++ /dev/null @@ -1,292 +0,0 @@ -static void bh_update_bounds( bh_tree *bh, u32 inode ) -{ - bh_node *node = &bh->nodes[ inode ]; - box_init_inf( node->bbx ); - for( u32 i=0; icount; i++ ) - { - u32 idx = node->start+i; - bh->system->expand_bound( bh->user, node->bbx, idx ); - } -} - -static void bh_subdivide( bh_tree *bh, u32 inode ) -{ - bh_node *node = &bh->nodes[ inode ]; - if( node->count <= bh->max_per_leaf ) - return; - - v3f extent; - v3_sub( node->bbx[1], node->bbx[0], extent ); - - int axis = 0; - if( extent[1] > extent[0] ) axis = 1; - if( extent[2] > extent[axis] ) axis = 2; - - float split = node->bbx[0][axis] + extent[axis]*0.5f; - float avg = 0.0; - for( u32 t=0; tcount; t++ ){ - u32 idx = node->start+t; - avg += bh->system->item_centroid( bh->user, idx, axis ); - } - avg /= (float)node->count; - split = avg; - - - i32 i = node->start, - j = i + node->count-1; - - while( i <= j ) - { - f32 centroid = bh->system->item_centroid( bh->user, i, axis ); - if( centroid < split ) - i ++; - else - { - bh->system->item_swap( bh->user, i, j ); - j --; - } - } - - u32 left_count = i - node->start; - if( left_count == 0 || left_count == node->count ) - return; - - u32 il = bh->node_count ++, - ir = bh->node_count ++; - bh_node *lnode = &bh->nodes[il], - *rnode = &bh->nodes[ir]; - lnode->start = node->start; - lnode->count = left_count; - rnode->start = i; - rnode->count = node->count - left_count; - - node->il = il; - node->ir = ir; - node->count = 0; - - bh_update_bounds( bh, il ); - bh_update_bounds( bh, ir ); - bh_subdivide( bh, il ); - bh_subdivide( bh, ir ); -} - -static void bh_rebuild( bh_tree *bh, u32 item_count ) -{ - bh_node *root = &bh->nodes[0]; - bh->node_count = 1; - - root->il = 0; - root->ir = 0; - root->count = item_count; - root->start = 0; - - bh_update_bounds( bh, 0 ); - - if( item_count > 2 ) - bh_subdivide( bh, 0 ); -} - -VG_TIER_1 void bh_create( bh_tree *bh, bh_system *system, void *user, u32 item_count, - u32 max_per_leaf, vg_stack_allocator *stack ) -{ - vg_zero_mem( bh, sizeof(bh_tree) ); - - u32 alloc_count = VG_MAX( 1, item_count ); - i32 max_size = sizeof(bh_node)*(alloc_count*2-1); - bh->nodes = vg_stack_allocate( stack, max_size, 8, "BVH Tree" ); - bh->system = system; - bh->user = user; - bh->max_per_leaf = max_per_leaf; - bh_rebuild( bh, item_count ); - - i32 used_size = sizeof(bh_node) * bh->node_count; - vg_stack_extend_last( stack, used_size - max_size ); - vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) ); -} - -VG_TIER_0 void bh_debug_leaf( bh_tree *bh, bh_node *node ) -{ - vg_line_boxf( node->bbx, 0xff00ff00 ); - - if( bh->system->item_debug ) - { - for( u32 i=0; icount; i++ ) - { - u32 idx = node->start+i; - bh->system->item_debug( bh->user, idx ); - } - } -} - -/* - * Trace the bh tree all the way down to the leaf nodes where pos is inside - */ -VG_TIER_0 void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour ) -{ - bh_node *node = &bh->nodes[ inode ]; - if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) && - (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) ) - { - if( !node->count ) - { - vg_line_boxf( node->bbx, colour ); - bh_debug_trace( bh, node->il, pos, colour ); - bh_debug_trace( bh, node->ir, pos, colour ); - } - else - if( bh->system->item_debug ) - bh_debug_leaf( bh, node ); - } -} - -VG_TIER_0 void bh_iter_init_generic( i32 root, bh_iter *it ) -{ - it->stack[0].id = root; - it->stack[0].depth = 0; - it->depth = 0; - it->i = 0; -} - -VG_TIER_0 void bh_iter_init_box( i32 root, bh_iter *it, boxf box ) -{ - bh_iter_init_generic( root, it ); - it->query = k_bh_query_box; - box_copy( box, it->box.box ); -} - -VG_TIER_0 void bh_iter_init_ray( i32 root, bh_iter *it, v3f co, v3f dir, f32 max_dist ) -{ - bh_iter_init_generic( root, it ); - it->query = k_bh_query_ray; - v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir ); - v3_copy( co, it->ray.co ); - it->ray.max_dist = max_dist; -} - -VG_TIER_0 void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range ) -{ - bh_iter_init_generic( root, it ); - it->query = k_bh_query_range; - - v3_copy( co, it->range.co ); - it->range.dist_sqr = range*range; -} - -VG_TIER_0 i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em ) -{ - while( it->depth >= 0 ) - { - bh_node *inode = &bh->nodes[ it->stack[it->depth].id ]; - - /* Only process overlapping nodes */ - i32 q = 0; - - if( it->i ) /* already checked */ - q = 1; - else - { - if( it->query == k_bh_query_box ) - q = box_overlap( inode->bbx, it->box.box ); - else if( it->query == k_bh_query_ray ) - q = ray_aabb1( inode->bbx, it->ray.co, it->ray.inv_dir, it->ray.max_dist ); - else - { - v3f nearest; - closest_point_aabb( it->range.co, inode->bbx, nearest ); - if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr ) - q = 1; - } - } - - if( !q ) - { - it->depth --; - continue; - } - - if( inode->count ) - { - if( it->i < inode->count ) - { - *em = inode->start+it->i; - it->i ++; - return 1; - } - else - { - it->depth --; - it->i = 0; - } - } - else - { - if( it->depth+1 >= VG_ARRAY_LEN(it->stack) ) - { - vg_error( "Maximum stack reached!\n" ); - return 0; - } - - it->stack[it->depth ].id = inode->il; - it->stack[it->depth+1].id = inode->ir; - it->depth ++; - it->i = 0; - } - } - return 0; -} - -VG_TIER_0 i32 bh_closest_point( bh_tree *bh, v3f pos, v3f closest, f32 max_dist ) -{ - if( bh->node_count < 2 ) - return -1; - - max_dist = max_dist*max_dist; - - i32 queue[ 128 ], - depth = 0, - best_item = -1; - - queue[0] = 0; - - while( depth >= 0 ) - { - bh_node *inode = &bh->nodes[ queue[depth] ]; - - v3f p1; - closest_point_aabb( pos, inode->bbx, p1 ); - - /* branch into node if its closer than current best */ - f32 node_dist = v3_dist2( pos, p1 ); - if( node_dist < max_dist ) - { - if( inode->count ) - { - for( i32 i=0; icount; i++ ) - { - v3f p2; - bh->system->item_closest( bh->user, inode->start+i, pos, p2 ); - - f32 item_dist = v3_dist2( pos, p2 ); - if( item_dist < max_dist ) - { - max_dist = item_dist; - v3_copy( p2, closest ); - best_item = inode->start+i; - } - } - - depth --; - } - else - { - queue[depth] = inode->il; - queue[depth+1] = inode->ir; - depth ++; - } - } - else - depth --; - } - - return best_item; -} 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_perlin.c b/vg_perlin.c deleted file mode 100644 index 6647a91..0000000 --- a/vg_perlin.c +++ /dev/null @@ -1,149 +0,0 @@ -static int perlin_hash[] = { -0x46,0xD5,0xB8,0xD3,0xF2,0xE5,0xCC,0x07,0xD0,0xB3,0x7A,0xA2,0xC3,0xDA,0xDC,0x7F, -0xE0,0xB7,0x42,0xA0,0xBF,0x41,0x92,0x32,0x6F,0x0D,0x45,0xC7,0x54,0xDB,0x30,0xC2, -0xD5,0xDA,0x55,0x09,0xDE,0x74,0x48,0x20,0xE1,0x24,0x5C,0x4D,0x6F,0x36,0xD8,0xE9, -0x8D,0x8F,0x54,0x99,0x98,0x51,0xFE,0xDB,0x26,0x04,0x65,0x57,0x56,0xF3,0x53,0x30, -0x3D,0x16,0xC0,0xB6,0xF2,0x47,0xCF,0x62,0xB0,0x6C,0x8F,0x4F,0x8C,0x4C,0x17,0xF0, -0x19,0x7E,0x2D,0x81,0x8D,0xFB,0x10,0xD3,0x49,0x50,0x60,0xFD,0x38,0x15,0x3B,0xEE, -0x05,0xC1,0xCF,0x62,0x97,0x75,0xDF,0x4E,0x4D,0x89,0x5E,0x88,0x5C,0x30,0x8C,0x54, -0x1D,0x39,0x41,0xEA,0xA2,0x63,0x12,0x1B,0x8E,0x35,0x22,0x9B,0x98,0xA3,0x7F,0x80, -0xD6,0x27,0x94,0x66,0xB5,0x1D,0x7E,0xDF,0x96,0x28,0x38,0x3A,0xA0,0xE8,0x71,0x09, -0x62,0x5E,0x9D,0x53,0x58,0x1B,0x7D,0x0D,0x2D,0x99,0x77,0x83,0xC3,0x89,0xC2,0xA2, -0xA7,0x1D,0x78,0x80,0x37,0xC1,0x87,0xFF,0x65,0xBF,0x2C,0xF1,0xE5,0xB3,0x09,0xE0, -0x25,0x92,0x83,0x0F,0x8A,0x57,0x3C,0x0B,0xC6,0xBC,0x44,0x16,0xE3,0xCE,0xC3,0x0D, -0x69,0xD3,0xC6,0x99,0xB8,0x46,0x44,0xC4,0xF3,0x1E,0xBF,0xF5,0xB4,0xDB,0xFB,0x93, -0xA1,0x7B,0xC9,0x08,0x77,0x22,0xE5,0x02,0xEF,0x9E,0x90,0x94,0x8A,0xA6,0x3D,0x7E, -0xA2,0xA0,0x10,0x82,0x47,0x5C,0xAA,0xF8,0x2F,0x0D,0x9F,0x76,0xDA,0x99,0x0F,0xCB, -0xE2,0x02,0x0C,0x75,0xCA,0x35,0x29,0xA6,0x49,0x83,0x6D,0x91,0xB4,0xEC,0x31,0x69, -0xBA,0x13,0xF3,0xC7,0x21,0x06,0xC8,0x79,0xEF,0xB1,0x9C,0x6A,0xEE,0x64,0x9A,0xDC, -0x1E,0xC6,0x18,0x93,0xA9,0x7E,0x89,0x7D,0x96,0xE5,0x44,0xB8,0x00,0x15,0xAF,0x8C, -0x78,0x8F,0xA8,0x05,0xA7,0x07,0x25,0x9A,0xC8,0x5D,0x90,0x1A,0x41,0x53,0x30,0xD3, -0x24,0x33,0x71,0xB4,0x50,0x6E,0xE4,0xEA,0x0D,0x2B,0x6D,0xF5,0x17,0x08,0x74,0x49, -0x71,0xC2,0xAC,0xF7,0xDC,0xB2,0x7E,0xCC,0xB6,0x1B,0xB8,0xA9,0x52,0xCF,0x6B,0x51, -0xD2,0x4E,0xC9,0x43,0xEE,0x2E,0x92,0x24,0xBB,0x47,0x4D,0x0C,0x3E,0x21,0x53,0x19, -0xD4,0x82,0xE2,0xC6,0x93,0x85,0x0A,0xF8,0xFA,0x04,0x07,0xD3,0x1D,0xEC,0x03,0x66, -0xFD,0xB1,0xFB,0x8F,0xC5,0xDE,0xE8,0x29,0xDF,0x23,0x09,0x9D,0x7C,0x43,0x3D,0x4D, -0x89,0xB9,0x6F,0xB4,0x6B,0x4A,0x51,0xC3,0x94,0xF4,0x7C,0x5E,0x19,0x87,0x79,0xC1, -0x80,0x0C,0x45,0x12,0xEC,0x95,0xF3,0x31,0x68,0x42,0xE1,0x06,0x57,0x0E,0xA7,0xFB, -0x78,0x96,0x87,0x23,0xA5,0x20,0x7A,0x09,0x3A,0x45,0xE6,0xD9,0x5E,0x6A,0xD6,0xAA, -0x29,0x50,0x92,0x4E,0xD0,0xB5,0x91,0xC2,0x9A,0xCF,0x07,0xFE,0xB2,0x15,0xEB,0xE4, -0x84,0x40,0x14,0x47,0xFA,0x93,0xB9,0x06,0x69,0xDB,0xBD,0x4E,0xEA,0x52,0x9B,0xDE, -0x5B,0x50,0x36,0xAB,0xB3,0x1F,0xD2,0xCD,0x9C,0x13,0x07,0x7E,0x8B,0xED,0x72,0x62, -0x74,0x77,0x3B,0x88,0xAC,0x5B,0x6A,0xBC,0xDA,0x99,0xE8,0x24,0x90,0x5A,0xCA,0x8D, -0x5C,0x2B,0xF8,0xF1,0xE1,0x1D,0x94,0x11,0xEA,0xCC,0x02,0x09,0x1E,0xA2,0x48,0x67, -0x87,0x5A,0x7E,0xC6,0xCC,0xA3,0xFB,0xC5,0x36,0xEB,0x5C,0xE1,0xAF,0x1E,0xBE,0xE7, -0xD8,0x8F,0x70,0xAE,0x42,0x05,0xF5,0xCD,0x2D,0xA2,0xB0,0xFD,0xEF,0x65,0x2C,0x22, -0xCB,0x8C,0x8B,0xAA,0x3D,0x86,0xE2,0xCD,0xBE,0xC3,0x42,0x38,0xE3,0x9C,0x08,0xB5, -0xAE,0xBD,0x54,0x73,0x83,0x70,0x24,0x47,0xCA,0x4C,0x04,0xC4,0xE0,0x1D,0x40,0xED, -0xF4,0x2B,0x50,0x8E,0x97,0xB3,0xF0,0xA6,0x76,0xDB,0x49,0x30,0xE5,0xD9,0x71,0x07, -0xB2,0xF1,0x0F,0xD6,0x77,0xAA,0x72,0xC0,0xAF,0x66,0xD8,0x40,0xC6,0x08,0x19,0x8C, -0xD9,0x8F,0x5A,0x75,0xAC,0xBE,0xC2,0x40,0x5B,0xBD,0x0D,0x1D,0x00,0xAF,0x26,0x5E, -0x78,0x43,0xAA,0xC6,0x4F,0xF3,0xD8,0xE2,0x7F,0x0C,0x1E,0x77,0x4D,0x35,0x96,0x23, -0x32,0x44,0x03,0x8D,0x92,0xE7,0xFD,0x48,0x07,0xD0,0x58,0xFC,0x6D,0xC9,0x91,0x33, -0xF0,0x23,0x45,0xA4,0x29,0xB9,0xF5,0xB0,0x68,0x8F,0x7B,0x59,0x15,0x8E,0xA6,0x66, -0x15,0xA0,0x76,0x9B,0x69,0xCB,0x38,0xA5,0xF4,0xB4,0x6B,0xDC,0x1F,0xAB,0xAE,0x12, -0x77,0xC0,0x8C,0x4A,0x03,0xB9,0x73,0xD3,0x6D,0x52,0xC5,0xF5,0x6E,0x4E,0x4B,0xA3, -0x24,0x02,0x58,0xEE,0x5F,0xF9,0xD6,0xD0,0x1D,0xBC,0xF4,0xB8,0x4F,0xFD,0x4B,0x2D, -0x34,0x77,0x46,0xE5,0xD4,0x33,0x7B,0x9C,0x35,0xCD,0xB0,0x5D,0x06,0x39,0x99,0xEB, -0x0C,0xD0,0x0F,0xF7,0x92,0xB5,0x58,0x5B,0x5E,0x79,0x12,0xF4,0x05,0xF6,0x21,0x07, -0x0B,0x49,0x1A,0xFB,0xD4,0x98,0xC4,0xEF,0x7A,0xD6,0xCA,0xA1,0xDA,0xB3,0x51,0x00, -0x76,0xEC,0x08,0x48,0x40,0x35,0xD7,0x94,0xBE,0xF5,0x7B,0xA4,0x20,0x81,0x5F,0x82, -0xF3,0x6F,0x96,0x24,0x98,0xB6,0x49,0x18,0xC8,0xC5,0x8C,0xD2,0x38,0x7F,0xC4,0xF6, -0xAA,0x87,0xDC,0x73,0x5B,0xA1,0xAF,0xE5,0x3D,0x37,0x6B,0x85,0xED,0x38,0x62,0x7D, -0x57,0xBD,0xCF,0xB5,0x1B,0xA8,0xBB,0x32,0x33,0xD3,0x34,0x5A,0xC1,0x5D,0xFB,0x28, -0x6E,0xE1,0x67,0x51,0xBB,0x31,0x92,0x83,0xAC,0xAA,0x72,0x52,0xFD,0x13,0x4F,0x73, -0xD3,0xF0,0x5E,0xFC,0xBA,0xB1,0x3C,0x7B,0x08,0x76,0x03,0x38,0x1E,0xD1,0xCC,0x33, -0xA3,0x1E,0xFC,0xE0,0x82,0x30,0x27,0x93,0x71,0x35,0x75,0x77,0xBA,0x78,0x10,0x33, -0xCD,0xAB,0xCF,0x8E,0xAD,0xF9,0x32,0xC9,0x15,0x9F,0xD6,0x6D,0xA8,0xAE,0xB1,0x3F, -0x90,0xEB,0xD4,0xF9,0x31,0x81,0xA3,0x53,0x99,0x4B,0x3C,0x93,0x3B,0xFE,0x55,0xFF, -0x25,0x9F,0xCC,0x07,0xC5,0x2C,0x14,0xA7,0xA4,0x1E,0x6C,0xB6,0x91,0x2A,0xE0,0x3E, -0x7F,0x39,0x0A,0xD9,0x24,0x3C,0x01,0xA0,0x30,0x99,0x8E,0xB8,0x1D,0xF9,0xA7,0x78, -0x86,0x95,0x35,0x0E,0x21,0xDA,0x7A,0x7B,0xAD,0x9F,0x4E,0xF6,0x63,0x5B,0x96,0xBB, -0x87,0x36,0x3F,0xA7,0x1A,0x66,0x91,0xCD,0xB0,0x3B,0xC0,0x4F,0x54,0xD2,0x5F,0xBB, -0x38,0x89,0x1C,0x79,0x7E,0xA2,0x02,0xE4,0x80,0x84,0x1E,0x33,0xAB,0x74,0xFA,0xBE, -0x31,0x46,0x2E,0xC5,0x15,0xB9,0x12,0xE9,0xD3,0x73,0x43,0xEA,0x74,0x11,0xA7,0xC0, -0xD5,0xD8,0x39,0x08,0x9F,0x4F,0xC7,0x71,0x25,0x09,0x51,0x65,0xD6,0xA8,0x02,0x1F -}; - -// Note: Hash must be power of 2! -#define PERLIN_HASH_LENGTH 1024 -#define PERLIN_HASH_MASK (PERLIN_HASH_LENGTH-1) - -static int perlin_noise2( int x, int y, int seed ) -{ - return perlin_hash[ (perlin_hash[(y+seed) & PERLIN_HASH_MASK] + x) - & PERLIN_HASH_MASK ]; -} - -static int perlin_noise1( int i, int seed ) -{ - return perlin_hash[ (seed + i) & PERLIN_HASH_MASK ]; -} - -static f32 perlin_smooth( f32 x, f32 y, f32 t ) -{ - return vg_lerpf( x, y, t*t*(3.0f-2.0f*t) ); -} - -f32 vg_perlin_noise_2d( f32 x, f32 y, int seed ) -{ - int ix = x, iy = y; - f32 x_frac = x - ix, - y_frac = y - iy; - - int s = perlin_noise2( ix, iy, seed ), - t = perlin_noise2( ix+1, iy, seed ), - u = perlin_noise2( ix, iy+1, seed ), - v = perlin_noise2( ix+1, iy+1, seed ); - - f32 low = perlin_smooth( s,t,x_frac ), - high = perlin_smooth( u,v,x_frac ); - - return perlin_smooth( low, high, y_frac ); -} - -f32 vg_perlin_noise_1d( f32 v, int seed ) -{ - int iv = v; - f32 frac = v-iv; - int s = perlin_noise1( iv, seed ), - t = perlin_noise1( iv+1, seed ); - - return perlin_smooth( s, t, frac ); -} - -f32 vg_perlin_fract_1d( f32 v, f32 freq, int octaves, int seed ) -{ - f32 xa = v*freq, - amp = 1.0f, - fin = 0.f, - div = 0.f; - - for( int i=0; iname = name; - profiler->default_budget_ms = default_budget_ms; - _vg_profiler.count ++; - return vg_stack_offset( &_vg_profiler.stack, profiler ); -} - -VG_API_INTERNAL static struct vg_profiler *_vg_profiler_get( u32 id ) -{ - return vg_stack_pointer( &_vg_profiler.stack, id ); -} - -VG_API void _vg_profiler_tick( u32 profiler_id ) -{ - struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); - - u64 new_tick = SDL_GetPerformanceCounter(), - duration = new_tick - profiler->tick_last; - - f64 to_ms = 1000.0 / (f64)SDL_GetPerformanceFrequency(); - - VG_MUTEX_LOCK( _vg_profiler.tick_lock ); - for( u32 i=0; ihistory_ms[ profiler->buffer_current ][ i ] = (f32)((f64)profiler->row_accumulated[i] * to_ms); - profiler->row_accumulated[i] = 0; - } - profiler->buffer_current ++; - if( profiler->buffer_current >= PROFILER_HISTORY_LENGTH ) - profiler->buffer_current = 0; - VG_MUTEX_UNLOCK( _vg_profiler.tick_lock ); - - profiler->tick_last = new_tick; -} - -static u32 vg_profiler_block_id( struct vg_profiler *profiler, const c8 *block_name ) -{ - for( u32 i=0; irow_length; i ++ ) - if( profiler->row_names[i] == block_name ) - return i + 1; - if( profiler->row_length < PROFILER_ROW_MAX ) - { - profiler->row_names[ profiler->row_length ++ ] = block_name; - return profiler->row_length; - } - else return 0; -} - -VG_API void _vg_profiler_enter_block( u32 profiler_id, const c8 *block_name ) -{ - u64 seg_end = SDL_GetPerformanceCounter(); - - struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); - VG_ASSERT( profiler->stack_height < PROFILER_STACK_MAX ); - - if( profiler->stack_height ) - { - u32 lower_block = profiler->block_stack[ profiler->stack_height ]; - if( lower_block ) - profiler->row_accumulated[ lower_block-1 ] += (seg_end - profiler->segment_last); - } - - u32 block_id = vg_profiler_block_id( profiler, block_name ); - profiler->block_stack[ profiler->stack_height ] = block_id; - profiler->stack_height ++; - profiler->segment_last = seg_end; -} - -VG_API void _vg_profiler_exit_block( u32 profiler_id ) -{ - u64 seg_end = SDL_GetPerformanceCounter(); - - struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); - VG_ASSERT( profiler->stack_height ); - - profiler->stack_height --; - u32 block_id = profiler->block_stack[ profiler->stack_height ]; - - if( block_id ) - profiler->row_accumulated[ block_id-1 ] += (seg_end - profiler->segment_last ); - - profiler->segment_last = seg_end; -} - -VG_API void _vg_profiler_draw( ui_context *ctx, u32 profiler_id, f32 budget_ms, ui_rect panel, i32 dir, bool normalize ) -{ - struct vg_profiler *profiler = _vg_profiler_get( profiler_id ); - if( panel[2] == 0 ) - panel[2] = 256; - if( panel[3] == 0 ) - panel[3] = PROFILER_HISTORY_LENGTH * 2; - - f32 sh = (f32)panel[3^dir] / (f32)PROFILER_HISTORY_LENGTH, - sw = (f32)panel[2^dir]; - - ui_fill( ctx, panel, 0xa0000000 ); - - u32 colours[ PROFILER_ROW_MAX ]; - for( u32 i=0; irow_length; i ++ ) - colours[i] = vg_strdjb2( profiler->row_names[i] ) | 0xff000000; - - for( i32 i=0; irow_length; j ++ ) - { - f32 sample_ms = profiler->history_ms[i][j], - pos_x = (total_ms / budget_ms) * sw, - width = (sample_ms / budget_ms) * sw; - - ui_rect block; - block[0^dir] = panel[0^dir] + pos_x; - block[1^dir] = panel[1^dir] + (f32)i*sh; - block[2^dir] = VG_MAX( 1, width-1 ); - block[3^dir] = ceilf(sh)-1; - ui_fill( ctx, block, colours[j] ); - total_ms += sample_ms; - } - } - -#if 0 - c8 infbuf[64]; - snprintf( infbuf, 64, "accuracy: %.7fms", rate_mul ); - ui_text( ctx, (ui_rect){ panel[0], panel[1], panel[2]-4, 24 }, infbuf, 1, k_ui_align_right, 0 ); - - for( int i=0; iname; - snprintf( infbuf, 64, "%.4fms %s", avgs[i] * (1.0f/(VG_PROFILE_SAMPLE_COUNT-1)), name ); - ui_text( ctx, (ui_rect){ panel[0], panel[1] + 24 + i*14, panel[2]-4, 14 }, infbuf, 1, k_ui_align_right, 0 ); - } -#endif -} - -static void cb_vg_profiler( ui_context *ctx, ui_rect rect, struct vg_magi_panel *magi ) -{ - struct vg_profiler *profiler = magi->data; - _vg_profiler_draw( ctx, vg_stack_offset( &_vg_profiler.stack, profiler ), profiler->default_budget_ms, rect, 0, 0 ); -} - -static int cmd_vg_profile( int argc, const char *argv[] ) -{ - if( argc == 1 ) - { - for( u32 i=0; i<_vg_profiler.count; i ++ ) - { - struct vg_profiler *profiler = vg_stack_pointer( &_vg_profiler.stack, i*sizeof(struct vg_profiler) ); - if( !strcmp( argv[0], profiler->name ) ) - { - ui_px w = 256, h = PROFILER_HISTORY_LENGTH * 2; - struct vg_magi_panel *magi = _vg_magi_open( w,h, VG_MAGI_MOVEABLE|VG_MAGI_PERSISTENT ); - magi->title = "Profiler"; - magi->data = profiler; - magi->ui_cb = cb_vg_profiler; - magi->close_cb = NULL; - return 1; - } - } - } - else - vg_error( "Usage: vg_profile \n" ); - - return 0; -} - -static void cmd_vg_profile_poll( int argc, const c8 *argv[] ) -{ - const c8 *term = argv[ argc-1 ]; - if( argc == 1 ) - { - for( u32 i=0; i<_vg_profiler.count; i ++ ) - { - struct vg_profiler *profiler = vg_stack_pointer( &_vg_profiler.stack, i*sizeof(struct vg_profiler) ); - console_suggest_score_text( profiler->name, term, 0 ); - } - } -} - -VG_API void _vg_profiler_register(void) -{ - vg_console_reg_cmd( "vg_profile", cmd_vg_profile, cmd_vg_profile_poll ); -} diff --git a/vg_rigidbody.c b/vg_rigidbody.c deleted file mode 100644 index 4afccfb..0000000 --- a/vg_rigidbody.c +++ /dev/null @@ -1,206 +0,0 @@ -static float - k_limit_bias = 0.02f, - k_joint_correction = 0.01f, - k_joint_impulse = 1.0f, - k_joint_bias = 0.08f; /* positional joints */ - -f32 k_gravity = 9.6f; - -VG_API void _vg_rigidbody_register(void) -{ - VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT ); - VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT ); - VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT ); - VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT ); - VG_VAR_F32( k_gravity, flags=VG_VAR_CHEAT ); -} - -void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h, f32 density, f32 inertia_scale ) -{ - f32 vol = vg_capsule_volume( r, h ), - mass = vol*density; - - rb->inv_mass = 1.0f/mass; - - m3x3f I; - vg_capsule_inertia( r, h, mass * inertia_scale, I ); - m3x3_inv( I, rb->iI ); -} - -void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale ) -{ - f32 vol = vg_box_volume( box ), - mass = vol*density; - - rb->inv_mass = 1.0f/mass; - - m3x3f I; - vg_box_inertia( box, mass * inertia_scale, I ); - m3x3_inv( I, rb->iI ); -} - -void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale ) -{ - f32 vol = vg_sphere_volume( r ), - mass = vol*density; - - rb->inv_mass = 1.0f/mass; - m3x3f I; - vg_sphere_inertia( r, mass * inertia_scale, I ); - m3x3_inv( I, rb->iI ); -} - -void rb_update_matrices( rigidbody *rb ) -{ - //q_normalize( rb->q ); - q_m3x3( rb->q, rb->to_world ); - v3_copy( rb->co, rb->to_world[3] ); - m4x3_invert_affine( rb->to_world, rb->to_local ); - - /* I = R I_0 R^T */ - m3x3_mul( rb->to_world, rb->iI, rb->iIw ); - m3x3_mul( rb->iIw, rb->to_local, rb->iIw ); -} - -void rb_extrapolate( rigidbody *rb, v3f co, v4f q ) -{ - float substep = vg.time_fixed_extrapolate; - v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co ); - - if( v3_length2( rb->w ) > 0.0f ){ - v4f rotation; - v3f axis; - v3_copy( rb->w, axis ); - - float mag = v3_length( axis ); - v3_divs( axis, mag, axis ); - q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep ); - q_mul( rotation, rb->q, q ); - q_normalize( q ); - } - else{ - v4_copy( rb->q, q ); - } -} - -void rb_iter( rigidbody *rb ) -{ - if( !vg_validf(rb->v[0]) || !vg_validf(rb->v[1]) || !vg_validf(rb->v[2]) ) - { - vg_fatal_error( - "Aborting the program because velocity has invalid value in one " - "or more components: %f %f %f\n", rb->v[0],rb->v[1],rb->v[2] ); - } - - v3f gravity = { 0.0f, -k_gravity, 0.0f }; - v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v ); - - /* intergrate velocity */ - v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co ); -#if 0 - v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w ); -#endif - - /* inegrate inertia */ - if( v3_length2( rb->w ) > 0.0f ){ - v4f rotation; - v3f axis; - v3_copy( rb->w, axis ); - - float mag = v3_length( axis ); - v3_divs( axis, mag, axis ); - q_axis_angle( rotation, axis, mag*vg.time_fixed_delta ); - q_mul( rotation, rb->q, rb->q ); - q_normalize( rb->q ); - } -} - -/* - * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf, - * page 76. - */ -void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ) -{ - /* convert to body coordinates */ - v3f w_local; - m3x3_mulv( rb->to_local, rb->w, w_local ); - - /* Residual vector */ - v3f f, v0; - m3x3_mulv( I, w_local, v0 ); - v3_cross( w_local, v0, f ); - v3_muls( f, h, f ); - - /* Jacobian */ - m3x3f iJ, J, skew_w_local, skew_v0, m0; - m3x3_skew_symetric( skew_w_local, w_local ); - m3x3_mul( skew_w_local, I, m0 ); - - m3x3_skew_symetric( skew_v0, v0 ); - m3x3_sub( m0, skew_v0, J ); - m3x3_scalef( J, h ); - m3x3_add( I, J, J ); - - /* Single Newton-Raphson update */ - v3f w1; - m3x3_inv( J, iJ ); - m3x3_mulv( iJ, f, w1 ); - v3_sub( w_local, w1, rb->w ); - - m3x3_mulv( rb->to_world, rb->w, rb->w ); -} - -void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ) -{ - v3f rva, rvb; - v3_cross( rba->w, ra, rva ); - v3_add( rba->v, rva, rva ); - v3_cross( rbb->w, rb, rvb ); - v3_add( rbb->v, rvb, rvb ); - - v3_sub( rva, rvb, rv ); -} - -void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ) -{ - /* linear */ - v3_muladds( rb->v, impulse, rb->inv_mass, rb->v ); - - /* Angular velocity */ - v3f wa; - v3_cross( delta, impulse, wa ); - - m3x3_mulv( rb->iIw, wa, wa ); - v3_add( rb->w, wa, rb->w ); -} - - -void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag ) -{ - /* float */ - float depth = v3_dot( plane, ra->co ) - plane[3], - lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt; - - v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v ); - - if( depth < 0.0f ) - v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v ); -} - -/* apply a spring&dampener force to match ra(worldspace) on rigidbody, to - * rt(worldspace) - */ -void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, - f32 spring, f32 dampening, f32 timestep ) -{ - float d = v3_dot( rt, ra ); - float a = acosf( vg_clampf( d, -1.0f, 1.0f ) ); - - v3f axis; - v3_cross( rt, ra, axis ); - - float Fs = -a * spring, - Fd = -v3_dot( rba->w, axis ) * dampening; - - v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w ); -} 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.c b/vg_rigidbody_collision.c deleted file mode 100644 index 156416e..0000000 --- a/vg_rigidbody_collision.c +++ /dev/null @@ -1,880 +0,0 @@ -int rb_contact_count = 0; -struct rb_ct rb_contact_buffer[VG_MAX_CONTACTS]; - -/* - * Contact generators - * - * These do not automatically allocate contacts, an appropriately sized - * buffer must be supplied. The function returns the size of the manifold - * which was generated. - * - * The values set on the contacts are: n, co, p, rba, rbb - */ - -/* - * By collecting the minimum(time) and maximum(time) pairs of points, we - * build a reduced and stable exact manifold. - * - * tx: time at point - * rx: minimum distance of these points - * dx: the delta between the two points - * - * pairs will only ammend these if they are creating a collision - */ -typedef struct capsule_manifold capsule_manifold; -struct capsule_manifold{ - f32 t0, t1; - f32 r0, r1; - v3f d0, d1; -}; - -/* - * Expand a line manifold with a new pair. t value is the time along segment - * on the oriented object which created this pair. - */ -static void rb_capsule_manifold( v3f pa, v3f pb, f32 t, f32 r, - capsule_manifold *manifold ){ - v3f delta; - v3_sub( pa, pb, delta ); - - if( v3_length2(delta) < r*r ){ - if( t < manifold->t0 ){ - v3_copy( delta, manifold->d0 ); - manifold->t0 = t; - manifold->r0 = r; - } - - if( t > manifold->t1 ){ - v3_copy( delta, manifold->d1 ); - manifold->t1 = t; - manifold->r1 = r; - } - } -} - -static void rb_capsule_manifold_init( capsule_manifold *manifold ){ - manifold->t0 = INFINITY; - manifold->t1 = -INFINITY; -} - -static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c, - capsule_manifold *manifold, - rb_ct *buf ){ - v3f p0, p1; - v3_muladds( mtx[3], mtx[1], -c->h*0.5f+c->r, p0 ); - v3_muladds( mtx[3], mtx[1], c->h*0.5f-c->r, p1 ); - - int count = 0; - if( manifold->t0 <= 1.0f ){ - rb_ct *ct = buf; - - v3f pa; - v3_muls( p0, 1.0f-manifold->t0, pa ); - v3_muladds( pa, p1, manifold->t0, pa ); - - f32 d = v3_length( manifold->d0 ); - v3_muls( manifold->d0, 1.0f/d, ct->n ); - v3_muladds( pa, ct->n, -c->r, ct->co ); - - ct->p = manifold->r0 - d; - ct->type = k_contact_type_default; - count ++; - } - - if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) ){ - rb_ct *ct = buf+count; - - v3f pa; - v3_muls( p0, 1.0f-manifold->t1, pa ); - v3_muladds( pa, p1, manifold->t1, pa ); - - f32 d = v3_length( manifold->d1 ); - v3_muls( manifold->d1, 1.0f/d, ct->n ); - v3_muladds( pa, ct->n, -c->r, ct->co ); - - ct->p = manifold->r1 - d; - ct->type = k_contact_type_default; - - count ++; - } - - /* - * Debugging - */ - -#if 0 - if( count == 2 ) - vg_line( buf[0].co, buf[1].co, 0xff0000ff ); -#endif - - return count; -} - -int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca, - v3f coB, f32 rb, rb_ct *buf ){ - f32 ha = ca->h, - ra = ca->r, - r = ra + rb; - - v3f p0, p1; - v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); - v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); - - v3f c, delta; - closest_point_segment( p0, p1, coB, c ); - v3_sub( c, coB, delta ); - f32 d2 = v3_length2(delta); - - if( d2 < r*r ){ - f32 d = sqrtf(d2); - - rb_ct *ct = buf; - v3_muls( delta, 1.0f/d, ct->n ); - ct->p = r-d; - - v3f p0, p1; - v3_muladds( c, ct->n, -ra, p0 ); - v3_muladds( coB, ct->n, rb, p1 ); - v3_add( p0, p1, ct->co ); - v3_muls( ct->co, 0.5f, ct->co ); - ct->type = k_contact_type_default; - return 1; - } - else return 0; -} - -int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, rb_capsule *cb, rb_ct *buf ) -{ - f32 ha = ca->h, - hb = cb->h, - ra = ca->r, - rb = cb->r, - r = ra+rb; - - v3f p0, p1, p2, p3; - v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 ); - v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 ); - v3_muladds( mtxB[3], mtxB[1], -hb*0.5f+rb, p2 ); - v3_muladds( mtxB[3], mtxB[1], hb*0.5f-rb, p3 ); - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f pa, pb; - f32 ta, tb; - closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb ); - rb_capsule_manifold( pa, pb, ta, r, &manifold ); - - ta = closest_point_segment( p0, p1, p2, pa ); - tb = closest_point_segment( p0, p1, p3, pb ); - rb_capsule_manifold( pa, p2, ta, r, &manifold ); - rb_capsule_manifold( pb, p3, tb, r, &manifold ); - - closest_point_segment( p2, p3, p0, pa ); - closest_point_segment( p2, p3, p1, pb ); - rb_capsule_manifold( p0, pa, 0.0f, r, &manifold ); - rb_capsule_manifold( p1, pb, 1.0f, r, &manifold ); - - return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); -} - -/* - * Generates up to two contacts; optimised for the most stable manifold - */ -int rb_capsule__box( m4x3f mtxA, rb_capsule *ca, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ) -{ - f32 h = ca->h, r = ca->r; - - /* - * Solving this in symetric local space of the cube saves us some time and a - * couple branches when it comes to the quad stage. - */ - v3f centroid; - v3_add( box[0], box[1], centroid ); - v3_muls( centroid, 0.5f, centroid ); - - boxf bbx; - v3_sub( box[0], centroid, bbx[0] ); - v3_sub( box[1], centroid, bbx[1] ); - - v3f pc, p0w, p1w, p0, p1; - v3_muladds( mtxA[3], mtxA[1], -h*0.5f+r, p0w ); - v3_muladds( mtxA[3], mtxA[1], h*0.5f-r, p1w ); - - m4x3_mulv( mtxB_inverse, p0w, p0 ); - m4x3_mulv( mtxB_inverse, p1w, p1 ); - v3_sub( p0, centroid, p0 ); - v3_sub( p1, centroid, p1 ); - v3_add( p0, p1, pc ); - v3_muls( pc, 0.5f, pc ); - - /* - * Finding an appropriate quad to collide lines with - */ - v3f region; - v3_div( pc, bbx[1], region ); - - v3f quad[4]; - if( (fabsf(region[0]) > fabsf(region[1])) && - (fabsf(region[0]) > fabsf(region[2])) ) - { - f32 px = vg_signf(region[0]) * bbx[1][0]; - v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] ); - v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] ); - v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] ); - v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] ); - } - else if( fabsf(region[1]) > fabsf(region[2]) ) - { - f32 py = vg_signf(region[1]) * bbx[1][1]; - v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] ); - v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] ); - v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] ); - v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] ); - } - else - { - f32 pz = vg_signf(region[2]) * bbx[1][2]; - v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] ); - v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] ); - v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] ); - v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] ); - } - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f c0, c1; - closest_point_aabb( p0, bbx, c0 ); - closest_point_aabb( p1, bbx, c1 ); - - v3f d0, d1, da; - v3_sub( c0, p0, d0 ); - v3_sub( c1, p1, d1 ); - v3_sub( p1, p0, da ); - - v3_normalize(d0); - v3_normalize(d1); - v3_normalize(da); - - if( v3_dot( da, d0 ) <= 0.01f ) - rb_capsule_manifold( p0, c0, 0.0f, r, &manifold ); - - if( v3_dot( da, d1 ) >= -0.01f ) - rb_capsule_manifold( p1, c1, 1.0f, r, &manifold ); - - for( i32 i=0; i<4; i++ ){ - i32 i0 = i, - i1 = (i+1)%4; - - v3f ca, cb; - f32 ta, tb; - closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb ); - rb_capsule_manifold( ca, cb, ta, r, &manifold ); - } - - /* - * Create final contacts based on line manifold - */ - m3x3_mulv( mtxB, manifold.d0, manifold.d0 ); - m3x3_mulv( mtxB, manifold.d1, manifold.d1 ); - return rb_capsule__manifold_done( mtxA, ca, &manifold, buf ); -} - -int rb_sphere__box( v3f coA, f32 ra, - m4x3f mtxB, m4x3f mtxB_inverse, boxf box, - rb_ct *buf ) -{ - v3f co, delta; - closest_point_obb( coA, box, mtxB, mtxB_inverse, co ); - v3_sub( coA, co, delta ); - - f32 d2 = v3_length2(delta); - - if( d2 <= ra*ra ){ - f32 d; - - rb_ct *ct = buf; - if( d2 <= 0.0001f ){ - v3f e, coB; - v3_sub( box[1], box[0], e ); - v3_muls( e, 0.5f, e ); - v3_add( box[0], e, coB ); - v3_sub( coA, coB, delta ); - - /* - * some extra testing is required to find the best axis to push the - * object back outside the box. Since there isnt a clear seperating - * vector already, especially on really high aspect boxes. - */ - f32 lx = v3_dot( mtxB[0], delta ), - ly = v3_dot( mtxB[1], delta ), - lz = v3_dot( mtxB[2], delta ), - px = e[0] - fabsf(lx), - py = e[1] - fabsf(ly), - pz = e[2] - fabsf(lz); - - if( px < py && px < pz ) v3_muls( mtxB[0], vg_signf(lx), ct->n ); - else if( py < pz ) v3_muls( mtxB[1], vg_signf(ly), ct->n ); - else v3_muls( mtxB[2], vg_signf(lz), ct->n ); - - v3_muladds( coA, ct->n, -ra, ct->co ); - ct->p = ra; - } - else{ - d = sqrtf(d2); - v3_muls( delta, 1.0f/d, ct->n ); - ct->p = ra-d; - v3_copy( co, ct->co ); - } - - ct->type = k_contact_type_default; - return 1; - } - else return 0; -} - -int rb_sphere__sphere( v3f coA, f32 ra, v3f coB, f32 rb, rb_ct *buf ) -{ - v3f delta; - v3_sub( coA, coB, delta ); - - f32 d2 = v3_length2(delta), - r = ra+rb; - - if( d2 < r*r ){ - f32 d = sqrtf(d2); - - rb_ct *ct = buf; - v3_muls( delta, 1.0f/d, ct->n ); - - v3f p0, p1; - v3_muladds( coA, ct->n,-ra, p0 ); - v3_muladds( coB, ct->n, rb, p1 ); - v3_add( p0, p1, ct->co ); - v3_muls( ct->co, 0.5f, ct->co ); - ct->type = k_contact_type_default; - ct->p = r-d; - return 1; - } - else return 0; -} - -int rb_sphere__triangle( m4x3f mtxA, f32 r, v3f tri[3], rb_ct *buf ) -{ - v3f delta, co; - enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co ); - v3_sub( mtxA[3], co, delta ); - f32 d2 = v3_length2( delta ); - - if( d2 <= r*r ){ - rb_ct *ct = buf; - - v3f ab, ac, tn; - v3_sub( tri[2], tri[0], ab ); - v3_sub( tri[1], tri[0], ac ); - v3_cross( ac, ab, tn ); - v3_copy( tn, ct->n ); - - if( v3_length2( ct->n ) <= 0.00001f ){ -#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING - vg_error( "Zero area triangle!\n" ); -#endif - return 0; - } - - v3_normalize( ct->n ); - - f32 d = sqrtf(d2); - - v3_copy( co, ct->co ); - ct->type = type; - ct->p = r-d; - return 1; - } - - return 0; -} - -int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c, v3f tri[3], rb_ct *buf ) -{ - v3f pc, p0w, p1w; - v3_muladds( mtxA[3], mtxA[1], -c->h*0.5f+c->r, p0w ); - v3_muladds( mtxA[3], mtxA[1], c->h*0.5f-c->r, p1w ); - - capsule_manifold manifold; - rb_capsule_manifold_init( &manifold ); - - v3f v0, v1, n; - v3_sub( tri[1], tri[0], v0 ); - v3_sub( tri[2], tri[0], v1 ); - v3_cross( v0, v1, n ); - - if( v3_length2( n ) <= 0.00001f ){ -#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING - vg_error( "Zero area triangle!\n" ); -#endif - return 0; - } - - v3_normalize( n ); - -#if 1 - /* deep penetration recovery. for when we clip through the triangles. so its - * not very 'correct' */ - f32 dist; - if( ray_tri( tri, p0w, mtxA[1], &dist, 1 ) ){ - f32 l = c->h - c->r*2.0f; - if( (dist >= 0.0f) && (dist < l) ){ - v3f co; - v3_muladds( p0w, mtxA[1], dist, co ); - vg_line_point( co, 0.02f, 0xffffff00 ); - - v3f d0, d1; - v3_sub( p0w, co, d0 ); - v3_sub( p1w, co, d1 ); - - f32 p = vg_minf( v3_dot( n, d0 ), v3_dot( n, d1 ) ) - c->r; - - rb_ct *ct = buf; - ct->p = -p; - ct->type = k_contact_type_default; - v3_copy( n, ct->n ); - v3_muladds( co, n, p, ct->co ); - - return 1; - } - } -#endif - - v3f c0, c1; - closest_on_triangle_1( p0w, tri, c0 ); - closest_on_triangle_1( p1w, tri, c1 ); - - v3f d0, d1, da; - v3_sub( c0, p0w, d0 ); - v3_sub( c1, p1w, d1 ); - v3_sub( p1w, p0w, da ); - - v3_normalize(d0); - v3_normalize(d1); - v3_normalize(da); - - /* the two balls at the ends */ - if( v3_dot( da, d0 ) <= 0.01f ) - rb_capsule_manifold( p0w, c0, 0.0f, c->r, &manifold ); - if( v3_dot( da, d1 ) >= -0.01f ) - rb_capsule_manifold( p1w, c1, 1.0f, c->r, &manifold ); - - /* the edges to edges */ - for( int i=0; i<3; i++ ){ - int i0 = i, - i1 = (i+1)%3; - - v3f ca, cb; - f32 ta, tb; - closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb ); - rb_capsule_manifold( ca, cb, ta, c->r, &manifold ); - } - - int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf ); - for( int i=0; i VG_ARRAY_LEN(rb_contact_buffer) ) - return 0; - - return 1; -} - -rb_ct *rb_global_buffer( void ) -{ - return &rb_contact_buffer[ rb_contact_count ]; -} - -/* - * ----------------------------------------------------------------------------- - * Boolean shape overlap functions - * ----------------------------------------------------------------------------- - */ - -/* - * Project AABB, and triangle interval onto axis to check if they overlap - */ -static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] ){ - float - - r = extent[0] * fabsf(axis[0]) + - extent[1] * fabsf(axis[1]) + - extent[2] * fabsf(axis[2]), - - p0 = v3_dot( axis, tri[0] ), - p1 = v3_dot( axis, tri[1] ), - p2 = v3_dot( axis, tri[2] ), - - e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2))); - - if( e > r ) return 0; - else return 1; -} - -/* - * Seperating axis test box vs triangle - */ -int rb_box_triangle_sat( v3f extent, v3f center, - m4x3f to_local, v3f tri_src[3] ) -{ - v3f tri[3]; - - for( int i=0; i<3; i++ ){ - m4x3_mulv( to_local, tri_src[i], tri[i] ); - v3_sub( tri[i], center, tri[i] ); - } - - v3f f0,f1,f2,n; - v3_sub( tri[1], tri[0], f0 ); - v3_sub( tri[2], tri[1], f1 ); - v3_sub( tri[0], tri[2], f2 ); - - - v3f axis[9]; - v3_cross( (v3f){1.0f,0.0f,0.0f}, f0, axis[0] ); - v3_cross( (v3f){1.0f,0.0f,0.0f}, f1, axis[1] ); - v3_cross( (v3f){1.0f,0.0f,0.0f}, f2, axis[2] ); - v3_cross( (v3f){0.0f,1.0f,0.0f}, f0, axis[3] ); - v3_cross( (v3f){0.0f,1.0f,0.0f}, f1, axis[4] ); - v3_cross( (v3f){0.0f,1.0f,0.0f}, f2, axis[5] ); - v3_cross( (v3f){0.0f,0.0f,1.0f}, f0, axis[6] ); - v3_cross( (v3f){0.0f,0.0f,1.0f}, f1, axis[7] ); - v3_cross( (v3f){0.0f,0.0f,1.0f}, f2, axis[8] ); - - for( int i=0; i<9; i++ ) - if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0; - - /* u0, u1, u2 */ - if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0; - if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0; - if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0; - - /* normal */ - v3_cross( f0, f1, n ); - if(!rb_box_triangle_interval( extent, n, tri )) return 0; - - return 1; -} - -/* - * ----------------------------------------------------------------------------- - * Manifold - * ----------------------------------------------------------------------------- - */ - -int rb_manifold_apply_filtered( rb_ct *man, int len ) -{ - int k = 0; - - for( int i=0; itype == k_contact_type_disabled ) - continue; - - man[k ++] = man[i]; - } - - return k; -} - -void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r ) -{ - if( v3_dist2( ci->co, cj->co ) < r*r ){ - cj->type = k_contact_type_disabled; - ci->p = (ci->p + cj->p) * 0.5f; - - v3_add( ci->co, cj->co, ci->co ); - v3_muls( ci->co, 0.5f, ci->co ); - - v3f delta; - v3_sub( ci->rba->co, ci->co, delta ); - - float c0 = v3_dot( ci->n, delta ), - c1 = v3_dot( cj->n, delta ); - - if( c0 < 0.0f || c1 < 0.0f ){ - /* error */ - ci->type = k_contact_type_disabled; - } - else{ - v3f n; - v3_muls( ci->n, c0, n ); - v3_muladds( n, cj->n, c1, n ); - v3_normalize( n ); - v3_copy( n, ci->n ); - } - } -} - -/* - * - */ -void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r ) -{ - for( int i=0; itype != k_contact_type_edge ) - continue; - - for( int j=i+1; jtype != k_contact_type_edge ) - continue; - - rb_manifold_contact_weld( ci, cj, r ); - } - } -} - -void rb_manifold_filter_pairs( rb_ct *man, int len, float r ) -{ - for( int i=0; itype == k_contact_type_disabled ) continue; - - for( int j=i+1; jtype == k_contact_type_disabled ) continue; - - if( v3_dist2( ci->co, cj->co ) < r*r ){ - cj->type = k_contact_type_disabled; - v3_add( cj->n, ci->n, ci->n ); - ci->p += cj->p; - similar ++; - } - } - - if( similar ){ - float n = 1.0f/((float)similar+1.0f); - v3_muls( ci->n, n, ci->n ); - ci->p *= n; - - if( v3_length2(ci->n) < 0.1f*0.1f ) - ci->type = k_contact_type_disabled; - else - v3_normalize( ci->n ); - } - } -} - -void rb_manifold_filter_backface( rb_ct *man, int len ) -{ - for( int i=0; itype == k_contact_type_disabled ) - continue; - - v3f delta; - v3_sub( ct->co, ct->rba->co, delta ); - - if( v3_dot( delta, ct->n ) > -0.001f ) - ct->type = k_contact_type_disabled; - } -} - -void rb_manifold_filter_coplanar( rb_ct *man, int len, float w ) -{ - for( int i=0; itype == k_contact_type_disabled || - ci->type == k_contact_type_edge ) - continue; - - float d1 = v3_dot( ci->co, ci->n ); - - for( int j=0; jtype == k_contact_type_disabled ) - continue; - - float d2 = v3_dot( cj->co, ci->n ), - d = d2-d1; - - if( fabsf( d ) <= w ){ - cj->type = k_contact_type_disabled; - } - } - } -} - -void rb_debug_contact( rb_ct *ct ) -{ - v3f p1; - v3_muladds( ct->co, ct->n, 0.05f, p1 ); - - if( ct->type == k_contact_type_default ){ - vg_line_point( ct->co, 0.0125f, 0xff0000ff ); - vg_line( ct->co, p1, 0xffffffff ); - } - else if( ct->type == k_contact_type_edge ){ - vg_line_point( ct->co, 0.0125f, 0xff00ffc0 ); - vg_line( ct->co, p1, 0xffffffff ); - } -} - -void rb_solver_reset(void) -{ - rb_contact_count = 0; -} - -rb_ct *rb_global_ct(void) -{ - return rb_contact_buffer + rb_contact_count; -} - -void rb_prepare_contact( rb_ct *ct, f32 dt ) -{ - ct->bias = -k_phys_baumgarte * (dt*3600.0f) - * vg_minf( 0.0f, -ct->p+k_penetration_slop ); - - v3_tangent_basis( ct->n, ct->t[0], ct->t[1] ); - ct->norm_impulse = 0.0f; - ct->tangent_impulse[0] = 0.0f; - ct->tangent_impulse[1] = 0.0f; -} - -/* - * calculate total move to depenetrate object from contacts. - * manifold should belong to ONE object only - */ -void rb_depenetrate( rb_ct *manifold, int len, v3f dt ) -{ - v3_zero( dt ); - - for( int j=0; j<7; j++ ){ - for( int i=0; in, dt ), - remaining = (ct->p-k_penetration_slop) - resolved_amt, - apply = vg_maxf( remaining, 0.0f ) * 0.4f; - - v3_muladds( dt, ct->n, apply, dt ); - } - } -} - -/* - * Initializing things like tangent vectors - */ -void rb_presolve_contacts( rb_ct *buffer, f32 dt, int len ) -{ - for( int i=0; ico, ct->rba->co, ra ); - v3_sub( ct->co, ct->rbb->co, rb ); - v3_cross( ra, ct->n, raCn ); - v3_cross( rb, ct->n, rbCn ); - - /* orient inverse inertia tensors */ - v3f raCnI, rbCnI; - m3x3_mulv( ct->rba->iIw, raCn, raCnI ); - m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI ); - - ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass; - ct->normal_mass += v3_dot( raCn, raCnI ); - ct->normal_mass += v3_dot( rbCn, rbCnI ); - ct->normal_mass = 1.0f/ct->normal_mass; - - for( int j=0; j<2; j++ ){ - v3f raCtI, rbCtI; - v3_cross( ct->t[j], ra, raCt ); - v3_cross( ct->t[j], rb, rbCt ); - m3x3_mulv( ct->rba->iIw, raCt, raCtI ); - m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI ); - - ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass; - ct->tangent_mass[j] += v3_dot( raCt, raCtI ); - ct->tangent_mass[j] += v3_dot( rbCt, rbCtI ); - ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j]; - } - } -} - -void rb_contact_restitution( rb_ct *ct, float cr ) -{ - v3f rv, ra, rb; - v3_sub( ct->co, ct->rba->co, ra ); - v3_sub( ct->co, ct->rbb->co, rb ); - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - - float v = v3_dot( rv, ct->n ); - - if( v < -1.0f ){ - ct->bias += -cr * v; - } -} - -/* - * One iteration to solve the contact constraint - */ -void rb_solve_contacts( rb_ct *buf, int len ) -{ - for( int i=0; ico, ct->rba->co, ra ); - v3_sub( ct->co, ct->rbb->co, rb ); - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - - /* Friction */ - for( int j=0; j<2; j++ ){ - float f = k_friction * ct->norm_impulse, - vt = v3_dot( rv, ct->t[j] ), - lambda = ct->tangent_mass[j] * -vt; - - float temp = ct->tangent_impulse[j]; - ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f ); - lambda = ct->tangent_impulse[j] - temp; - - v3f impulse; - v3_muls( ct->t[j], lambda, impulse ); - rb_linear_impulse( ct->rba, ra, impulse ); - - v3_muls( ct->t[j], -lambda, impulse ); - rb_linear_impulse( ct->rbb, rb, impulse ); - } - - /* Normal */ - rb_rcv( ct->rba, ct->rbb, ra, rb, rv ); - float vn = v3_dot( rv, ct->n ), - lambda = ct->normal_mass * (-vn + ct->bias); - - float temp = ct->norm_impulse; - ct->norm_impulse = vg_maxf( temp + lambda, 0.0f ); - lambda = ct->norm_impulse - temp; - - v3f impulse; - v3_muls( ct->n, lambda, impulse ); - rb_linear_impulse( ct->rba, ra, impulse ); - - v3_muls( ct->n, -lambda, impulse ); - rb_linear_impulse( ct->rbb, rb, impulse ); - } -} 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.c b/vg_rigidbody_constraints.c deleted file mode 100644 index ea26487..0000000 --- a/vg_rigidbody_constraints.c +++ /dev/null @@ -1,490 +0,0 @@ -/* - * ----------------------------------------------------------------------------- - * Constraints - * ----------------------------------------------------------------------------- - */ - -void rb_debug_position_constraints( rb_constr_pos *buffer, int len ) -{ - for( int i=0; irba, *rbb = constr->rbb; - - v3f wca, wcb; - m3x3_mulv( rba->to_world, constr->lca, wca ); - m3x3_mulv( rbb->to_world, constr->lcb, wcb ); - - v3f p0, p1; - v3_add( wca, rba->co, p0 ); - v3_add( wcb, rbb->co, p1 ); - vg_line_point( p0, 0.0025f, 0xff000000 ); - vg_line_point( p1, 0.0025f, 0xffffffff ); - vg_line2( p0, p1, 0xff000000, 0xffffffff ); - } -} - -void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) -{ - for( int i=0; irba->to_world, st->conevx, vx ); - m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); - m3x3_mulv( st->rba->to_world, st->conevy, vy ); - m3x3_mulv( st->rbb->to_world, st->coneva, va ); - m4x3_mulv( st->rba->to_world, st->view_offset, center ); - v3_cross( vy, vx, axis ); - - /* Constraint violated ? */ - float fx = v3_dot( vx, va ), /* projection world */ - fy = v3_dot( vy, va ), - fn = v3_dot( va, axis ), - - rx = st->conevx[3], /* elipse radii */ - ry = st->conevy[3], - - lx = fx/rx, /* projection local (fn==lz) */ - ly = fy/ry; - - st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f); - if( st->tangent_violation ){ - /* Calculate a good position and the axis to solve on */ - v2f closest, tangent, - p = { fx/fabsf(fn), fy/fabsf(fn) }; - - closest_point_elipse( p, (v2f){rx,ry}, closest ); - tangent[0] = -closest[1] / (ry*ry); - tangent[1] = closest[0] / (rx*rx); - v2_normalize( tangent ); - - v3f v0, v1; - v3_muladds( axis, vx, closest[0], v0 ); - v3_muladds( v0, vy, closest[1], v0 ); - v3_normalize( v0 ); - - v3_muls( vx, tangent[0], v1 ); - v3_muladds( v1, vy, tangent[1], v1 ); - - v3_copy( v0, st->tangent_target ); - v3_copy( v1, st->tangent_axis ); - - /* calculate mass */ - v3f aIw, bIw; - m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw ); - m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw ); - st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) + - v3_dot( st->tangent_axis, bIw )); - - float angle = v3_dot( va, st->tangent_target ); - } - - v3f refaxis; - v3_cross( vy, va, refaxis ); /* our default rotation */ - v3_normalize( refaxis ); - - float angle = v3_dot( refaxis, vxb ); - st->axis_violation = fabsf(angle) < st->conet; - - if( st->axis_violation ){ - v3f dir_test; - v3_cross( refaxis, vxb, dir_test ); - - if( v3_dot(dir_test, va) < 0.0f ) - st->axis_violation = -st->axis_violation; - - float newang = (float)st->axis_violation * acosf(st->conet-0.0001f); - - v3f refaxis_up; - v3_cross( va, refaxis, refaxis_up ); - v3_muls( refaxis_up, sinf(newang), st->axis_target ); - v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target ); - - /* calculate mass */ - v3_copy( va, st->axis ); - v3f aIw, bIw; - m3x3_mulv( st->rba->iIw, st->axis, aIw ); - m3x3_mulv( st->rbb->iIw, st->axis, bIw ); - st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) + - v3_dot( st->axis, bIw )); - } - } -} - -void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) -{ - float size = 0.12f; - - for( int i=0; irba->to_world, st->conevx, vx ); - m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); - m3x3_mulv( st->rba->to_world, st->conevy, vy ); - m3x3_mulv( st->rbb->to_world, st->coneva, va ); - m4x3_mulv( st->rba->to_world, st->view_offset, center ); - v3_cross( vy, vx, axis ); - - float rx = st->conevx[3], /* elipse radii */ - ry = st->conevy[3]; - - v3f p0, p1; - v3_muladds( center, va, size, p1 ); - vg_line( center, p1, 0xffffffff ); - vg_line_point( p1, 0.00025f, 0xffffffff ); - - if( st->tangent_violation ){ - v3_muladds( center, st->tangent_target, size, p0 ); - - vg_line( center, p0, 0xff00ff00 ); - vg_line_point( p0, 0.00025f, 0xff00ff00 ); - vg_line( p1, p0, 0xff000000 ); - } - - for( int x=0; x<32; x++ ){ - float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf, - t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf, - c0 = cosf( t0 ), - s0 = sinf( t0 ), - c1 = cosf( t1 ), - s1 = sinf( t1 ); - - v3f v0, v1; - v3_muladds( axis, vx, c0*rx, v0 ); - v3_muladds( v0, vy, s0*ry, v0 ); - v3_muladds( axis, vx, c1*rx, v1 ); - v3_muladds( v1, vy, s1*ry, v1 ); - - v3_normalize( v0 ); - v3_normalize( v1 ); - - v3_muladds( center, v0, size, p0 ); - v3_muladds( center, v1, size, p1 ); - - u32 col0r = fabsf(c0) * 255.0f, - col0g = fabsf(s0) * 255.0f, - col1r = fabsf(c1) * 255.0f, - col1g = fabsf(s1) * 255.0f, - col = st->tangent_violation? 0xff0000ff: 0xff000000, - col0 = col | (col0r<<16) | (col0g << 8), - col1 = col | (col1r<<16) | (col1g << 8); - - vg_line2( center, p0, VG__NONE, col0 ); - vg_line2( p0, p1, col0, col1 ); - } - - /* Draw twist */ - v3_muladds( center, va, size, p0 ); - v3_muladds( p0, vxb, size, p1 ); - - vg_line( p0, p1, 0xff0000ff ); - - if( st->axis_violation ){ - v3_muladds( p0, st->axis_target, size*1.25f, p1 ); - vg_line( p0, p1, 0xffffff00 ); - vg_line_point( p1, 0.0025f, 0xffffff80 ); - } - - v3f refaxis; - v3_cross( vy, va, refaxis ); /* our default rotation */ - v3_normalize( refaxis ); - v3f refaxis_up; - v3_cross( va, refaxis, refaxis_up ); - float newang = acosf(st->conet-0.0001f); - - v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 ); - v3_muladds( p1, refaxis, -cosf(newang)*size, p1 ); - vg_line( p0, p1, 0xff000000 ); - - v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 ); - v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 ); - vg_line( p0, p1, 0xff404040 ); - } -} - -void rb_solve_position_constraints( rb_constr_pos *buf, int len ) -{ - for( int i=0; irba, *rbb = constr->rbb; - - v3f wa, wb; - m3x3_mulv( rba->to_world, constr->lca, wa ); - m3x3_mulv( rbb->to_world, constr->lcb, wb ); - - m3x3f ssra, ssrat, ssrb, ssrbt; - - m3x3_skew_symetric( ssrat, wa ); - m3x3_skew_symetric( ssrbt, wb ); - m3x3_transpose( ssrat, ssra ); - m3x3_transpose( ssrbt, ssrb ); - - v3f b, b_wa, b_wb, b_a, b_b; - m3x3_mulv( ssra, rba->w, b_wa ); - m3x3_mulv( ssrb, rbb->w, b_wb ); - v3_add( rba->v, b_wa, b ); - v3_sub( b, rbb->v, b ); - v3_sub( b, b_wb, b ); - v3_muls( b, -1.0f, b ); - - m3x3f invMa, invMb; - m3x3_diagonal( invMa, rba->inv_mass ); - m3x3_diagonal( invMb, rbb->inv_mass ); - - m3x3f ia, ib; - m3x3_mul( ssra, rba->iIw, ia ); - m3x3_mul( ia, ssrat, ia ); - m3x3_mul( ssrb, rbb->iIw, ib ); - m3x3_mul( ib, ssrbt, ib ); - - m3x3f cma, cmb; - m3x3_add( invMa, ia, cma ); - m3x3_add( invMb, ib, cmb ); - - m3x3f A; - m3x3_add( cma, cmb, A ); - - /* Solve Ax = b ( A^-1*b = x ) */ - v3f impulse; - m3x3f invA; - m3x3_inv( A, invA ); - m3x3_mulv( invA, b, impulse ); - - v3f delta_va, delta_wa, delta_vb, delta_wb; - m3x3f iwa, iwb; - m3x3_mul( rba->iIw, ssrat, iwa ); - m3x3_mul( rbb->iIw, ssrbt, iwb ); - - m3x3_mulv( invMa, impulse, delta_va ); - m3x3_mulv( invMb, impulse, delta_vb ); - m3x3_mulv( iwa, impulse, delta_wa ); - m3x3_mulv( iwb, impulse, delta_wb ); - - v3_add( rba->v, delta_va, rba->v ); - v3_add( rba->w, delta_wa, rba->w ); - v3_sub( rbb->v, delta_vb, rbb->v ); - v3_sub( rbb->w, delta_wb, rbb->w ); - } -} - -void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len ) -{ - for( int i=0; iaxis_violation ) - continue; - - float rv = v3_dot( st->axis, st->rbb->w ) - - v3_dot( st->axis, st->rba->w ); - - if( rv * (float)st->axis_violation > 0.0f ) - continue; - - v3f impulse, wa, wb; - v3_muls( st->axis, rv*st->axis_mass, impulse ); - m3x3_mulv( st->rba->iIw, impulse, wa ); - v3_add( st->rba->w, wa, st->rba->w ); - - v3_muls( impulse, -1.0f, impulse ); - m3x3_mulv( st->rbb->iIw, impulse, wb ); - v3_add( st->rbb->w, wb, st->rbb->w ); - - float rv2 = v3_dot( st->axis, st->rbb->w ) - - v3_dot( st->axis, st->rba->w ); - } - - for( int i=0; itangent_violation ) - continue; - - float rv = v3_dot( st->tangent_axis, st->rbb->w ) - - v3_dot( st->tangent_axis, st->rba->w ); - - if( rv > 0.0f ) - continue; - - v3f impulse, wa, wb; - v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse ); - m3x3_mulv( st->rba->iIw, impulse, wa ); - v3_add( st->rba->w, wa, st->rba->w ); - - v3_muls( impulse, -1.0f, impulse ); - m3x3_mulv( st->rbb->iIw, impulse, wb ); - v3_add( st->rbb->w, wb, st->rbb->w ); - - float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) - - v3_dot( st->tangent_axis, st->rba->w ); - } -} - -/* debugging */ -void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len ) -{ - for( int i=0; iaxis_violation ){ - st->conv_axis = 0.0f; - continue; - } - - f32 rv = v3_dot( st->axis, st->rbb->w ) - - v3_dot( st->axis, st->rba->w ); - - if( rv * (f32)st->axis_violation > 0.0f ) - st->conv_axis = 0.0f; - else - st->conv_axis = rv; - } - - for( int i=0; itangent_violation ){ - st->conv_tangent = 0.0f; - continue; - } - - f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) - - v3_dot( st->tangent_axis, st->rba->w ); - - if( rv > 0.0f ) - st->conv_tangent = 0.0f; - else - st->conv_tangent = rv; - } -} - -void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb ) -{ - m3x3f ssra, ssrb, ssrat, ssrbt; - m3x3f cma, cmb; - - m3x3_skew_symetric( ssrat, ra ); - m3x3_skew_symetric( ssrbt, rb ); - m3x3_transpose( ssrat, ssra ); - m3x3_transpose( ssrbt, ssrb ); - - m3x3_mul( ssra, rba->iIw, cma ); - m3x3_mul( cma, ssrat, cma ); - m3x3_mul( ssrb, rbb->iIw, cmb ); - m3x3_mul( cmb, ssrbt, cmb ); - - m3x3f A, invA; - m3x3_add( cma, cmb, A ); - m3x3_inv( A, invA ); - - v3f b_wa, b_wb, b; - m3x3_mulv( ssra, rba->w, b_wa ); - m3x3_mulv( ssrb, rbb->w, b_wb ); - v3_add( b_wa, b_wb, b ); - v3_negate( b, b ); - - v3f impulse; - m3x3_mulv( invA, b, impulse ); - - v3f delta_wa, delta_wb; - m3x3f iwa, iwb; - m3x3_mul( rba->iIw, ssrat, iwa ); - m3x3_mul( rbb->iIw, ssrbt, iwb ); - m3x3_mulv( iwa, impulse, delta_wa ); - m3x3_mulv( iwb, impulse, delta_wb ); - v3_add( rba->w, delta_wa, rba->w ); - v3_sub( rbb->w, delta_wb, rbb->w ); -} - -void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt ) -{ - for( int i=0; irba, *rbb = constr->rbb; - - v3f p0, p1, d; - m3x3_mulv( rba->to_world, constr->lca, p0 ); - m3x3_mulv( rbb->to_world, constr->lcb, p1 ); - v3_add( rba->co, p0, p0 ); - v3_add( rbb->co, p1, p1 ); - v3_sub( p1, p0, d ); - -#if 1 - v3_muladds( rbb->co, d, -1.0f * amt, rbb->co ); - rb_update_matrices( rbb ); -#else - f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass), - a = mt * (k_phys_baumgarte/k_rb_delta); - - v3_muladds( rba->v, d, a* rba->inv_mass, rba->v ); - v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v ); -#endif - } -} - -void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf, int len, float amt ) -{ - for( int i=0; itangent_violation ) - continue; - - v3f va; - m3x3_mulv( st->rbb->to_world, st->coneva, va ); - - f32 angle = v3_dot( va, st->tangent_target ); - - if( fabsf(angle) < 0.9999f ){ - v3f axis; - v3_cross( va, st->tangent_target, axis ); -#if 1 - angle = acosf(angle) * amt; - v4f correction; - q_axis_angle( correction, axis, angle ); - q_mul( correction, st->rbb->q, st->rbb->q ); - q_normalize( st->rbb->q ); - rb_update_matrices( st->rbb ); -#else - f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass), - wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta); - //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w ); - v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w ); -#endif - } - } - - for( int i=0; iaxis_violation ) - continue; - - v3f vxb; - m3x3_mulv( st->rbb->to_world, st->conevxb, vxb ); - - f32 angle = v3_dot( vxb, st->axis_target ); - - if( fabsf(angle) < 0.9999f ){ - v3f axis; - v3_cross( vxb, st->axis_target, axis ); - -#if 1 - angle = acosf(angle) * amt; - v4f correction; - q_axis_angle( correction, axis, angle ); - q_mul( correction, st->rbb->q, st->rbb->q ); - q_normalize( st->rbb->q ); - rb_update_matrices( st->rbb ); -#else - f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass), - wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta); - //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w ); - v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w ); -#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 diff --git a/vg_shader.c b/vg_shader.c deleted file mode 100644 index e530014..0000000 --- a/vg_shader.c +++ /dev/null @@ -1,212 +0,0 @@ -const char *vg_shader_gl_ver = "#version 330 core\n"; - -/* - * Compile OpenGL subshader from GLSL source. Type is subshader type. - * If critical is set to 1, the program will fatal exit on compile failure. - */ -static GLuint vg_compile_opengl_subshader( GLint type, const c8 *src, bool critical, const c8 *debug_path ) -{ - GLuint shader = glCreateShader( type ); - - if( shader == 0 ) - { - vg_fatal_error( "glCreateShader returned 0.\n" ); - return 0; - } - - glShaderSource( shader, 2, (const char*[2]){ vg_shader_gl_ver, src }, NULL ); - glCompileShader( shader ); - - GLint status; - glGetShaderiv( shader, GL_COMPILE_STATUS, &status ); - - if( status == GL_TRUE ) - { - return shader; - } - else - { - GLchar info[1024]; - GLsizei len; - glGetShaderInfoLog( shader, sizeof(info), &len, info ); - - const char *type_str = "?"; - - if( type == GL_VERTEX_SHADER ) type_str = "GL_VERTEX_SHADER"; - else if( type == GL_FRAGMENT_SHADER ) type_str = "GL_FRAGMENT_SHADER"; - - if( critical ) - vg_fatal_error( "shader source path: %s\n %s subshader compile error:\n\n%s\n", debug_path, type_str, info ); - return 0; - } -} - -/* - * Final compilation by linking, if critical is 1, a fatal exit will occur on - * link failure - */ -static int vg_link_opengl_program( GLuint program, bool critical ) -{ - glLinkProgram( program ); - - GLint success; - glGetProgramiv( program, GL_LINK_STATUS, &success ); - - if( success ) return 1; - else - { - char info[ 512 ]; - glGetProgramInfoLog( program, sizeof(info), NULL, info ); - if( critical ) - vg_fatal_error( "Shader program link error:\n\n%s\n", info ); - return 0; - } -} - -/* - * Compile vg_shader from static source code. Will fatal exit if there is a - * compile error - */ -void vg_compile_shader( struct vg_shader *shader ) -{ - VG_ASSERT( shader->compiled == 0 ); - - const c8 *vs = _vg_shaders_glsl + shader->vs.glsl, - *fs = _vg_shaders_glsl + shader->fs.glsl; - GLuint vert = vg_compile_opengl_subshader( GL_VERTEX_SHADER, vs, 1, _vg_shaders_infos + shader->vs.src ), - frag = vg_compile_opengl_subshader( GL_FRAGMENT_SHADER, fs, 1, _vg_shaders_infos + shader->fs.src ), - program = glCreateProgram(); - - glAttachShader( program, vert ); - glAttachShader( program, frag ); - - vg_link_opengl_program( program, 1 ); - - glDeleteShader( vert ); - glDeleteShader( frag ); - - _vg_shader_names[ shader->names_start ] = program; - shader->compiled = 1; -} - -/* - * Recompile vg_shader from its original source files. This won't work in the - * shipped version of the engine. - */ -void vg_recompile_shader( struct vg_shader *shader ) -{ - VG_ASSERT( shader->compiled == 1 ); - - char error[260]; - char path_buf[260]; - vg_str path; - - vg_strnull( &path, path_buf, sizeof(path_buf) ); - vg_strcat( &path, "../../" ); - vg_strcat( &path, _vg_shaders_infos + shader->vs.src ); - VG_ASSERT( vg_strgood( &path ) ); - - c8 *vs = stb_include_file( path_buf, "", "../../shaders", error ); - - vg_strnull( &path, path_buf, sizeof(path_buf) ); - vg_strcat( &path, "../../" ); - vg_strcat( &path, _vg_shaders_infos + shader->fs.src ); - VG_ASSERT( vg_strgood( &path ) ); - - c8 *fs = stb_include_file( path_buf, "", "../../shaders", error ); - - if( !vs || !fs ) - { - vg_warn( "Could not recompile shader due to missing source files:\n" ); - - if( !vs ) vg_info( " Vertex: %s\n", _vg_shaders_infos + shader->vs.src ); - if( !fs ) vg_info( " Fragment: %s\n", _vg_shaders_infos + shader->fs.src ); - free( vs ); - free( fs ); - return; - } - - GLuint vert = vg_compile_opengl_subshader( GL_VERTEX_SHADER, vs, 0, _vg_shaders_infos + shader->vs.src ), - frag = vg_compile_opengl_subshader( GL_FRAGMENT_SHADER, fs, 0, _vg_shaders_infos + shader->fs.src ); - - free( vs ); - free( fs ); - - if( !vert || !frag ) return; - - GLuint program = glCreateProgram(); - - glAttachShader( program, vert ); - glAttachShader( program, frag ); - - if( vg_link_opengl_program( program, 0 ) ) - { - /* replace existing */ - glDeleteProgram( _vg_shader_names[ shader->names_start ] ); - _vg_shader_names[ shader->names_start ] = program; - } - else - { - /* womp womp */ - glDeleteProgram( program ); - } - - glDeleteShader( vert ); - glDeleteShader( frag ); -} - -void vg_free_shader( struct vg_shader *shader ) -{ - if( shader->compiled ) - { - shader->compiled = 0; - glDeleteProgram( _vg_shader_names[ shader->names_start ] ); - _vg_shader_names[ shader->names_start ] = 0; - for( u32 i=0; iuniform_count; i ++ ) - _vg_shader_names[ shader->names_start + 1 + i ] = 0; - } -} - -static void vg_shader_link( struct vg_shader *shader ) -{ - GLuint program = _vg_shader_names[ shader->names_start ]; - const c8 *uniform_alias = _vg_shaders_uniform_names + shader->uniform_aliases_offset; - - for( u32 i=0; iuniform_count; i ++ ) - { - u32 index = shader->names_start + 1 + i; - if( uniform_alias[0] == '$' ) _vg_shader_names[ index ] = glGetUniformBlockIndex( program, uniform_alias+1 ); - else _vg_shader_names[ index ] = glGetUniformLocation( program, uniform_alias ); - while( *uniform_alias ) - uniform_alias ++; - uniform_alias ++; - } -} - -VG_API void _vg_shaders_init(void) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_MAIN ) ); - - vg_info( "Compiling shaders\n" ); - for( int i=0; im_eResult == k_EResultOK ) - vg_logsteam( " New app ticket ready\n" ); - else - vg_logsteam( KYEL " Could not request new encrypted app ticket (%u)\n", response->m_eResult ); - - if( SteamAPI_ISteamUser_GetEncryptedAppTicket( _steam_api.pSteamUser, _steam_api.app_symmetric_key, - VG_ARRAY_LEN(_steam_api.app_symmetric_key), &_steam_api.app_key_length )) - { - vg_logsteam( KGRN " Loaded app ticket\n" ); - } - else - { - vg_logsteam( KRED " No ticket availible\n" ); - _steam_api.app_key_length = 0; - } -} -#endif - -#if defined( VG_SERVER ) -static u8 vg_char_base16( c8 c ) -{ - if( c >= '0' && c <= '9' ) - return c-'0'; - if( c >= 'a' && c <= 'f' ) - return (c-'a') + 10; - return 0; -} -#endif - -#if defined( VG_SERVER ) -VG_API bool _vg_steam_init( u32 unIP, u16 usGamePort, u16 usQueryPort, EServerMode eServerMode, - const c8 *pchVersionString, const c8 *appid_str ) -#else -VG_API bool _vg_steam_init(void) -#endif -{ - if( _steam_api.disabled ) - return 0; - - SteamErrMsg err; - - /* Steamworks init step - * ---------------------------------------------------------------------------- */ -#if defined( VG_ENGINE ) - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_STEAM ) ); - - const char *pszInternalCheckInterfaceVersions = - STEAMUTILS_INTERFACE_VERSION "\0" - STEAMNETWORKINGUTILS_INTERFACE_VERSION "\0" - STEAMAPPS_INTERFACE_VERSION "\0" - STEAMCONTROLLER_INTERFACE_VERSION "\0" - STEAMFRIENDS_INTERFACE_VERSION "\0" - STEAMGAMESEARCH_INTERFACE_VERSION "\0" - STEAMHTMLSURFACE_INTERFACE_VERSION "\0" - STEAMHTTP_INTERFACE_VERSION "\0" - STEAMINPUT_INTERFACE_VERSION "\0" - STEAMINVENTORY_INTERFACE_VERSION "\0" - STEAMMATCHMAKINGSERVERS_INTERFACE_VERSION "\0" - STEAMMATCHMAKING_INTERFACE_VERSION "\0" - STEAMMUSICREMOTE_INTERFACE_VERSION "\0" - STEAMMUSIC_INTERFACE_VERSION "\0" - STEAMNETWORKINGMESSAGES_INTERFACE_VERSION "\0" - STEAMNETWORKINGSOCKETS_INTERFACE_VERSION "\0" - STEAMNETWORKING_INTERFACE_VERSION "\0" - STEAMPARENTALSETTINGS_INTERFACE_VERSION "\0" - STEAMPARTIES_INTERFACE_VERSION "\0" - STEAMREMOTEPLAY_INTERFACE_VERSION "\0" - STEAMREMOTESTORAGE_INTERFACE_VERSION "\0" - STEAMSCREENSHOTS_INTERFACE_VERSION "\0" - STEAMUGC_INTERFACE_VERSION "\0" - STEAMUSERSTATS_INTERFACE_VERSION "\0" - STEAMUSER_INTERFACE_VERSION "\0" - STEAMVIDEO_INTERFACE_VERSION "\0" - - "\0"; - - if( SteamInternal_SteamAPI_Init( pszInternalCheckInterfaceVersions, &err ) != k_ESteamAPIInitResult_OK ) - { - _steam_api.disabled = 1; - vg_logsteam( KRED "SteamInternal_SteamAPI_Init() failed: '%s'\nAll steam interactions disabled for this session\n", err ); - return 0; - } -#endif - -#if defined( VG_SERVER ) - FILE *txt = fopen( "steam_appid.txt", "w" ); - fputs( appid_str, txt ); - fclose( txt ); - - // FIXME: Add no-auth launch option (hoist from skaterift, as we have it there, to vg steam module?) - vg_stack_allocator stack; - vg_stack_init( &stack, NULL, VG_KB(256), NULL ); - u32 size; - c8 *src = vg_file_read( &stack, "application_key", &size, 0 ); - if( src ) - { - if( size < k_nSteamEncryptedAppTicketSymmetricKeyLen ) - { - vg_error( "Application key was invalid size\n" ); - return 0; - } - - for( int i=0; iuserdata = NULL; - call->cb = cb_auth_ticket_recieved; - call->id = SteamAPI_ISteamUser_RequestEncryptedAppTicket( _steam_api.pSteamUser, NULL, 0 ); - } -# endif -#endif - - return 1; -} - -static const c8 *string_ESteamNetworkingConnectionState( ESteamNetworkingConnectionState s ) -{ - if( s == k_ESteamNetworkingConnectionState_None ) return "None"; - if( s == k_ESteamNetworkingConnectionState_Connecting) return "Connecting"; - if( s == k_ESteamNetworkingConnectionState_FindingRoute) return "Finding route"; - if( s == k_ESteamNetworkingConnectionState_Connected) return "Connected"; - if( s == k_ESteamNetworkingConnectionState_ClosedByPeer) return "Closed By Peer"; - if( s == k_ESteamNetworkingConnectionState_ProblemDetectedLocally) return "Problem detected locally"; - if( s == k_ESteamNetworkingConnectionState_FinWait) return "Finwait"; - if( s == k_ESteamNetworkingConnectionState_Linger) return "Linger"; - if( s == k_ESteamNetworkingConnectionState_Dead) return "Dead"; - return "enum-out-of-range"; -} - -static const c8 *string_ESteamAPICallFailure( ESteamAPICallFailure e ) -{ - if( e == k_ESteamAPICallFailureNone ) return "None"; - if( e == k_ESteamAPICallFailureSteamGone ) return "Steam Gone"; - if( e == k_ESteamAPICallFailureNetworkFailure ) return "Network Failure"; - if( e == k_ESteamAPICallFailureInvalidHandle ) return KBLK "Invalid Handle"; - if( e == k_ESteamAPICallFailureMismatchedCallback ) return "Mismatched Callback"; - return "enum-out-of-range"; -} - -void vg_steam_frame(void) -{ - if( _steam_api.disabled ) - return; - - SteamAPI_ManualDispatch_RunFrame( _steam_api.hPipe ); - - CallbackMsg_t callback; - while( SteamAPI_ManualDispatch_GetNextCallback( _steam_api.hPipe, &callback ) ) - { - /* Check for dispatching API call results */ - i32 type = callback.m_iCallback; - if( type == k_iSteamUtils_SteamAPICallCompleted ) - { - SteamAPICallCompleted_t *inf = callback.m_pubParam; - - bool bFailed; - void *call_data = alloca( inf->m_cubParam ); - - if( SteamAPI_ManualDispatch_GetAPICallResult( _steam_api.hPipe, inf->m_hAsyncCall, - call_data, inf->m_cubParam, - inf->m_iCallback, &bFailed ) ) - { - vg_logsteam( "api_call_completed %lu\n", inf->m_hAsyncCall ); - - int j=0; - for( int i=0; i<_steam_api.api_call_count; i++ ) - { - if( _steam_api.api_calls[i].id != inf->m_hAsyncCall ) - _steam_api.api_calls[j ++] = _steam_api.api_calls[i]; - else - _steam_api.api_calls[i].cb( call_data, _steam_api.api_calls[i].userdata ); - } - - if( _steam_api.api_call_count == j ) - vg_error( "No tracker was register for API call\n" ); - - _steam_api.api_call_count = j; - } - else - { - enum ESteamAPICallFailure e = - SteamAPI_ISteamUtils_GetAPICallFailureReason( _steam_api.pSteamUtils, inf->m_hAsyncCall ); - const c8 *fail_str = string_ESteamAPICallFailure( e ); - vg_logsteam( KRED "Error getting call result on %lu (code %d)\n", inf->m_hAsyncCall, fail_str ); - } - } - else - { - /* - * Look at callback.m_iCallback to see what kind of callback it is, - * and dispatch to appropriate handler(s) - * void *data = callback.m_pubParam; - */ - if( type == k_iSteamUser_SteamServersConnected ) - vg_success( "Steam servers connected" ); - else if( type == k_iSteamUser_SteamConnectFailure ) - { - SteamServerConnectFailure_t *inf = callback.m_pubParam; - vg_logsteam( KRED "Steam server connect failure, code %d, retrying: %d\n", inf->m_eResult, inf->m_bStillRetrying ); - } - else if( type == k_iSteamUser_SteamServersDisconnected ) - { - SteamServersDisconnected_t *inf = callback.m_pubParam; - vg_logsteam( "Steam servers disconnect, code %d\n", inf->m_eResult ); - } - else if( type == k_iSteamNetworkingSockets_SteamNetConnectionStatusChangedCallback ) - { - SteamNetConnectionStatusChangedCallback_t *inf = callback.m_pubParam; - const c8 *status_string = string_ESteamNetworkingConnectionState( inf->m_info.m_eState ); - vg_logsteam( "Connection status changed: %x -> %s\n Debug: '%s'\n EndDebug: '%s'\n", - inf->m_hConn, status_string, inf->m_info.m_szConnectionDescription, - inf->m_info.m_szEndDebug ); - - if( _steam_api.cb_connection_changed ) - _steam_api.cb_connection_changed( inf ); - } - else if( type == k_iSteamNetworkingSockets_SteamNetAuthenticationStatus ) - { - SteamNetAuthenticationStatus_t *inf = callback.m_pubParam; - vg_logsteam( "Steam Authentication Status: %d\n Debug: '%s'\n", inf->m_eAvail, inf->m_debugMsg ); - } - } - - SteamAPI_ManualDispatch_FreeLastCallback( _steam_api.hPipe ); - } -} - -VG_API void _vg_steam_shutdown(void) -{ -#if defined( VG_SERVER ) - if( _steam_api.is_connected ) - { - SteamAPI_ISteamGameServer_LogOff( _steam_api.pSteamGameServer ); - _steam_api.is_connected = 0; - } - SteamGameServer_Shutdown(); -#else - SteamAPI_Shutdown(); -#endif -} - -vg_steam_api_call *vg_alloc_async_steam_api_call(void) -{ - if( _steam_api.api_call_count == VG_ARRAY_LEN(_steam_api.api_calls) ) - return NULL; - return &_steam_api.api_calls[ _steam_api.api_call_count ++ ]; -} - -#if defined( VG_ENGINE ) -void vg_steam_set_achievement( const c8 *name, bool yes ) -{ - if( _steam_api.disabled || _steam_api.demo_mode ) - return; - - if( yes ) - { - if( SteamAPI_ISteamUserStats_SetAchievement( _steam_api.pSteamUserStats, name ) ) - { - vg_logsteam( KGRN "Set achievement '%s'\n", name ); - SteamAPI_ISteamUserStats_StoreStats( _steam_api.pSteamUserStats ); - } - } - else - { - if( SteamAPI_ISteamUserStats_ClearAchievement( _steam_api.pSteamUserStats, name ) ) - { - vg_logsteam( KBLK "Clear achievement '%s'\n", name ); - SteamAPI_ISteamUserStats_StoreStats( _steam_api.pSteamUserStats ); - } - } -} -#endif diff --git a/vg_tex.c b/vg_tex.c deleted file mode 100644 index 7acd81c..0000000 --- a/vg_tex.c +++ /dev/null @@ -1,430 +0,0 @@ -#define QOI_OP_INDEX 0x00 /* 00xxxxxx */ -#define QOI_OP_DIFF 0x40 /* 01xxxxxx */ -#define QOI_OP_LUMA 0x80 /* 10xxxxxx */ -#define QOI_OP_RUN 0xc0 /* 11xxxxxx */ -#define QOI_OP_RGB 0xfe /* 11111110 */ -#define QOI_OP_RGBA 0xff /* 11111111 */ -#define QOI_MASK_2 0xc0 /* 11000000 */ - -#define QOI_COLOR_HASH(C) (C.rgba[0]*3 + C.rgba[1]*5 + C.rgba[2]*7 + C.rgba[3]*11) -#define QOI_MAGIC \ - (((u32)'q') | ((u32)'o') << 8 | \ - ((u32)'i') << 16 | ((u32)'f') << 24) - -static const u8 qoi_padding[8] = {0,0,0,0,0,0,0,1}; - -#define cpu_to_big32 big32_to_cpu -VG_TIER_0 static inline u32 big32_to_cpu( u32 x ) -{ - return ((x & 0xFF) << 24) | ((x & 0xFF00) << 8) | ((x & 0xFF0000) >> 8) | (x >> 24); -} - -VG_TIER_0 bool vg_qoi_validate( const qoi_desc *desc ) -{ - if( (desc->width == 0) || (desc->height == 0) || - (desc->width >= 2048) || (desc->height >= 2048) ) - { - vg_error( "QOI file is invalid; Unpermitted size (%ux%u)\n", desc->width, desc->height ); - return 0; - } - - if( !(desc->channels == 3 || desc->channels == 4) ) - { - vg_error( "QOI file is invalid; Only 3 or 4 channels permitted. File has %u\n", desc->channels ); - return 0; - } - - return 1; -} - -/* Initalize stream context and return the number of bytes required to store the final RGB/A image data. */ -VG_TIER_0 u32 vg_qoi_stream_init( qoi_desc *desc, vg_stream *stream ) -{ - vg_stream_read( stream, desc, sizeof(qoi_desc) ); - if( desc->magic != QOI_MAGIC ) - { - vg_error( "QOI file is invalid; Magic Number incorrect.\n" ); - return 0; - } - - desc->width = big32_to_cpu( desc->width ); - desc->height = big32_to_cpu( desc->height ); - - if( vg_qoi_validate( desc ) ) - return desc->width * desc->height * desc->channels; - else return 0; -} - -VG_TIER_0 void vg_qoi_stream_decode( qoi_desc *desc, vg_stream *stream, u8 *pixels, bool v_flip ) -{ - qoi_rgba_t index[64], px; - vg_zero_mem( index, sizeof(qoi_rgba_t)*64 ); - vg_zero_mem( &px, sizeof(qoi_rgba_t) ); - px.rgba[3] = 255; - - u32 run=0; - - for( u32 y=0; yheight; y ++ ) - { - for( u32 x=0; xwidth; x ++ ) - { - if( run > 0 ) - run --; - else - { - u8 b1; - vg_stream_read( stream, &b1, 1 ); - - if( b1 == QOI_OP_RGB ) - vg_stream_read( stream, px.rgba, 3 ); - else if( b1 == QOI_OP_RGBA ) - vg_stream_read( stream, px.rgba, 4 ); - else if( (b1 & QOI_MASK_2) == QOI_OP_INDEX ) - px = index[b1]; - else if( (b1 & QOI_MASK_2) == QOI_OP_DIFF ) - { - px.rgba[0] += (i32)((b1 >> 4) & 0x03) - 2; - px.rgba[1] += (i32)((b1 >> 2) & 0x03) - 2; - px.rgba[2] += (i32)( b1 & 0x03) - 2; - } - else if( (b1 & QOI_MASK_2) == QOI_OP_LUMA ) - { - u8 b2; - vg_stream_read( stream, &b2, 1 ); - i32 vg = (i32)(b1 & 0x3f) - 32; - px.rgba[0] += vg - 8 + (i32)((b2 >> 4) & 0x0f); - px.rgba[1] += vg; - px.rgba[2] += vg - 8 + (i32)(b2 & 0x0f); - } - else if( (b1 & QOI_MASK_2) == QOI_OP_RUN ) - run = (b1 & 0x3f); - index[ QOI_COLOR_HASH(px) % 64 ] = px; - } - - u32 row = v_flip? desc->height-(y+1): y; - for( u32 i=0; i < desc->channels; i ++ ) - pixels[ ((row*desc->width) + x)*desc->channels + i ] = px.rgba[i]; - } - } -} - -VG_TIER_0 u32 vg_query_qoi_max_compressed_size( const qoi_desc *desc ) -{ - return desc->width * desc->height * (desc->channels + 1) + sizeof(qoi_desc) + sizeof(qoi_padding); -} - -VG_TIER_0 u32 vg_qoi_stream_encode( const qoi_desc *desc, const u8 *pixels, vg_stream *stream, bool v_flip ) -{ - if( !vg_qoi_validate( desc ) ) - return 0; - - qoi_desc file_header = *desc; - file_header.magic = QOI_MAGIC; - file_header.width = cpu_to_big32( file_header.width ); - file_header.height = cpu_to_big32( file_header.height ); - vg_stream_write( stream, &file_header, sizeof(qoi_desc) ); - - qoi_rgba_t index[64]; - vg_zero_mem( index, sizeof(qoi_rgba_t)*64 ); - qoi_rgba_t px_prev = { .rgba = { 0, 0, 0, 255 } }; - qoi_rgba_t px = px_prev; - - u32 run = 0; - for( u32 y=0; yheight; y ++ ) - { - for( u32 x=0; xwidth; x ++ ) - { - u32 row = v_flip? desc->height-(y+1): y; - for( u32 i=0; i < desc->channels; i ++ ) - px.rgba[i] = pixels[ ((row*desc->width) + x)*desc->channels + i ]; - - if( px.v == px_prev.v ) - { - run ++; - if( run == 62 || ((y+1 == desc->height) && (x+1 == desc->width)) ) - { - u8 b1 = QOI_OP_RUN | (run - 1); - vg_stream_write( stream, &b1, 1 ); - run = 0; - } - } - else - { - if( run > 0 ) - { - u8 b1 = QOI_OP_RUN | (run - 1); - vg_stream_write( stream, &b1, 1 ); - run = 0; - } - - u32 index_pos = QOI_COLOR_HASH( px ) % 64; - if( index[ index_pos ].v == px.v ) - { - u8 b1 = QOI_OP_INDEX | index_pos; - vg_stream_write( stream, &b1, 1 ); - } - else - { - index[ index_pos ] = px; - if( px.rgba[3] == px_prev.rgba[3] ) - { - i8 vr = px.rgba[0] - px_prev.rgba[0], - vg = px.rgba[1] - px_prev.rgba[1], - vb = px.rgba[2] - px_prev.rgba[2], - vg_r = vr - vg, - vg_b = vb - vg; - - if( vr > -3 && vr < 2 && - vg > -3 && vg < 2 && - vb > -3 && vb < 2 ) - { - vg_stream_write( stream, (u8[]){ QOI_OP_DIFF | (vr + 2) << 4 | (vg + 2) << 2 | (vb + 2) }, 1 ); - } - else if( vg_r > -9 && vg_r < 8 && - vg > -33 && vg < 32 && - vg_b > -9 && vg_b < 8 ) - { - vg_stream_write( stream, (u8[]){ QOI_OP_LUMA | (vg + 32), - (vg_r + 8) << 4 | (vg_b + 8) }, 2 ); - } - else - { - vg_stream_write( stream, (u8[]){ QOI_OP_RGB }, 1 ); - vg_stream_write( stream, px.rgba, 3 ); - } - } - else - { - vg_stream_write( stream, (u8 []){ QOI_OP_RGBA }, 1 ); - vg_stream_write( stream, px.rgba, 4 ); - } - } - } - px_prev = px; - } - } - vg_stream_write( stream, qoi_padding, sizeof(qoi_padding) ); - return 1; -} - -/* VG_PART - * ------------------------------------------------------------------------------------------------------------------ */ - -struct -{ - GLuint error2d, errorcube; -} -_vg_tex; - -VG_API void _vg_tex_init(void) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_OPENGL ) ); - - static u8 const_vg_tex2d_err[] = - { - 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, - 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, - 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, - 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, 0x00,0x00,0x00,0xff, 0xff,0x00,0xff,0xff, - }; - - glGenTextures( 1, &_vg_tex.error2d ); - glBindTexture( GL_TEXTURE_2D, _vg_tex.error2d ); - glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 4, 4, 0, GL_RGBA, GL_UNSIGNED_BYTE, const_vg_tex2d_err ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT ); - - glGenTextures( 1, &_vg_tex.errorcube ); - glBindTexture( GL_TEXTURE_CUBE_MAP, _vg_tex.errorcube ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, GL_LINEAR ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, GL_LINEAR ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE ); - - for( u32 j=0; j<6; j ++ ) - { - glTexImage2D( GL_TEXTURE_CUBE_MAP_POSITIVE_X + j, 0, GL_RGBA, 4, 4, - 0, GL_RGBA, GL_UNSIGNED_BYTE, const_vg_tex2d_err ); - } -} - -struct tex_upload_task -{ - vg_tex *tex; - u32 width, height, channels, flags; - u8 image_buffer[]; -}; - -VG_API_INTERNAL static void _vg_tex_upload( struct tex_upload_task *in_args, vg_async_info *async ) -{ - VG_ASSERT( _vg_tex.errorcube && _vg_tex.error2d ); - - u32 flags = in_args->flags; - vg_tex *tex = in_args->tex; - - if( flags & VG_TEX_ERROR ) - { - tex->name = (flags & VG_TEX_CUBEMAP)? _vg_tex.errorcube: _vg_tex.error2d; - tex->flags = (flags & VG_TEX_CUBEMAP) | VG_TEX_ERROR | VG_TEX_COMPLETE; - } - else - { - u32 pixel_format = 0; - if( in_args->channels == 3 ) pixel_format = GL_RGB; - else if( in_args->channels == 4 ) pixel_format = GL_RGBA; - else vg_fatal_error( "Can't upload texture with '%u' channels.\n", in_args->channels ); - - glGenTextures( 1, &tex->name ); - - u32 filter_min = 0, - filter_mag = 0; - if( flags & VG_TEX_LINEAR ) - { - if( flags & VG_TEX_NOMIP ) filter_min = GL_LINEAR; - else filter_min = GL_LINEAR_MIPMAP_LINEAR; - filter_mag = GL_LINEAR; - } - else - { - VG_ASSERT( flags & VG_TEX_NEAREST ); - filter_min = GL_NEAREST; - filter_mag = GL_NEAREST; - } - - if( flags & VG_TEX_CUBEMAP ) - { - u32 w = in_args->width, - h = in_args->height/6; - - glBindTexture( GL_TEXTURE_CUBE_MAP, tex->name ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MIN_FILTER, filter_min ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_MAG_FILTER, filter_mag ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE ); /* can this be anything else? */ - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE ); - glTexParameteri( GL_TEXTURE_CUBE_MAP, GL_TEXTURE_WRAP_R, GL_CLAMP_TO_EDGE ); - - for( u32 j=0; j<6; j ++ ) - { - u32 offset = w*h*j*in_args->channels; - glTexImage2D( GL_TEXTURE_CUBE_MAP_POSITIVE_X + j, 0, pixel_format, - w, h, - 0, pixel_format, GL_UNSIGNED_BYTE, in_args->image_buffer + offset ); - } - - if( !(flags & VG_TEX_NOMIP) ) - glGenerateMipmap( GL_TEXTURE_CUBE_MAP ); - } - else - { - glBindTexture( GL_TEXTURE_2D, tex->name ); - glTexImage2D( GL_TEXTURE_2D, 0, pixel_format, in_args->width, in_args->height, - 0, pixel_format, GL_UNSIGNED_BYTE, in_args->image_buffer ); - - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, filter_min ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, filter_mag ); - - u32 wrap_s = 0, - wrap_t = 0; - - if( flags & VG_TEX_CLAMP ) - { - wrap_s = GL_CLAMP_TO_EDGE; - wrap_t = GL_CLAMP_TO_EDGE; - } - else - { - VG_ASSERT( flags & VG_TEX_REPEAT ); - wrap_s = GL_REPEAT; - wrap_t = GL_REPEAT; - } - - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrap_s ); - glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrap_t ); - - if( !(flags & VG_TEX_NOMIP) ) - glGenerateMipmap( GL_TEXTURE_2D ); - } - tex->flags = flags | VG_TEX_COMPLETE; - } -} - -VG_API bool _vg_tex_load_stream( vg_tex *out_tex, vg_stream *in_stream, u32 flags ) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); - - qoi_desc qoi; - u32 size = vg_qoi_stream_init( &qoi, in_stream ); - if( size ) - { - _vg_async_context_push_groups( VG_ASYNC_GROUP_OPENGL, 0 ); - struct tex_upload_task *out_args = _vg_async_alloc( VG_THREAD_MAIN_ID, sizeof( struct tex_upload_task ) + size ); - vg_qoi_stream_decode( &qoi, in_stream, out_args->image_buffer, flags & VG_TEX_FLIP_V? 1: 0 ); - out_args->tex = out_tex; - out_args->width = qoi.width; - out_args->height = qoi.height; - out_args->channels = qoi.channels; - out_args->flags = flags; - _vg_async_send( out_args, (vg_async_fn)_vg_tex_upload ); - _vg_async_context_pop_groups(); - return 1; - } - else - return 0; -} - -VG_API void _vg_tex_load( vg_tex *out_tex, const c8 *path, u32 flags ) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_BACKGROUND ) ); - - bool error = 0; - vg_stream file; - if( vg_file_stream_open( &file, path, VG_STREAM_READ ) ) - { - if( !_vg_tex_load_stream( out_tex, &file, flags ) ) - error = 1; - vg_file_stream_close( &file ); - } - else - error = 1; - - if( error ) - { - _vg_async_context_push_groups( VG_ASYNC_GROUP_OPENGL, 0 ); - struct tex_upload_task *out_args = _vg_async_alloc( VG_THREAD_MAIN_ID, sizeof( struct tex_upload_task ) ); - out_args->tex = out_tex; - out_args->width = 0; - out_args->height = 0; - out_args->channels = 0; - out_args->flags = VG_TEX_ERROR; - _vg_async_send( out_args, (vg_async_fn)_vg_tex_upload ); - _vg_async_context_pop_groups(); - } -} - -VG_API u32 vg_tex_name( GLuint target, vg_tex *tex ) -{ - if( !tex ) - { - return (target == GL_TEXTURE_2D)? _vg_tex.error2d: _vg_tex.errorcube; - } - if( tex->flags & VG_TEX_COMPLETE ) return tex->name; - else return (target == GL_TEXTURE_2D)? _vg_tex.error2d: _vg_tex.errorcube; -} - -VG_API void vg_tex_bind( GLuint target, vg_tex *tex, u32 slot ) -{ - glActiveTexture( GL_TEXTURE0 + slot ); - glBindTexture( target, vg_tex_name( target, tex ) ); -} - -VG_API void vg_tex_delete( vg_tex *tex ) -{ - VG_ASSERT( _vg_thread_has_flags( VG_THREAD_OWNS_OPENGL ) ); - VG_ASSERT( tex->flags & VG_TEX_COMPLETE ); - if( !(tex->flags & VG_TEX_ERROR) ) - glDeleteTextures( 1, &tex->name ); - vg_zero_mem( tex, sizeof(vg_tex) ); -}