things!
This commit is contained in:
veclavtlica
@@ -0,0 +1,30 @@
|
||||
#ifndef COM_BITS_H
|
||||
#define COM_BITS_H
|
||||
|
||||
#include "def.h"
|
||||
#include <assert.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* How many bits needed to represent a given number */
|
||||
/* Alternitive semntic: nearest upper power of two */
|
||||
static inline int com_bits_needed(uint32_t v) {
|
||||
#ifdef COM_DEF_COMPILE_MODERN
|
||||
/* Use intrinsic, required for fast sqrt impl */
|
||||
return v == 0 ? 1 : 32 - __builtin_clz(v);
|
||||
#else
|
||||
#endif
|
||||
if (v == 0)
|
||||
return 1;
|
||||
|
||||
v--;
|
||||
v |= v >> 1;
|
||||
v |= v >> 2;
|
||||
v |= v >> 4;
|
||||
v |= v >> 8;
|
||||
v |= v >> 16;
|
||||
v++;
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,19 @@
|
||||
/*
|
||||
Compiler definitions helpers, for portability.
|
||||
*/
|
||||
|
||||
#ifndef COM_DEF_H
|
||||
#define COM_DEF_H
|
||||
|
||||
#if (defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201112L) || \
|
||||
defined(__GNUC__) || defined(__clang__)
|
||||
#define COM_DEF_COMPILE_MODERN 1
|
||||
#endif
|
||||
|
||||
#ifdef COM_DEF_COMPILE_MODERN
|
||||
#define com_def_alignedas(v_as) _Alignas(v_as)
|
||||
#else
|
||||
#define com_def_alignedas(v_as)
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,71 @@
|
||||
#include "fixed.h"
|
||||
#include "bits.h"
|
||||
#include <assert.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
// 4-bit LUT (16 entries) for the normalized range [0.5, 2.0)
|
||||
// It stores the initial guess scaled to Q16.16.
|
||||
static const uint32_t com_fixed_sqrt_lut[16] = {
|
||||
46340, 49547, 52521, 55314, 57954, 60464, 62862, 65161,
|
||||
67373, 69508, 71572, 73572, 75514, 77402, 79240, 81033};
|
||||
|
||||
com_fixed_t com_fixed_sqrt(com_fixed_t a) {
|
||||
// 1. Handle sign and edge cases
|
||||
assert(a >= 0);
|
||||
if (a == 0)
|
||||
return 0;
|
||||
|
||||
// 2. Normalize input to the range [0.5, 2.0) to maximize LUT precision
|
||||
// clz = count leading zeros. On modern hardware, use __builtin_clz
|
||||
int leading_zeros = com_bits_needed(a);
|
||||
|
||||
// Calculate how much we need to shift to place the highest bit properly
|
||||
// We want the value to land squarely within an optimal window
|
||||
int shift = (31 - leading_zeros) - COM_FIXED_FRACBITS;
|
||||
|
||||
// Normalize shift to always be even so we can cleanly pull out 2^(shift/2)
|
||||
if (shift & 1)
|
||||
shift -= 1;
|
||||
|
||||
uint32_t normalized_a;
|
||||
if (shift > 0) {
|
||||
normalized_a = a >> shift;
|
||||
} else {
|
||||
normalized_a = a << (-shift);
|
||||
}
|
||||
|
||||
// 3. LUT Lookup using 4 MSBs of the normalized value
|
||||
// Extracted index corresponds to the interval [0.5, 2.0)
|
||||
uint32_t lut_index = (normalized_a >> (COM_FIXED_FRACBITS - 3)) & 0xF;
|
||||
uint64_t x = com_fixed_sqrt_lut[lut_index];
|
||||
|
||||
// 4. Newton-Raphson Iterations: x = 0.5 * (x + normalized_a / x)
|
||||
// We upscale to 64-bit to prevent intermediate overflow during division
|
||||
x = (x + ((uint64_t)normalized_a << COM_FIXED_FRACBITS) / x) >>
|
||||
1; // Iteration 1
|
||||
x = (x + ((uint64_t)normalized_a << COM_FIXED_FRACBITS) / x) >>
|
||||
1; // Iteration 2
|
||||
|
||||
// 5. Denormalize back to the target scale: result = x * 2^(shift / 2)
|
||||
int32_t final_shift = shift / 2;
|
||||
if (final_shift > 0) {
|
||||
return (int32_t)(x << final_shift);
|
||||
} else {
|
||||
return (int32_t)(x >> (-final_shift));
|
||||
}
|
||||
}
|
||||
|
||||
void com_fixed_print(com_fixed_t a) {
|
||||
if (a < 0) {
|
||||
printf("-");
|
||||
a = -a;
|
||||
}
|
||||
int32_t int_part = a >> 16;
|
||||
int32_t frac_part = a & 0xFFFF;
|
||||
|
||||
// Convert fractional 16-bit part to a decimal value (up to 4 decimal places)
|
||||
uint32_t decimal_val = (frac_part * 10000) >> 16;
|
||||
|
||||
printf("%d.%04u\n", int_part, decimal_val);
|
||||
}
|
||||
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
Doom-inspired Q15.16 format used for most of everything.
|
||||
https://hackmd.io/9uRB9YbTSBW4Qz_2b8TyDw#Examples-2-DOOM
|
||||
*/
|
||||
|
||||
#ifndef COM_FIXED_H
|
||||
#define COM_FIXED_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define COM_FIXED_FRACBITS 16
|
||||
#define COM_FIXED_FRACUNIT (1 << COM_FIXED_FRACBITS)
|
||||
|
||||
typedef int32_t com_fixed_t;
|
||||
|
||||
static inline com_fixed_t com_fixed_add(com_fixed_t a, com_fixed_t b) {
|
||||
return a + b;
|
||||
}
|
||||
|
||||
static inline com_fixed_t com_fixed_sub(com_fixed_t a, com_fixed_t b) {
|
||||
return a - b;
|
||||
}
|
||||
|
||||
static inline com_fixed_t com_fixed_mul(com_fixed_t a, com_fixed_t b) {
|
||||
return (com_fixed_t)(((int64_t)a * (int64_t)b) >> COM_FIXED_FRACBITS);
|
||||
}
|
||||
|
||||
/* Note: this does not clamp for over/underflow cases */
|
||||
static inline com_fixed_t com_fixed_div(com_fixed_t a, com_fixed_t b) {
|
||||
return (com_fixed_t)(((int64_t)a << COM_FIXED_FRACBITS) / (int64_t)b);
|
||||
}
|
||||
|
||||
/* Approximate square root using Newton-Raphson in 2 iterations over small LUT*/
|
||||
com_fixed_t com_fixed_sqrt(com_fixed_t a);
|
||||
|
||||
void com_fixed_print(com_fixed_t a);
|
||||
|
||||
#endif
|
||||
+154
@@ -0,0 +1,154 @@
|
||||
/*
|
||||
LZW compression that is required for GIF implementation.
|
||||
https://en.wikipedia.org/wiki/Lempel%E2%80%93Ziv%E2%80%93Welch
|
||||
*/
|
||||
|
||||
#include "lzw.h"
|
||||
#include "bits.h"
|
||||
#include <assert.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
/* References: */
|
||||
/* https://www.w3.org/Graphics/GIF/spec-gif89a.txt */
|
||||
/* https://www.daubnet.com/en/file-format-gif */
|
||||
|
||||
/* After it is reached the table is supposed to be reset */
|
||||
/* As this is known, the upper boundary of memory used is known */
|
||||
#define COM_LZW_CODEPOINT_LIMIT 8192
|
||||
#define COM_TABLE_BIT_WIDTH_LIMIT 12 /* typical GIF impl */
|
||||
|
||||
#define COM_LZW_OUTPUT_CAP_GROWTH 256
|
||||
#define COM_LZW_TABLE_CAP_GROW 128
|
||||
|
||||
struct com_lzw_table com_lzw_infer_table(const char *datain, uint32_t sizein) {
|
||||
struct com_lzw_table_entry *table = NULL;
|
||||
uint32_t table_init_size = 0;
|
||||
uint32_t table_size = 0;
|
||||
uint32_t table_cap = 0;
|
||||
|
||||
/* Infer initial table from the data */
|
||||
for (uint32_t i = 0; i < sizein && table_init_size < 255; i++) {
|
||||
bool code_found = false;
|
||||
char code = datain[i];
|
||||
/* Try searching the code */
|
||||
for (uint32_t t = 0; t < table_init_size; t++) {
|
||||
if (table[t].code == code) {
|
||||
code_found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Append non-existing codepoint */
|
||||
if (!code_found) {
|
||||
if (table_size >= table_cap) {
|
||||
/* TODO: Ref to prev table gets missed, memory leak scenario */
|
||||
if (!(table = realloc(table, sizeof(struct com_lzw_table_entry) *
|
||||
(table_cap + COM_LZW_TABLE_CAP_GROW))))
|
||||
goto ERR_ALLOC_INIT_TABLE;
|
||||
|
||||
table_cap += COM_LZW_TABLE_CAP_GROW;
|
||||
}
|
||||
|
||||
table[table_size] = (struct com_lzw_table_entry){.code = code};
|
||||
table_init_size++;
|
||||
table_size++;
|
||||
}
|
||||
}
|
||||
|
||||
return (struct com_lzw_table){
|
||||
.table = table,
|
||||
.cap = table_cap,
|
||||
.size = table_size,
|
||||
.init_size = table_init_size,
|
||||
.continuous = false,
|
||||
};
|
||||
|
||||
ERR_ALLOC_INIT_TABLE:
|
||||
if (table_cap > 0)
|
||||
free(table);
|
||||
return (struct com_lzw_table){0};
|
||||
}
|
||||
|
||||
void com_lzw_free_table(struct com_lzw_table *table) {
|
||||
if (!table->table)
|
||||
return;
|
||||
free(table->table);
|
||||
table->size = 0;
|
||||
table->cap = 0;
|
||||
table->init_size = 0;
|
||||
}
|
||||
|
||||
/* For GIFs, color should already be collapsed to indices at this point */
|
||||
bool com_lzw_compress(const struct com_lzw_table *table, const char *datain,
|
||||
uint32_t sizein, char **dataout, uint32_t *sizeout) {
|
||||
assert(datain && dataout && sizeout && sizein > 0);
|
||||
assert(*dataout == NULL && *sizeout == 0);
|
||||
|
||||
/* TODO: Make sure the table is cleared */
|
||||
|
||||
/* Encode step */
|
||||
uint8_t codesize = com_bits_needed(table->init_size) + 1;
|
||||
assert(codesize < COM_TABLE_BIT_WIDTH_LIMIT);
|
||||
|
||||
uint32_t cur_table_idx =
|
||||
0; /* Head table in which vector we should be looking into */
|
||||
uint32_t feedback =
|
||||
0; /* How many bytes feeded from datain to the current string */
|
||||
|
||||
char *output = NULL;
|
||||
uint32_t output_size = 0;
|
||||
uint32_t output_cap = 0;
|
||||
uint8_t output_bitshift = 0;
|
||||
|
||||
for (uint32_t i = 0; i < sizein; i++) {
|
||||
char code = datain[i];
|
||||
if (feedback == 0) {
|
||||
/* Should not be possible not to find the first code */
|
||||
for (uint32_t t = 0; t < table->init_size; t++) {
|
||||
if (table->table[t].code == code) {
|
||||
cur_table_idx = t;
|
||||
feedback++;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// for (uint32_t v = 0; v < table->table[cur_table_idx].tree_vector_size;
|
||||
// v++) {
|
||||
// if (table->table[table->table[cur_table_idx].tree_vector[v]].code ==
|
||||
// code) {
|
||||
// cur_table_idx = table->table[cur_table_idx].tree_vector[v];
|
||||
// feedback++;
|
||||
// continue;
|
||||
// }
|
||||
// }
|
||||
|
||||
/* Emit output, drop the string */
|
||||
assert(cur_table_idx < (1 << codesize));
|
||||
assert(cur_table_idx < table->size);
|
||||
|
||||
if (output_size == output_cap && output_bitshift + codesize > 8) {
|
||||
/* TODO: catch alloc failure */
|
||||
output = realloc(output, output_cap + COM_LZW_OUTPUT_CAP_GROWTH);
|
||||
output_cap += COM_LZW_OUTPUT_CAP_GROWTH;
|
||||
}
|
||||
|
||||
// uint16_t code = cur_table_idx;
|
||||
// uint8_t bits_to_write = codesize;
|
||||
// while (bits_to_write > 0) {
|
||||
// bits_to_write -=
|
||||
// }
|
||||
|
||||
/* Return to prev char as it wasn't processed */
|
||||
feedback = 0;
|
||||
i--;
|
||||
}
|
||||
}
|
||||
|
||||
*sizeout = output_size;
|
||||
*dataout = output;
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
#ifndef COM_LZW_H
|
||||
#define COM_LZW_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* Alphabet table for use in LZW algorithms */
|
||||
struct com_lzw_table {
|
||||
/* table_size indexed entry is reserved for clear code */
|
||||
/* table_size+1 indexed entry is reserved for terminator */
|
||||
struct com_lzw_table_entry *table;
|
||||
uint32_t init_size; /* Alphabet size, table clear is done over it */
|
||||
uint32_t size;
|
||||
uint32_t cap;
|
||||
bool continuous; /* If true, first order search is simple indexing, where
|
||||
codepoint = index, up to table_init_size */
|
||||
};
|
||||
|
||||
struct com_lzw_table_entry {
|
||||
uint16_t child_chain; /* Start of linked list inside table, or 0 if none */
|
||||
uint16_t next_child; /* Index of next child, or 0 if none */
|
||||
char code; /* Single byte, children bytes are appended to it */
|
||||
};
|
||||
|
||||
/* Returns table that is optimized for continuous range of codepoints */
|
||||
/* Useful for binary compression, such as .GIF and .SAM formats */
|
||||
struct com_lzw_table com_lzw_continous_table(uint8_t codepoints);
|
||||
|
||||
/* Returns table with alphabet inferred from the incoming data */
|
||||
/* Useful for alphanumeric compression, where not all codepoints are in use */
|
||||
/* TODO: Sort table by frequency? */
|
||||
struct com_lzw_table com_lzw_infer_table(const char *datain, uint32_t sizein);
|
||||
|
||||
/* Each bit corresponds to a byte value, position-wise */
|
||||
struct com_lzw_encoded_alphabet {
|
||||
uint64_t b0;
|
||||
uint64_t b1;
|
||||
uint64_t b2;
|
||||
uint64_t b3;
|
||||
};
|
||||
struct com_lzw_encoded_alphabet
|
||||
com_lzw_encode_alphabet(const struct com_lzw_table *table);
|
||||
|
||||
struct com_lzw_table
|
||||
com_lzw_decode_table(struct com_lzw_encoded_alphabet alphabet);
|
||||
|
||||
/* Return table to its original form */
|
||||
void com_lzw_clear_table(struct com_lzw_table *table);
|
||||
|
||||
void com_lzw_free_table(struct com_lzw_table *table);
|
||||
|
||||
bool com_lzw_compress(const struct com_lzw_table *table, const char *datain,
|
||||
uint32_t sizein, char **dataout, uint32_t *sizeout);
|
||||
|
||||
#endif
|
||||
+137
@@ -0,0 +1,137 @@
|
||||
/*
|
||||
Fixed point implementation of 4 dimensional matrix.
|
||||
Some optimizing cases are present, such as reordered matrices and assumed
|
||||
identity components.
|
||||
|
||||
Column-major ordering is assumed unless stated otherwise:
|
||||
|
||||
a e k o
|
||||
b f l p
|
||||
c g m q
|
||||
d h n r
|
||||
|
||||
In memory: a b c d e f g h ...
|
||||
*/
|
||||
|
||||
#ifndef COM_MAT_H
|
||||
#define COM_MAT_H
|
||||
|
||||
#include "fixed.h"
|
||||
#include "vec.h"
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
typedef union {
|
||||
com_fixed_t com_def_alignedas(64) a[4 * 4];
|
||||
} com_mat_t;
|
||||
|
||||
static inline com_mat_t com_mat_identity(void) {
|
||||
com_mat_t result = {0};
|
||||
|
||||
result.a[0 * 4 + 0] = COM_FIXED_FRACUNIT;
|
||||
result.a[1 * 4 + 1] = COM_FIXED_FRACUNIT;
|
||||
result.a[2 * 4 + 2] = COM_FIXED_FRACUNIT;
|
||||
result.a[3 * 4 + 3] = COM_FIXED_FRACUNIT;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* https://michalpitr.substack.com/p/optimizing-matrix-multiplication */
|
||||
static inline com_mat_t com_mat_mul(com_mat_t a, com_mat_t b) {
|
||||
// com_mat_t result = {0};
|
||||
|
||||
// for (int c = 0; c < 4; ++c) {
|
||||
// for (int k = 0; k < 4; ++k) {
|
||||
// for (int r = 0; r < 4; ++r) {
|
||||
// result.a[r + c * 4] += com_fixed_mul(a.a[r + k * 4], b.a[k + c * 4]);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
com_mat_t result;
|
||||
|
||||
for (int c = 0; c < 4; ++c) {
|
||||
for (int r = 0; r < 4; ++r) {
|
||||
result.a[r + c * 4] = (((int64_t)a.a[r + 0 * 4] * b.a[0 + c * 4]) +
|
||||
((int64_t)a.a[r + 1 * 4] * b.a[1 + c * 4]) +
|
||||
((int64_t)a.a[r + 2 * 4] * b.a[2 + c * 4]) +
|
||||
((int64_t)a.a[r + 3 * 4] * b.a[3 + c * 4])) >>
|
||||
COM_FIXED_FRACBITS;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* This case might be slightly more optimized, as we can reorder one frequently
|
||||
* reused matrix, such as VP.
|
||||
*/
|
||||
/* Note: second a matrix is assumed to be row-major, reverse of typical. */
|
||||
static inline com_mat_t com_mat_mul_reodered(com_mat_t a, com_mat_t b) {
|
||||
com_mat_t result;
|
||||
|
||||
for (int c = 0; c < 4; ++c) {
|
||||
for (int r = 0; r < 4; ++r) {
|
||||
result.a[r + c * 4] = (((int64_t)a.a[0 + r * 4] * b.a[0 + c * 4]) +
|
||||
((int64_t)a.a[1 + r * 4] * b.a[1 + c * 4]) +
|
||||
((int64_t)a.a[2 + r * 4] * b.a[2 + c * 4]) +
|
||||
((int64_t)a.a[3 + r * 4] * b.a[3 + c * 4])) >>
|
||||
COM_FIXED_FRACBITS;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Projects vertex position to a screen via reordered row-major MVP matrix,
|
||||
* which implies division by w in-place */
|
||||
static inline com_vec_t com_mat_vec_project(com_mat_t a, com_vec_t b) {
|
||||
com_fixed_t t[4];
|
||||
com_vec_t result;
|
||||
|
||||
for (int c = 0; c < 4; ++c) {
|
||||
t[c] =
|
||||
(((int64_t)a.a[c * 4 + 0] * b.s.x) + ((int64_t)a.a[c * 4 + 1] * b.s.y) +
|
||||
((int64_t)a.a[c * 4 + 2] * b.s.z) + a.a[c * 4 + 3]) >>
|
||||
COM_FIXED_FRACBITS;
|
||||
}
|
||||
|
||||
/* Creates perspective effect, could be skipped for orthographic */
|
||||
result.a[0] = com_fixed_div(t[0], t[3]);
|
||||
result.a[1] = com_fixed_div(t[1], t[3]);
|
||||
result.a[2] = com_fixed_div(t[2], t[3]);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* Slightly optimized case of assumed identity scaling, might be useful for MVP
|
||||
* calculations, if model matrix does not scale. View matrix is always
|
||||
* unscaled as well.
|
||||
*/
|
||||
// static inline com_mat_t com_mat_mul_no_scale(com_mat_t a, com_mat_t b) {
|
||||
// com_mat_t result;
|
||||
|
||||
// /* TODO: calc the rest */
|
||||
|
||||
// result.a[0 * 4 + 3] = 0;
|
||||
// result.a[1 * 4 + 3] = 0;
|
||||
// result.a[2 * 4 + 3] = 0;
|
||||
// result.a[3 * 4 + 3] = COM_FIXED_FRACUNIT;
|
||||
|
||||
// return result;
|
||||
// }
|
||||
|
||||
/* Reorder between column and row major, it's also called transposing */
|
||||
static inline com_mat_t com_mat_reoder(com_mat_t a) {
|
||||
com_mat_t result;
|
||||
|
||||
for (int r = 0; r < 4; ++r) {
|
||||
for (int c = 0; c < 4; ++c) {
|
||||
result.a[c * 4 + r] = a.a[c + r * 4];
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
Fixed point implementation of 3 dimensional vectors.
|
||||
SSE2 extension availability is assumed, making it more viable.
|
||||
For mul and div we don't use com_fixed functions to make fewer bitshifts.
|
||||
*/
|
||||
|
||||
#ifndef COM_VEC_H
|
||||
#define COM_VEC_H
|
||||
|
||||
#include "def.h"
|
||||
#include "fixed.h"
|
||||
#include <stdint.h>
|
||||
|
||||
typedef union {
|
||||
com_fixed_t com_def_alignedas(16) a[3];
|
||||
com_def_alignedas(16) struct {
|
||||
com_fixed_t x;
|
||||
com_fixed_t y;
|
||||
com_fixed_t z;
|
||||
} s;
|
||||
} com_vec_t;
|
||||
|
||||
static inline com_vec_t com_vec_add(com_vec_t a, com_vec_t b) {
|
||||
return (com_vec_t){.s = {.x = com_fixed_add(a.s.x, b.s.x),
|
||||
.y = com_fixed_add(a.s.y, b.s.y),
|
||||
.z = com_fixed_add(a.s.z, b.s.z)}};
|
||||
}
|
||||
|
||||
static inline com_vec_t com_vec_sub(com_vec_t a, com_vec_t b) {
|
||||
return (com_vec_t){.s = {.x = com_fixed_sub(a.s.x, b.s.x),
|
||||
.y = com_fixed_sub(a.s.y, b.s.y),
|
||||
.z = com_fixed_sub(a.s.z, b.s.z)}};
|
||||
}
|
||||
|
||||
static inline com_vec_t com_vec_mul(com_vec_t a, com_vec_t b) {
|
||||
return (com_vec_t){.s = {.x = com_fixed_mul(a.s.x, b.s.x),
|
||||
.y = com_fixed_mul(a.s.y, b.s.y),
|
||||
.z = com_fixed_mul(a.s.z, b.s.z)}};
|
||||
}
|
||||
|
||||
/* Note: this does not clamp for over/underflow cases */
|
||||
static inline com_vec_t com_vec_div(com_vec_t a, com_vec_t b) {
|
||||
return (com_vec_t){.s = {.x = com_fixed_div(a.s.x, b.s.x),
|
||||
.y = com_fixed_div(a.s.y, b.s.y),
|
||||
.z = com_fixed_div(a.s.z, b.s.z)}};
|
||||
}
|
||||
|
||||
/* Scale vector by a fixed point number */
|
||||
static inline com_vec_t com_vec_scl(com_vec_t a, com_fixed_t b) {
|
||||
return (com_vec_t){.s = {.x = com_fixed_mul(a.s.x, b),
|
||||
.y = com_fixed_mul(a.s.y, b),
|
||||
.z = com_fixed_mul(a.s.z, b)}};
|
||||
}
|
||||
|
||||
/* Shows how much given vectors are correlated in direction to each other */
|
||||
/* Resulted range depends on input, it's in -1 to 1 for normalized
|
||||
* input and otherwise is -ab to +ab */
|
||||
static inline com_fixed_t com_vec_dot(com_vec_t a, com_vec_t b) {
|
||||
return (((int64_t)a.s.x * b.s.x) + ((int64_t)a.s.y * b.s.y) +
|
||||
((int64_t)a.s.z * b.s.z)) >>
|
||||
COM_FIXED_FRACBITS;
|
||||
}
|
||||
|
||||
/* Cross product produces a perpendicular for normalized vectors, or 0 for
|
||||
* parallel vectors */
|
||||
static inline com_vec_t com_vec_crs(com_vec_t a, com_vec_t b) {
|
||||
int64_t const cx = ((int64_t)a.s.y * b.s.z) - ((int64_t)a.s.z - b.s.y);
|
||||
int64_t const cy = ((int64_t)a.s.z * b.s.x) - ((int64_t)a.s.x - b.s.z);
|
||||
int64_t const cz = ((int64_t)a.s.x * b.s.y) - ((int64_t)a.s.y - b.s.x);
|
||||
return (com_vec_t){.s = {.x = (com_fixed_t)(cx >> COM_FIXED_FRACBITS),
|
||||
.y = (com_fixed_t)(cy >> COM_FIXED_FRACBITS),
|
||||
.z = (com_fixed_t)(cz >> COM_FIXED_FRACBITS)}};
|
||||
}
|
||||
|
||||
static inline com_vec_t com_vec_nrm(com_vec_t a) {
|
||||
com_fixed_t const n = com_fixed_sqrt(com_vec_dot(a, a));
|
||||
return com_vec_scl(a, com_fixed_div(COM_FIXED_FRACUNIT, n));
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user