Kimelas/bgc-c
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Оптимизация под GNU C компилятор / Optimization for GNU C Compiller

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-26 13:20:12 +07:00
parent e4d75824f3
commit 081f794eb1
10 changed files with 671 additions and 755 deletions
Download patch file Download diff file Expand all files Collapse all files

104
basic-geometry/matrix2x2.c
View file

@ -1,105 +1 @@
#include "matrix2x2.h"
// ================= Inversion ================== //
int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(matrix);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = matrix->r2c2;
const float r1c2 = -matrix->r1c2;
const float r2c1 = -matrix->r2c1;
const float r2c2 = matrix->r1c1;
const float multiplier = 1.0f / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(matrix);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = matrix->r2c2;
const double r1c2 = -matrix->r1c2;
const double r2c1 = -matrix->r2c1;
const double r2c2 = matrix->r1c1;
const double multiplier = 1.0 / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
// ================ Set Inverted ================ //
int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(from);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = from->r2c2;
const float r1c2 = -from->r1c2;
const float r2c1 = -from->r2c1;
const float r2c2 = from->r1c1;
const float multiplier = 1.0f / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}
int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(from);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = from->r2c2;
const double r1c2 = -from->r1c2;
const double r2c1 = -from->r2c1;
const double r2c2 = from->r1c1;
const double multiplier = 1.0 / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}

100
basic-geometry/matrix2x2.h
View file

@ -171,9 +171,55 @@ static inline void bg_fp64_matrix2x2_transpose(BgFP64Matrix2x2* matrix)
// ================= Inversion ================== //
int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix);
static inline int bg_fp32_matrix2x2_invert(BgFP32Matrix2x2* matrix)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(matrix);
int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = matrix->r2c2;
const float r1c2 = -matrix->r1c2;
const float r2c1 = -matrix->r2c1;
const float r2c2 = matrix->r1c1;
const float multiplier = 1.0f / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
static inline int bg_fp64_matrix2x2_invert(BgFP64Matrix2x2* matrix)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(matrix);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = matrix->r2c2;
const double r1c2 = -matrix->r1c2;
const double r2c1 = -matrix->r2c1;
const double r2c2 = matrix->r1c1;
const double multiplier = 1.0 / determinant;
matrix->r1c1 = r1c1 * multiplier;
matrix->r1c2 = r1c2 * multiplier;
matrix->r2c1 = r2c1 * multiplier;
matrix->r2c2 = r2c2 * multiplier;
return 1;
}
// =============== Set Transposed =============== //
@ -201,9 +247,55 @@ static inline void bg_fp64_matrix2x2_set_transposed(const BgFP64Matrix2x2* from,
// ================ Set Inverted ================ //
int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to);
static inline int bg_fp32_matrix2x2_set_inverted(const BgFP32Matrix2x2* from, BgFP32Matrix2x2* to)
{
const float determinant = bg_fp32_matrix2x2_get_determinant(from);
int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to);
if (-BG_FP32_EPSYLON <= determinant && determinant <= BG_FP32_EPSYLON) {
return 0;
}
const float r1c1 = from->r2c2;
const float r1c2 = -from->r1c2;
const float r2c1 = -from->r2c1;
const float r2c2 = from->r1c1;
const float multiplier = 1.0f / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}
static inline int bg_fp64_matrix2x2_set_inverted(const BgFP64Matrix2x2* from, BgFP64Matrix2x2* to)
{
const double determinant = bg_fp64_matrix2x2_get_determinant(from);
if (-BG_FP64_EPSYLON <= determinant && determinant <= BG_FP64_EPSYLON) {
return 0;
}
const double r1c1 = from->r2c2;
const double r1c2 = -from->r1c2;
const double r2c1 = -from->r2c1;
const double r2c2 = from->r1c1;
const double multiplier = 1.0 / determinant;
to->r1c1 = r1c1 * multiplier;
to->r1c2 = r1c2 * multiplier;
to->r2c1 = r2c1 * multiplier;
to->r2c2 = r2c2 * multiplier;
return 1;
}
// ================= Set Row 1 ================== //

