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

Базовая версия библиотеки. Версия 0.2.0-dev

Browse source
This commit is contained in:
Andrey Pokidov 2024-11-12 00:55:03 +07:00
parent b086af7f66
commit 6a56e85052
39 changed files with 6200 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

193
src/quaternion.c Normal file
View file

@ -0,0 +1,193 @@
#include "quaternion.h"
// ============ Make Rotation Matrix ============ //
void sp_quaternion_make_matrix(const SPQuaternion* quaternion, SPMatrix3x3* 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_module = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-SP_EPSYLON <= square_module && square_module <= SP_EPSYLON)
{
sp_matrix3x3_make_identity(matrix);
return;
}
float corrector1;
float corrector2;
if (1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON) {
corrector1 = 2.0f - square_module;
corrector2 = 2.0f * corrector1;
}
else {
corrector1 = 1.0f / square_module;
corrector2 = 2.0f / square_module;
}
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 dp_quaternion_make_matrix(const DPQuaternion* quaternion, DPMatrix3x3* 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_module = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-DP_EPSYLON <= square_module && square_module <= DP_EPSYLON)
{
dp_matrix3x3_make_identity(matrix);
return;
}
double corrector1;
double corrector2;
if (1.0 - DP_TWO_EPSYLON <= square_module && square_module <= 1.0 + DP_TWO_EPSYLON) {
corrector1 = 2.0 - square_module;
corrector2 = 2.0 * corrector1;
}
else {
corrector1 = 1.0 / square_module;
corrector2 = 2.0 / square_module;
}
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 sp_quaternion_make_reverse_matrix(const SPQuaternion* quaternion, SPMatrix3x3* 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_module = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-SP_EPSYLON <= square_module && square_module <= SP_EPSYLON)
{
sp_matrix3x3_make_identity(matrix);
return;
}
float corrector1;
float corrector2;
if (1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON) {
corrector1 = 2.0f - square_module;
corrector2 = 2.0f * corrector1;
}
else {
corrector1 = 1.0f / square_module;
corrector2 = 2.0f / square_module;
}
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 dp_quaternion_make_reverse_matrix(const DPQuaternion* quaternion, DPMatrix3x3* 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_module = (s0s0 + x1x1) + (x2x2 + x3x3);
if (-DP_EPSYLON <= square_module && square_module <= DP_EPSYLON)
{
dp_matrix3x3_make_identity(matrix);
return;
}
double corrector1;
double corrector2;
if (1.0 - DP_TWO_EPSYLON <= square_module && square_module <= 1.0 + DP_TWO_EPSYLON) {
corrector1 = 2.0 - square_module;
corrector2 = 2.0 * corrector1;
}
else {
corrector1 = 1.0 / square_module;
corrector2 = 2.0 / square_module;
}
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);
}