diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..18bf290 --- /dev/null +++ b/.gitignore @@ -0,0 +1,66 @@ +# ---> C +# Prerequisites +*.d + +# Object files +*.o +*.ko +*.obj +*.elf + +# Linker output +*.ilk +*.map +*.exp + +# Precompiled Headers +*.gch +*.pch + +# Libraries +*.lib +*.a +*.la +*.lo + +# Shared objects (inc. Windows DLLs) +*.dll +*.so +*.so.* +*.dylib + +# Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +# Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +# Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +.vs +x64 +x86 +Debug +Release +bin +obj +modules.order +Module.symvers +Mkfile.old +dkms.conf +bin +obj +logs +*.layout +*.depend +*.user \ No newline at end of file diff --git a/Butis-C.sln b/Butis-C.sln new file mode 100644 index 0000000..e1e5d90 --- /dev/null +++ b/Butis-C.sln @@ -0,0 +1,44 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.1.32421.90 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "butis", "butis\butis.vcxproj", "{3B57B227-EFF3-4A3F-A692-A1FD75CB752A}" + ProjectSection(ProjectDependencies) = postProject + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7} = {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "basic-geometry", "basic-geometry\basic-geometry.vcxproj", "{40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 + Release|x64 = Release|x64 + Release|x86 = Release|x86 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Debug|x64.ActiveCfg = Debug|x64 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Debug|x64.Build.0 = Debug|x64 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Debug|x86.ActiveCfg = Debug|Win32 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Debug|x86.Build.0 = Debug|Win32 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Release|x64.ActiveCfg = Release|x64 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Release|x64.Build.0 = Release|x64 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Release|x86.ActiveCfg = Release|Win32 + {3B57B227-EFF3-4A3F-A692-A1FD75CB752A}.Release|x86.Build.0 = Release|Win32 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Debug|x64.ActiveCfg = Debug|x64 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Debug|x64.Build.0 = Debug|x64 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Debug|x86.ActiveCfg = Debug|Win32 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Debug|x86.Build.0 = Debug|Win32 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Release|x64.ActiveCfg = Release|x64 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Release|x64.Build.0 = Release|x64 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Release|x86.ActiveCfg = Release|Win32 + {40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {ED50005A-8144-45FE-A461-637C9607FEA8} + EndGlobalSection +EndGlobal diff --git a/butis/butis.c b/butis/butis.c new file mode 100644 index 0000000..2d6f6e0 --- /dev/null +++ b/butis/butis.c @@ -0,0 +1,2 @@ + +#include "butis.h" diff --git a/butis/butis.h b/butis/butis.h new file mode 100644 index 0000000..c6c9dbc --- /dev/null +++ b/butis/butis.h @@ -0,0 +1,6 @@ +#ifndef _BUTIS_H_INCLUDE_ +#define _BUTIS_H_INCLUDE_ + +#include "motor.h" + +#endif diff --git a/butis/butis.vcxproj b/butis/butis.vcxproj new file mode 100644 index 0000000..9f0ebf2 --- /dev/null +++ b/butis/butis.vcxproj @@ -0,0 +1,176 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {3b57b227-eff3-4a3f-a692-a1fd75cb752a} + butis + 10.0 + + + + StaticLibrary + true + v143 + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + StaticLibrary + true + v143 + Unicode + + + StaticLibrary + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + + + false + + + + Level3 + true + WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + pch.h + CompileAsC + $(SolutionDir)basic-geometry;%(AdditionalIncludeDirectories) + + + + + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + pch.h + CompileAsC + $(SolutionDir)basic-geometry;%(AdditionalIncludeDirectories) + + + + + true + true + true + + + + + Level3 + true + _DEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + pch.h + CompileAsC + $(SolutionDir)basic-geometry;%(AdditionalIncludeDirectories) + + + + + true + + + + + Level3 + true + true + true + NDEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + pch.h + CompileAsC + $(SolutionDir)basic-geometry;%(AdditionalIncludeDirectories) + + + + + true + true + true + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/butis/butis.vcxproj.filters b/butis/butis.vcxproj.filters new file mode 100644 index 0000000..b5a0706 --- /dev/null +++ b/butis/butis.vcxproj.filters @@ -0,0 +1,45 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd + + + {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} + rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms + + + + + Файлы заголовков + + + Файлы заголовков + + + Файлы заголовков + + + Файлы заголовков + + + + + Исходные файлы + + + Исходные файлы + + + Исходные файлы + + + Исходные файлы + + + \ No newline at end of file diff --git a/butis/butis.vcxproj.user b/butis/butis.vcxproj.user new file mode 100644 index 0000000..88a5509 --- /dev/null +++ b/butis/butis.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/butis/motor.c b/butis/motor.c new file mode 100644 index 0000000..c007cb3 --- /dev/null +++ b/butis/motor.c @@ -0,0 +1,34 @@ +#include "motor.h" + +extern inline void bt_motor_reset_fp32(BtMotorFP32* motor); +extern inline void bt_motor_reset_fp64(BtMotorFP64* motor); + +extern inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination); +extern inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination); + +extern inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination); +extern inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination); + +extern inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2); +extern inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* motor2); + +extern inline void bt_motor_shift_at_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* moved); +extern inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved); + +extern inline void bt_motor_shift_to_fp32(const BtMotorFP32* motor, const BgcVector3FP32* destination, BtMotorFP32* moved); +extern inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved); + +extern inline void bt_motor_multiply_at_number_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product); +extern inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product); + +extern inline void bt_motor_divide_at_number_fp32(const BtMotorFP32* divident, const float divisor, BtMotorFP32* quotient); +extern inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient); + +extern inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor); +extern inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor); + +extern inline void bt_motor_make_screw_fp32(BtMotorFP32* motor); +extern inline void bt_motor_make_screw_fp64(BtMotorFP64* motor); + +extern inline void bt_motor_get_screw_fp32(const BtMotorFP32* motor, BtMotorFP32* screw); +extern inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw); diff --git a/butis/motor.h b/butis/motor.h new file mode 100644 index 0000000..e7779f1 --- /dev/null +++ b/butis/motor.h @@ -0,0 +1,248 @@ +#ifndef _BUTIS_MOTOR_H_INCLUDE_ +#define _BUTIS_MOTOR_H_INCLUDE_ + +#include + +// =================== Types ==================== // + +typedef struct { + BgcVector3FP32 point, vector, momentum; +} BtMotorFP32; + +typedef struct { + BgcVector3FP64 point, vector, momentum; +} BtMotorFP64; + +// =================== Reset ==================== // + +inline void bt_motor_reset_fp32(BtMotorFP32* motor) +{ + bgc_vector3_reset_fp32(&motor->point); + bgc_vector3_reset_fp32(&motor->vector); + bgc_vector3_reset_fp32(&motor->momentum); +} + +inline void bt_motor_reset_fp64(BtMotorFP64* motor) +{ + bgc_vector3_reset_fp64(&motor->point); + bgc_vector3_reset_fp64(&motor->vector); + bgc_vector3_reset_fp64(&motor->momentum); +} + +// ==================== Copy ==================== // + +inline void bt_motor_copy_fp32(const BtMotorFP32* source, BtMotorFP32* destination) +{ + bgc_vector3_copy_fp32(&source->point, &destination->point); + bgc_vector3_copy_fp32(&source->vector, &destination->vector); + bgc_vector3_copy_fp32(&source->momentum, &destination->momentum); +} + +inline void bt_motor_copy_fp64(const BtMotorFP64* source, BtMotorFP64* destination) +{ + bgc_vector3_copy_fp64(&source->point, &destination->point); + bgc_vector3_copy_fp64(&source->vector, &destination->vector); + bgc_vector3_copy_fp64(&source->momentum, &destination->momentum); +} + +// ================== Convert =================== // + +inline void bt_motor_convert_fp32_to_fp64(const BtMotorFP32* source, BtMotorFP64* destination) +{ + bgc_vector3_convert_fp32_to_fp64(&source->point, &destination->point); + bgc_vector3_convert_fp32_to_fp64(&source->vector, &destination->vector); + bgc_vector3_convert_fp32_to_fp64(&source->momentum, &destination->momentum); +} + +inline void bt_motor_convert_fp64_to_fp32(const BtMotorFP64* source, BtMotorFP32* destination) +{ + bgc_vector3_convert_fp64_to_fp32(&source->point, &destination->point); + bgc_vector3_convert_fp64_to_fp32(&source->vector, &destination->vector); + bgc_vector3_convert_fp64_to_fp32(&source->momentum, &destination->momentum); +} + +// ==================== Swap ==================== // + +inline void bt_motor_swap_fp32(BtMotorFP32* motor1, BtMotorFP32* motor2) +{ + bgc_vector3_swap_fp32(&motor2->point, &motor2->point); + bgc_vector3_swap_fp32(&motor2->vector, &motor2->vector); + bgc_vector3_swap_fp32(&motor2->momentum, &motor2->momentum); +} + +inline void bt_motor_swap_fp64(BtMotorFP64* motor1, BtMotorFP64* motor2) +{ + bgc_vector3_swap_fp64(&motor2->point, &motor2->point); + bgc_vector3_swap_fp64(&motor2->vector, &motor2->vector); + bgc_vector3_swap_fp64(&motor2->momentum, &motor2->momentum); +} + +// ================== Shift At ================== // + +inline void bt_motor_shift_at_fp32(const BtMotorFP32* motor, const BgcVector3FP32* shift, BtMotorFP32* moved) +{ + BgcVector3FP32 momentum_difference; + bgc_vector3_get_cross_product_fp32(shift, &motor->vector, &momentum_difference); + bgc_vector3_copy_fp32(&motor->vector, &moved->vector); + bgc_vector3_subtract_fp32(&motor->momentum, &momentum_difference, &moved->vector); + bgc_vector3_add_fp32(&motor->point, shift, &moved->point); +} + +inline void bt_motor_shift_at_fp64(const BtMotorFP64* motor, const BgcVector3FP64* shift, BtMotorFP64* moved) +{ + BgcVector3FP64 momentum_difference; + bgc_vector3_get_cross_product_fp64(shift, &motor->vector, &momentum_difference); + bgc_vector3_subtract_fp64(&motor->momentum, &momentum_difference, &moved->vector); + bgc_vector3_copy_fp64(&motor->vector, &moved->vector); + bgc_vector3_add_fp64(&motor->point, shift, &moved->point); +} + +// ================== Shift To ================== // + +inline void bt_motor_shift_to_fp32(const BtMotorFP32* motor, const BgcVector3FP32* destination, BtMotorFP32* moved) +{ + BgcVector3FP32 shift; + BgcVector3FP32 momentum_change; + bgc_vector3_subtract_fp32(&motor->point, destination, &shift); + bgc_vector3_get_cross_product_fp32(&shift, &motor->vector, &momentum_change); + bgc_vector3_copy_fp32(destination, &moved->point); + bgc_vector3_copy_fp32(&motor->vector, &moved->vector); + bgc_vector3_add_fp32(&motor->momentum, &momentum_change, &moved->momentum); +} + +inline void bt_motor_shift_to_fp64(const BtMotorFP64* motor, const BgcVector3FP64* destination, BtMotorFP64* moved) +{ + BgcVector3FP64 shift; + BgcVector3FP64 momentum_change; + bgc_vector3_subtract_fp64(&motor->point, destination, &shift); + bgc_vector3_get_cross_product_fp64(&shift, &motor->vector, &momentum_change); + bgc_vector3_copy_fp64(destination, &moved->point); + bgc_vector3_copy_fp64(&motor->vector, &moved->vector); + bgc_vector3_add_fp64(&motor->momentum, &momentum_change, &moved->momentum); +} + +// ================== Multiply ================== // + +inline void bt_motor_multiply_at_number_fp32(const BtMotorFP32* multiplicand, const float multiplier, BtMotorFP32* product) +{ + bgc_vector3_copy_fp32(&multiplicand->point, &product->point); + bgc_vector3_multiply_fp32(&multiplicand->vector, multiplier, &product->vector); + bgc_vector3_multiply_fp32(&multiplicand->momentum, multiplier, &product->momentum); +} + +inline void bt_motor_multiply_at_number_fp64(const BtMotorFP64* multiplicand, const double multiplier, BtMotorFP64* product) +{ + bgc_vector3_copy_fp64(&multiplicand->point, &product->point); + bgc_vector3_multiply_fp64(&multiplicand->vector, multiplier, &product->vector); + bgc_vector3_multiply_fp64(&multiplicand->momentum, multiplier, &product->momentum); +} + +// =================== Divide =================== // + +inline void bt_motor_divide_at_number_fp32(const BtMotorFP32* divident, const float divisor, BtMotorFP32* quotient) +{ + bt_motor_multiply_at_number_fp32(divident, 1.0f / divisor, quotient); +} + +inline void bt_motor_divide_at_number_fp64(const BtMotorFP64* divident, const double divisor, BtMotorFP64* quotient) +{ + bt_motor_multiply_at_number_fp64(divident, 1.0 / divisor, quotient); +} + +// ================== Is Screw ================== // + +inline int bt_motor_is_screw_fp32(const BtMotorFP32* motor) +{ + return bgc_vector3_are_parallel_fp32(&motor->vector, &motor->momentum); +} + +inline int bt_motor_is_screw_fp64(const BtMotorFP64* motor) +{ + return bgc_vector3_are_parallel_fp64(&motor->vector, &motor->momentum); +} + +// ================= Make Screw ================= // + +inline void bt_motor_make_screw_fp32(BtMotorFP32* motor) +{ + const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector); + + if (square_vector <= BGC_SQUARE_EPSYLON_FP32) { + return; + } + + const float multiplier = 1.0f / square_vector; + BgcVector3FP32 cross_product; + + bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product); + + bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &motor->point); + + bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &motor->momentum); +} + +inline void bt_motor_make_screw_fp64(BtMotorFP64* motor) +{ + const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector); + + if (square_vector <= BGC_SQUARE_EPSYLON_FP64) { + return; + } + + const double multiplier = 1.0 / square_vector; + BgcVector3FP64 cross_product; + + bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product); + + bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &motor->point); + + bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &motor->momentum); +} + +// ================= Get Screw ================== // + +inline void bt_motor_get_screw_fp32(const BtMotorFP32* motor, BtMotorFP32* screw) +{ + const float square_vector = bgc_vector3_get_square_modulus_fp32(&motor->vector); + + bgc_vector3_copy_fp32(&motor->vector, &screw->vector); + + if (square_vector <= BGC_SQUARE_EPSYLON_FP32) { + bgc_vector3_copy_fp32(&motor->point, &screw->point); + bgc_vector3_copy_fp32(&motor->momentum, &screw->momentum); + return; + } + + const float multiplier = 1.0f / square_vector; + BgcVector3FP32 cross_product; + + bgc_vector3_get_cross_product_fp32(&motor->vector, &motor->momentum, &cross_product); + + bgc_vector3_add_scaled_fp32(&motor->point, &cross_product, multiplier, &screw->point); + + bgc_vector3_multiply_fp32(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp32(&motor->vector, &motor->momentum), &screw->momentum); +} + +inline void bt_motor_get_screw_fp64(const BtMotorFP64* motor, BtMotorFP64* screw) +{ + const double square_vector = bgc_vector3_get_square_modulus_fp64(&motor->vector); + + bgc_vector3_copy_fp64(&motor->vector, &screw->vector); + + if (square_vector <= BGC_SQUARE_EPSYLON_FP64) { + bgc_vector3_copy_fp64(&motor->point, &screw->point); + bgc_vector3_copy_fp64(&motor->momentum, &screw->momentum); + return; + } + + const double multiplier = 1.0 / square_vector; + BgcVector3FP64 cross_product; + + bgc_vector3_get_cross_product_fp64(&motor->vector, &motor->momentum, &cross_product); + + bgc_vector3_add_scaled_fp64(&motor->point, &cross_product, multiplier, &screw->point); + + bgc_vector3_multiply_fp64(&motor->vector, multiplier * bgc_vector3_get_scalar_product_fp64(&motor->vector, &motor->momentum), &screw->momentum); +} + +#endif diff --git a/butis/motor_matrix.c b/butis/motor_matrix.c new file mode 100644 index 0000000..b3fb851 --- /dev/null +++ b/butis/motor_matrix.c @@ -0,0 +1,4 @@ +#include "motor_matrix.h" + +extern inline void bt_matrix_reset_fp32(BtMatrixFP32* matrix); +extern inline void bt_matrix_reset_fp64(BtMatrixFP64* matrix); diff --git a/butis/motor_matrix.h b/butis/motor_matrix.h new file mode 100644 index 0000000..6ac8045 --- /dev/null +++ b/butis/motor_matrix.h @@ -0,0 +1,118 @@ +#ifndef _BUTIS_MOTOR_MATRIX_H_INCLUDE_ +#define _BUTIS_MOTOR_MATRIX_H_INCLUDE_ + +#include + +// =================== Types ==================== // + +typedef struct { + float r1c1, r1c2, r1c3, r1c4, r1c5, r1c6; + float r2c1, r2c2, r2c3, r2c4, r2c5, r2c6; + float r3c1, r3c2, r3c3, r3c4, r3c5, r3c6; + float r4c1, r4c2, r4c3, r4c4, r4c5, r4c6; + float r5c1, r5c2, r5c3, r5c4, r5c5, r5c6; + float r6c1, r6c2, r6c3, r6c4, r6c5, r6c6; +} BtMatrixFP32; + +typedef struct { + double r1c1, r1c2, r1c3, r1c4, r1c5, r1c6; + double r2c1, r2c2, r2c3, r2c4, r2c5, r2c6; + double r3c1, r3c2, r3c3, r3c4, r3c5, r3c6; + double r4c1, r4c2, r4c3, r4c4, r4c5, r4c6; + double r5c1, r5c2, r5c3, r5c4, r5c5, r5c6; + double r6c1, r6c2, r6c3, r6c4, r6c5, r6c6; +} BtMatrixFP64; + +// =================== Reset ==================== // + +inline void bt_matrix_reset_fp32(BtMatrixFP32* matrix) +{ + matrix->r1c1 = 1.0f; + matrix->r1c2 = 0.0f; + matrix->r1c3 = 0.0f; + matrix->r1c4 = 0.0f; + matrix->r1c5 = 0.0f; + matrix->r1c6 = 0.0f; + + matrix->r2c1 = 0.0f; + matrix->r2c2 = 1.0f; + matrix->r2c3 = 0.0f; + matrix->r2c4 = 0.0f; + matrix->r2c5 = 0.0f; + matrix->r2c6 = 0.0f; + + matrix->r3c1 = 0.0f; + matrix->r3c2 = 0.0f; + matrix->r3c3 = 1.0f; + matrix->r3c4 = 0.0f; + matrix->r3c5 = 0.0f; + matrix->r3c6 = 0.0f; + + matrix->r4c1 = 0.0f; + matrix->r4c2 = 0.0f; + matrix->r4c3 = 0.0f; + matrix->r4c4 = 1.0f; + matrix->r4c5 = 0.0f; + matrix->r4c6 = 0.0f; + + matrix->r5c1 = 0.0f; + matrix->r5c2 = 0.0f; + matrix->r5c3 = 0.0f; + matrix->r5c4 = 0.0f; + matrix->r5c5 = 1.0f; + matrix->r5c6 = 0.0f; + + matrix->r6c1 = 0.0f; + matrix->r6c2 = 0.0f; + matrix->r6c3 = 0.0f; + matrix->r6c4 = 0.0f; + matrix->r6c5 = 0.0f; + matrix->r6c6 = 1.0f; +} + +inline void bt_matrix_reset_fp64(BtMatrixFP64* matrix) +{ + matrix->r1c1 = 1.0; + matrix->r1c2 = 0.0; + matrix->r1c3 = 0.0; + matrix->r1c4 = 0.0; + matrix->r1c5 = 0.0; + matrix->r1c6 = 0.0; + + matrix->r2c1 = 0.0; + matrix->r2c2 = 1.0; + matrix->r2c3 = 0.0; + matrix->r2c4 = 0.0; + matrix->r2c5 = 0.0; + matrix->r2c6 = 0.0; + + matrix->r3c1 = 0.0; + matrix->r3c2 = 0.0; + matrix->r3c3 = 1.0; + matrix->r3c4 = 0.0; + matrix->r3c5 = 0.0; + matrix->r3c6 = 0.0; + + matrix->r4c1 = 0.0; + matrix->r4c2 = 0.0; + matrix->r4c3 = 0.0; + matrix->r4c4 = 1.0; + matrix->r4c5 = 0.0; + matrix->r4c6 = 0.0; + + matrix->r5c1 = 0.0; + matrix->r5c2 = 0.0; + matrix->r5c3 = 0.0; + matrix->r5c4 = 0.0; + matrix->r5c5 = 1.0; + matrix->r5c6 = 0.0; + + matrix->r6c1 = 0.0; + matrix->r6c2 = 0.0; + matrix->r6c3 = 0.0; + matrix->r6c4 = 0.0; + matrix->r6c5 = 0.0; + matrix->r6c6 = 1.0; +} + +#endif diff --git a/butis/screw.c b/butis/screw.c new file mode 100644 index 0000000..8036959 --- /dev/null +++ b/butis/screw.c @@ -0,0 +1,4 @@ +#include "screw.h" + +extern inline void bt_screw_reset_fp32(BtScrewFP32* screw); +extern inline void bt_screw_reset_fp64(BtScrewFP64* screw); diff --git a/butis/screw.h b/butis/screw.h new file mode 100644 index 0000000..f140543 --- /dev/null +++ b/butis/screw.h @@ -0,0 +1,48 @@ +#ifndef _BUTIS_SCREW_H_INCLUDE_ +#define _BUTIS_SCREW_H_INCLUDE_ + +#include + +// =================== Types ==================== // + +typedef struct { + BgcVector3FP32 point, direction; + float value, momentum; +} BtScrewFP32; + +typedef struct { + BgcVector3FP64 point, direction; + double value, momentum; +} BtScrewFP64; + +// =================== Reset ==================== // + +inline void bt_screw_reset_fp32(BtScrewFP32* screw) +{ + screw->point.x1 = 0.0f; + screw->point.x2 = 0.0f; + screw->point.x3 = 0.0f; + + screw->direction.x1 = 1.0f; + screw->direction.x2 = 0.0f; + screw->direction.x3 = 0.0f; + + screw->value = 0.0f; + screw->momentum = 0.0f; +} + +inline void bt_screw_reset_fp64(BtScrewFP64* screw) +{ + screw->point.x1 = 0.0; + screw->point.x2 = 0.0; + screw->point.x3 = 0.0; + + screw->direction.x1 = 1.0; + screw->direction.x2 = 0.0; + screw->direction.x3 = 0.0; + + screw->value = 0.0f; + screw->momentum = 0.0f; +} + +#endif