231
basic-geometry/quaternion.c
View file

@ -1,233 +1,2 @@
#include "quaternion.h"
// =============== Normalization ================ //
int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion)
{
const float square_modulus = bg_fp32_quaternion_get_square_modulus(quaternion);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_quaternion_reset(quaternion);
return 0;
}
const float multiplier = sqrtf(1.0f / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion)
{
const double square_modulus = bg_fp64_quaternion_get_square_modulus(quaternion);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp64_quaternion_reset(quaternion);
return 0;
}
const double multiplier = sqrt(1.0 / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
// ============ Make Rotation Matrix ============ //
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP32_EPSYLON <= square_modulus && square_modulus <= BG_FP32_EPSYLON)
{
bg_fp32_matrix3x3_set_to_identity(matrix);
return;
}
const float corrector1 = 1.0f / square_modulus;
const float corrector2 = 2.0f * corrector1;
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 - s0x3);
matrix->r2c3 = corrector2 * (x2x3 - s0x1);
matrix->r3c1 = corrector2 * (x1x3 - s0x2);
matrix->r2c1 = corrector2 * (x1x2 + s0x3);
matrix->r3c2 = corrector2 * (x2x3 + s0x1);
matrix->r1c3 = corrector2 * (x1x3 + s0x2);
}
void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP64_EPSYLON <= square_modulus && square_modulus <= BG_FP64_EPSYLON)
{
bg_fp64_matrix3x3_set_to_identity(matrix);
return;
}
const double corrector1 = 1.0f / square_modulus;
const double corrector2 = 2.0f * corrector1;
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 - s0x3);
matrix->r2c3 = corrector2 * (x2x3 - s0x1);
matrix->r3c1 = corrector2 * (x1x3 - s0x2);
matrix->r2c1 = corrector2 * (x1x2 + s0x3);
matrix->r3c2 = corrector2 * (x2x3 + s0x1);
matrix->r1c3 = corrector2 * (x1x3 + s0x2);
}
// ============ Make Reverse Matrix ============= //
void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP32_EPSYLON <= square_modulus && square_modulus <= BG_FP32_EPSYLON)
{
bg_fp32_matrix3x3_set_to_identity(matrix);
return;
}
const float corrector1 = 1.0f / square_modulus;
const float corrector2 = 2.0f * corrector1;
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 + s0x3);
matrix->r2c3 = corrector2 * (x2x3 + s0x1);
matrix->r3c1 = corrector2 * (x1x3 + s0x2);
matrix->r2c1 = corrector2 * (x1x2 - s0x3);
matrix->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
}
void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP64_EPSYLON <= square_modulus && square_modulus <= BG_FP64_EPSYLON)
{
bg_fp64_matrix3x3_set_to_identity(matrix);
return;
}
const double corrector1 = 1.0f / square_modulus;
const double corrector2 = 2.0f * corrector1;
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 + s0x3);
matrix->r2c3 = corrector2 * (x2x3 + s0x1);
matrix->r3c1 = corrector2 * (x1x3 + s0x2);
matrix->r2c1 = corrector2 * (x1x2 - s0x3);
matrix->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
}
// ================== Product =================== //
void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product)
{
const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}
void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product)
{
const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}

224
basic-geometry/quaternion.h
View file

