Удаление combite3 функций, изменение названий параметров функицй combine и exclude
This commit is contained in:
parent
54c762da14
commit
5425206401
10 changed files with 277 additions and 220 deletions
|
|
@ -53,6 +53,10 @@
|
|||
<Option compilerVar="CC" />
|
||||
</Unit>
|
||||
<Unit filename="printing_utils.h" />
|
||||
<Unit filename="turn3_combination.c">
|
||||
<Option compilerVar="CC" />
|
||||
</Unit>
|
||||
<Unit filename="turn3_combination.h" />
|
||||
<Unit filename="vector3_pair_difference.c">
|
||||
<Option compilerVar="CC" />
|
||||
</Unit>
|
||||
|
|
|
|||
|
|
@ -1,128 +1,12 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <basic-geometry.h>
|
||||
|
||||
#ifdef _WIN64
|
||||
#include <windows.h>
|
||||
#else
|
||||
#include <time.h>
|
||||
#endif // _WINDOWS_
|
||||
|
||||
#include "printing_utils.h"
|
||||
#include "vector3_pair_difference.h"
|
||||
#include "turn3_combination.h"
|
||||
#include "affine3.h"
|
||||
|
||||
typedef struct {
|
||||
BGC_FP32_Turn3 versor1, versor2, result;
|
||||
} structure_fp32_t;
|
||||
|
||||
structure_fp32_t* allocate_structures(const unsigned int amount)
|
||||
{
|
||||
return calloc(amount, sizeof(structure_fp32_t));
|
||||
}
|
||||
|
||||
structure_fp32_t* make_structures(const unsigned int amount)
|
||||
{
|
||||
structure_fp32_t* list = allocate_structures(amount);
|
||||
|
||||
if (list == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const float multiplier = 2.0f / RAND_MAX;
|
||||
|
||||
for (unsigned int i = 0; i < amount; i++) {
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor1,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f
|
||||
);
|
||||
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor2,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f
|
||||
);
|
||||
|
||||
bgc_fp32_turn3_reset(&list[i].result);
|
||||
}
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
|
||||
void list_work(const uint_fast32_t amount, structure_fp32_t* list)
|
||||
{
|
||||
for (uint_fast32_t j = 0; j < 1000; j++) {
|
||||
for (uint_fast32_t i = 0; i < amount; i++) {
|
||||
bgc_fp32_turn3_combine(&list[i].result, &list[i].versor1, &list[i].versor2);
|
||||
}
|
||||
}
|
||||
}
|
||||
/*
|
||||
int main()
|
||||
{
|
||||
const unsigned int amount = 1000000;
|
||||
structure_fp32_t* list = make_structures(amount);
|
||||
|
||||
#ifdef _WIN64
|
||||
ULONGLONG start, end;
|
||||
|
||||
start = GetTickCount64();
|
||||
|
||||
srand((unsigned int)(start & 0xfffffff));
|
||||
|
||||
start = GetTickCount64();
|
||||
#else
|
||||
struct timespec start, end;
|
||||
clock_gettime(0, &start);
|
||||
srand((unsigned int)(start.tv_nsec & 0xfffffff));
|
||||
|
||||
clock_gettime(CLOCK_REALTIME, &start);
|
||||
#endif // _WIN64
|
||||
|
||||
list_work(amount, list);
|
||||
|
||||
#ifdef _WIN64
|
||||
end = GetTickCount64();
|
||||
|
||||
printf("Time: %lld\n", end - start);
|
||||
#else
|
||||
clock_gettime(CLOCK_REALTIME, &end);
|
||||
|
||||
printf("Time: %lf\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001);
|
||||
#endif // _WIN64
|
||||
|
||||
print_fp32_quaternion(&list[10].versor1._versor);
|
||||
print_fp32_quaternion(&list[10].versor2._versor);
|
||||
print_fp32_quaternion(&list[10].result._versor);
|
||||
|
||||
free(list);
|
||||
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
|
||||
/*
|
||||
int main() {
|
||||
BGC_FP32_Complex complex, exponent, result;
|
||||
|
||||
bgc_fp32_complex_set_values(0, 1, &complex);
|
||||
|
||||
bgc_fp32_complex_set_values(4, 0, &exponent);
|
||||
|
||||
bgc_fp32_complex_get_exponation(&complex, exponent.real, exponent.imaginary, &result);
|
||||
|
||||
printf("(%f, %f) ^ (%f, %f) = (%f, %f)\n", complex.real, complex.imaginary, exponent.real, exponent.imaginary, result.real, result.imaginary);
|
||||
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
/*
|
||||
int main() {
|
||||
BGC_FP32_Turn3 start = { 1.0f, 0.0f, 0.0f, 0.0f };
|
||||
|
|
@ -177,13 +61,11 @@ int main()
|
|||
//test_fp32_pair_difference();
|
||||
//test_fp64_pair_difference();
|
||||
|
||||
//printf("Affine3 performance test: %f\n", test_bgc_affine3_performance(10000000, 10));
|
||||
//test_fp32_quaternion_set_matrix();
|
||||
//test_fp64_quaternion_set_matrix();
|
||||
|
||||
//printf("sizeof(BGC_FP32_Affine3) = %zu\n", sizeof(BGC_FP32_Affine3));
|
||||
//printf("offsetof(shift) = %zu\n", offsetof(BGC_FP32_Affine3, shift));
|
||||
//printf("sizeof(BGC_FP32_Matrix3x3) = %zu\n", sizeof(BGC_FP32_Matrix3x3));
|
||||
|
||||
test_fp32_quaternion_set_matrix();
|
||||
//test_fp32_turn3_combination();
|
||||
//test_fp64_turn3_combination();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
218
basic-geometry-dev/turn3_combination.c
Normal file
218
basic-geometry-dev/turn3_combination.c
Normal file
|
|
@ -0,0 +1,218 @@
|
|||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef _WIN64
|
||||
#include <windows.h>
|
||||
#else
|
||||
#include <time.h>
|
||||
#endif // _WINDOWS_
|
||||
|
||||
#include "printing_utils.h"
|
||||
#include "turn3_combination.h"
|
||||
|
||||
// =================== Types ==================== //
|
||||
|
||||
typedef struct {
|
||||
BGC_FP32_Turn3 versor1, versor2, result;
|
||||
} Testing_FP32_Turn3Structure;
|
||||
|
||||
typedef struct {
|
||||
BGC_FP64_Turn3 versor1, versor2, result;
|
||||
} Testing_FP64_Turn3Structure;
|
||||
|
||||
// ================= Allocation ================= //
|
||||
|
||||
static Testing_FP32_Turn3Structure* _fp32_allocate_structures(const unsigned int amount)
|
||||
{
|
||||
return malloc((size_t)amount * sizeof(Testing_FP32_Turn3Structure));
|
||||
}
|
||||
|
||||
static Testing_FP64_Turn3Structure* _fp64_allocate_structures(const unsigned int amount)
|
||||
{
|
||||
return malloc((size_t)amount * sizeof(Testing_FP64_Turn3Structure));
|
||||
}
|
||||
|
||||
// =============== Randomization ================ //
|
||||
|
||||
static Testing_FP32_Turn3Structure* _fp32_make_structures(const unsigned int amount)
|
||||
{
|
||||
Testing_FP32_Turn3Structure* list = _fp32_allocate_structures(amount);
|
||||
|
||||
if (list == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const float multiplier = 2.0f / RAND_MAX;
|
||||
|
||||
for (unsigned int i = 0; i < amount; i++) {
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor1,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f
|
||||
);
|
||||
|
||||
bgc_fp32_turn3_set_values(
|
||||
&list[i].versor2,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f,
|
||||
rand() * multiplier - 1.0f
|
||||
);
|
||||
|
||||
bgc_fp32_turn3_reset(&list[i].result);
|
||||
}
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
static Testing_FP64_Turn3Structure* _fp64_make_structures(const unsigned int amount)
|
||||
{
|
||||
Testing_FP64_Turn3Structure* list = _fp64_allocate_structures(amount);
|
||||
|
||||
if (list == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const double multiplier = 2.0 / RAND_MAX;
|
||||
|
||||
for (unsigned int i = 0; i < amount; i++) {
|
||||
bgc_fp64_turn3_set_values(
|
||||
&list[i].versor1,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0
|
||||
);
|
||||
|
||||
bgc_fp64_turn3_set_values(
|
||||
&list[i].versor2,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0,
|
||||
rand() * multiplier - 1.0
|
||||
);
|
||||
|
||||
bgc_fp64_turn3_reset(&list[i].result);
|
||||
}
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
// ================ Circle Work ================= //
|
||||
|
||||
static int_fast64_t _fp32_circle_work(const uint_fast32_t amount, Testing_FP32_Turn3Structure* list)
|
||||
{
|
||||
int_fast64_t total = 0;
|
||||
|
||||
for (uint_fast32_t j = 0; j < 1000; j++) {
|
||||
for (uint_fast32_t i = 0; i < amount; i++) {
|
||||
bgc_fp32_turn3_combine(&list[i].result, &list[i].versor1, &list[i].versor2);
|
||||
total++;
|
||||
}
|
||||
}
|
||||
|
||||
return total;
|
||||
}
|
||||
|
||||
static int_fast64_t _fp64_circle_work(const uint_fast32_t amount, Testing_FP64_Turn3Structure* list)
|
||||
{
|
||||
int_fast64_t total = 0;
|
||||
|
||||
for (uint_fast32_t j = 0; j < 1000; j++) {
|
||||
for (uint_fast32_t i = 0; i < amount; i++) {
|
||||
bgc_fp64_turn3_combine(&list[i].result, &list[i].versor1, &list[i].versor2);
|
||||
total++;
|
||||
}
|
||||
}
|
||||
|
||||
return total;
|
||||
}
|
||||
|
||||
// ==================== Test ==================== //
|
||||
|
||||
void test_fp32_turn3_combination()
|
||||
{
|
||||
const unsigned int amount = 1000000;
|
||||
int_fast64_t total = 0;
|
||||
Testing_FP32_Turn3Structure* list = _fp32_make_structures(amount);
|
||||
|
||||
#ifdef _WIN64
|
||||
ULONGLONG start, end;
|
||||
|
||||
start = GetTickCount64();
|
||||
|
||||
srand((unsigned int)(start & 0xfffffff));
|
||||
|
||||
start = GetTickCount64();
|
||||
#else
|
||||
struct timespec start, end;
|
||||
clock_gettime(0, &start);
|
||||
srand((unsigned int)(start.tv_nsec & 0xfffffff));
|
||||
|
||||
clock_gettime(CLOCK_REALTIME, &start);
|
||||
#endif // _WIN64
|
||||
|
||||
total = _fp32_circle_work(amount, list);
|
||||
|
||||
#ifdef _WIN64
|
||||
end = GetTickCount64();
|
||||
|
||||
printf("Time: %lld, Total iterations: %ld\n", end - start, total);
|
||||
#else
|
||||
clock_gettime(CLOCK_REALTIME, &end);
|
||||
|
||||
printf("Time: %lf, Total iterations: %ld\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001, total);
|
||||
#endif // _WIN64
|
||||
|
||||
print_fp32_quaternion(&list[10].versor1._versor);
|
||||
print_fp32_quaternion(&list[10].versor2._versor);
|
||||
print_fp32_quaternion(&list[10].result._versor);
|
||||
|
||||
free(list);
|
||||
}
|
||||
|
||||
|
||||
void test_fp64_turn3_combination()
|
||||
{
|
||||
const unsigned int amount = 1000000;
|
||||
int_fast64_t total = 0;
|
||||
Testing_FP64_Turn3Structure* list = _fp64_make_structures(amount);
|
||||
|
||||
#ifdef _WIN64
|
||||
ULONGLONG start, end;
|
||||
|
||||
start = GetTickCount64();
|
||||
|
||||
srand((unsigned int)(start & 0xfffffff));
|
||||
|
||||
start = GetTickCount64();
|
||||
#else
|
||||
struct timespec start, end;
|
||||
clock_gettime(0, &start);
|
||||
srand((unsigned int)(start.tv_nsec & 0xfffffff));
|
||||
|
||||
clock_gettime(CLOCK_REALTIME, &start);
|
||||
#endif // _WIN64
|
||||
|
||||
total = _fp64_circle_work(amount, list);
|
||||
|
||||
#ifdef _WIN64
|
||||
end = GetTickCount64();
|
||||
|
||||
printf("Time: %lld, Total iterations: %ld\n", end - start, total);
|
||||
#else
|
||||
clock_gettime(CLOCK_REALTIME, &end);
|
||||
|
||||
printf("Time: %lf, Total iterations: %ld\n", (end.tv_sec - start.tv_sec) * 1000.0 + (end.tv_nsec - start.tv_nsec) * 0.000001, total);
|
||||
#endif // _WIN64
|
||||
|
||||
print_fp64_quaternion(&list[10].versor1._versor);
|
||||
print_fp64_quaternion(&list[10].versor2._versor);
|
||||
print_fp64_quaternion(&list[10].result._versor);
|
||||
|
||||
free(list);
|
||||
}
|
||||
9
basic-geometry-dev/turn3_combination.h
Normal file
9
basic-geometry-dev/turn3_combination.h
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
#ifndef TURN3_COMBINATION_H_INCLUDED
|
||||
#define TURN3_COMBINATION_H_INCLUDED
|
||||
|
||||
#include <basic-geometry.h>
|
||||
|
||||
void test_fp32_turn3_combination();
|
||||
void test_fp64_turn3_combination();
|
||||
|
||||
#endif // TURN3_COMBINATION_H_INCLUDED
|
||||
|
|
@ -48,11 +48,11 @@ extern inline void bgc_fp64_rigid_pose3_revert(BGC_FP64_RigidPose3* const pose);
|
|||
extern inline void bgc_fp32_rigid_pose3_get_reverse_pose(BGC_FP32_RigidPose3* const reverse, const BGC_FP32_RigidPose3* const pose);
|
||||
extern inline void bgc_fp64_rigid_pose3_get_reverse_pose(BGC_FP64_RigidPose3* const reverse, const BGC_FP64_RigidPose3* const pose);
|
||||
|
||||
extern inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* const combination, const BGC_FP32_RigidPose3* const first, const BGC_FP32_RigidPose3* const second);
|
||||
extern inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* const combination, const BGC_FP64_RigidPose3* const first, const BGC_FP64_RigidPose3* const second);
|
||||
extern inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* const combination, const BGC_FP32_RigidPose3* const external_pose, const BGC_FP32_RigidPose3* const internal_pose);
|
||||
extern inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* const combination, const BGC_FP64_RigidPose3* const external_pose, const BGC_FP64_RigidPose3* const internal_pose);
|
||||
|
||||
extern inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* const difference, const BGC_FP32_RigidPose3* const base, const BGC_FP32_RigidPose3* const excludant);
|
||||
extern inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* const difference, const BGC_FP64_RigidPose3* const base, const BGC_FP64_RigidPose3* const excludant);
|
||||
extern inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* const difference, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_RigidPose3* const excludant);
|
||||
extern inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* const difference, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_RigidPose3* const excludant);
|
||||
|
||||
extern inline void bgc_fp32_rigid_pose3_get_matrix(BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_RigidPose3* const pose);
|
||||
extern inline void bgc_fp64_rigid_pose3_get_matrix(BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_RigidPose3* const pose);
|
||||
|
|
|
|||
|
|
@ -280,41 +280,41 @@ inline void bgc_fp64_rigid_pose3_get_reverse_pose(BGC_FP64_RigidPose3* const rev
|
|||
|
||||
// ================== Combine =================== //
|
||||
|
||||
inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* const combination, const BGC_FP32_RigidPose3* const first, const BGC_FP32_RigidPose3* const second)
|
||||
inline void bgc_fp32_rigid_pose3_combine(BGC_FP32_RigidPose3* const combination, const BGC_FP32_RigidPose3* const external_pose, const BGC_FP32_RigidPose3* const internal_pose)
|
||||
{
|
||||
bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
bgc_fp32_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &external_pose->_versor, &internal_pose->_versor);
|
||||
_bgc_fp32_rigid_pose3_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* const combination, const BGC_FP64_RigidPose3* const first, const BGC_FP64_RigidPose3* const second)
|
||||
inline void bgc_fp64_rigid_pose3_combine(BGC_FP64_RigidPose3* const combination, const BGC_FP64_RigidPose3* const external_pose, const BGC_FP64_RigidPose3* const internal_pose)
|
||||
{
|
||||
bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
bgc_fp64_dual_quaternion_multiply_by_dual_quaternion(&combination->_versor, &external_pose->_versor, &internal_pose->_versor);
|
||||
_bgc_fp64_rigid_pose3_normalize(combination);
|
||||
}
|
||||
|
||||
// ================== Exclude =================== //
|
||||
|
||||
inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* const difference, const BGC_FP32_RigidPose3* const base, const BGC_FP32_RigidPose3* const excludant)
|
||||
inline void bgc_fp32_rigid_pose3_exclude(BGC_FP32_RigidPose3* const difference, const BGC_FP32_RigidPose3* const pose, const BGC_FP32_RigidPose3* const excludant)
|
||||
{
|
||||
BGC_FP32_Quaternion dual_part1, dual_part2;
|
||||
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&dual_part1, &base->_versor.real_part, &excludant->_versor.dual_part);
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&dual_part2, &base->_versor.dual_part, &excludant->_versor.real_part);
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&dual_part1, &pose->_versor.real_part, &excludant->_versor.dual_part);
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&dual_part2, &pose->_versor.dual_part, &excludant->_versor.real_part);
|
||||
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &base->_versor.real_part, &excludant->_versor.real_part);
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &pose->_versor.real_part, &excludant->_versor.real_part);
|
||||
bgc_fp32_quaternion_add(&difference->_versor.dual_part, &dual_part1, &dual_part2);
|
||||
|
||||
_bgc_fp32_rigid_pose3_normalize(difference);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* const difference, const BGC_FP64_RigidPose3* const base, const BGC_FP64_RigidPose3* const excludant)
|
||||
inline void bgc_fp64_rigid_pose3_exclude(BGC_FP64_RigidPose3* const difference, const BGC_FP64_RigidPose3* const pose, const BGC_FP64_RigidPose3* const excludant)
|
||||
{
|
||||
BGC_FP64_Quaternion dual_part1, dual_part2;
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&dual_part1, &base->_versor.real_part, &excludant->_versor.dual_part);
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&dual_part2, &base->_versor.dual_part, &excludant->_versor.real_part);
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&dual_part1, &pose->_versor.real_part, &excludant->_versor.dual_part);
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&dual_part2, &pose->_versor.dual_part, &excludant->_versor.real_part);
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &base->_versor.real_part, &excludant->_versor.real_part);
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor.real_part, &pose->_versor.real_part, &excludant->_versor.real_part);
|
||||
bgc_fp64_quaternion_add(&difference->_versor.dual_part, &dual_part1, &dual_part2);
|
||||
|
||||
_bgc_fp64_rigid_pose3_normalize(difference);
|
||||
|
|
|
|||
|
|
@ -43,11 +43,8 @@ extern inline void bgc_fp64_turn2_get_power(BGC_FP64_Turn2* const power, const B
|
|||
extern inline void bgc_fp32_turn2_combine(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2);
|
||||
extern inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2);
|
||||
|
||||
extern inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3);
|
||||
extern inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3);
|
||||
|
||||
extern inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant);
|
||||
extern inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant);
|
||||
extern inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const turn, const BGC_FP32_Turn2* const excludant);
|
||||
extern inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const turn, const BGC_FP64_Turn2* const excludant);
|
||||
|
||||
extern inline void bgc_fp32_turn2_get_rotation_matrix(BGC_FP32_Matrix2x2* const matrix, const BGC_FP32_Turn2* const turn);
|
||||
extern inline void bgc_fp64_turn2_get_rotation_matrix(BGC_FP64_Matrix2x2* const matrix, const BGC_FP64_Turn2* const turn);
|
||||
|
|
|
|||
|
|
@ -253,42 +253,12 @@ inline void bgc_fp64_turn2_combine(BGC_FP64_Turn2* const combination, const BGC_
|
|||
_bgc_fp64_turn2_normalize(combination);
|
||||
}
|
||||
|
||||
// ============ Combination of three ============ //
|
||||
|
||||
inline void bgc_fp32_turn2_combine3(BGC_FP32_Turn2* const combination, const BGC_FP32_Turn2* const turn1, const BGC_FP32_Turn2* const turn2, const BGC_FP32_Turn2* const turn3)
|
||||
{
|
||||
const float _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
|
||||
const float _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
|
||||
|
||||
const float _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin;
|
||||
const float _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos;
|
||||
|
||||
combination->_cos = _cos_of_three;
|
||||
combination->_sin = _sin_of_three;
|
||||
|
||||
_bgc_fp32_turn2_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn2_combine3(BGC_FP64_Turn2* const combination, const BGC_FP64_Turn2* const turn1, const BGC_FP64_Turn2* const turn2, const BGC_FP64_Turn2* const turn3)
|
||||
{
|
||||
const double _cos_of_two = turn1->_cos * turn2->_cos - turn1->_sin * turn2->_sin;
|
||||
const double _sin_of_two = turn1->_cos * turn2->_sin + turn1->_sin * turn2->_cos;
|
||||
|
||||
const double _cos_of_three = _cos_of_two * turn3->_cos - _sin_of_two * turn3->_sin;
|
||||
const double _sin_of_three = _cos_of_two * turn3->_sin + _sin_of_two * turn3->_cos;
|
||||
|
||||
combination->_cos = _cos_of_three;
|
||||
combination->_sin = _sin_of_three;
|
||||
|
||||
_bgc_fp64_turn2_normalize(combination);
|
||||
}
|
||||
|
||||
// ================= Exclusion ================== //
|
||||
|
||||
inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const base, const BGC_FP32_Turn2* const excludant)
|
||||
inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_FP32_Turn2* const turn, const BGC_FP32_Turn2* const excludant)
|
||||
{
|
||||
const float _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin;
|
||||
const float _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin;
|
||||
const float _cos = turn->_cos * excludant->_cos + turn->_sin * excludant->_sin;
|
||||
const float _sin = turn->_sin * excludant->_cos - turn->_cos * excludant->_sin;
|
||||
|
||||
difference->_cos = _cos;
|
||||
difference->_sin = _sin;
|
||||
|
|
@ -296,10 +266,10 @@ inline void bgc_fp32_turn2_exclude(BGC_FP32_Turn2* const difference, const BGC_F
|
|||
_bgc_fp32_turn2_normalize(difference);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const base, const BGC_FP64_Turn2* const excludant)
|
||||
inline void bgc_fp64_turn2_exclude(BGC_FP64_Turn2* const difference, const BGC_FP64_Turn2* const turn, const BGC_FP64_Turn2* const excludant)
|
||||
{
|
||||
const double _cos = base->_cos * excludant->_cos + base->_sin * excludant->_sin;
|
||||
const double _sin = base->_sin * excludant->_cos - base->_cos * excludant->_sin;
|
||||
const double _cos = turn->_cos * excludant->_cos + turn->_sin * excludant->_sin;
|
||||
const double _sin = turn->_sin * excludant->_cos - turn->_cos * excludant->_sin;
|
||||
|
||||
difference->_cos = _cos;
|
||||
difference->_sin = _sin;
|
||||
|
|
|
|||
|
|
@ -6,6 +6,8 @@ const BGC_FP32_Turn3 BGC_FP32_IDLE_TURN3 = {{ 1.0f, 0.0f, 0.0f, 0.0f }};
|
|||
|
||||
const BGC_FP64_Turn3 BGC_FP64_IDLE_TURN3 = {{ 1.0, 0.0, 0.0, 0.0 }};
|
||||
|
||||
int64_t turn3_normalize_counter = 0;
|
||||
|
||||
extern inline void bgc_fp32_turn3_reset(BGC_FP32_Turn3* const turn);
|
||||
extern inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* const turn);
|
||||
|
||||
|
|
@ -51,14 +53,11 @@ extern inline void bgc_fp64_turn3_revert(BGC_FP64_Turn3* const turn);
|
|||
extern inline void bgc_fp32_turn3_get_reverse(BGC_FP32_Turn3* const inverse, const BGC_FP32_Turn3* const turn);
|
||||
extern inline void bgc_fp64_turn3_get_reverse(BGC_FP64_Turn3* const inverse, const BGC_FP64_Turn3* const turn);
|
||||
|
||||
extern inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const first, const BGC_FP32_Turn3* const second);
|
||||
extern inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second);
|
||||
extern inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const external_turn, const BGC_FP32_Turn3* const internal_turn);
|
||||
extern inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const external_turn, const BGC_FP64_Turn3* const internal_turn);
|
||||
|
||||
extern inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const first, const BGC_FP32_Turn3* const second, const BGC_FP32_Turn3* const third);
|
||||
extern inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second, const BGC_FP64_Turn3* const third);
|
||||
|
||||
extern inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_FP32_Turn3* const base, const BGC_FP32_Turn3* const excludant);
|
||||
extern inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const base, const BGC_FP64_Turn3* const excludant);
|
||||
extern inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_FP32_Turn3* const turn, const BGC_FP32_Turn3* const excludant);
|
||||
extern inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const turn, const BGC_FP64_Turn3* const excludant);
|
||||
|
||||
extern inline void bgc_fp32_turn3_get_rotation_matrix(BGC_FP32_Matrix3x3* const matrix, const BGC_FP32_Turn3* const turn);
|
||||
extern inline void bgc_fp64_turn3_get_rotation_matrix(BGC_FP64_Matrix3x3* const matrix, const BGC_FP64_Turn3* const turn);
|
||||
|
|
|
|||
|
|
@ -52,6 +52,8 @@ inline void bgc_fp64_turn3_reset(BGC_FP64_Turn3* const turn)
|
|||
|
||||
// ============= Private: Normalize ============= //
|
||||
|
||||
extern int64_t turn3_normalize_counter;
|
||||
|
||||
inline void _bgc_fp32_turn3_normalize(BGC_FP32_Turn3* const turn)
|
||||
{
|
||||
const float square_magnitude = bgc_fp32_quaternion_get_square_magnitude(&turn->_versor);
|
||||
|
|
@ -323,54 +325,30 @@ void bgc_fp64_turn3_get_power(BGC_FP64_Turn3* const power, const BGC_FP64_Turn3*
|
|||
|
||||
// ================ Combination ================= //
|
||||
|
||||
inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const first, const BGC_FP32_Turn3* const second)
|
||||
inline void bgc_fp32_turn3_combine(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const external_turn, const BGC_FP32_Turn3* const internal_turn)
|
||||
{
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &external_turn->_versor, &internal_turn->_versor);
|
||||
_bgc_fp32_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second)
|
||||
inline void bgc_fp64_turn3_combine(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const external_turn, const BGC_FP64_Turn3* const internal_turn)
|
||||
{
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &second->_versor, &first->_versor);
|
||||
_bgc_fp64_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
// ============ Combination of three ============ //
|
||||
|
||||
inline void bgc_fp32_turn3_combine3(BGC_FP32_Turn3* const combination, const BGC_FP32_Turn3* const first, const BGC_FP32_Turn3* const second, const BGC_FP32_Turn3* const third)
|
||||
{
|
||||
BGC_FP32_Quaternion product;
|
||||
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
|
||||
|
||||
bgc_fp32_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);
|
||||
|
||||
_bgc_fp32_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_combine3(BGC_FP64_Turn3* const combination, const BGC_FP64_Turn3* const first, const BGC_FP64_Turn3* const second, const BGC_FP64_Turn3* const third)
|
||||
{
|
||||
BGC_FP64_Quaternion product;
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&product, &second->_versor, &first->_versor);
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &third->_versor, &product);
|
||||
|
||||
bgc_fp64_quaternion_multiply_by_quaternion(&combination->_versor, &external_turn->_versor, &internal_turn->_versor);
|
||||
_bgc_fp64_turn3_normalize(combination);
|
||||
}
|
||||
|
||||
// ================= Exclusion ================== //
|
||||
|
||||
inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_FP32_Turn3* const base, const BGC_FP32_Turn3* const excludant)
|
||||
inline void bgc_fp32_turn3_exclude(BGC_FP32_Turn3* const difference, const BGC_FP32_Turn3* const turn, const BGC_FP32_Turn3* const excludant)
|
||||
{
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor, &base->_versor, &excludant->_versor);
|
||||
bgc_fp32_quaternion_multiply_by_conjugate(&difference->_versor, &turn->_versor, &excludant->_versor);
|
||||
|
||||
_bgc_fp32_turn3_normalize(difference);
|
||||
}
|
||||
|
||||
inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const base, const BGC_FP64_Turn3* const excludant)
|
||||
inline void bgc_fp64_turn3_exclude(BGC_FP64_Turn3* const difference, const BGC_FP64_Turn3* const turn, const BGC_FP64_Turn3* const excludant)
|
||||
{
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor, &base->_versor, &excludant->_versor);
|
||||
bgc_fp64_quaternion_multiply_by_conjugate(&difference->_versor, &turn->_versor, &excludant->_versor);
|
||||
|
||||
_bgc_fp64_turn3_normalize(difference);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue