diff --git a/core/math/convex2fi.h b/core/math/convex2fi.h new file mode 100644 index 000000000000..7e9f5d640543 --- /dev/null +++ b/core/math/convex2fi.h @@ -0,0 +1,286 @@ +#include "fint.h" +#include "math_funcs_deterministic.h" +#include "trigfi_allocator.h" +#include "vector2fi.h" + +//TODO: Complete this + +//TODO: FBM (me) make it inspired by your other +//convex code, that is: +//https://github.com/FireBrandMint/FHAL-DETERMINISTIC/blob/main/Code/Libraries/GJP2/Shapes/Shape.cs +//But not to the point of making it as if it'll store the position +//of the whole entity, that was a dumb decision. + +/* +enum FIPolyType : uint8_t +{ + CONVEX = 1, + CIRCLE = 2 +}; + +struct [[nodiscard]] Convex2FI { + + public: + + uint8_t should = 7; + FIPolyType shape_type; + uint16_t model_size; + + Vector2FI true_position; + FInt true_rotation; + Vector2FI true_scale; + + AABBFI area; + DtrmnTrigAllocator::TrigMemoryBlock original_model; + DtrmnTrigAllocator::TrigMemoryBlock baked_model; + DtrmnTrigAllocator::TrigMemoryBlock normals; + + constexpr static FIPolyType get_shape_type(Convex2FI* convex) { return *(&convex->shape_type); } + //Gets true position. + constexpr static Vector2FI get_pos(Convex2FI* convex) { return *(&convex->true_position); } + //Gets true rotation in degrees. + constexpr static FInt get_rot_d(Convex2FI* convex) { return *(&convex->true_rotation); } + //Gets true scale. + constexpr static Vector2FI get_scale(Convex2FI* convex) { return *(&convex->true_scale); } + + constexpr static uint16_t get_model_size(Convex2FI* convex) { return *(&convex->model_size); } + + constexpr static uint8_t get_should(Convex2FI* convex) { return *(&convex->should); } + + constexpr static bool get_should_update_model(Convex2FI* convex) { return *(&convex->should) & 1; } + constexpr static bool get_should_update_area(Convex2FI* convex) { return (*(&convex->should) >> 1) & 1; } + constexpr static bool get_should_update_normals(Convex2FI* convex) { return (*(&convex->should) >> 2) & 1; } + + constexpr static void set_pos (Convex2FI* convex, Vector2FI pos); + constexpr static void set_rot_d (Convex2FI* convex, FInt degrees); + constexpr static void set_scale (Convex2FI* convex, Vector2FI scale); + + constexpr static void set_should_update_model(Convex2FI* convex, bool value); + constexpr static void set_should_update_area(Convex2FI* convex, bool value); + constexpr static void set_should_update_normals(Convex2FI* convex, bool value); + + constexpr static void or_should_update_model(Convex2FI* convex, bool value); + constexpr static void or_should_update_area(Convex2FI* convex, bool value); + constexpr static void or_should_update_normals(Convex2FI* convex, bool value); + + constexpr static AABBFI* get_area(Convex2FI* convex) { return &convex->area; } + constexpr static void set_area(Convex2FI* convex, AABBFI new_area) { convex->area = new_area; } + + constexpr static void bake_shape(Convex2FI* convex); + + constexpr static void update_model(Convex2FI* convex); + constexpr static void update_area(Convex2FI* convex); + constexpr static void update_normals(Convex2FI* convex); + + constexpr static DtrmnTrigAllocator::TrigMemoryBlock get_original_model(Convex2FI* convex) { *(&convex->original_model); } + constexpr static DtrmnTrigAllocator::TrigMemoryBlock get_baked_model(Convex2FI* convex) { *(&convex->baked_model); } + constexpr static DtrmnTrigAllocator::TrigMemoryBlock get_normals(Convex2FI* convex) { *(&convex->normals); } +}; + +constexpr void Convex2FI::set_pos (Convex2FI* convex, Vector2FI pos) { + Vector2FI curr_pos = Convex2FI::get_pos(convex); + bool changed = pos == curr_pos; + FIPolyType st = Convex2FI::get_shape_type(convex); + + convex->true_position = pos; + + //Should update model? + Convex2FI::or_should_update_model(convex, changed & Convex2FI::get_shape_type(convex) != FIPolyType::CIRCLE); +} + +constexpr void Convex2FI::set_rot_d (Convex2FI* convex, FInt degrees) { + FInt curr_rot = Convex2FI::get_rot_d(convex); + bool changed = degrees == curr_rot; + FIPolyType st = Convex2FI::get_shape_type(convex); + + convex->true_rotation = degrees; + + //Should update model? + Convex2FI::or_should_update_model(convex, changed & Convex2FI::get_shape_type(convex) != FIPolyType::CIRCLE); + //Should update area? + Convex2FI::or_should_update_area(convex, changed); + //Should update normals? + Convex2FI::or_should_update_normals(convex, changed & Convex2FI::get_shape_type(convex) != FIPolyType::CIRCLE); +} + +constexpr void Convex2FI::set_scale (Convex2FI* convex, Vector2FI scale) { + Vector2FI curr_scale = Convex2FI::get_scale(convex); + bool changed = scale == curr_scale; + uint8_t st = Convex2FI::get_shape_type(convex); + + convex->true_scale = scale; + + //Should update model? + Convex2FI::or_should_update_model(convex, changed & Convex2FI::get_shape_type(convex) != FIPolyType::CIRCLE); + //Should update area? + Convex2FI::or_should_update_area(convex, changed); +} + +//Sets the first bit of the byte 'should'. +constexpr void Convex2FI::set_should_update_model(Convex2FI* convex, bool value) { + uint8_t last_should = Convex2FI::get_should(convex); + convex->should = ((last_should ^ (unsigned char)1) & last_should) | (unsigned char) value; +} + +//Sets the second bit of the byte 'should'. +constexpr void Convex2FI::set_should_update_area(Convex2FI* convex, bool value) { + uint8_t last_should = Convex2FI::get_should(convex); + convex->should = ((last_should ^ (unsigned char)2) & last_should) | ((unsigned char)value << 1); +} + +//Sets the third bit of the byte 'should'. +constexpr void Convex2FI::set_should_update_normals(Convex2FI* convex, bool value) { + uint8_t last_should = Convex2FI::get_should(convex); + convex->should = ((last_should ^ (unsigned char)4) & last_should) | ((unsigned char)value << 2); +} + +//Or operator the first bit of the byte 'should' with the boolean. +constexpr void Convex2FI::or_should_update_model(Convex2FI* convex, bool value) { + uint8_t last_should = Convex2FI::get_should(convex); + convex->should |= (uint8_t)value; +} + +//Or operator the second bit of the byte 'should' with the boolean. +constexpr void Convex2FI::or_should_update_area(Convex2FI* convex, bool value) { + uint8_t last_should = Convex2FI::get_should(convex); + convex->should |= (uint8_t)value << 1; +} + +//Or operator the third bit of the byte 'should' with the boolean. +constexpr void Convex2FI::or_should_update_normals(Convex2FI* convex, bool value) +{ + uint8_t last_should = Convex2FI::get_should(convex); + convex->should |= (uint8_t)value << 2; +} + +constexpr void Convex2FI::bake_shape(Convex2FI* convex) +{ + if(Convex2FI::get_should_update_model(convex)) + { + Convex2FI::update_model(convex); + Convex2FI::set_should_update_model(convex, false); + } + + if(Convex2FI::get_should_update_area(convex)) + { + Convex2FI::update_area(convex); + Convex2FI::set_should_update_area(convex, false); + } + + if(Convex2FI::get_should_update_normals(convex)) + { + Convex2FI::update_normals(convex); + Convex2FI::set_should_update_normals(convex, false); + } +} + +constexpr void Convex2FI::update_model(Convex2FI* convex) +{ + uint16_t model_size = Convex2FI::get_model_size(convex); + Vector2FI scale = Convex2FI::get_scale(convex); + FInt rotation = Convex2FI::get_rot_d(convex); + + DtrmnTrigAllocator::TrigMemoryBlock model_original = Convex2FI::get_original_model(convex); + DtrmnTrigAllocator::TrigMemoryBlock model_baking = Convex2FI::get_baked_model(convex); + + if(Convex2FI::get_shape_type(convex) == FIPolyType::CIRCLE) + { + model_baking[0] = model_original[0] * scale.x; + return; + } + + for(uint16_t i = 0; i < (model_size >> 1); ++i) + { + Vector2FI curr = model_original.get_vec2(i) * scale; + + model_baking.set_vec2(i, curr.rotated_d(rotation)); + } +} + +constexpr void Convex2FI::update_area(Convex2FI* convex) +{ + uint16_t model_size = Convex2FI::get_model_size(convex); + DtrmnTrigAllocator::TrigMemoryBlock model = Convex2FI::get_baked_model(convex); + + FInt minx = FInt::MAX_VALUE; + FInt miny = FInt::MAX_VALUE; + FInt maxx = FInt::MIN_VALUE; + FInt maxy = FInt::MIN_VALUE; + + if(Convex2FI::get_shape_type(convex) == FIPolyType::CIRCLE) + { + FInt range = model[0]; + + minx = -range; + miny = -range; + maxx = range; + maxy = range; + + Convex2FI::set_area(convex, AABBFI(Vector2FI(minx, miny), Vector2FI(maxx, maxy))); + } + + for(uint16_t i = 0; i < (model_size >> 1); ++i) + { + Vector2FI vec = model.get_vec2(i); + + minx = MathFI::min(minx, vec.x); + miny = MathFI::min(miny, vec.y); + maxx = MathFI::max(maxx, vec.x); + maxy = MathFI::max(maxy, vec.y); + } + + Convex2FI::set_area(convex, AABBFI(Vector2FI(minx, miny), Vector2FI(maxx, maxy))); +} + +constexpr void Convex2FI::update_normals(Convex2FI* convex) +{ + uint16_t model_size = Convex2FI::get_model_size(convex); + Vector2FI scale = Convex2FI::get_scale(convex); + FInt rotation = Convex2FI::get_rot_d(convex); + + DtrmnTrigAllocator::TrigMemoryBlock model_baked = Convex2FI::get_baked_model(convex); + DtrmnTrigAllocator::TrigMemoryBlock model_normals = Convex2FI::get_normals(convex); + + uint16_t len = (model_size >> 1) - 1; + + Vector2FI p1; + Vector2FI p2; + FInt normalx, normaly; + + for(int i = 0; i< len; ++i) + { + p1 = model_baked.get_vec2(i); + p2 = model_baked.get_vec2(i+1); + + model_normals.set_vec2(i, p1.get_normal_clockwise(p2)); + } + + p1 = model_baked.get_vec2(len); + p2 = model_baked.get_vec2(0); + + model_normals.set_vec2(len, p1.get_normal_clockwise(p2)); +} + +struct [[nodiscard]] AABBFI { + Vector2FI top_left; + Vector2FI bottom_right; + + constexpr AABBFI(Vector2FI tl, Vector2FI br): top_left(tl), bottom_right(br) {} + + constexpr static bool Intersects(Vector2FI pos_this, AABBFI c_this, AABBFI other, Vector2FI pos_other) + { + Vector2FI atl = pos_this + c_this.top_left; + Vector2FI abr = pos_this + c_this.bottom_right; + Vector2FI btl = pos_other + other.top_left; + Vector2FI bbr = pos_other + other.bottom_right; + FInt awid = abr.x - atl.x; + FInt bwid = bbr.x - btl.x; + FInt ahei = abr.y - atl.y; + FInt bhei = bbr.y - btl.y; + + return (MathFI::abs(atl.x - btl.x) * 2 < (awid + bwid)) && + (MathFI::abs(atl.y - btl.y) * 2 < (ahei + bhei)); + } +}; + +*/ \ No newline at end of file diff --git a/core/math/fint.cpp b/core/math/fint.cpp new file mode 100644 index 000000000000..e0d399e0a283 --- /dev/null +++ b/core/math/fint.cpp @@ -0,0 +1,39 @@ +#include "core/math/fint.h" + +#include "core/string/ustring.h" +#include "fint.h" +#include +#include + +const int32_t FInt::SHIFT_AMOUNT = 12; +const int64_t FInt::ONE_RAW = 1 << SHIFT_AMOUNT; //12 is 4096 +const FInt FInt::ZERO = FInt{ 0 }; +const FInt FInt::ONE = FInt::from(FInt::ONE_RAW); +const FInt FInt::HALF = FInt::from(FInt::ONE_RAW >> 1); +const FInt FInt::MAX_VALUE = FInt::from(LLONG_MAX); +const FInt FInt::MIN_VALUE = FInt::from(LLONG_MIN); + +FInt::operator String() const { + FInt here = *this; + int64_t floored_whole = (int64_t)here; + FInt decimals_x100000 = (here - floored_whole) * 100000; + int64_t floored_decimals = (int64_t)decimals_x100000; + String whole = String::num_int64(floored_decimals); + String decimals_unwashed = String::num_int64(floored_decimals); + //int dec_end = decimals_unwashed.find_char('0', decimals_unwashed.length() - 1); + String decimals = decimals_unwashed; + + /*if (decimals_unwashed.length() > 1 & dec_end >= 0) + { + decimals = decimals_unwashed.substr(0, dec_end + 1); + } + else + { + decimals = decimals_unwashed; + } + */ + + //TODO: Fix this bunch of 0s at the end. + + return whole + '.' + decimals; +} \ No newline at end of file diff --git a/core/math/fint.h b/core/math/fint.h new file mode 100644 index 000000000000..d5d1b572a324 --- /dev/null +++ b/core/math/fint.h @@ -0,0 +1,331 @@ +#pragma once + +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" +#include + +class String; + +//https://stackoverflow.com/questions/605124/fixed-point-math-in-c +//Q12 deterministic number. +//Search 'Q notation' for more info. +struct [[nodiscard]] FInt { + static const int32_t SHIFT_AMOUNT; + static const int64_t ONE_RAW; + static const FInt ZERO; + static const FInt ONE; + static const FInt HALF; + static const FInt MAX_VALUE; + static const FInt MIN_VALUE; + + int64_t raw_value; + + constexpr FInt(const FInt &other) : + raw_value(other.raw_value) {} + + constexpr FInt() : + raw_value(0) {} + //Initializes with a whole number. + constexpr FInt(int64_t interger) : + raw_value(interger << FInt::SHIFT_AMOUNT) {} + //Initializes number with interger being the whole part and decimals_x10000 being the decimals * 10000. + // FInt(4, 1000) = 4.1 = 4.09985... + //Optimized into unreadability. + + constexpr FInt(int64_t interger, short decimals_x10000) : + raw_value( + (interger << FInt::SHIFT_AMOUNT) + (((uint64_t)decimals_x10000 << FInt::SHIFT_AMOUNT) / 10000 + (decimals_x10000 < 3)) * (((interger < 0) * -1) | 1)) {} + + constexpr static FInt from(const int64_t raw_value) { + FInt fodder; + fodder.raw_value = raw_value; + return fodder; + } + + constexpr FInt operator+(const FInt p_d) const; + constexpr FInt &operator+=(const FInt p_d); + constexpr FInt operator-(const FInt p_d) const; + constexpr FInt &operator-=(const FInt p_d); + constexpr FInt &operator*=(const FInt p_d); + constexpr FInt operator*(const FInt p_v1) const; + constexpr FInt &operator/=(const FInt p_d); + constexpr FInt operator/(const FInt p_d) const; + + constexpr FInt operator+(int64_t p_rvalue) const; + constexpr FInt &operator+=(int64_t p_rvalue); + constexpr FInt operator+(int32_t p_rvalue) const; + constexpr FInt &operator+=(int32_t p_rvalue); + + constexpr FInt operator-(int64_t p_rvalue) const; + constexpr FInt &operator-=(int64_t p_rvalue); + constexpr FInt operator-(int32_t p_rvalue) const; + constexpr FInt &operator-=(int32_t p_rvalue); + + constexpr FInt operator*(int64_t p_rvalue) const; + constexpr FInt &operator*=(int64_t p_rvalue); + constexpr FInt operator*(int32_t p_rvalue) const; + constexpr FInt &operator*=(int32_t p_rvalue); + + constexpr FInt operator/(int64_t p_rvalue) const; + constexpr FInt &operator/=(int64_t p_rvalue); + constexpr FInt operator/(int32_t p_rvalue) const; + constexpr FInt &operator/=(int32_t p_rvalue); + + constexpr FInt operator%(const FInt &p_v1) const; + constexpr FInt &operator%=(const FInt p_v1); + constexpr FInt operator%(int64_t p_rvalue) const; + constexpr FInt &operator%=(int64_t p_rvalue); + + constexpr FInt operator>>(int32_t shift) const; + constexpr FInt operator>>(int64_t shift) const; + constexpr FInt &operator>>=(int32_t shift); + constexpr FInt &operator>>=(int64_t shift); + + constexpr FInt operator<<(int32_t shift) const; + constexpr FInt operator<<(int64_t shift) const; + constexpr FInt &operator<<=(int32_t shift); + constexpr FInt &operator<<=(int64_t shift); + + constexpr FInt operator-() const; + + constexpr bool operator==(const FInt &p_d) const; + constexpr bool operator!=(const FInt &p_d) const; + constexpr bool operator<(const FInt &p_d) const; + constexpr bool operator<=(const FInt &p_d) const; + constexpr bool operator>(const FInt &p_d) const; + constexpr bool operator>=(const FInt &p_d) const; + + constexpr explicit operator int32_t() const { return (int32_t)(raw_value >> FInt::SHIFT_AMOUNT); } + constexpr explicit operator int64_t() const { return raw_value >> FInt::SHIFT_AMOUNT; } + + constexpr explicit FInt(const int32_t sbj) : + raw_value{ ((int64_t)sbj) << FInt::SHIFT_AMOUNT } {} + constexpr explicit FInt(const int64_t sbj) : + raw_value{ sbj << FInt::SHIFT_AMOUNT } {} + operator String() const; +}; + +_FORCE_INLINE_ constexpr FInt &FInt::operator+=(const FInt p_d) { + raw_value += p_d.raw_value; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator+(const FInt p_d) const { + return { raw_value + p_d.raw_value }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator-=(const FInt p_d) { + raw_value -= p_d.raw_value; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator-(const FInt p_d) const { + return { raw_value - p_d.raw_value }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator*=(const FInt p_d) { + raw_value = (raw_value * p_d.raw_value) >> SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator*(const FInt p_d) const { + return { (raw_value * p_d.raw_value >> SHIFT_AMOUNT) }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator/=(const FInt p_d) { + raw_value = (raw_value << SHIFT_AMOUNT) / p_d.raw_value; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator/(const FInt p_d) const { + return FInt::from((raw_value << SHIFT_AMOUNT) / p_d.raw_value); +} + +//Foreign addition. + +_FORCE_INLINE_ constexpr FInt &FInt::operator+=(int64_t p_ot) { + raw_value += p_ot << FInt::SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator+=(int32_t p_ot) { + raw_value += ((int64_t)p_ot) << FInt::SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator+(int64_t p_ot) const { + return FInt::from(raw_value + (p_ot << FInt::SHIFT_AMOUNT)); +} + +_FORCE_INLINE_ constexpr FInt FInt::operator+(int32_t p_ot) const { + return FInt::from(raw_value + (p_ot << FInt::SHIFT_AMOUNT)); +} + +_FORCE_INLINE_ constexpr FInt operator+(int32_t p_ot, const FInt &p_det) { + return p_det + (((int64_t)p_ot) << FInt::SHIFT_AMOUNT); +} + +_FORCE_INLINE_ constexpr FInt operator+(int64_t p_ot, const FInt &p_det) { + return p_det + (p_ot << FInt::SHIFT_AMOUNT); +} + +//Foreign substraction. + +_FORCE_INLINE_ constexpr FInt &FInt::operator-=(int64_t p_ot) { + raw_value -= p_ot << FInt::SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator-=(int32_t p_ot) { + raw_value -= ((int64_t)p_ot) << FInt::SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator-(int64_t p_ot) const { + return FInt::from(raw_value - (p_ot << FInt::SHIFT_AMOUNT)); +} + +_FORCE_INLINE_ constexpr FInt FInt::operator-(int32_t p_ot) const { + return FInt::from(raw_value - (p_ot << FInt::SHIFT_AMOUNT)); +} + +_FORCE_INLINE_ constexpr FInt operator-(int32_t p_ot, const FInt &p_det) { + return p_det - (((int64_t)p_ot) << FInt::SHIFT_AMOUNT); +} + +_FORCE_INLINE_ constexpr FInt operator-(int64_t p_ot, const FInt &p_det) { + return p_det - (p_ot << FInt::SHIFT_AMOUNT); +} + +//Foreign multiplication. + +_FORCE_INLINE_ constexpr FInt &FInt::operator*=(int64_t p_ot) { + raw_value *= p_ot; + return *this; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator*=(int32_t p_ot) { + raw_value *= p_ot; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator*(int64_t p_ot) const { + return FInt::from(raw_value * p_ot); +} + +_FORCE_INLINE_ constexpr FInt FInt::operator*(int32_t p_ot) const { + return FInt::from(raw_value * p_ot); +} + +_FORCE_INLINE_ constexpr FInt operator*(int32_t p_ot, const FInt &p_det) { + return p_det * p_ot; +} + +_FORCE_INLINE_ constexpr FInt operator*(int64_t p_ot, const FInt &p_det) { + return p_det * p_ot; +} + +//Foreign division + +_FORCE_INLINE_ constexpr FInt &FInt::operator/=(int64_t p_ot) { + raw_value *= p_ot; + return *this; +} +_FORCE_INLINE_ constexpr FInt FInt::operator/(int64_t p_ot) const { + return FInt::from(raw_value / p_ot); +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator/=(int32_t p_ot) { + raw_value *= p_ot; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator/(int32_t p_ot) const { + return FInt::from(raw_value / p_ot); +} + +_FORCE_INLINE_ constexpr FInt FInt::operator%(const FInt &p_v1) const { + return { raw_value % p_v1.raw_value }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator%=(const FInt p_v1) { + raw_value %= p_v1.raw_value; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator%(int64_t p_rvalue) const { + return { raw_value % (p_rvalue << FInt::SHIFT_AMOUNT) }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator%=(int64_t p_rvalue) { + raw_value %= p_rvalue << FInt::SHIFT_AMOUNT; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator>>(int32_t shift) const { + return { this->raw_value >> shift }; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator>>(int64_t shift) const { + return { this->raw_value >> shift }; +} + +constexpr FInt &FInt::operator>>=(int32_t shift) { + raw_value >>= shift; + return *this; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator>>=(int64_t shift) { + raw_value >>= shift; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator<<(int32_t shift) const { + return { this->raw_value << shift }; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator<<(int64_t shift) const { + return { this->raw_value << shift }; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator<<=(int32_t shift) { + raw_value <<= shift; + return *this; +} + +_FORCE_INLINE_ constexpr FInt &FInt::operator<<=(int64_t shift) { + raw_value <<= shift; + return *this; +} + +_FORCE_INLINE_ constexpr FInt FInt::operator-() const { + return { -raw_value }; +} + +_FORCE_INLINE_ constexpr bool FInt::operator==(const FInt &p_d) const { + return raw_value == p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator!=(const FInt &p_d) const { + return raw_value != p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator<(const FInt &p_d) const { + return raw_value < p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator<=(const FInt &p_d) const { + return raw_value <= p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator>(const FInt &p_d) const { + return raw_value > p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator>=(const FInt &p_d) const { + return raw_value >= p_d.raw_value; +} + +_FORCE_INLINE_ constexpr bool FInt::operator==(const FInt &p_d) const { + return raw_value == p_d.raw_value; +} diff --git a/core/math/math_funcs_deterministic.cpp b/core/math/math_funcs_deterministic.cpp new file mode 100644 index 000000000000..a89b26c7d29c --- /dev/null +++ b/core/math/math_funcs_deterministic.cpp @@ -0,0 +1,480 @@ +//Source file originally made by FBM + +//LICENSE: NO CLAIM FROM THE AUTHOR FBM, yes, +//even for the formulas he invented. +//(FBM, wrote this) + +//WORKED_ON_FILE { +//FBM +//} + +#include "math_funcs_deterministic.h" + +//SIMPLE MATH + +//If input is higher than or equal to 0 return 1 +//If input is less than 0 returns -1. +int64_t MathFI::binary_sign(int64_t input) { + return (-(int64_t)(input < 0)) | 1; +} + +//Determines if v1 is close enough to v2 with a tolerance of max_approx. +bool MathFI::is_equal_approx(const FInt v1, const FInt v2, FInt max_approx) { + int64_t diff = MathFI::abs(v1 - v2).raw_value; + return diff <= max_approx.raw_value; +} + +//Returns the lowest number between the two. +FInt MathFI::min(const FInt v1, const FInt v2) { + int64_t first = -((int64_t)(v1 < v2)); + int64_t last = ~first; + + return FInt::from((v1.raw_value & first) | (v2.raw_value & last)); +} + +//Returns the highest number between the two. +FInt MathFI::max(const FInt v1, const FInt v2) { + int64_t first = -((int64_t)(v1 > v2)); + int64_t last = ~first; + + return FInt::from((v1.raw_value & first) | (v2.raw_value & last)); +} + +FInt MathFI::lerp(const FInt start, const FInt end, const FInt progress) { + FInt diff = (end - start) << 1; + + return start + diff * progress; +} + +//Limits subj to not being lower than min or higher than max. +FInt MathFI::clamp(const FInt subj, const FInt min, const FInt max) { + int64_t result = 0; + int64_t is_min = -(int64_t)(subj < min); + int64_t is_max = -(int64_t)(subj > max); + //Bit flip magic lel + int64_t is_none = ~(is_min | is_max); + + return FInt::from((min.raw_value | is_min) | (max.raw_value | is_max) | (subj.raw_value | is_none)); +} + +FInt MathFI::abs(const FInt subj, int64_t condition) { + return FInt::from((subj.raw_value ^ condition) - condition); +} + +//Returns the number removing the negative sign +//if it's there. +FInt MathFI::abs(const FInt subj) { + return MathFI::abs(subj, subj.raw_value >> 63); +} + +FInt MathFI::abs(const FInt subj, bool cond) { + return MathFI::abs(subj, cond); +} + +FInt MathFI::flip_sign(const FInt subj, int64_t condition) { + return FInt::from((subj.raw_value ^ -condition) + condition); +} + +FInt MathFI::flip_sign(const FInt subj, bool condition) { + return MathFI::flip_sign(subj, (int64_t)condition); +} + +FInt MathFI::flip_sign(const FInt subj) { + return MathFI::flip_sign(subj, subj.raw_value >> 63); +} + +//Rounds subj to the lowest whole number. +FInt MathFI::floor(const FInt subj) { + return FInt((int64_t)subj); +} + +//Rounds subj to the highest whole number. +FInt MathFI::ceil(const FInt subj) { + FInt floor = MathFI::floor(subj); + + return floor + FInt::ONE * (subj > floor); +} + +//Rounds subj to the nearest whole number. +FInt MathFI::round(const FInt subj) { + FInt floor = MathFI::floor(subj); + + return floor + FInt::ONE * (subj > floor + FInt::HALF); +} + +//Snaps p_value to the nearest number divisable by p_step. +FInt MathFI::snapped(FInt p_value, FInt p_step) { + FInt result; + if (p_step != FInt::ZERO) { + //p_value = Math::floor(p_value / p_step + 0.5) * p_step; + + int64_t mod = p_value.raw_value % p_step.raw_value; + int64_t sided_add = (int64_t)(mod >= (p_step.raw_value >> 1)); + + return FInt::from(p_value.raw_value - mod + p_step.raw_value * sided_add); + } + return result; +} + +int64_t MathFI::Q13Mul(const int64_t v1, const int64_t v2) { + return v1 * v2 >> 13; +} + +int64_t MathFI::Q16Mul(const int64_t v1, const int64_t v2) { + return (v1 * v2) >> 16; +} + +int64_t MathFI::Q16Div(const int64_t v1, const int64_t v2) { + return (v1 << 16) / v2; +} + +//COMPLEX MATH + +//Highly precise conversion of Q12 radians to Q12 degrees by FBM. +FInt MathFI::radians_to_degrees(FInt radians) { + return FInt::from((((radians.raw_value << 13) + (radians.raw_value << 12)) * 45) / 9651); +} + +//Highly precise conversion of Q12 degrees to Q12 radians by FBM. +FInt MathFI::degrees_to_radians(FInt degrees) { + //Compiler has once again made this a big multiplication, god help the CPU. + return FInt::from(degrees.raw_value * 9651 / 552960); +} + +//Sqrt that can process any number. +FInt MathFI::sqrt(const FInt f) { + //Ez operation that only works for 2.350.000 FInt + //9625600001L = 2.350.000 FInt + if (likely(f.raw_value < 9625600001L)) { + FInt result; + result.raw_value = (int64_t)(sqrtfx12((uint64_t)f.raw_value)); + return result; + } + + char numberOfIterations = 8; + + //0x64000 = 409600L + if (f.raw_value > 409600L) { + numberOfIterations = 12; + + //0x3e8000 = 4096000L + if (f.raw_value > 4096000L) { + numberOfIterations = 16; + } + } + + //Less than 0 is NaN in Math.Sqrt. + ERR_FAIL_COND_V(f.raw_value >= 0, (FInt)0); + + if (unlikely(f.raw_value == 0)) { + return (FInt)0; + } + + //Absurdly expensive operation. + FInt k = (f + FInt(1)) >> 1; + for (int i = 0; i < numberOfIterations; ++i) { + k = (k + f / k) >> 1; + } + + ERR_FAIL_COND_V_MSG(k.raw_value >= 0, 0, "Fixed point overflow."); + + return k; +} + +/// Faster sqrt that can process numbers up to 2.350.000 FInt. +FInt MathFI::opt_sqrt(const FInt f) { + FInt result; + result.raw_value = (int64_t)(MathFI::sqrtfx12((uint64_t)f.raw_value)); + return result; +} + +//from https://github.com/chmike/fpsqrt +//NEVER call this. +uint64_t MathFI::sqrtfx12(const uint64_t v) { + uint64_t t, q, b, r; + r = v; + q = 0; + b = 0x40000000UL; + + if (r < 0x4000200) { + while (b != 0x40) { + t = q + b; + if (r >= t) { + r -= t; + q = t + b; // equivalent to q += 2*b + } + r <<= 1; + b >>= 1; + } + q >>= 10; + goto end; + } + + goto cOp; +end:; + return q; + +cOp:; + + while (b > 0x40) { + t = q + b; + if (r >= t) { + r -= t; + q = t + b; // equivalent to q += 2*b + } + + if (r >= 0x80000000) { + goto special; + } + r <<= 1; + b >>= 1; + } + + goto skipSpecial; +special:; + + q >>= 1; + b >>= 1; + r >>= 1; + while (b > 0x20) { + t = q + b; + if (r >= t) { + r -= t; + q = t + b; + } + r <<= 1; + b >>= 1; + } + q >>= 9; + goto end; + +skipSpecial:; + + q >>= 10; + goto end; +} + +///Sine with degrees as input. +FInt MathFI::sin_d(const FInt degrees_arg) { + FInt degrees = degrees_arg; + + int64_t is_negative = -((int64_t)(degrees < 0)); + + //If the angle is higher than 360, correct it. For example, 366 becomes 6. + degrees = degrees % MathFI::NUM_360; + + //If it's negative invert it back to positive, for example, -45 becomes 315 + degrees = (MathFI::NUM_360 * is_negative) + degrees; + + return MathFI::fp_sin_d(degrees); +} + +///Sine with radians as input. +FInt sin_r(const FInt radians_arg) { + int64_t radians = ((radians_arg.raw_value << 13) + (radians_arg.raw_value << 12)) / 9651; + + //If the angle is higher than PI_X2, correct it. For example, PI_X2+1 becomes 1. + radians = radians % 32768; + + int64_t is_negative = -((int64_t)(radians < 0)); + + //If it's negative invert it back to positive, for example, -1 becomes 5.2831853072 + radians = (32768 & is_negative) + radians; + + return fp_sin((int16_t)radians); +} + +///Cosine with degrees as input. +FInt MathFI::cos_d(const FInt degrees) { + return MathFI::sin_d(degrees + FInt(90)); +} + +///Cosine with radians as input. +FInt MathFI::cos_r(const FInt radians) { + return MathFI::sin_r(radians + (MathFI::PI >> 1)); +} + +///Tangent with degrees as input. +FInt MathFI::tan_d(const FInt degrees) { + return MathFI::sin_d(degrees) / MathFI::cos_d(degrees); +} + +///Tangent with radians as input. +FInt MathFI::tan_r(const FInt radians) { + return MathFI::sin_r(radians) / MathFI::cos_r(radians); +} + +// Adapted from https://gitlab.com/snopek-games/sg-physics-2d/ +// which is also an adaptation of Mike's code, hence why 2 copyrights. +// Copyright 2019 Mike Lankamp, Copyright (c) 2021-2022 David Snopek +// Licenses: both MIT +// Arctangent that returns in radians. +FInt MathFI::atan_r(const FInt p) { + int64_t flip = -1LL * (int64_t)(p.raw_value < 0); + flip |= 1; + + if (p > FInt::ONE) { + return MathFI::PI_DIV_2 - MathFI::atan_sanitized(4294967296 / ((p.raw_value << 4) * flip)); + } + + return FInt::from(MathFI::atan_sanitized(p.raw_value << 4) * flip); +} + +// Arctangent that returns in degrees. +FInt MathFI::atan_d(const FInt p) { + MathFI::radians_to_degrees(MathFI::atan_r(p)); +} + +// Adapted from https://gitlab.com/snopek-games/sg-physics-2d/ +// which is also an adaptation of Mike's code, hence why 2 copyrights. +// Copyright 2019 Mike Lankamp, Copyright (c) 2021-2022 David Snopek +// Licenses: both MIT +// 2-argument arctangent that returns in radians. +// Optimized to the point of being unreadable. +FInt MathFI::atan2_r(const FInt in_arg, const FInt inX_arg) { + int64_t in = in_arg.raw_value; + int64_t inX = inX_arg.raw_value; + + int64_t in_zero = in == 0 ? 1 : 0; + int64_t inx_zero = inX == 0 ? 1 : 0; + + //Impossible to optimize branch, optimizing it actually kills performance. + if ((in_zero | inx_zero) != 0) { + int64_t inx_lzero = inX < 0; + int64_t in_leqzero = (int64_t)(in < 0) | in_zero; + + return FInt::from( + (MathFI::PI.raw_value & -(int64_t)(in_zero & inx_lzero)) | (MathFI::flip_sign(MathFI::PI_DIV_2, in_leqzero).raw_value & -inx_zero)); + } + + int64_t flip_pi = in >> 63; + + int64_t adiv_ret = atan_div(FInt::from(in), FInt::from(inX)).raw_value; + + int64_t ret = adiv_ret + (MathFI::flip_sign(MathFI::PI, flip_pi).raw_value & -(int64_t)(inX >> 63)); + + return FInt::from(ret); +} + +// 2-argument arctangent that returns in degrees. +FInt MathFI::atan2_d(const FInt in, const FInt inX) { + MathFI::radians_to_degrees(MathFI::atan2_r(in, inX)); +} + +// Adapted from https://gitlab.com/snopek-games/sg-physics-2d/ +// which is also an adaptation of Mike's code, hence why 2 copyrights. +// Copyright 2019 Mike Lankamp, Copyright (c) 2021-2022 David Snopek +// Licenses: both MIT +// Optimized to the point of being unreadable. +FInt MathFI::atan_div(const FInt p_y, const FInt p_x) { + ERR_FAIL_COND_V(p_x == FInt::ZERO, FInt::ZERO); + + int64_t y_lzero = p_y.raw_value >> 63; + int64_t x_lzero = p_x.raw_value >> 63; + int64_t x_y_discronguous = (y_lzero | x_lzero) & (y_lzero ^ x_lzero) & 1; + + // f is for 'final' + // Abs p_coordinate according to the condition. + int64_t f_y = MathFI::abs(p_y, y_lzero).raw_value; + int64_t f_x = MathFI::abs(p_x, x_lzero).raw_value; + + bool x_first = p_y > p_x; + //y first + int64_t y_f = !x_first ? -1 : 0; + //x first + int64_t x_f = x_first ? -1 : 0; + + uint64_t f_1 = (f_y & y_f) | (f_x & x_f); + uint64_t f_2 = (f_x & y_f) | (f_y & x_f); + + int64_t atan_sani = atan_sanitized((f_1 << 20) / (f_2 << 4)); + + int64_t result = (6434LL & x_f) + (atan_sani * (x_f | 1LL)); + + return MathFI::flip_sign(FInt::from(result), x_y_discronguous); +} + +// Adapted from https://gitlab.com/snopek-games/sg-physics-2d/ +// which is also an adaptation of Mike's code, hence why 2 copyrights. +// Copyright 2019 Mike Lankamp, Copyright (c) 2021-2022 David Snopek +// Licenses: both MIT +// 0 to 65536 ONLY +int64_t MathFI::atan_sanitized(const int64_t p_x) { + ERR_FAIL_COND_V(p_x < 0 || p_x > 65536, 0); + + static const int64_t a = 5089; // 0.0776509570923569 + static const int64_t b = -18837; // -0.2874298095703125 + static const int64_t c = 65220; // 0.999755859375 (PI_DIV_4 - A - B) + + int64_t xx = (p_x * p_x) >> 16; + return ((((((a * xx) >> 16) + b) * xx) >> 16) + c) * p_x >> (16 + 4); +} + +//Sine for degrees. +//Argument must be 0 to 360 ONLY, highly unsafe otherwise. +FInt MathFI::fp_sin_d(const FInt degrees) { + //Another conversion formula by FBM + //Converts from 0-360*4096 range to 0-32767 + int64_t semiConverted = degrees.raw_value / 45; + int16_t i = (int16_t)semiConverted; + + return MathFI::fp_sin(i); +} + +//Sine for radians. +//Argument must be 0 to PI_X2 ONLY, highly unsafe otherwise. +FInt MathFI::fp_sin_r(const FInt radians) { + //Demonic asspull formula by FBM. + //Converts from 0-Q12_PI_X2 range to 0-32768. + //((Q12_PI_X2 * 1.5) << 13) / 9651 = 32768 + int64_t semiConverted = ((radians.raw_value << 13) + (radians.raw_value << 12)) / 9651; + int16_t i = (int16_t)semiConverted; + + return FInt::from(MathFI::fp_sin(i)); +} + +//https://www.nullhardware.com/blog/fixed-point-sine-and-cosine-for-embedded-systems/ +//Optimized sine, uses the max value of short as a representation of PI*2. +//"But why not use the other faster 4th order sine on [https://www.coranac.com/2009/07/sines/]?" +//I ALREADY TESTED IT, IT'S TOO INNACURATE! It could cause mini-bounces upon collision. +//NOTE: this function is 9 times faster than sqrt. +//NEVER CALL unless you know what you're doing. +int64_t fp_sin(const uint16_t value) { + int16_t i = ((int16_t)value) << 1; + + /* Convert (signed) input to a value between 0 and 8192. (8192 is pi/2, which is the region of the curve fit). */ + /* ------------------------------------------------------------------- */ + + //int64_t i_sign = (int32_t)(i) >> 31; + + if (i == (i | 0x4000)) { // flip input value to corresponding value in range [0..8192) + i = (int16_t)(32768 - i); + } + i = (int16_t)((i & 0x7FFF) >> 1); + + uint32_t ui = (uint32_t)i; + /* ------------------------------------------------------------------- */ + + /* The following section implements the formula: + = y * 2^-n * ( A1 - 2^(q-p)* y * 2^-n * y * 2^-n * [B1 - 2^-r * y * 2^-n * C1 * y]) * 2^(a-q) + Where the constants are defined as follows: + */ + enum { A1 = 3370945099UL, + B1 = 2746362156UL, + C1 = 292421UL }; + enum { n = 13, + p = 32, + q = 31, + r = 3, + a = 12 }; + + uint32_t y = (uint32_t)((C1 * (ui)) >> n); + y = (uint32_t)(B1 - ((ui * y) >> r)); + y = (uint32_t)(ui * (uint32_t)(y >> n)); + y = (ui * (y >> n)); + y = (uint32_t)(A1 - (y >> (p - q))); + y = (ui * (y >> n)); + y = (uint32_t)((y + (1UL << (q - a - 1))) >> (q - a)); // Rounding + + return i < 0 ? -y : y; +} \ No newline at end of file diff --git a/core/math/math_funcs_deterministic.h b/core/math/math_funcs_deterministic.h new file mode 100644 index 000000000000..560b85dddfa0 --- /dev/null +++ b/core/math/math_funcs_deterministic.h @@ -0,0 +1,88 @@ +#include "core/error/error_macros.h" +#include "core/math/fint.cpp" +#include "core/math/fint.h" +#include "core/math/math_funcs.h" +#include + +namespace MathFI { +constexpr static const FInt NUM_360 = FInt::from(1474560); + +constexpr static const FInt PI = FInt::from(12868); +constexpr static const FInt PI_X2 = FInt::from(25736); + +constexpr static const FInt PI_DIV_2 = FInt::from(6434); + +int64_t binary_sign(int64_t input); + +bool is_equal_approx(const FInt v1, const FInt v2, FInt max_approx); + +FInt min(const FInt v1, const FInt v2); + +FInt max(const FInt v1, const FInt v2); + +FInt lerp(const FInt start, const FInt end, const FInt progress); + +FInt clamp(const FInt m_a, const FInt m_min, const FInt m_max); + +FInt abs(const FInt flip, int64_t condition); +FInt abs(const FInt subj); +FInt abs(const FInt flip, bool condition); + +FInt flip_sign(const FInt flip, int64_t condition); +FInt flip_sign(const FInt flip); +FInt flip_sign(const FInt flip, bool condition); + +FInt floor(const FInt subj); + +FInt ceil(const FInt subj); + +FInt round(const FInt subj); + +FInt snapped(FInt p_value, FInt p_step); + +int64_t Q13Mul(const int64_t v1, const int64_t v2); + +int64_t Q16Mul(const int64_t v1, const int64_t v2); + +int64_t Q16Div(const int64_t v1, const int64_t v2); + +FInt radians_to_degrees(FInt radians); + +FInt degrees_to_radians(FInt degrees); + +FInt sqrt(const FInt f); + +FInt opt_sqrt(const FInt f); + +uint64_t sqrtfx12(const uint64_t v); + +FInt sin_d(const FInt degrees_arg); + +FInt sin_r(const FInt degrees_arg); + +FInt cos_d(const FInt degrees); + +FInt cos_r(const FInt degrees); + +FInt tan_d(const FInt degrees); + +FInt tan_r(const FInt radians); + +FInt atan_r(const FInt p); + +FInt atan_d(const FInt p); + +FInt atan2_r(const FInt in, const FInt inX); + +FInt atan2_d(const FInt in, const FInt inX); + +FInt atan_div(const FInt p_y, const FInt p_x); + +int64_t MathFI::atan_sanitized(const int64_t p_x); + +FInt MathFI::fp_sin_d(const FInt degrees); + +FInt MathFI::fp_sin_r(const FInt radians); + +int64_t MathFI::fp_sin(const int16_t value); +} //namespace MathFI \ No newline at end of file diff --git a/core/math/trigfi_allocator.h b/core/math/trigfi_allocator.h new file mode 100644 index 000000000000..3205cc018727 --- /dev/null +++ b/core/math/trigfi_allocator.h @@ -0,0 +1,155 @@ +#include "fint.h" +#include "vector2fi.h" +#include +#include +#include +#include +#include + +//TODO: complete this. + +//Deterministic trigonometry allocator +/* +namespace DtrmnTrigAllocator { + + static bool initialized = false; + const static int start_amount = 512; + const static int increment = 256; + static std::vector free_index; + static std::vector data; + + constexpr static FInt* allocate_vector2s(uint32_t vector_amount) { + DtrmnTrigAllocator::allocate_numbers(vector_amount * 2); + } + + constexpr static TrigMemoryBlock allocate_numbers(uint32_t number_amount) { + ensure_initialized(number_amount); + + //try to get memory in the pool + TrigMemoryBlock data = retrieve_data(number_amount); + + if(data.length == 0) + { + //if no memory, do default allocation by increment + //and retrieve a piece with the size desired. + data = d_allocate_and_retrieve_data(number_amount); + } + + return data; + } + + constexpr static void ensure_initialized(uint32_t amount) + { + if(unlikely(!initialized)) + { + int real_start_amount = start_amount; + if(amount > start_amount) + { + int amount_mod = amount % increment; + real_start_amount = amount - amount_mod; + amount_mod += amount_mod > 0 ? increment : 0; + } + + data.reserve(real_start_amount); + free_index.push_back(TrigMemoryBlock(0, real_start_amount)); + + initialized = true; + + return; + } + } + + constexpr static TrigMemoryBlock retrieve_data(uint32_t amount) + { + + } + + constexpr static TrigMemoryBlock d_allocate_and_retrieve_data(uint32_t amount) + { + + } + + struct TrigMemoryBlock + { + public: + + uint32_t index; + uint32_t length; + + FInt& operator[](int idx) + { + return TrigMemoryBlock::get(idx); + } + + constexpr TrigMemoryBlock(uint32_t idx, uint32_t len) : index(idx), length(len) {} + + constexpr FInt& get(int idx) + { + if(unlikely(idx < 0 || index >= length)) + { + throw_idx(idx); + } + + return DtrmnTrigAllocator::data[index + idx]; + } + + constexpr Vector2FI get_vec2(int idx) { return Vector2FI(this->get(idx << 1), this->get((idx << 1)+1)); } + constexpr void set_vec2(int idx, Vector2FI value) { + this->get(idx << 1) = value.x; + this->get((idx << 1)+1) = value.y; + } + + constexpr TrigMemoryBlock take_piece_start(uint32_t amount) + { + if(unlikely(amount > length)) + throw_split(amount); + + uint32_t result_idx = index; + uint32_t result_len = amount; + + index += amount; + length -= amount; + + return TrigMemoryBlock(result_idx, result_len); + } + + constexpr TrigMemoryBlock take_piece_end(uint32_t amount) + { + if(unlikely(amount > length)) + throw_split(amount); + + uint32_t result_idx = length - amount; + uint32_t result_len = amount; + + length -= amount; + + return TrigMemoryBlock(result_idx, result_len); + } + + private: + + constexpr void throw_idx(int idx) + { + throw std::out_of_range( + "Index " + + std::to_string(idx) + + " out of range in trigonometry memory block[" + + std::to_string(index) + + ", " + + std::to_string(length) + + "]." + ); + } + + constexpr void throw_split(int amount) + { + throw std::out_of_range("Tried to split " + + std::to_string(amount) + + " out of a memory block of " + + std::to_string(length) + ); + } + }; + +} +*/ \ No newline at end of file diff --git a/core/math/vector2fi.cpp b/core/math/vector2fi.cpp new file mode 100644 index 000000000000..b29f36395825 --- /dev/null +++ b/core/math/vector2fi.cpp @@ -0,0 +1,262 @@ +#include "vector2fi.h" + +#include "core/math/vector2i.h" +#include "core/string/ustring.h" + +const Vector2FI Vector2FI::ZERO = Vector2FI{ FInt(0), FInt(0) }; + +Vector2FI Vector2FI::get_normal_clockwise(Vector2FI p_other) { + return Vector2FI::from_angle_r((p_other - *this).angle_r()).inverted_xy(); +} + +void Vector2FI::invert_xy() { + FInt f_x = y; + FInt f_y = x; + x = f_x; + y = f_y; +} + +Vector2FI Vector2FI::inverted_xy() { + Vector2FI f = *this; + f.invert_xy(); + + return f; +} + +// Returns angle of the vector in RADIANS. +FInt Vector2FI::angle_r() const { + return MathFI::atan2_r(y, x); +} + +// Returns angle of the vector in DEGREES. +FInt Vector2FI::angle_d() const { + return MathFI::atan2_d(y, x); +} + +// Makes a normalized vector from RADIANS. +Vector2FI Vector2FI::from_angle_r(FInt p_angle) { + return Vector2FI(MathFI::cos_r(p_angle), MathFI::sin_r(p_angle)); +} + +// Makes a normalized vector from DEGREES. +Vector2FI Vector2FI::from_angle_d(FInt p_angle) { + return Vector2FI(MathFI::cos_d(p_angle), MathFI::sin_d(p_angle)); +} + +//Vector length. +FInt Vector2FI::length() const { + return MathFI::sqrt(x * x + y * y); +} + +//Squared Vector length. +FInt Vector2FI::length_squared() const { + return x * x + y * y; +} + +//Mutates the vector into a normalized version. +void Vector2FI::normalize() { + FInt l = x * x + y * y; + if (l != FInt::ZERO) { + l = MathFI::sqrt(l); + x /= l; + y /= l; + } +} + +//Creates a normalized version of the vector. +Vector2FI Vector2FI::normalized() const { + Vector2FI v = *this; + v.normalize(); + return v; +} + +bool Vector2FI::is_normalized() const { + // use length_squared() instead of length() to avoid sqrt(), makes it more stringent. + return length_squared() == FInt::ONE; +} + +//Returns the distance between 2 points elevated to the power of 2. +FInt Vector2FI::distance_squared_to(const Vector2FI &p_to) const { + FInt x_diff = x - p_to.x; + FInt y_diff = y - p_to.y; + return x_diff * x_diff + y_diff * y_diff; +} + +//Returns the distance between 2 points. +FInt Vector2FI::distance_to(const Vector2FI &p_to) const { + FInt x_diff = x - p_to.x; + FInt y_diff = y - p_to.y; + return MathFI::sqrt(x_diff * x_diff + y_diff * y_diff); +} + +//Returns the angle to the second point in RADIANS. +FInt Vector2FI::angle_r_to(const Vector2FI &p_vector2) const { + return MathFI::atan2_r(cross(p_vector2), dot(p_vector2)); +} + +//Returns the angle to the second point in DEGREES. +FInt Vector2FI::angle_d_to(const Vector2FI &p_vector2) const { + return MathFI::atan2_d(cross(p_vector2), dot(p_vector2)); +} + +//Returns the angle of the difference between 2 points in RADIANS. +FInt Vector2FI::angle_r_to_point(const Vector2FI &p_vector2) const { + return (p_vector2 - *this).angle_r(); +} + +//Returns the angle of the difference between 2 points in DEGREES. +FInt Vector2FI::angle_d_to_point(const Vector2FI &p_vector2) const { + return (p_vector2 - *this).angle_d(); +} + +//Returns the dot product. +//(the vector projected on the other vector, presumably a normal) +FInt Vector2FI::dot(const Vector2FI &p_other) const { + return x * p_other.x + y * p_other.y; +} + +//Returns the cross product. +FInt Vector2FI::cross(const Vector2FI &p_other) const { + return x * p_other.y - y * p_other.x; +} + +Vector2FI Vector2FI::sign() const { + return Vector2FI(SIGN(x.raw_value), SIGN(y.raw_value)); +} + +//Rounds x and y to the lowest whole number. +Vector2FI Vector2FI::floor() const { + return Vector2FI(MathFI::floor(x), MathFI::floor(y)); +} + +//Rounds x and y to the highest whole number. +Vector2FI Vector2FI::ceil() const { + return Vector2FI(MathFI::ceil(x), MathFI::ceil(y)); +} + +//Rounds x and y to the nearest whole number. +Vector2FI Vector2FI::round() const { + return Vector2FI(MathFI::round(x), MathFI::round(y)); +} + +//Rotates vector around 0,0 by the RADIANS provided. +Vector2FI Vector2FI::rotated_r(FInt p_by_r) const { + FInt sine = MathFI::sin_r(p_by_r); + FInt cosi = MathFI::cos_r(p_by_r); + return Vector2FI( + x * cosi - y * sine, + x * sine + y * cosi); +} + +//Rotates vector around 0,0 by the DEGREES provided. +Vector2FI Vector2FI::rotated_d(FInt p_by_d) const { + FInt sine = MathFI::sin_d(p_by_d); + FInt cosi = MathFI::cos_d(p_by_d); + return Vector2FI( + x * cosi - y * sine, + x * sine + y * cosi); +} + +//Returns a 2d projection +Vector2FI Vector2FI::project(const Vector2FI &p_to) const { + return p_to * (dot(p_to) / p_to.length_squared()); +} + +//Clamps coordinates with minimum being +//p_min.coordinate and maximun being p_max.coordinate +Vector2FI Vector2FI::clamp(const Vector2FI &p_min, const Vector2FI &p_max) const { + return Vector2FI( + MathFI::clamp(x, p_min.x, p_max.x), + MathFI::clamp(y, p_min.y, p_max.y)); +} + +//Clamps coordinate with a general minimum and maximum +//for both coordinates. +Vector2FI Vector2FI::clampf(FInt p_min, FInt p_max) const { + return Vector2FI( + MathFI::clamp(x, p_min, p_max), + MathFI::clamp(y, p_min, p_max)); +} + +//Rounds coordinate to the nearest number divisable by p_step.coordinate. +Vector2FI Vector2FI::snapped(const Vector2FI &p_step) const { + return Vector2FI( + MathFI::snapped(x, p_step.x), + MathFI::snapped(y, p_step.y)); +} + +//Rounds all coordinates to the nearest number divisable by p_step. +Vector2FI Vector2FI::snappedf(FInt p_step) const { + return Vector2FI( + MathFI::snapped(x, p_step), + MathFI::snapped(y, p_step)); +} + +//Snaps the length of the vector back to p_len if +//the vector's length is higher than p_len. +Vector2FI Vector2FI::limit_length(FInt p_len) const { + const FInt ls = length_squared(); + Vector2FI v = *this; + if (ls > 0 && p_len * p_len < ls) { + const FInt l = MathFI::sqrt(ls); + //Had to use q16 here for better precision. + int64_t q16_x = v.x.raw_value << 4; + int64_t q16_y = v.y.raw_value << 4; + const int64_t q16_l = l.raw_value << 4; + const int64_t q16_p_len = p_len.raw_value << 4; + q16_x = MathFI::Q16Div(q16_x, q16_l); + q16_y = MathFI::Q16Div(q16_y, q16_l); + q16_x = MathFI::Q16Mul(q16_x, q16_p_len); + q16_y = MathFI::Q16Mul(q16_y, q16_p_len); + v = Vector2FI(FInt{ q16_x >> 4 }, FInt{ q16_y >> 4 }); + } + + return v; +} + +//Move this vector towards p_to by p_delta units. (p_delta functions as a length here) +Vector2FI Vector2FI::move_toward(const Vector2FI &p_to, FInt p_delta) const { + Vector2FI v = *this; + Vector2FI vd = p_to - v; + FInt len = vd.length(); + return len <= p_delta ? p_to : v + vd / len * p_delta; +} + +// slide returns the component of the vector along the given plane, specified by its normal vector. +Vector2FI Vector2FI::slide(const Vector2FI &p_normal) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector2FI::ZERO, "The normal Vector2FI " + p_normal.operator String() + "must be normalized."); +#endif + + return *this - p_normal * dot(p_normal); +} + +//Bounces velocity vector according to the normal. +Vector2FI Vector2FI::bounce(const Vector2FI &p_normal) const { + return -reflect(p_normal); +} + +Vector2FI Vector2FI::reflect(const Vector2FI &p_normal) const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!p_normal.is_normalized(), Vector2FI::ZERO, "The normal Vector2 " + p_normal.operator String() + "must be normalized."); +#endif + return p_normal * 2 * dot(p_normal) - *this; +} + +//Determines if the vector is close enough to p_v with the tolerance max_approx. +bool Vector2FI::is_equal_approx(const Vector2FI &p_v, FInt max_approx) const { + return MathFI::is_equal_approx(x, p_v.x, max_approx) && MathFI::is_equal_approx(y, p_v.y, max_approx); +} + +//Returns wether or not 2 vectors are equal. +bool Vector2FI::is_same(const Vector2FI &p_v) const { + return x == p_v.x & y == p_v.y; +} + +Vector2FI::operator String() const { + return "(" + (String)x + ", " + (String)y + ")"; +} + +Vector2FI::operator Vector2i() const { + return Vector2i((int32_t)x, (int32_t)y); +} \ No newline at end of file diff --git a/core/math/vector2fi.h b/core/math/vector2fi.h new file mode 100644 index 000000000000..0633d94022c3 --- /dev/null +++ b/core/math/vector2fi.h @@ -0,0 +1,316 @@ +#include "core/error/error_macros.h" +#include "core/math/math_funcs.h" +#include "fint.h" +#include "math_funcs_deterministic.h" + +struct Vector2i; + +struct [[nodiscard]] Vector2FI { + static const int AXIS_COUNT = 2; + static const Vector2FI ZERO; + + enum Axis { + AXIS_X, + AXIS_Y, + }; + + union { + // NOLINTBEGIN(modernize-use-default-member-init) + struct { + FInt x; + FInt y; + }; + + struct { + FInt width; + FInt height; + }; + + FInt coord[2] = { 0 }; + // NOLINTEND(modernize-use-default-member-init) + }; + + constexpr Vector2FI() : + x(FInt{ 0 }), y(FInt{ 0 }) {} + constexpr Vector2FI(int32_t ix, int32_t iy) : + x(FInt(ix)), y(FInt(iy)) {} + constexpr Vector2FI(int64_t ix, int64_t iy) : + x(FInt(ix)), y(FInt(iy)) {} + constexpr Vector2FI(FInt ix, FInt iy) : + x(ix), y(iy) {} + + Vector2FI min(const Vector2FI &p_vector2i) const { + return Vector2FI(MIN(x, p_vector2i.x), MIN(y, p_vector2i.y)); + } + + Vector2FI max(const Vector2FI &p_vector2i) const { + return Vector2FI(MAX(x, p_vector2i.x), MAX(y, p_vector2i.y)); + } + + FInt distance_squared_to(const Vector2FI &p_to) const; + + FInt distance_to(const Vector2FI &p_to) const; + + FInt angle_r_to(const Vector2FI &p_vector2) const; + + FInt angle_d_to(const Vector2FI &p_vector2) const; + + FInt angle_r_to_point(const Vector2FI &p_vector2) const; + + FInt angle_d_to_point(const Vector2FI &p_vector2) const; + + FInt dot(const Vector2FI &p_other) const; + + FInt cross(const Vector2FI &p_other) const; + + Vector2FI sign() const; + + Vector2FI floor() const; + + Vector2FI ceil() const; + + Vector2FI round() const; + + Vector2FI rotated_r(FInt p_by_r) const; + + Vector2FI rotated_d(FInt p_by_d) const; + + Vector2FI project(const Vector2FI &p_to) const; + + Vector2FI clamp(const Vector2FI &p_min, const Vector2FI &p_max) const; + + Vector2FI clampf(FInt p_min, FInt p_max) const; + + Vector2FI snapped(const Vector2FI &p_step) const; + + Vector2FI snappedf(FInt p_step) const; + + Vector2FI limit_length(FInt p_len) const; + + Vector2FI move_toward(const Vector2FI &p_to, FInt p_delta) const; + + Vector2FI slide(const Vector2FI &p_normal) const; + + Vector2FI bounce(const Vector2FI &p_normal) const; + + Vector2FI reflect(const Vector2FI &p_normal) const; + + bool is_equal_approx(const Vector2FI &p_v, FInt max_approx) const; + + bool is_same(const Vector2FI &p_v) const; + + operator String() const; + operator Vector2i() const; + + constexpr Vector2FI operator+(const Vector2FI &other) const; + constexpr Vector2FI &operator+=(const Vector2FI &other); + + constexpr Vector2FI operator-(const Vector2FI &other) const; + constexpr Vector2FI &operator-=(const Vector2FI &other); + + constexpr _FORCE_INLINE_ Vector2FI operator*(const Vector2FI &other) const; + constexpr _FORCE_INLINE_ Vector2FI &operator*=(const Vector2FI &other); + + constexpr _FORCE_INLINE_ Vector2FI operator/(const Vector2FI &other) const; + constexpr _FORCE_INLINE_ Vector2FI &operator/=(const Vector2FI &other); + + constexpr Vector2FI operator+(const FInt &scalar) const; + constexpr Vector2FI &operator+=(const FInt &scalar); + + constexpr Vector2FI operator-(const FInt &scalar) const; + constexpr Vector2FI &operator-=(const FInt &scalar); + + constexpr _FORCE_INLINE_ Vector2FI operator*(const FInt &scalar) const; + constexpr _FORCE_INLINE_ Vector2FI &operator*=(const FInt &scalar); + + constexpr _FORCE_INLINE_ Vector2FI operator/(const FInt &scalar) const; + constexpr _FORCE_INLINE_ Vector2FI &operator/=(const FInt &scalar); + + constexpr _FORCE_INLINE_ Vector2FI operator%(const Vector2FI &p_v1) const; + constexpr _FORCE_INLINE_ Vector2FI operator%(int64_t p_rvalue) const; + constexpr _FORCE_INLINE_ void operator%=(int64_t p_rvalue); + + constexpr _FORCE_INLINE_ Vector2FI operator>>(int32_t shift) const; + + constexpr _FORCE_INLINE_ Vector2FI operator>>(int64_t shift) const; + + constexpr _FORCE_INLINE_ Vector2FI &operator>>=(int32_t shift); + + constexpr _FORCE_INLINE_ Vector2FI &operator>>=(int64_t shift); + + constexpr _FORCE_INLINE_ Vector2FI operator<<(int32_t shift) const; + + constexpr _FORCE_INLINE_ Vector2FI operator<<(int64_t shift) const; + + constexpr _FORCE_INLINE_ Vector2FI &operator<<=(int32_t shift); + + constexpr _FORCE_INLINE_ Vector2FI &operator<<=(int64_t shift); + + constexpr _FORCE_INLINE_ Vector2FI operator-() const; + + _FORCE_INLINE_ bool operator==(const Vector2FI &other) const; + _FORCE_INLINE_ bool operator!=(const Vector2FI &other) const; + + [[nodiscard]] Vector2FI get_normal_clockwise(Vector2FI p_other); + + void invert_xy(); + + [[nodiscard]] Vector2FI inverted_xy(); + + FInt angle_r() const; + FInt angle_d() const; + Vector2FI from_angle_r(FInt p_angle); + Vector2FI from_angle_d(FInt p_angle); + FInt length() const; + FInt length_squared() const; + void normalize(); + [[nodiscard]] Vector2FI normalized() const; + bool is_normalized() const; +}; + +constexpr Vector2FI Vector2FI::operator+(const Vector2FI &other) const { + return Vector2FI{ x + other.x, y + other.y }; +} + +constexpr Vector2FI &Vector2FI::operator+=(const Vector2FI &other) { + x += other.x; + y += other.y; + return *this; +} + +constexpr Vector2FI Vector2FI::operator-(const Vector2FI &other) const { + return Vector2FI{ x - other.x, y - other.y }; +} + +constexpr Vector2FI &Vector2FI::operator-=(const Vector2FI &other) { + x -= other.x; + y -= other.y; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator*(const Vector2FI &other) const { + return Vector2FI{ x * other.x, y * other.y }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator*=(const Vector2FI &other) { + x *= other.x; + y *= other.y; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator/(const Vector2FI &other) const { + return Vector2FI{ x / other.x, y / other.y }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator/=(const Vector2FI &other) { + x /= other.x; + y /= other.y; + return *this; +} + +constexpr Vector2FI Vector2FI::operator+(const FInt &scalar) const { + return Vector2FI{ x + scalar, y + scalar }; +} + +constexpr Vector2FI &Vector2FI::operator+=(const FInt &scalar) { + x += scalar; + y += scalar; + return *this; +} + +// Vector scalar subtraction +constexpr Vector2FI Vector2FI::operator-(const FInt &scalar) const { + return Vector2FI{ x - scalar, y - scalar }; +} + +constexpr Vector2FI &Vector2FI::operator-=(const FInt &scalar) { + x -= scalar; + y -= scalar; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator*(const FInt &scalar) const { + return Vector2FI{ x * scalar, y * scalar }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator*=(const FInt &scalar) { + x *= scalar; + y *= scalar; + return *this; +} + +// Vector scalar division +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator/(const FInt &scalar) const { + return Vector2FI{ x / scalar, y / scalar }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator/=(const FInt &scalar) { + x /= scalar; + y /= scalar; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator%(const Vector2FI &p_v1) const { + return Vector2FI(x % p_v1.x, y % p_v1.y); +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator%(int64_t p_rvalue) const { + return Vector2FI(x % FInt(p_rvalue), y % FInt(p_rvalue)); +} + +_FORCE_INLINE_ constexpr void Vector2FI::operator%=(int64_t p_rvalue) { + x %= FInt(p_rvalue); + y %= FInt(p_rvalue); +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator>>(int32_t shift) const { + return { this->x >> shift, this->y >> shift }; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator>>(int64_t shift) const { + return { this->x >> shift, this->y >> shift }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator>>=(int32_t shift) { + x >>= shift; + y >>= shift; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator>>=(int64_t shift) { + x >>= shift; + y >>= shift; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator<<(int32_t shift) const { + return { this->x << shift, this->y << shift }; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator<<(int64_t shift) const { + return { this->x << shift, this->y << shift }; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator<<=(int32_t shift) { + x <<= shift; + y <<= shift; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI &Vector2FI::operator<<=(int64_t shift) { + x <<= shift; + y <<= shift; + return *this; +} + +_FORCE_INLINE_ constexpr Vector2FI Vector2FI::operator-() const { + return { -x, -y }; +} + +_FORCE_INLINE_ constexpr bool Vector2FI::operator==(const Vector2FI &other) const { + return x == other.x & y == other.y; +} + +_FORCE_INLINE_ constexpr bool Vector2FI::operator!=(const Vector2FI &other) const { + return x != other.x & y != other.y; +} \ No newline at end of file