@ -183,21 +183,207 @@ static inline double bg_fp64_quaternion_get_modulus(const BgFP64Quaternion* quat
// =============== Normalization ================ //
int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion);
static inline int bg_fp32_quaternion_normalize(BgFP32Quaternion* quaternion)
{
const float square_modulus = bg_fp32_quaternion_get_square_modulus(quaternion);
int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_quaternion_reset(quaternion);
return 0;
}
const float multiplier = sqrtf(1.0f / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
static inline int bg_fp64_quaternion_normalize(BgFP64Quaternion* quaternion)
{
const double square_modulus = bg_fp64_quaternion_get_square_modulus(quaternion);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp64_quaternion_reset(quaternion);
return 0;
}
const double multiplier = sqrt(1.0 / square_modulus);
quaternion->s0 *= multiplier;
quaternion->x1 *= multiplier;
quaternion->x2 *= multiplier;
quaternion->x3 *= multiplier;
return 1;
}
// ============ Make Rotation Matrix ============ //
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
void bg_fp32_quaternion_get_rotation_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP32_EPSYLON <= square_modulus && square_modulus <= BG_FP32_EPSYLON)
{
bg_fp32_matrix3x3_set_to_identity(matrix);
return;
}
const float corrector1 = 1.0f / square_modulus;
const float corrector2 = 2.0f * corrector1;
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 - s0x3);
matrix->r2c3 = corrector2 * (x2x3 - s0x1);
matrix->r3c1 = corrector2 * (x1x3 - s0x2);
matrix->r2c1 = corrector2 * (x1x2 + s0x3);
matrix->r3c2 = corrector2 * (x2x3 + s0x1);
matrix->r1c3 = corrector2 * (x1x3 + s0x2);
}
void bg_fp64_quaternion_get_rotation_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP64_EPSYLON <= square_modulus && square_modulus <= BG_FP64_EPSYLON)
{
bg_fp64_matrix3x3_set_to_identity(matrix);
return;
}
const double corrector1 = 1.0f / square_modulus;
const double corrector2 = 2.0f * corrector1;
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 - s0x3);
matrix->r2c3 = corrector2 * (x2x3 - s0x1);
matrix->r3c1 = corrector2 * (x1x3 - s0x2);
matrix->r2c1 = corrector2 * (x1x2 + s0x3);
matrix->r3c2 = corrector2 * (x2x3 + s0x1);
matrix->r1c3 = corrector2 * (x1x3 + s0x2);
}
// ============ Make Reverse Matrix ============= //
void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix);
static inline void bg_fp32_quaternion_get_reverse_matrix(const BgFP32Quaternion* quaternion, BgFP32Matrix3x3* matrix)
{
const float s0s0 = quaternion->s0 * quaternion->s0;
const float x1x1 = quaternion->x1 * quaternion->x1;
const float x2x2 = quaternion->x2 * quaternion->x2;
const float x3x3 = quaternion->x3 * quaternion->x3;
void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix);
const float square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP32_EPSYLON <= square_modulus && square_modulus <= BG_FP32_EPSYLON)
{
bg_fp32_matrix3x3_set_to_identity(matrix);
return;
}
const float corrector1 = 1.0f / square_modulus;
const float corrector2 = 2.0f * corrector1;
const float s0x1 = quaternion->s0 * quaternion->x1;
const float s0x2 = quaternion->s0 * quaternion->x2;
const float s0x3 = quaternion->s0 * quaternion->x3;
const float x1x2 = quaternion->x1 * quaternion->x2;
const float x1x3 = quaternion->x1 * quaternion->x3;
const float x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 + s0x3);
matrix->r2c3 = corrector2 * (x2x3 + s0x1);
matrix->r3c1 = corrector2 * (x1x3 + s0x2);
matrix->r2c1 = corrector2 * (x1x2 - s0x3);
matrix->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
}
static inline void bg_fp64_quaternion_get_reverse_matrix(const BgFP64Quaternion* quaternion, BgFP64Matrix3x3* matrix)
{
const double s0s0 = quaternion->s0 * quaternion->s0;
const double x1x1 = quaternion->x1 * quaternion->x1;
const double x2x2 = quaternion->x2 * quaternion->x2;
const double x3x3 = quaternion->x3 * quaternion->x3;
const double square_modulus = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-BG_FP64_EPSYLON <= square_modulus && square_modulus <= BG_FP64_EPSYLON)
{
bg_fp64_matrix3x3_set_to_identity(matrix);
return;
}
const double corrector1 = 1.0f / square_modulus;
const double corrector2 = 2.0f * corrector1;
const double s0x1 = quaternion->s0 * quaternion->x1;
const double s0x2 = quaternion->s0 * quaternion->x2;
const double s0x3 = quaternion->s0 * quaternion->x3;
const double x1x2 = quaternion->x1 * quaternion->x2;
const double x1x3 = quaternion->x1 * quaternion->x3;
const double x2x3 = quaternion->x2 * quaternion->x3;
matrix->r1c1 = corrector1 * ((s0s0 + x1x1) - (x2x2 + x3x3));
matrix->r2c2 = corrector1 * ((s0s0 + x2x2) - (x1x1 + x3x3));
matrix->r3c3 = corrector1 * ((s0s0 + x3x3) - (x1x1 + x2x2));
matrix->r1c2 = corrector2 * (x1x2 + s0x3);
matrix->r2c3 = corrector2 * (x2x3 + s0x1);
matrix->r3c1 = corrector2 * (x1x3 + s0x2);
matrix->r2c1 = corrector2 * (x1x2 - s0x3);
matrix->r3c2 = corrector2 * (x2x3 - s0x1);
matrix->r1c3 = corrector2 * (x1x3 - s0x2);
}
// ==================== Add ===================== //
@ -267,8 +453,30 @@ static inline void bg_fp64_quaternion_divide(const BgFP64Quaternion* dividend, c
// ================== Product =================== //
void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product);
static inline void bg_fp32_quaternion_get_product(const BgFP32Quaternion* left, const BgFP32Quaternion* right, BgFP32Quaternion* product)
{
const float s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const float x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const float x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const float x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}
static inline void bg_fp64_quaternion_get_product(const BgFP64Quaternion* left, const BgFP64Quaternion* right, BgFP64Quaternion* product)
{
const double s0 = (left->s0 * right->s0 - left->x1 * right->x1) - (left->x2 * right->x2 + left->x3 * right->x3);
const double x1 = (left->x1 * right->s0 + left->s0 * right->x1) - (left->x3 * right->x2 - left->x2 * right->x3);
const double x2 = (left->x2 * right->s0 + left->s0 * right->x2) - (left->x1 * right->x3 - left->x3 * right->x1);
const double x3 = (left->x3 * right->s0 + left->s0 * right->x3) - (left->x2 * right->x1 - left->x1 * right->x2);
product->s0 = s0;
product->x1 = x1;
product->x2 = x2;
product->x3 = x3;
}
#endif // _GEOMETRY_QUATERNION_H_

36
basic-geometry/vector2.c
View file

@ -1,41 +1,5 @@
#include "vector2.h"
// =============== Normalization ================ //
int bg_fp32_vector2_normalize(BgFP32Vector2* vector)
{
const float square_modulus = bg_fp32_vector2_get_square_modulus(vector);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector2_reset(vector);
return 0;
}
bg_fp32_vector2_multiply(vector, sqrtf(1.0f / square_modulus), vector);
return 1;
}
int bg_fp64_vector2_normalize(BgFP64Vector2* vector)
{
const double square_modulus = bg_fp64_vector2_get_square_modulus(vector);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
bg_fp64_vector2_reset(vector);
return 0;
}
bg_fp64_vector2_multiply(vector, sqrt(1.0 / square_modulus), vector);
return 1;
}
// =================== Angle ==================== //
float bg_fp32_vector2_get_angle(const BgFP32Vector2* vector1, const BgFP32Vector2* vector2, const angle_unit_t unit)

34
basic-geometry/vector2.h
View file

@ -270,9 +270,39 @@ static inline double bg_fp64_vector2_cross_product(const BgFP64Vector2* vector1,
// =============== Normalization ================ //
int bg_fp32_vector2_normalize(BgFP32Vector2* vector);
static inline int bg_fp32_vector2_normalize(BgFP32Vector2* vector)
{
const float square_modulus = bg_fp32_vector2_get_square_modulus(vector);
int bg_fp64_vector2_normalize(BgFP64Vector2* vector);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector2_reset(vector);
return 0;
}
bg_fp32_vector2_multiply(vector, sqrtf(1.0f / square_modulus), vector);
return 1;
}
static inline int bg_fp64_vector2_normalize(BgFP64Vector2* vector)
{
const double square_modulus = bg_fp64_vector2_get_square_modulus(vector);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
bg_fp64_vector2_reset(vector);
return 0;
}
bg_fp64_vector2_multiply(vector, sqrt(1.0 / square_modulus), vector);
return 1;
}
// =============== Get Normalized =============== //

36
basic-geometry/vector3.c
View file

@ -1,41 +1,5 @@
#include "vector3.h"
// =============== Normalization ================ //
int bg_fp32_vector3_normalize(BgFP32Vector3* vector)
{
const float square_modulus = bg_fp32_vector3_get_square_modulus(vector);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector3_reset(vector);
return 0;
}
bg_fp32_vector3_multiply(vector, sqrtf(1.0f / square_modulus), vector);
return 1;
}
int bg_fp64_vector3_normalize(BgFP64Vector3* vector)
{
const double square_modulus = bg_fp64_vector3_get_square_modulus(vector);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
bg_fp64_vector3_reset(vector);
return 0;
}
bg_fp64_vector3_multiply(vector, sqrt(1.0 / square_modulus), vector);
return 1;
}
// =================== Angle ==================== //
float bg_fp32_vector3_get_angle(const BgFP32Vector3* vector1, const BgFP32Vector3* vector2, const angle_unit_t unit)

34
basic-geometry/vector3.h
View file

@ -346,9 +346,39 @@ static inline void bg_fp64_vector3_double_cross(const BgFP64Vector3* vector1, co
// =============== Normalization ================ //
int bg_fp32_vector3_normalize(BgFP32Vector3* vector);
static inline int bg_fp32_vector3_normalize(BgFP32Vector3* vector)
{
const float square_modulus = bg_fp32_vector3_get_square_modulus(vector);
int bg_fp64_vector3_normalize(BgFP64Vector3* vector);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
bg_fp32_vector3_reset(vector);
return 0;
}
bg_fp32_vector3_multiply(vector, sqrtf(1.0f / square_modulus), vector);
return 1;
}
static inline int bg_fp64_vector3_normalize(BgFP64Vector3* vector)
{
const double square_modulus = bg_fp64_vector3_get_square_modulus(vector);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return 1;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
bg_fp64_vector3_reset(vector);
return 0;
}
bg_fp64_vector3_multiply(vector, sqrt(1.0 / square_modulus), vector);
return 1;
}
// =============== Get Normalized =============== //

319
basic-geometry/versor.c
View file

@ -7,70 +7,6 @@ const BgFP32Versor BG_FP32_IDLE_VERSOR = { 1.0f, 0.0f, 0.0f, 0.0f };
const BgFP64Versor BG_FP64_IDLE_VERSOR = { 1.0, 0.0, 0.0, 0.0 };
// ==================== Set ===================== //
void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor)
{
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
twin->s0 = 1.0f;
twin->x1 = 0.0f;
twin->x2 = 0.0f;
twin->x3 = 0.0f;
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor)
{
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
twin->s0 = 1.0;
twin->x1 = 0.0;
twin->x2 = 0.0;
twin->x3 = 0.0;
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
// =============== Set Crude Turn =============== //
void bg_fp32_versor_set_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, BgFP32Versor* result)
@ -164,258 +100,3 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re
result->axis.x2 = versor->x2 * multiplier;
result->axis.x3 = versor->x3 * multiplier;
}
// ================ Combination ================= //
void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result)
{
const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
twin->s0 = 1.0f;
twin->x1 = 0.0f;
twin->x2 = 0.0f;
twin->x3 = 0.0f;
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result)
{
const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
twin->s0 = 1.0;
twin->x1 = 0.0;
twin->x2 = 0.0;
twin->x3 = 0.0;
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
// =========== Make Rotation Matrix3x3 ========== //
void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
// =========== Make Reverse Matrix3x3 =========== //
void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
// ================ Turn Vector ================= //
void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ============== Turn Vector Back ============== //
void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}

306
basic-geometry/versor.h
View file

@ -58,9 +58,67 @@ static inline void bg_fp64_versor_reset(BgFP64Versor* versor)
// ==================== Set ===================== //
void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor);
static inline void bg_fp32_versor_set_values(const float s0, const float x1, const float x2, const float x3, BgFP32Versor* versor)
{
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)versor;
void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor);
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
twin->s0 = 1.0f;
twin->x1 = 0.0f;
twin->x2 = 0.0f;
twin->x3 = 0.0f;
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
static inline void bg_fp64_versor_set_values(const double s0, const double x1, const double x2, const double x3, BgFP64Versor* versor)
{
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)versor;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
twin->s0 = 1.0;
twin->x1 = 0.0;
twin->x2 = 0.0;
twin->x3 = 0.0;
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
// ==================== Copy ==================== //
@ -238,9 +296,77 @@ static inline void bg_fp64_versor_set_inverted_fp32(const BgFP32Versor* versor,
// ================ Combination ================= //
void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result);
static inline void bg_fp32_versor_combine(const BgFP32Versor* second, const BgFP32Versor* first, BgFP32Versor* result)
{
const float s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const float x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const float x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const float x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result);
const float square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
__BgFP32DarkTwinVersor* twin = (__BgFP32DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0f - BG_FP32_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0f + BG_FP32_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP32_SQUARE_EPSYLON) {
twin->s0 = 1.0f;
twin->x1 = 0.0f;
twin->x2 = 0.0f;
twin->x3 = 0.0f;
return;
}
const float multiplier = sqrtf(1.0f / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
static inline void bg_fp64_versor_combine(const BgFP64Versor* second, const BgFP64Versor* first, BgFP64Versor* result)
{
const double s0 = (second->s0 * first->s0 - second->x1 * first->x1) - (second->x2 * first->x2 + second->x3 * first->x3);
const double x1 = (second->x1 * first->s0 + second->s0 * first->x1) - (second->x3 * first->x2 - second->x2 * first->x3);
const double x2 = (second->x2 * first->s0 + second->s0 * first->x2) - (second->x1 * first->x3 - second->x3 * first->x1);
const double x3 = (second->x3 * first->s0 + second->s0 * first->x3) - (second->x2 * first->x1 - second->x1 * first->x2);
const double square_modulus = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3);
__BgFP64DarkTwinVersor* twin = (__BgFP64DarkTwinVersor*)result;
twin->s0 = s0;
twin->x1 = x1;
twin->x2 = x2;
twin->x3 = x3;
if (1.0 - BG_FP64_TWO_EPSYLON <= square_modulus && square_modulus <= 1.0 + BG_FP64_TWO_EPSYLON) {
return;
}
if (square_modulus <= BG_FP64_SQUARE_EPSYLON) {
twin->s0 = 1.0;
twin->x1 = 0.0;
twin->x2 = 0.0;
twin->x3 = 0.0;
return;
}
const double multiplier = sqrt(1.0 / square_modulus);
twin->s0 *= multiplier;
twin->x1 *= multiplier;
twin->x2 *= multiplier;
twin->x3 *= multiplier;
}
// ================= Rotation3 ================== //
@ -250,26 +376,182 @@ void bg_fp64_versor_get_rotation(const BgFP64Versor* versor, BgFP64Rotation3* re
// =========== Make Rotation Matrix3x3 ========== //
void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix);
static inline void bg_fp32_versor_get_rotation_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix);
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
static inline void bg_fp64_versor_get_rotation_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 - s0x3;
matrix->r2c3 = x2x3 - s0x1;
matrix->r3c1 = x1x3 - s0x2;
matrix->r2c1 = x1x2 + s0x3;
matrix->r3c2 = x2x3 + s0x1;
matrix->r1c3 = x1x3 + s0x2;
}
// =========== Make Reverse Matrix3x3 =========== //
void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix);
static inline void bg_fp32_versor_get_reverse_matrix(const BgFP32Versor* versor, BgFP32Matrix3x3* matrix)
{
const float s0s0 = versor->s0 * versor->s0;
const float x1x1 = versor->x1 * versor->x1;
const float x2x2 = versor->x2 * versor->x2;
const float x3x3 = versor->x3 * versor->x3;
void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix);
const float s0x1 = 2.0f * versor->s0 * versor->x1;
const float s0x2 = 2.0f * versor->s0 * versor->x2;
const float s0x3 = 2.0f * versor->s0 * versor->x3;
const float x1x2 = 2.0f * versor->x1 * versor->x2;
const float x1x3 = 2.0f * versor->x1 * versor->x3;
const float x2x3 = 2.0f * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
static inline void bg_fp64_versor_get_reverse_matrix(const BgFP64Versor* versor, BgFP64Matrix3x3* matrix)
{
const double s0s0 = versor->s0 * versor->s0;
const double x1x1 = versor->x1 * versor->x1;
const double x2x2 = versor->x2 * versor->x2;
const double x3x3 = versor->x3 * versor->x3;
const double s0x1 = 2.0 * versor->s0 * versor->x1;
const double s0x2 = 2.0 * versor->s0 * versor->x2;
const double s0x3 = 2.0 * versor->s0 * versor->x3;
const double x1x2 = 2.0 * versor->x1 * versor->x2;
const double x1x3 = 2.0 * versor->x1 * versor->x3;
const double x2x3 = 2.0 * versor->x2 * versor->x3;
matrix->r1c1 = (s0s0 + x1x1) - (x2x2 + x3x3);
matrix->r2c2 = (s0s0 + x2x2) - (x1x1 + x3x3);
matrix->r3c3 = (s0s0 + x3x3) - (x1x1 + x2x2);
matrix->r1c2 = x1x2 + s0x3;
matrix->r2c3 = x2x3 + s0x1;
matrix->r3c1 = x1x3 + s0x2;
matrix->r2c1 = x1x2 - s0x3;
matrix->r3c2 = x2x3 - s0x1;
matrix->r1c3 = x1x3 - s0x2;
}
// ================ Turn Vector ================= //
void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result);
static inline void bg_fp32_versor_turn(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result);
const float x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 + tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 + tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 + tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
// ============== Turn Vector Back ============== //
void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result);
static inline void bg_fp32_versor_turn_back(const BgFP32Versor* versor, const BgFP32Vector3* vector, BgFP32Vector3* result)
{
const float tx1 = 2.0f * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const float tx2 = 2.0f * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const float tx3 = 2.0f * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result);
const float x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const float x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const float x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
static inline void bg_fp64_versor_turn_back(const BgFP64Versor* versor, const BgFP64Vector3* vector, BgFP64Vector3* result)
{
const double tx1 = 2.0 * (versor->x2 * vector->x3 - versor->x3 * vector->x2);
const double tx2 = 2.0 * (versor->x3 * vector->x1 - versor->x1 * vector->x3);
const double tx3 = 2.0 * (versor->x1 * vector->x2 - versor->x2 * vector->x1);
const double x1 = (vector->x1 - tx1 * versor->s0) + (versor->x2 * tx3 - versor->x3 * tx2);
const double x2 = (vector->x2 - tx2 * versor->s0) + (versor->x3 * tx1 - versor->x1 * tx3);
const double x3 = (vector->x3 - tx3 * versor->s0) + (versor->x1 * tx2 - versor->x2 * tx1);
result->x1 = x1;
result->x2 = x2;
result->x3 = x3;
}
#endif