diff --git a/.gitignore b/.gitignore index cd531cf..328ca76 100644 --- a/.gitignore +++ b/.gitignore @@ -47,8 +47,14 @@ *.mod* *.cmd .tmp_versions/ +.vs +x64 +x86 modules.order Module.symvers Mkfile.old dkms.conf - +bin +obj +dev +logs diff --git a/Geometry.workspace b/Geometry.workspace new file mode 100644 index 0000000..ed07c48 --- /dev/null +++ b/Geometry.workspace @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/Geometry.workspace.layout b/Geometry.workspace.layout new file mode 100644 index 0000000..e46768a --- /dev/null +++ b/Geometry.workspace.layout @@ -0,0 +1,5 @@ + + + + + diff --git a/GeometryC.sln b/GeometryC.sln new file mode 100644 index 0000000..aec3cde --- /dev/null +++ b/GeometryC.sln @@ -0,0 +1,51 @@ + +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}") = "geometry", "src\geometry.vcxproj", "{40CA6FB4-135F-4D54-A8D9-7338BA56E6A7}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "geometry-test", "test\geometry-test.vcxproj", "{48DAE315-715F-4044-ADF5-0308483B887C}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "geometry-dev", "dev\geometry-dev.vcxproj", "{46DE6C8F-3179-4652-95CF-28D44AC4774A}" +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 + {48DAE315-715F-4044-ADF5-0308483B887C}.Debug|x64.ActiveCfg = Debug|x64 + {48DAE315-715F-4044-ADF5-0308483B887C}.Debug|x64.Build.0 = Debug|x64 + {48DAE315-715F-4044-ADF5-0308483B887C}.Debug|x86.ActiveCfg = Debug|Win32 + {48DAE315-715F-4044-ADF5-0308483B887C}.Debug|x86.Build.0 = Debug|Win32 + {48DAE315-715F-4044-ADF5-0308483B887C}.Release|x64.ActiveCfg = Release|x64 + {48DAE315-715F-4044-ADF5-0308483B887C}.Release|x64.Build.0 = Release|x64 + {48DAE315-715F-4044-ADF5-0308483B887C}.Release|x86.ActiveCfg = Release|Win32 + {48DAE315-715F-4044-ADF5-0308483B887C}.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 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Debug|x64.ActiveCfg = Debug|x64 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Debug|x64.Build.0 = Debug|x64 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Debug|x86.ActiveCfg = Debug|Win32 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Debug|x86.Build.0 = Debug|Win32 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Release|x64.ActiveCfg = Release|x64 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Release|x64.Build.0 = Release|x64 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Release|x86.ActiveCfg = Release|Win32 + {46DE6C8F-3179-4652-95CF-28D44AC4774A}.Release|x86.Build.0 = Release|Win32 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {482D2212-4373-4231-88E5-34BD6B8CFF46} + EndGlobalSection +EndGlobal diff --git a/src/angle.c b/src/angle.c new file mode 100644 index 0000000..b7b0313 --- /dev/null +++ b/src/angle.c @@ -0,0 +1,3 @@ +#include "basis.h" +#include "angle.h" + diff --git a/src/angle.h b/src/angle.h new file mode 100644 index 0000000..de36be5 --- /dev/null +++ b/src/angle.h @@ -0,0 +1,549 @@ +#ifndef _GEOMETRY_ANGLE_H_ +#define _GEOMETRY_ANGLE_H_ + +#include +#include "basis.h" + +#define SP_PI 3.1415926536f +#define SP_TWO_PI 6.2831853072f +#define SP_HALF_OF_PI 1.5707963268f +#define SP_THIRD_OF_PI 1.0471975512f +#define SP_FOURTH_OF_PI 0.7853981634f +#define SP_SIXTH_OF_PI 0.5235987756f + +#define SP_DEGREES_IN_RADIAN 57.295779513f +#define SP_TURNS_IN_RADIAN 0.1591549431f +#define SP_RADIANS_IN_DEGREE 1.745329252E-2f +#define SP_TURNS_IN_DEGREE 2.7777777778E-3f + +#define DP_PI 3.14159265358979324 +#define DP_TWO_PI 6.28318530717958648 +#define DP_HALF_OF_PI 1.57079632679489662 +#define DP_THIRD_OF_PI 1.04719755119659775 +#define DP_FOURTH_OF_PI 0.78539816339744831 +#define DP_SIXTH_OF_PI 0.523598775598298873 + +#define DP_DEGREES_IN_RADIAN 57.2957795130823209 +#define DP_TURNS_IN_RADIAN 0.159154943091895336 +#define DP_RADIANS_IN_DEGREE 1.74532925199432958E-2 +#define DP_TURNS_IN_DEGREE 2.77777777777777778E-3 + +typedef enum { + ANGLE_UNIT_RADIANS = 1, + ANGLE_UNIT_DEGREES = 2, + ANGLE_UNIT_TURNS = 3 +} angle_unit_t; + +typedef enum { + /** + * The measure of an angle with a range of: + * [0, 360) degrees, [0, 2xPI) radians, [0, 1) turns, [0, 400) gradians + */ + ANGLE_RANGE_UNSIGNED = 1, + + /** + * The measure of an angle with a range of: + * (-180, 180] degrees, (-PI, PI] radians, (-0.5, 0.5] turns, (-200, 200] gradians + */ + ANGLE_RANGE_SIGNED = 2 +} angle_range_t; + +// ========= Convert radians to degrees ========= // + +static inline float sp_radians_to_degrees(const float radians) +{ + return radians * SP_DEGREES_IN_RADIAN; +} + +static inline double dp_radians_to_degrees(const double radians) +{ + return radians * DP_DEGREES_IN_RADIAN; +} + +// ========== Convert radians to turns ========== // + +static inline float sp_radians_to_turns(const float radians) +{ + return radians * SP_TURNS_IN_RADIAN; +} + +static inline double dp_radians_to_turns(const double radians) +{ + return radians * DP_TURNS_IN_RADIAN; +} + +// ========= Convert degrees to radians ========= // + +static inline float sp_degrees_to_radians(const float degrees) +{ + return degrees * SP_RADIANS_IN_DEGREE; +} + +static inline double dp_degrees_to_radians(const double degrees) +{ + return degrees * DP_RADIANS_IN_DEGREE; +} + +// ========== Convert degrees to turns ========== // + +static inline float sp_degrees_to_turns(const float radians) +{ + return radians * SP_TURNS_IN_DEGREE; +} + +static inline double dp_degrees_to_turns(const double radians) +{ + return radians * DP_TURNS_IN_DEGREE; +} + +// ========== Convert turns to radians ========== // + +static inline float sp_turns_to_radians(const float turns) +{ + return turns * SP_TWO_PI; +} + +static inline double dp_turns_to_radians(const double turns) +{ + return turns * DP_TWO_PI; +} + +// ========== Convert turns to degrees ========== // + +static inline float sp_turns_to_degrees(const float turns) +{ + return turns * 360.0f; +} + +static inline double dp_turns_to_degrees(const double turns) +{ + return turns * 360.0; +} + +// ========= Convert radians to any unit ======== // + +static inline float sp_convert_from_radians(const float radians, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_DEGREES) { + return radians * SP_DEGREES_IN_RADIAN; + } + + if (to_unit == ANGLE_UNIT_TURNS) { + return radians * SP_TURNS_IN_RADIAN; + } + + return radians; +} + +static inline double dp_convert_from_radians(const double radians, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_DEGREES) { + return radians * DP_DEGREES_IN_RADIAN; + } + + if (to_unit == ANGLE_UNIT_TURNS) { + return radians * DP_TURNS_IN_RADIAN; + } + + return radians; +} + +// ========= Convert degreess to any unit ======== // + +static inline float sp_convert_from_degrees(const float degrees, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_RADIANS) { + return degrees * SP_RADIANS_IN_DEGREE; + } + + if (to_unit == ANGLE_UNIT_TURNS) { + return degrees * SP_TURNS_IN_DEGREE; + } + + return degrees; +} + +static inline double dp_convert_from_degrees(const double degrees, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_RADIANS) { + return degrees * DP_RADIANS_IN_DEGREE; + } + + if (to_unit == ANGLE_UNIT_TURNS) { + return degrees * DP_TURNS_IN_DEGREE; + } + + return degrees; +} + +// ========= Convert turns to any unit ======== // + +static inline float sp_convert_from_turns(const float turns, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_RADIANS) { + return turns * SP_TWO_PI; + } + + if (to_unit == ANGLE_UNIT_DEGREES) { + return turns * 360.0f; + } + + return turns; +} + +static inline double dp_convert_from_turns(const double turns, const angle_unit_t to_unit) +{ + if (to_unit == ANGLE_UNIT_RADIANS) { + return turns * DP_TWO_PI; + } + + if (to_unit == ANGLE_UNIT_DEGREES) { + return turns * 360.0; + } + + return turns; +} + +// ========= Convert any unit to radians ======== // + +static inline float sp_convert_to_radians(const float angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_DEGREES) { + return angle * SP_RADIANS_IN_DEGREE; + } + + if (unit == ANGLE_UNIT_TURNS) { + return angle * SP_TWO_PI; + } + + return angle; +} + +static inline double dp_convert_to_radians(const double angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_DEGREES) { + return angle * DP_RADIANS_IN_DEGREE; + } + + if (unit == ANGLE_UNIT_TURNS) { + return angle * DP_TWO_PI; + } + + return angle; +} + +// ========= Convert any unit to degreess ======== // + +static inline float sp_convert_to_degrees(const float angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_DEGREES) { + return angle * SP_DEGREES_IN_RADIAN; + } + + if (unit == ANGLE_UNIT_TURNS) { + return angle * 360.0f; + } + + return angle; +} + +static inline double dp_convert_to_degrees(const double angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return angle * DP_DEGREES_IN_RADIAN; + } + + if (unit == ANGLE_UNIT_TURNS) { + return angle * 360.0; + } + + return angle; +} + +// ========= Convert any unit to turns ======== // + +static inline float sp_convert_to_turns(const float angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return angle * SP_TURNS_IN_RADIAN; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return angle * SP_TURNS_IN_DEGREE; + } + + return angle; +} + +static inline double dp_convert_to_turns(const double angle, const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return angle * DP_TURNS_IN_RADIAN; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return angle * DP_TURNS_IN_DEGREE; + } + + return angle; +} + +// ============= Get Full Circle ============== // + +static inline float sp_get_full_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return SP_TWO_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 360.0f; + } + + return 1.0f; +} + +static inline double dp_get_full_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return DP_TWO_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 360.0; + } + + return 1.0; +} + +// ============= Get Half Circle ============== // + +static inline float sp_get_half_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return SP_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 180.0f; + } + + return 0.5f; +} + +static inline double dp_get_half_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return DP_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 180.0; + } + + return 0.5; +} + +// ============= Get Half Circle ============== // + +static inline float sp_get_quater_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return SP_HALF_OF_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 90.0f; + } + + return 0.25f; +} + +static inline double dp_get_quater_circle(const angle_unit_t unit) +{ + if (unit == ANGLE_UNIT_RADIANS) { + return DP_HALF_OF_PI; + } + + if (unit == ANGLE_UNIT_DEGREES) { + return 90.0; + } + + return 0.25; +} + +// ============ Normalize radians ============= // + +static inline float sp_normalize_radians(const float radians, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0f <= radians && radians < SP_TWO_PI) { + return radians; + } + } + else { + if (-SP_PI < radians && radians <= SP_PI) { + return radians; + } + } + + float turns = radians * SP_TURNS_IN_RADIAN; + + turns -= floorf(turns); + + if (range == ANGLE_RANGE_SIGNED && turns > 0.5f) { + turns -= 1.0f; + } + + return turns * SP_TWO_PI; +} + +static inline double dp_normalize_radians(const double radians, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0 <= radians && radians < DP_TWO_PI) { + return radians; + } + } + else { + if (-DP_PI < radians && radians <= DP_PI) { + return radians; + } + } + + double turns = radians * DP_TURNS_IN_RADIAN; + + turns -= floor(turns); + + if (range == ANGLE_RANGE_SIGNED && turns > 0.5) { + turns -= 1.0; + } + + return turns * DP_TWO_PI; +} + +// ============ Normalize degrees ============= // + +static inline float sp_normalize_degrees(const float degrees, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0f <= degrees && degrees < 360.0f) { + return degrees; + } + } + else { + if (-180.0f < degrees && degrees <= 180.0f) { + return degrees; + } + } + + float turns = degrees * SP_TURNS_IN_DEGREE; + + turns -= floorf(turns); + + if (range == ANGLE_RANGE_SIGNED && turns > 0.5f) { + turns -= 1.0f; + } + + return turns * 360.0f; +} + +static inline double dp_normalize_degrees(const double degrees, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0 <= degrees && degrees < 360.0) { + return degrees; + } + } + else { + if (-180.0 < degrees && degrees <= 180.0) { + return degrees; + } + } + + double turns = degrees * DP_TURNS_IN_DEGREE; + + turns -= floor(turns); + + if (range == ANGLE_RANGE_SIGNED && turns > 0.5) { + turns -= 1.0; + } + + return turns * 360.0; +} + +// ============= Normalize turns ============== // + +static inline float sp_normalize_turns(const float turns, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0f <= turns && turns < 1.0f) { + return turns; + } + } + else { + if (-0.5f < turns && turns <= 0.5f) { + return turns; + } + } + + float rest = turns - floorf(turns); + + if (range == ANGLE_RANGE_SIGNED && rest > 0.5f) { + return rest - 1.0f; + } + + return rest; +} + +static inline double dp_normalize_turns(const double turns, const angle_range_t range) +{ + if (range == ANGLE_RANGE_UNSIGNED) { + if (0.0 <= turns && turns < 1.0) { + return turns; + } + } + else { + if (-0.5 < turns && turns <= 0.5) { + return turns; + } + } + + double rest = turns - floor(turns); + + if (range == ANGLE_RANGE_SIGNED && rest > 0.5) { + return rest - 1.0; + } + + return rest; +} + +// ================ Normalize ================= // + +static inline float sp_normalize_angle(const float angle, const angle_unit_t unit, const angle_range_t range) +{ + if (unit == ANGLE_UNIT_DEGREES) { + return sp_normalize_degrees(angle, range); + } + + if (unit == ANGLE_UNIT_TURNS) { + return sp_normalize_turns(angle, range); + } + + return sp_normalize_radians(angle, range); +} + +static inline double dp_normalize_angle(const double angle, const angle_unit_t unit, const angle_range_t range) +{ + if (unit == ANGLE_UNIT_DEGREES) { + return dp_normalize_degrees(angle, range); + } + + if (unit == ANGLE_UNIT_TURNS) { + return dp_normalize_turns(angle, range); + } + + return dp_normalize_radians(angle, range); +} + +#endif diff --git a/src/basis.c b/src/basis.c new file mode 100644 index 0000000..03a9af7 --- /dev/null +++ b/src/basis.c @@ -0,0 +1,2 @@ +#include "basis.h" + diff --git a/src/basis.h b/src/basis.h new file mode 100644 index 0000000..3058312 --- /dev/null +++ b/src/basis.h @@ -0,0 +1,56 @@ +#ifndef __GEOMETRY__TYPES_H_ +#define __GEOMETRY__TYPES_H_ + +#define SP_EPSYLON_EFFECTIVENESS_LIMIT 10.0f + +#define SP_EPSYLON 5E-7f +#define SP_TWO_EPSYLON 1E-6f +#define SP_SQUARE_EPSYLON 2.5E-13f + +#define SP_ONE_THIRD 0.333333333f +#define SP_ONE_SIXTH 0.166666667f +#define SP_ONE_NINETH 0.111111111f + +#define SP_GOLDEN_RATIO_HIGH 1.618034f +#define SP_GOLDEN_RATIO_LOW 0.618034f + +#define DP_EPSYLON_EFFECTIVENESS_LIMIT 10.0 + +#define DP_EPSYLON 5E-14 +#define DP_TWO_EPSYLON 1E-13 +#define DP_SQUARE_EPSYLON 2.5E-27 + +#define DP_ONE_THIRD 0.333333333333333333 +#define DP_ONE_SIXTH 0.166666666666666667 +#define DP_ONE_NINETH 0.111111111111111111 + +#define DP_GOLDEN_RATIO_HIGH 1.61803398874989485 +#define DP_GOLDEN_RATIO_LOW 0.61803398874989485 + +static inline int sp_are_equal(const float value1, const float value2) +{ + if (-SP_EPSYLON_EFFECTIVENESS_LIMIT < value1 && value1 < SP_EPSYLON_EFFECTIVENESS_LIMIT) { + return -SP_EPSYLON <= (value1 - value2) && (value1 - value2) <= SP_EPSYLON; + } + + if (value1 < 0.0f) { + return (1.0f + SP_EPSYLON) * value2 <= value1 && (1.0f + SP_EPSYLON) * value1 <= value2; + } + + return value2 <= value1 * (1.0f + SP_EPSYLON) && value1 <= value2 * (1.0f + SP_EPSYLON); +} + +static inline int dp_are_equal(const double value1, const double value2) +{ + if (-DP_EPSYLON_EFFECTIVENESS_LIMIT < value1 && value1 < DP_EPSYLON_EFFECTIVENESS_LIMIT) { + return -DP_EPSYLON <= (value1 - value2) && (value1 - value2) <= DP_EPSYLON; + } + + if (value1 < 0.0) { + return (1.0 + DP_EPSYLON) * value2 <= value1 && (1.0 + DP_EPSYLON) * value1 <= value2; + } + + return value2 <= value1 * (1.0 + DP_EPSYLON) && value1 <= value2 * (1.0 + DP_EPSYLON); +} + +#endif diff --git a/src/geometry.cbp b/src/geometry.cbp new file mode 100644 index 0000000..91658c5 --- /dev/null +++ b/src/geometry.cbp @@ -0,0 +1,87 @@ + + + + + + diff --git a/src/geometry.depend b/src/geometry.depend new file mode 100644 index 0000000..c4ac310 --- /dev/null +++ b/src/geometry.depend @@ -0,0 +1 @@ +# depslib dependency file v1.0 diff --git a/src/geometry.h b/src/geometry.h new file mode 100644 index 0000000..1cbdaee --- /dev/null +++ b/src/geometry.h @@ -0,0 +1,19 @@ +#ifndef __GEOMETRY_H__ +#define __GEOMETRY_H__ + +#include "basis.h" + +#include "angle.h" + +#include "vector2.h" +#include "vector3.h" + +#include "matrix2x2.h" +#include "matrix3x3.h" + +#include "rotation3.h" + +#include "quaternion.h" +#include "versor.h" + +#endif diff --git a/src/geometry.layout b/src/geometry.layout new file mode 100644 index 0000000..173e12c --- /dev/null +++ b/src/geometry.layout @@ -0,0 +1,100 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/geometry.vcxproj b/src/geometry.vcxproj new file mode 100644 index 0000000..7e406c9 --- /dev/null +++ b/src/geometry.vcxproj @@ -0,0 +1,175 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + + + + + + + + + + + + + + + + + + 16.0 + Win32Proj + {40ca6fb4-135f-4d54-a8d9-7338ba56e6a7} + geometry + 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 + + + + + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + + + + + true + true + true + + + + + Level3 + true + _DEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + + + + + true + + + + + Level3 + true + true + true + NDEBUG;_LIB;%(PreprocessorDefinitions) + true + NotUsing + + + + + true + true + true + + + + + + \ No newline at end of file diff --git a/src/geometry.vcxproj.filters b/src/geometry.vcxproj.filters new file mode 100644 index 0000000..8262b96 --- /dev/null +++ b/src/geometry.vcxproj.filters @@ -0,0 +1,78 @@ + + + + + {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/src/geometry.vcxproj.user b/src/geometry.vcxproj.user new file mode 100644 index 0000000..88a5509 --- /dev/null +++ b/src/geometry.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/src/matrix2x2.c b/src/matrix2x2.c new file mode 100644 index 0000000..6ba579f --- /dev/null +++ b/src/matrix2x2.c @@ -0,0 +1,24 @@ +#include "matrix2x2.h" + +#include +#include "angle.h" + +const SPMatrix2x2 SP_ZERO_MATRIX2X2 = { + 0.0f, 0.0f, + 0.0f, 0.0f +}; + +const DPMatrix2x2 DP_ZERO_MATRIX2X2 = { + 0.0, 0.0, + 0.0, 0.0 +}; + +const SPMatrix2x2 SP_IDENTITY_MATRIX2X2 = { + 1.0f, 0.0f, + 0.0f, 1.0f +}; + +const DPMatrix2x2 DP_IDENTITY_MATRIX2X2 = { + 1.0, 0.0, + 0.0, 1.0 +}; diff --git a/src/matrix2x2.h b/src/matrix2x2.h new file mode 100644 index 0000000..5153223 --- /dev/null +++ b/src/matrix2x2.h @@ -0,0 +1,755 @@ +#ifndef _GEOMETRY_MATRIX2X2_H_ +#define _GEOMETRY_MATRIX2X2_H_ + +#include "angle.h" +#include "vector2.h" + +typedef struct { + float r1c1, r1c2; + float r2c1, r2c2; +} SPMatrix2x2; + +typedef struct { + double r1c1, r1c2; + double r2c1, r2c2; +} DPMatrix2x2; + +extern const SPMatrix2x2 SP_ZERO_MATRIX2X2; + +extern const DPMatrix2x2 DP_ZERO_MATRIX2X2; + +extern const SPMatrix2x2 SP_IDENTITY_MATRIX2X2; + +extern const DPMatrix2x2 DP_IDENTITY_MATRIX2X2; + +// =================== Reset ==================== // + +static inline void sp_matrix2x2_reset(SPMatrix2x2* matrix) +{ + matrix->r1c1 = 0.0f; + matrix->r1c2 = 0.0f; + matrix->r2c1 = 0.0f; + matrix->r2c2 = 0.0f; +} + +static inline void dp_matrix2x2_reset(DPMatrix2x2* matrix) +{ + matrix->r1c1 = 0.0; + matrix->r1c2 = 0.0; + matrix->r2c1 = 0.0; + matrix->r2c2 = 0.0; +} + +// ================== Identity ================== // + +static inline void sp_matrix2x2_make_identity(SPMatrix2x2* matrix) +{ + matrix->r1c1 = 1.0f; + matrix->r1c2 = 0.0f; + matrix->r2c1 = 0.0f; + matrix->r2c2 = 1.0f; +} + +static inline void dp_matrix2x2_make_identity(DPMatrix2x2* matrix) +{ + matrix->r1c1 = 1.0; + matrix->r1c2 = 0.0; + matrix->r2c1 = 0.0; + matrix->r2c2 = 1.0; +} + +// ================ Make Diagonal =============== // + +static inline void sp_matrix2x2_make_diagonal(const float d1, const float d2, SPMatrix2x2* matrix) +{ + matrix->r1c1 = d1; + matrix->r1c2 = 0.0f; + matrix->r2c1 = 0.0f; + matrix->r2c2 = d2; +} + +static inline void dp_matrix2x2_make_diagonal(const double d1, const double d2, DPMatrix2x2* matrix) +{ + matrix->r1c1 = d1; + matrix->r1c2 = 0.0; + matrix->r2c1 = 0.0; + matrix->r2c2 = d2; +} + +// ============== Rotation Matrix =============== // + +static inline void sp_matrix2x2_make_turn(const float angle, const angle_unit_t unit, SPMatrix2x2* matrix) +{ + const float radians = sp_convert_to_radians(angle, unit); + const float cosine = cosf(radians); + const float sine = sinf(radians); + + matrix->r1c1 = cosine; + matrix->r1c2 = -sine; + matrix->r2c1 = sine; + matrix->r2c2 = cosine; +} + +static inline void dp_matrix2x2_make_turn(const double angle, const angle_unit_t unit, DPMatrix2x2* matrix) +{ + const double radians = dp_convert_to_radians(angle, unit); + const double cosine = cos(radians); + const double sine = sin(radians); + + matrix->r1c1 = cosine; + matrix->r1c2 = -sine; + matrix->r2c1 = sine; + matrix->r2c2 = cosine; +} + +// ==================== Copy ==================== // + +static inline void sp_matrix2x2_copy(const SPMatrix2x2* from, SPMatrix2x2* to) +{ + to->r1c1 = from->r1c1; + to->r1c2 = from->r1c2; + + to->r2c1 = from->r2c1; + to->r2c2 = from->r2c2; +} + +static inline void dp_matrix2x2_copy(const DPMatrix2x2* from, DPMatrix2x2* to) +{ + to->r1c1 = from->r1c1; + to->r1c2 = from->r1c2; + + to->r2c1 = from->r2c1; + to->r2c2 = from->r2c2; +} + +// ============= Copy to twin type ============== // + +static inline void sp_matrix2x2_copy_to_double(const SPMatrix2x2* from, DPMatrix2x2* to) +{ + to->r1c1 = (double)from->r1c1; + to->r1c2 = (double)from->r1c2; + to->r2c1 = (double)from->r2c1; + to->r2c2 = (double)from->r2c2; +} + +static inline void dp_matrix2x2_copy_to_single(const DPMatrix2x2* from, SPMatrix2x2* to) +{ + to->r1c1 = (float)from->r1c1; + to->r1c2 = (float)from->r1c2; + to->r2c1 = (float)from->r2c1; + to->r2c2 = (float)from->r2c2; +} + +// ================ Determinant ================= // + +static inline float sp_matrix2x2_get_determinant(const SPMatrix2x2* matrix) +{ + return matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; +} + +static inline double dp_matrix2x2_get_determinant(const DPMatrix2x2* matrix) +{ + return matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; +} + +// ================== Singular ================== // + +static inline int sp_matrix2x2_is_singular(const SPMatrix2x2* matrix) +{ + const float determinant = sp_matrix2x2_get_determinant(matrix); + + return -SP_EPSYLON <= determinant && determinant <= SP_EPSYLON; +} + +static inline int dp_matrix2x2_is_singular(const DPMatrix2x2* matrix) +{ + const double determinant = dp_matrix2x2_get_determinant(matrix); + + return -DP_EPSYLON <= determinant && determinant <= DP_EPSYLON; +} + +// =============== Transposition ================ // + +static inline void sp_matrix2x2_transpose(SPMatrix2x2* matrix) +{ + const float tmp = matrix->r1c2; + matrix->r1c2 = matrix->r2c1; + matrix->r2c1 = tmp; +} + +static inline void dp_matrix2x2_transpose(DPMatrix2x2* matrix) +{ + const double tmp = matrix->r1c2; + matrix->r1c2 = matrix->r2c1; + matrix->r2c1 = tmp; +} + +// ================= Inversion ================== // + +static inline int sp_matrix2x2_invert(SPMatrix2x2* matrix) +{ + const float determinant = sp_matrix2x2_get_determinant(matrix); + + if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) { + return 0; + } + + const float r1c1 = matrix->r2c2; + const float r1c2 = -matrix->r1c2; + + const float r2c1 = -matrix->r2c1; + const float r2c2 = matrix->r1c1; + + matrix->r1c1 = r1c1 / determinant; + matrix->r1c2 = r1c2 / determinant; + + matrix->r2c1 = r2c1 / determinant; + matrix->r2c2 = r2c2 / determinant; + + return 1; +} + +static inline int dp_matrix2x2_invert(DPMatrix2x2* matrix) +{ + const double determinant = dp_matrix2x2_get_determinant(matrix); + + if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) { + return 0; + } + + const double r1c1 = matrix->r2c2; + const double r1c2 = -matrix->r1c2; + + const double r2c1 = -matrix->r2c1; + const double r2c2 = matrix->r1c1; + + matrix->r1c1 = r1c1 / determinant; + matrix->r1c2 = r1c2 / determinant; + + matrix->r2c1 = r2c1 / determinant; + matrix->r2c2 = r2c2 / determinant; + + return 1; +} + +// =============== Make Transposed ============== // + +static inline void sp_matrix2x2_make_transposed(const SPMatrix2x2* matrix, SPMatrix2x2* result) +{ + if (matrix == result) { + sp_matrix2x2_transpose(result); + return; + } + + result->r1c1 = matrix->r1c1; + result->r1c2 = matrix->r2c1; + + result->r2c1 = matrix->r1c2; + result->r2c2 = matrix->r2c2; +} + +static inline void dp_matrix2x2_make_transposed(const DPMatrix2x2* matrix, DPMatrix2x2* result) +{ + if (matrix == result) { + dp_matrix2x2_transpose(result); + return; + } + + result->r1c1 = matrix->r1c1; + result->r1c2 = matrix->r1c2; + + result->r2c1 = matrix->r2c1; + result->r2c2 = matrix->r2c2; +} + +// ================ Make Inverted =============== // + +static inline int sp_matrix2x2_make_inverted(const SPMatrix2x2* matrix, SPMatrix2x2* result) +{ + const float determinant = sp_matrix2x2_get_determinant(matrix); + + if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) { + return 0; + } + + const float r1c1 = matrix->r2c2; + const float r1c2 = -matrix->r1c2; + + const float r2c1 = -matrix->r2c1; + const float r2c2 = matrix->r1c1; + + result->r1c1 = r1c1 / determinant; + result->r1c2 = r1c2 / determinant; + + result->r2c1 = r2c1 / determinant; + result->r2c2 = r2c2 / determinant; + + return 1; +} + +static inline int dp_matrix2x2_make_inverted(const DPMatrix2x2* matrix, DPMatrix2x2* result) +{ + const double determinant = dp_matrix2x2_get_determinant(matrix); + + if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) { + return 0; + } + + const double r1c1 = matrix->r2c2; + const double r1c2 = -matrix->r1c2; + + const double r2c1 = -matrix->r2c1; + const double r2c2 = matrix->r1c1; + + result->r1c1 = r1c1 / determinant; + result->r1c2 = r1c2 / determinant; + + result->r2c1 = r2c1 / determinant; + result->r2c2 = r2c2 / determinant; + + return 1; +} + +// ================ Set Diagonal ================ // + +static inline void sp_matrix2x2_set_main_diagonal(const float d1, const float d2, SPMatrix2x2* matrix) +{ + matrix->r1c1 = d1; + matrix->r2c2 = d2; +} + +static inline void dp_matrix2x2_set_main_diagonal(const double d1, const double d2, DPMatrix2x2* matrix) +{ + matrix->r1c1 = d1; + matrix->r2c2 = d2; +} + +// ================= Set Row 1 ================== // + +static inline void sp_matrix2x2_set_row1(const float c1, const float c2, SPMatrix2x2* matrix) +{ + matrix->r1c1 = c1; + matrix->r1c2 = c2; +} + +static inline void dp_matrix2x2_set_row1(const double c1, const double c2, DPMatrix2x2* matrix) +{ + matrix->r1c1 = c1; + matrix->r1c2 = c2; +} + +// ================= Set Row 2 ================== // + +static inline void sp_matrix2x2_set_row2(const float c1, const float c2, SPMatrix2x2* matrix) +{ + matrix->r2c1 = c1; + matrix->r2c2 = c2; +} + +static inline void dp_matrix2x2_set_row2(const double c1, const double c2, DPMatrix2x2* matrix) +{ + matrix->r2c1 = c1; + matrix->r2c2 = c2; +} + +// ================ Set Column 1 ================ // + +static inline void sp_matrix2x2_set_column1(const float r1, const float r2, SPMatrix2x2* matrix) +{ + matrix->r1c1 = r1; + matrix->r2c1 = r2; +} + +static inline void dp_matrix2x2_set_column1(const double r1, const double r2, DPMatrix2x2* matrix) +{ + matrix->r1c1 = r1; + matrix->r2c1 = r2; +} + +// ================ Set Column 2 ================ // + +static inline void sp_matrix2x2_set_column2(const float r1, const float r2, SPMatrix2x2* matrix) +{ + matrix->r1c2 = r1; + matrix->r2c2 = r2; +} + +static inline void dp_matrix2x2_set_column2(const double r1, const double r2, DPMatrix2x2* matrix) +{ + matrix->r1c2 = r1; + matrix->r2c2 = r2; +} + +// ================== Addition ================== // + +static inline void sp_matrix2x2_add(const SPMatrix2x2* matrix1, const SPMatrix2x2* matrix2, SPMatrix2x2* sum) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2; +} + +static inline void dp_matrix2x2_add(const DPMatrix2x2* matrix1, const DPMatrix2x2* matrix2, DPMatrix2x2* sum) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2; +} + +// ==================== Add3 ==================== // + +static inline void sp_matrix2x2_add3( + const SPMatrix2x2* matrix1, + const SPMatrix2x2* matrix2, + const SPMatrix2x2* matrix3, + SPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1 + matrix3->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2 + matrix3->r1c2; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1 + matrix3->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2 + matrix3->r2c2; +} + +static inline void dp_matrix2x2_add3( + const DPMatrix2x2* matrix1, + const DPMatrix2x2* matrix2, + const DPMatrix2x2* matrix3, + DPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1 + matrix3->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2 + matrix3->r1c2; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1 + matrix3->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2 + matrix3->r2c2; +} + +// ==================== Add4 ==================== // + +static inline void sp_matrix2x2_add4( + const SPMatrix2x2* matrix1, + const SPMatrix2x2* matrix2, + const SPMatrix2x2* matrix3, + const SPMatrix2x2* matrix4, + SPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1); + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2); + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1); + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2); +} + +static inline void dp_matrix2x2_add4( + const DPMatrix2x2* matrix1, + const DPMatrix2x2* matrix2, + const DPMatrix2x2* matrix3, + const DPMatrix2x2* matrix4, + DPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1); + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2); + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1); + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2); +} + +// ==================== Add5 ==================== // + +static inline void sp_matrix2x2_add5( + const SPMatrix2x2* matrix1, + const SPMatrix2x2* matrix2, + const SPMatrix2x2* matrix3, + const SPMatrix2x2* matrix4, + const SPMatrix2x2* matrix5, + SPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1) + matrix5->r1c1; + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2) + matrix5->r1c2; + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1) + matrix5->r2c1; + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2) + matrix5->r2c2; +} + +static inline void dp_matrix2x2_add5( + const DPMatrix2x2* matrix1, + const DPMatrix2x2* matrix2, + const DPMatrix2x2* matrix3, + const DPMatrix2x2* matrix4, + const DPMatrix2x2* matrix5, + DPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1) + matrix5->r1c1; + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2) + matrix5->r1c2; + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1) + matrix5->r2c1; + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2) + matrix5->r2c2; +} + +// ================ Subtraction ================= // + +static inline void sp_matrix2x2_subtract(const SPMatrix2x2* minuend, const SPMatrix2x2* subtrahend, SPMatrix2x2* difference) +{ + difference->r1c1 = minuend->r1c1 - subtrahend->r1c1; + difference->r1c2 = minuend->r1c2 - subtrahend->r1c2; + + difference->r2c1 = minuend->r2c1 - subtrahend->r2c1; + difference->r2c2 = minuend->r2c2 - subtrahend->r2c2; +} + +static inline void dp_matrix2x2_subtract(const DPMatrix2x2* minuend, const DPMatrix2x2* subtrahend, DPMatrix2x2* difference) +{ + difference->r1c1 = minuend->r1c1 - subtrahend->r1c1; + difference->r1c2 = minuend->r1c2 - subtrahend->r1c2; + + difference->r2c1 = minuend->r2c1 - subtrahend->r2c1; + difference->r2c2 = minuend->r2c2 - subtrahend->r2c2; +} + +// ============= Weighed Sum of two ============= // + +static inline void sp_matrix2x2_get_weighted_sum2( + const float weight1, const SPMatrix2x2* matrix1, + const float weight2, const SPMatrix2x2* matrix2, + SPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2; +} + +static inline void dp_matrix2x2_get_weighted_sum2( + const double weight1, const DPMatrix2x2* matrix1, + const double weight2, const DPMatrix2x2* matrix2, + DPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2; +} + +// ============ Weighed Sum of three ============ // + +static inline void sp_matrix2x2_get_weighted_sum3( + const float weight1, const SPMatrix2x2* matrix1, + const float weight2, const SPMatrix2x2* matrix2, + const float weight3, const SPMatrix2x2* matrix3, + SPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3; +} + +static inline void dp_matrix2x2_get_weighted_sum3( + const double weight1, const DPMatrix2x2* matrix1, + const double weight2, const DPMatrix2x2* matrix2, + const double weight3, const DPMatrix2x2* matrix3, + DPMatrix2x2* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3; +} + +// ============ Weighed Sum of four ============= // + +static inline void sp_matrix2x2_get_weighted_sum4( + const float weight1, const SPMatrix2x2* matrix1, + const float weight2, const SPMatrix2x2* matrix2, + const float weight3, const SPMatrix2x2* matrix3, + const float weight4, const SPMatrix2x2* matrix4, + SPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4); + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4); + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4); + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4); +} + +static inline void dp_matrix2x2_get_weighted_sum4( + const double weight1, const DPMatrix2x2* matrix1, + const double weight2, const DPMatrix2x2* matrix2, + const double weight3, const DPMatrix2x2* matrix3, + const double weight4, const DPMatrix2x2* matrix4, + DPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4); + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4); + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4); + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4); +} + +// ============ Weighed Sum of five ============= // + +static inline void sp_matrix2x2_get_weighted_sum5( + const float weight1, const SPMatrix2x2* matrix1, + const float weight2, const SPMatrix2x2* matrix2, + const float weight3, const SPMatrix2x2* matrix3, + const float weight4, const SPMatrix2x2* matrix4, + const float weight5, const SPMatrix2x2* matrix5, + SPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5; + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5; + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5; + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5; +} + +static inline void dp_matrix2x2_get_weighted_sum5( + const double weight1, const DPMatrix2x2* matrix1, + const double weight2, const DPMatrix2x2* matrix2, + const double weight3, const DPMatrix2x2* matrix3, + const double weight4, const DPMatrix2x2* matrix4, + const double weight5, const DPMatrix2x2* matrix5, + DPMatrix2x2* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5; + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5; + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5; + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5; +} + +// =============== Multiplication =============== // + +static inline void sp_matrix2x2_multiply(const SPMatrix2x2* multiplicand, const float multiplier, SPMatrix2x2* product) +{ + product->r1c1 = multiplicand->r1c1 * multiplier; + product->r1c2 = multiplicand->r1c2 * multiplier; + + product->r2c1 = multiplicand->r2c1 * multiplier; + product->r2c2 = multiplicand->r2c2 * multiplier; +} + +static inline void dp_matrix2x2_multiply(const DPMatrix2x2* multiplicand, const double multiplier, DPMatrix2x2* product) +{ + product->r1c1 = multiplicand->r1c1 * multiplier; + product->r1c2 = multiplicand->r1c2 * multiplier; + + product->r2c1 = multiplicand->r2c1 * multiplier; + product->r2c2 = multiplicand->r2c2 * multiplier; +} + +// ================== Division ================== // + +static inline void sp_matrix2x2_divide(const SPMatrix2x2* dividend, const float divisor, SPMatrix2x2* quotient) +{ + quotient->r1c1 = dividend->r1c1 / divisor; + quotient->r1c2 = dividend->r1c2 / divisor; + + quotient->r2c1 = dividend->r2c1 / divisor; + quotient->r2c2 = dividend->r2c2 / divisor; +} + +static inline void dp_matrix2x2_divide(const DPMatrix2x2* dividend, const double divisor, DPMatrix2x2* quotient) +{ + quotient->r1c1 = dividend->r1c1 / divisor; + quotient->r1c2 = dividend->r1c2 / divisor; + + quotient->r2c1 = dividend->r2c1 / divisor; + quotient->r2c2 = dividend->r2c2 / divisor; +} + +// ============ Left Vector Product ============= // + +static inline void sp_matrix2x2_left_product(const SPVector2* vector, const SPMatrix2x2* matrix, SPVector2* result) +{ + sp_vector2_set( + vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, + vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2, + result + ); +} + +static inline void dp_matrix2x2_left_product(const DPVector2* vector, const DPMatrix2x2* matrix, DPVector2* result) +{ + dp_vector2_set( + vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1, + vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2, + result + ); +} + +// ============ Right Vector Product ============ // + +static inline void sp_matrix2x2_right_product(const SPMatrix2x2* matrix, const SPVector2* vector, SPVector2* result) +{ + sp_vector2_set( + matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, + matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2, + result + ); +} + +static inline void dp_matrix2x2_right_product(const DPMatrix2x2* matrix, const DPVector2* vector, DPVector2* result) +{ + dp_vector2_set( + matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2, + matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2, + result + ); +} + +// =============== Matrix Product =============== // + +static inline void sp_matrix2x2_matrix_product(const SPMatrix2x2* matrix1, const SPMatrix2x2* matrix2, SPMatrix2x2* result) +{ + const float r1c1 = matrix1->r1c1 * matrix2->r1c1 + matrix1->r1c2 * matrix2->r2c1; + const float r1c2 = matrix1->r1c1 * matrix2->r1c2 + matrix1->r1c2 * matrix2->r2c2; + + const float r2c1 = matrix1->r2c1 * matrix2->r1c1 + matrix1->r2c2 * matrix2->r2c1; + const float r2c2 = matrix1->r2c1 * matrix2->r1c2 + matrix1->r2c2 * matrix2->r2c2; + + result->r1c1 = r1c1; + result->r1c2 = r1c2; + + result->r2c1 = r2c1; + result->r2c2 = r2c2; +} + +static inline void dp_matrix2x2_matrix_product(const DPMatrix2x2* matrix1, const DPMatrix2x2* matrix2, DPMatrix2x2* result) +{ + const double r1c1 = matrix1->r1c1 * matrix2->r1c1 + matrix1->r1c2 * matrix2->r2c1; + const double r1c2 = matrix1->r1c1 * matrix2->r1c2 + matrix1->r1c2 * matrix2->r2c2; + + const double r2c1 = matrix1->r2c1 * matrix2->r1c1 + matrix1->r2c2 * matrix2->r2c1; + const double r2c2 = matrix1->r2c1 * matrix2->r1c2 + matrix1->r2c2 * matrix2->r2c2; + + result->r1c1 = r1c1; + result->r1c2 = r1c2; + + result->r2c1 = r2c1; + result->r2c2 = r2c2; +} + +#endif diff --git a/src/matrix3x3.c b/src/matrix3x3.c new file mode 100644 index 0000000..224a5e6 --- /dev/null +++ b/src/matrix3x3.c @@ -0,0 +1,397 @@ +#include "matrix3x3.h" + +const SPMatrix3x3 SP_ZERO_MATRIX3X3 = { + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f +}; + +const DPMatrix3x3 DP_ZERO_MATRIX3X3 = { + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0 +}; + +const SPMatrix3x3 SP_IDENTITY_MATRIX3X3 = { + 1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 1.0f +}; + +const DPMatrix3x3 DP_IDENTITY_MATRIX3X3 = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 +}; + +// ================= Inversion ================== // + +int sp_matrix3x3_invert(SPMatrix3x3* matrix) +{ + const float determinant = sp_matrix3x3_get_determinant(matrix); + + if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) { + return 0; + } + + const float r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2; + const float r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3; + const float r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2; + + const float r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3; + const float r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1; + const float r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3; + + const float r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1; + const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; + const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; + + matrix->r1c1 = r1c1 / determinant; + matrix->r1c2 = r1c2 / determinant; + matrix->r1c3 = r1c3 / determinant; + + matrix->r2c1 = r2c1 / determinant; + matrix->r2c2 = r2c2 / determinant; + matrix->r2c3 = r2c3 / determinant; + + matrix->r3c1 = r3c1 / determinant; + matrix->r3c2 = r3c2 / determinant; + matrix->r3c3 = r3c3 / determinant; + + return 1; +} + +int dp_matrix3x3_invert(DPMatrix3x3* matrix) +{ + const double determinant = dp_matrix3x3_get_determinant(matrix); + + if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) { + return 0; + } + + const double r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2; + const double r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3; + const double r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2; + + const double r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3; + const double r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1; + const double r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3; + + const double r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1; + const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; + const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; + + matrix->r1c1 = r1c1 / determinant; + matrix->r1c2 = r1c2 / determinant; + matrix->r1c3 = r1c3 / determinant; + + matrix->r2c1 = r2c1 / determinant; + matrix->r2c2 = r2c2 / determinant; + matrix->r2c3 = r2c3 / determinant; + + matrix->r3c1 = r3c1 / determinant; + matrix->r3c2 = r3c2 / determinant; + matrix->r3c3 = r3c3 / determinant; + + return 1; +} + +// ================ Make Inverted =============== // + +int sp_matrix3x3_make_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result) +{ + const float determinant = sp_matrix3x3_get_determinant(matrix); + + if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) { + return 0; + } + + const float r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2; + const float r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3; + const float r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2; + + const float r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3; + const float r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1; + const float r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3; + + const float r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1; + const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; + const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; + + result->r1c1 = r1c1 / determinant; + result->r1c2 = r1c2 / determinant; + result->r1c3 = r1c3 / determinant; + + result->r2c1 = r2c1 / determinant; + result->r2c2 = r2c2 / determinant; + result->r2c3 = r2c3 / determinant; + + result->r3c1 = r3c1 / determinant; + result->r3c2 = r3c2 / determinant; + result->r3c3 = r3c3 / determinant; + + return 1; +} + +int dp_matrix3x3_make_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result) +{ + const double determinant = dp_matrix3x3_get_determinant(matrix); + + if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) { + return 0; + } + + const double r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2; + const double r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3; + const double r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2; + + const double r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3; + const double r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1; + const double r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3; + + const double r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1; + const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2; + const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1; + + result->r1c1 = r1c1 / determinant; + result->r1c2 = r1c2 / determinant; + result->r1c3 = r1c3 / determinant; + + result->r2c1 = r2c1 / determinant; + result->r2c2 = r2c2 / determinant; + result->r2c3 = r2c3 / determinant; + + result->r3c1 = r3c1 / determinant; + result->r3c2 = r3c2 / determinant; + result->r3c3 = r3c3 / determinant; + + return 1; +} + +// ============= Weighed Sum of two ============= // + +void sp_matrix3x3_get_weighted_sum2( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + SPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2; + sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2; + sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2; + + sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2; + sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2; + sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2; +} + +void dp_matrix3x3_get_weighted_sum2( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + DPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2; + sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2; + sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2; + + sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2; + sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2; + sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2; +} + +// ============ Weighed Sum of three ============ // + +void sp_matrix3x3_get_weighted_sum3( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + SPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3; + sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3; + sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3; + + sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3; + sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3; + sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3; +} + +void dp_matrix3x3_get_weighted_sum3( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + DPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3; + sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3; + sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3; + + sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3; + sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3; + sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3; + + sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3; + sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3; + sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3; +} + +// ============ Weighed Sum of four ============= // + +void sp_matrix3x3_get_weighted_sum4( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + const float weight4, const SPMatrix3x3* matrix4, + SPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4); + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4); + sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4); + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4); + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4); + sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4); + + sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4); + sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4); + sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4); +} + +void dp_matrix3x3_get_weighted_sum4( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + const double weight4, const DPMatrix3x3* matrix4, + DPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4); + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4); + sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4); + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4); + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4); + sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4); + + sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4); + sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4); + sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4); +} + +// ============ Weighed Sum of five ============= // + +void sp_matrix3x3_get_weighted_sum5( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + const float weight4, const SPMatrix3x3* matrix4, + const float weight5, const SPMatrix3x3* matrix5, + SPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5; + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5; + sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5; + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5; + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5; + sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5; + + sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5; + sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5; + sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5; +} + +void dp_matrix3x3_get_weighted_sum5( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + const double weight4, const DPMatrix3x3* matrix4, + const double weight5, const DPMatrix3x3* matrix5, + DPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5; + sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5; + sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5; + + sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5; + sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5; + sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5; + + sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5; + sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5; + sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5; +} + +// =============== Matrix Product =============== // + +void sp_matrix3x3_matrix_product(const SPMatrix3x3* matrix1, const SPMatrix3x3* matrix2, SPMatrix3x3* result) +{ + const float r1c1 = matrix1->r1c1 * matrix2->r1c1 + matrix1->r1c2 * matrix2->r2c1 + matrix1->r1c3 * matrix2->r3c1; + const float r1c2 = matrix1->r1c1 * matrix2->r1c2 + matrix1->r1c2 * matrix2->r2c2 + matrix1->r1c3 * matrix2->r3c2; + const float r1c3 = matrix1->r1c1 * matrix2->r1c3 + matrix1->r1c2 * matrix2->r2c3 + matrix1->r1c3 * matrix2->r3c3; + + const float r2c1 = matrix1->r2c1 * matrix2->r1c1 + matrix1->r2c2 * matrix2->r2c1 + matrix1->r2c3 * matrix2->r3c1; + const float r2c2 = matrix1->r2c1 * matrix2->r1c2 + matrix1->r2c2 * matrix2->r2c2 + matrix1->r2c3 * matrix2->r3c2; + const float r2c3 = matrix1->r2c1 * matrix2->r1c3 + matrix1->r2c2 * matrix2->r2c3 + matrix1->r2c3 * matrix2->r3c3; + + const float r3c1 = matrix1->r3c1 * matrix2->r1c1 + matrix1->r3c2 * matrix2->r2c1 + matrix1->r3c3 * matrix2->r3c1; + const float r3c2 = matrix1->r3c1 * matrix2->r1c2 + matrix1->r3c2 * matrix2->r2c2 + matrix1->r3c3 * matrix2->r3c2; + const float r3c3 = matrix1->r3c1 * matrix2->r1c3 + matrix1->r3c2 * matrix2->r2c3 + matrix1->r3c3 * matrix2->r3c3; + + result->r1c1 = r1c1; + result->r1c2 = r1c2; + result->r1c3 = r1c3; + + result->r2c1 = r2c1; + result->r2c2 = r2c2; + result->r2c3 = r2c3; + + result->r3c1 = r3c1; + result->r3c2 = r3c2; + result->r3c3 = r3c3; +} + +void dp_matrix3x3_matrix_product(const DPMatrix3x3* matrix1, const DPMatrix3x3* matrix2, DPMatrix3x3* result) +{ + const double r1c1 = matrix1->r1c1 * matrix2->r1c1 + matrix1->r1c2 * matrix2->r2c1 + matrix1->r1c3 * matrix2->r3c1; + const double r1c2 = matrix1->r1c1 * matrix2->r1c2 + matrix1->r1c2 * matrix2->r2c2 + matrix1->r1c3 * matrix2->r3c2; + const double r1c3 = matrix1->r1c1 * matrix2->r1c3 + matrix1->r1c2 * matrix2->r2c3 + matrix1->r1c3 * matrix2->r3c3; + + const double r2c1 = matrix1->r2c1 * matrix2->r1c1 + matrix1->r2c2 * matrix2->r2c1 + matrix1->r2c3 * matrix2->r3c1; + const double r2c2 = matrix1->r2c1 * matrix2->r1c2 + matrix1->r2c2 * matrix2->r2c2 + matrix1->r2c3 * matrix2->r3c2; + const double r2c3 = matrix1->r2c1 * matrix2->r1c3 + matrix1->r2c2 * matrix2->r2c3 + matrix1->r2c3 * matrix2->r3c3; + + const double r3c1 = matrix1->r3c1 * matrix2->r1c1 + matrix1->r3c2 * matrix2->r2c1 + matrix1->r3c3 * matrix2->r3c1; + const double r3c2 = matrix1->r3c1 * matrix2->r1c2 + matrix1->r3c2 * matrix2->r2c2 + matrix1->r3c3 * matrix2->r3c2; + const double r3c3 = matrix1->r3c1 * matrix2->r1c3 + matrix1->r3c2 * matrix2->r2c3 + matrix1->r3c3 * matrix2->r3c3; + + result->r1c1 = r1c1; + result->r1c2 = r1c2; + result->r1c3 = r1c3; + + result->r2c1 = r2c1; + result->r2c2 = r2c2; + result->r2c3 = r2c3; + + result->r3c1 = r3c1; + result->r3c2 = r3c2; + result->r3c3 = r3c3; +} diff --git a/src/matrix3x3.h b/src/matrix3x3.h new file mode 100644 index 0000000..25d5595 --- /dev/null +++ b/src/matrix3x3.h @@ -0,0 +1,794 @@ +#ifndef _GEOMETRY_MATRIX3X3_H_ +#define _GEOMETRY_MATRIX3X3_H_ + +#include "vector3.h" + +typedef struct { + float r1c1, r1c2, r1c3; + float r2c1, r2c2, r2c3; + float r3c1, r3c2, r3c3; +} SPMatrix3x3; + +typedef struct { + double r1c1, r1c2, r1c3; + double r2c1, r2c2, r2c3; + double r3c1, r3c2, r3c3; +} DPMatrix3x3; + +extern const SPMatrix3x3 SP_ZERO_MATRIX3X3; + +extern const DPMatrix3x3 DP_ZERO_MATRIX3X3; + +extern const SPMatrix3x3 SP_IDENTITY_MATRIX3X3; + +extern const DPMatrix3x3 DP_IDENTITY_MATRIX3X3; + +// =================== Reset ==================== // + +static inline void sp_matrix3x3_reset(SPMatrix3x3* matrix) +{ + matrix->r1c1 = 0.0f; + matrix->r1c2 = 0.0f; + matrix->r1c3 = 0.0f; + + matrix->r2c1 = 0.0f; + matrix->r2c2 = 0.0f; + matrix->r2c3 = 0.0f; + + matrix->r3c1 = 0.0f; + matrix->r3c2 = 0.0f; + matrix->r3c3 = 0.0f; +} + +static inline void dp_matrix3x3_reset(DPMatrix3x3* matrix) +{ + matrix->r1c1 = 0.0; + matrix->r1c2 = 0.0; + matrix->r1c3 = 0.0; + + matrix->r2c1 = 0.0; + matrix->r2c2 = 0.0; + matrix->r2c3 = 0.0; + + matrix->r3c1 = 0.0; + matrix->r3c2 = 0.0; + matrix->r3c3 = 0.0; +} + +// ================== Identity ================== // + +static inline void sp_matrix3x3_make_identity(SPMatrix3x3* matrix) +{ + matrix->r1c1 = 1.0f; + matrix->r1c2 = 0.0f; + matrix->r1c3 = 0.0f; + + matrix->r2c1 = 0.0f; + matrix->r2c2 = 1.0f; + matrix->r2c3 = 0.0f; + + matrix->r3c1 = 0.0f; + matrix->r3c2 = 0.0f; + matrix->r3c3 = 1.0f; +} + +static inline void dp_matrix3x3_make_identity(DPMatrix3x3* matrix) +{ + matrix->r1c1 = 1.0; + matrix->r1c2 = 0.0; + matrix->r1c3 = 0.0; + + matrix->r2c1 = 0.0; + matrix->r2c2 = 1.0; + matrix->r2c3 = 0.0; + + matrix->r3c1 = 0.0; + matrix->r3c2 = 0.0; + matrix->r3c3 = 1.0; +} + +// ================ Make Diagonal =============== // + +static inline void sp_matrix3x3_make_diagonal(const float d1, const float d2, const float d3, SPMatrix3x3* matrix) +{ + matrix->r1c1 = d1; + matrix->r1c2 = 0.0f; + matrix->r1c3 = 0.0f; + + matrix->r2c1 = 0.0f; + matrix->r2c2 = d2; + matrix->r2c3 = 0.0f; + + matrix->r3c1 = 0.0f; + matrix->r3c2 = 0.0f; + matrix->r3c3 = d2; +} + +static inline void dp_matrix3x3_make_diagonal(const double d1, const double d2, const double d3, DPMatrix3x3* matrix) +{ + matrix->r1c1 = d1; + matrix->r1c2 = 0.0; + matrix->r1c3 = 0.0; + + matrix->r2c1 = 0.0; + matrix->r2c2 = d2; + matrix->r2c3 = 0.0; + + matrix->r3c1 = 0.0; + matrix->r3c2 = 0.0; + matrix->r3c3 = d2; +} + +// ==================== Copy ==================== // + +static inline void sp_matrix3x3_copy(const SPMatrix3x3* from, SPMatrix3x3* to) +{ + to->r1c1 = from->r1c1; + to->r1c2 = from->r1c2; + to->r1c3 = from->r1c3; + + to->r2c1 = from->r2c1; + to->r2c2 = from->r2c2; + to->r2c3 = from->r2c3; + + to->r3c1 = from->r3c1; + to->r3c2 = from->r3c2; + to->r3c3 = from->r3c3; +} + +static inline void dp_matrix3x3_copy(const DPMatrix3x3* from, DPMatrix3x3* to) +{ + to->r1c1 = from->r1c1; + to->r1c2 = from->r1c2; + to->r1c3 = from->r1c3; + + to->r2c1 = from->r2c1; + to->r2c2 = from->r2c2; + to->r2c3 = from->r2c3; + + to->r3c1 = from->r3c1; + to->r3c2 = from->r3c2; + to->r3c3 = from->r3c3; +} + +// ============= Copy to twin type ============== // + +static inline void sp_matrix3x3_copy_to_double(const SPMatrix3x3* from, DPMatrix3x3* to) +{ + to->r1c1 = (double)from->r1c1; + to->r1c2 = (double)from->r1c2; + to->r1c3 = (double)from->r1c3; + + to->r2c1 = (double)from->r2c1; + to->r2c2 = (double)from->r2c2; + to->r2c3 = (double)from->r2c3; + + to->r3c1 = (double)from->r3c1; + to->r3c2 = (double)from->r3c2; + to->r3c3 = (double)from->r3c3; +} + +static inline void dp_matrix3x3_copy_to_single(const DPMatrix3x3* from, SPMatrix3x3* to) +{ + to->r1c1 = (float)from->r1c1; + to->r1c2 = (float)from->r1c2; + to->r1c3 = (float)from->r1c3; + + to->r2c1 = (float)from->r2c1; + to->r2c2 = (float)from->r2c2; + to->r2c3 = (float)from->r2c3; + + to->r3c1 = (float)from->r3c1; + to->r3c2 = (float)from->r3c2; + to->r3c3 = (float)from->r3c3; +} + +// ================ Determinant ================= // + +static inline float sp_matrix3x3_get_determinant(const SPMatrix3x3* matrix) +{ + return matrix->r1c1 * (matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2) + + matrix->r1c2 * (matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3) + + matrix->r1c3 * (matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1); +} + +static inline double dp_matrix3x3_get_determinant(const DPMatrix3x3* matrix) +{ + return matrix->r1c1 * (matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2) + + matrix->r1c2 * (matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3) + + matrix->r1c3 * (matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1); +} + +// ================== Singular ================== // + +static inline int sp_matrix3x3_is_singular(const SPMatrix3x3* matrix) +{ + const float determinant = sp_matrix3x3_get_determinant(matrix); + + return -SP_EPSYLON <= determinant && determinant <= SP_EPSYLON; +} + +static inline int dp_matrix3x3_is_singular(const DPMatrix3x3* matrix) +{ + const double determinant = dp_matrix3x3_get_determinant(matrix); + + return -DP_EPSYLON <= determinant && determinant <= DP_EPSYLON; +} + +// ================= Inversion ================== // + +int sp_matrix3x3_invert(SPMatrix3x3* matrix); + +int dp_matrix3x3_invert(DPMatrix3x3* matrix); + +// =============== Transposition ================ // + +static inline void sp_matrix3x3_transpose(SPMatrix3x3* matrix) +{ + float tmp = matrix->r1c2; + matrix->r1c2 = matrix->r2c1; + matrix->r2c1 = tmp; + + tmp = matrix->r1c3; + matrix->r1c3 = matrix->r3c1; + matrix->r3c1 = tmp; + + tmp = matrix->r2c3; + matrix->r2c3 = matrix->r3c2; + matrix->r3c2 = tmp; +} + +static inline void dp_matrix3x3_transpose(DPMatrix3x3* matrix) +{ + double tmp = matrix->r1c2; + matrix->r1c2 = matrix->r2c1; + matrix->r2c1 = tmp; + + tmp = matrix->r1c3; + matrix->r1c3 = matrix->r3c1; + matrix->r3c1 = tmp; + + tmp = matrix->r2c3; + matrix->r2c3 = matrix->r3c2; + matrix->r3c2 = tmp; +} + +// ================ Make Inverted =============== // + +int sp_matrix3x3_make_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result); + +int dp_matrix3x3_make_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result); + +// =============== Make Transposed ============== // + +static inline void sp_matrix3x3_make_transposed(const SPMatrix3x3* matrix, SPMatrix3x3* result) +{ + if (matrix == result) { + sp_matrix3x3_transpose(result); + return; + } + + result->r1c1 = matrix->r1c1; + result->r1c2 = matrix->r2c1; + result->r1c3 = matrix->r3c1; + + result->r2c1 = matrix->r1c2; + result->r2c2 = matrix->r2c2; + result->r2c3 = matrix->r3c2; + + result->r3c1 = matrix->r1c3; + result->r3c2 = matrix->r2c3; + result->r3c3 = matrix->r3c3; +} + +static inline void dp_matrix3x3_make_transposed(const DPMatrix3x3* matrix, DPMatrix3x3* result) +{ + if (matrix == result) { + dp_matrix3x3_transpose(result); + return; + } + + result->r1c1 = matrix->r1c1; + result->r1c2 = matrix->r2c1; + result->r1c3 = matrix->r3c1; + + result->r2c1 = matrix->r1c2; + result->r2c2 = matrix->r2c2; + result->r2c3 = matrix->r3c2; + + result->r3c1 = matrix->r1c3; + result->r3c2 = matrix->r2c3; + result->r3c3 = matrix->r3c3; +} + +// ================ Set Diagonal ================ // + +static inline void sp_matrix3x3_set_main_diagonal(const float d1, const float d2, const float d3, SPMatrix3x3* matrix) +{ + matrix->r1c1 = d1; + matrix->r2c2 = d2; + matrix->r3c3 = d3; +} + +static inline void dp_matrix3x3_set_main_diagonal(const double d1, const double d2, const double d3, DPMatrix3x3* matrix) +{ + matrix->r1c1 = d1; + matrix->r2c2 = d2; + matrix->r3c3 = d3; +} + +// ================= Set Row 1 ================== // + +static inline void sp_matrix3x3_set_row1(const float c1, const float c2, const float c3, SPMatrix3x3* matrix) +{ + matrix->r1c1 = c1; + matrix->r1c2 = c2; + matrix->r1c3 = c3; +} + +static inline void dp_matrix3x3_set_row1(const double c1, const double c2, const double c3, DPMatrix3x3* matrix) +{ + matrix->r1c1 = c1; + matrix->r1c2 = c2; + matrix->r1c3 = c3; +} + +// ================= Set Row 2 ================== // + +static inline void sp_matrix3x3_set_row2(const float c1, const float c2, const float c3, SPMatrix3x3* matrix) +{ + matrix->r2c1 = c1; + matrix->r2c2 = c2; + matrix->r2c3 = c3; +} + +static inline void dp_matrix3x3_set_row2(const double c1, const double c2, const double c3, DPMatrix3x3* matrix) +{ + matrix->r2c1 = c1; + matrix->r2c2 = c2; + matrix->r2c3 = c3; +} + +// ================= Set Row 3 ================== // + +static inline void sp_matrix3x3_set_row3(const float c1, const float c2, const float c3, SPMatrix3x3* matrix) +{ + matrix->r3c1 = c1; + matrix->r3c2 = c2; + matrix->r3c3 = c3; +} + +static inline void dp_matrix3x3_set_row3(const double c1, const double c2, const double c3, DPMatrix3x3* matrix) +{ + matrix->r3c1 = c1; + matrix->r3c2 = c2; + matrix->r3c3 = c3; +} + +// ================ Set Column 1 ================ // + +static inline void sp_matrix3x3_set_column1(const float r1, const float r2, const float r3, SPMatrix3x3* matrix) +{ + matrix->r1c1 = r1; + matrix->r2c1 = r2; + matrix->r3c1 = r3; +} + +static inline void dp_matrix3x3_set_column1(const double r1, const double r2, const double r3, DPMatrix3x3* matrix) +{ + matrix->r1c1 = r1; + matrix->r2c1 = r2; + matrix->r3c1 = r3; +} + +// ================ Set Column 2 ================ // + +static inline void sp_matrix3x3_set_column2(const float r1, const float r2, const float r3, SPMatrix3x3* matrix) +{ + matrix->r1c2 = r1; + matrix->r2c2 = r2; + matrix->r3c2 = r3; +} + +static inline void dp_matrix3x3_set_column2(const double r1, const double r2, const double r3, DPMatrix3x3* matrix) +{ + matrix->r1c2 = r1; + matrix->r2c2 = r2; + matrix->r3c2 = r3; +} + +// ================ Set Column 3 ================ // + +static inline void sp_matrix3x3_set_column3(const float r1, const float r2, const float r3, SPMatrix3x3* matrix) +{ + matrix->r1c3 = r1; + matrix->r2c3 = r2; + matrix->r3c3 = r3; +} + +static inline void dp_matrix3x3_set_column3(const double r1, const double r2, const double r3, DPMatrix3x3* matrix) +{ + matrix->r1c3 = r1; + matrix->r2c3 = r2; + matrix->r3c3 = r3; +} + +// ================== Addition ================== // + +static inline void sp_matrix3x3_add(const SPMatrix3x3* matrix1, const SPMatrix3x3* matrix2, SPMatrix3x3* sum) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2; + sum->r1c3 = matrix1->r1c3 + matrix2->r1c3; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2; + sum->r2c3 = matrix1->r2c3 + matrix2->r2c3; + + sum->r3c1 = matrix1->r3c1 + matrix2->r3c1; + sum->r3c2 = matrix1->r3c2 + matrix2->r3c2; + sum->r3c3 = matrix1->r3c3 + matrix2->r3c3; +} + +static inline void dp_matrix3x3_add(const DPMatrix3x3* matrix1, const DPMatrix3x3* matrix2, DPMatrix3x3* sum) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2; + sum->r1c3 = matrix1->r1c3 + matrix2->r1c3; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2; + sum->r2c3 = matrix1->r2c3 + matrix2->r2c3; + + sum->r3c1 = matrix1->r3c1 + matrix2->r3c1; + sum->r3c2 = matrix1->r3c2 + matrix2->r3c2; + sum->r3c3 = matrix1->r3c3 + matrix2->r3c3; +} + +// ==================== Add3 ==================== // + +static inline void sp_matrix3x3_add3( + const SPMatrix3x3* matrix1, + const SPMatrix3x3* matrix2, + const SPMatrix3x3* matrix3, + SPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1 + matrix3->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2 + matrix3->r1c2; + sum->r1c3 = matrix1->r1c3 + matrix2->r1c3 + matrix3->r1c3; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1 + matrix3->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2 + matrix3->r2c2; + sum->r2c3 = matrix1->r2c3 + matrix2->r2c3 + matrix3->r2c3; + + sum->r3c1 = matrix1->r3c1 + matrix2->r3c1 + matrix3->r3c1; + sum->r3c2 = matrix1->r3c2 + matrix2->r3c2 + matrix3->r3c2; + sum->r3c3 = matrix1->r3c3 + matrix2->r3c3 + matrix3->r3c3; +} + +static inline void dp_matrix3x3_add3( + const DPMatrix3x3* matrix1, + const DPMatrix3x3* matrix2, + const DPMatrix3x3* matrix3, + DPMatrix3x3* sum +) +{ + sum->r1c1 = matrix1->r1c1 + matrix2->r1c1 + matrix3->r1c1; + sum->r1c2 = matrix1->r1c2 + matrix2->r1c2 + matrix3->r1c2; + sum->r1c3 = matrix1->r1c3 + matrix2->r1c3 + matrix3->r1c3; + + sum->r2c1 = matrix1->r2c1 + matrix2->r2c1 + matrix3->r2c1; + sum->r2c2 = matrix1->r2c2 + matrix2->r2c2 + matrix3->r2c2; + sum->r2c3 = matrix1->r2c3 + matrix2->r2c3 + matrix3->r2c3; + + sum->r3c1 = matrix1->r3c1 + matrix2->r3c1 + matrix3->r3c1; + sum->r3c2 = matrix1->r3c2 + matrix2->r3c2 + matrix3->r3c2; + sum->r3c3 = matrix1->r3c3 + matrix2->r3c3 + matrix3->r3c3; +} + +// ==================== Add4 ==================== // + +static inline void sp_matrix3x3_add4( + const SPMatrix3x3* matrix1, + const SPMatrix3x3* matrix2, + const SPMatrix3x3* matrix3, + const SPMatrix3x3* matrix4, + SPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1); + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2); + sum->r1c2 = (matrix1->r1c3 + matrix2->r1c3) + (matrix3->r1c3 + matrix4->r1c3); + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1); + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2); + sum->r2c3 = (matrix1->r2c3 + matrix2->r2c3) + (matrix3->r2c3 + matrix4->r2c3); + + sum->r3c1 = (matrix1->r3c1 + matrix2->r3c1) + (matrix3->r3c1 + matrix4->r3c1); + sum->r3c2 = (matrix1->r3c2 + matrix2->r3c2) + (matrix3->r3c2 + matrix4->r3c2); + sum->r3c3 = (matrix1->r3c3 + matrix2->r3c3) + (matrix3->r3c3 + matrix4->r3c3); +} + +static inline void dp_matrix3x3_add4( + const DPMatrix3x3* matrix1, + const DPMatrix3x3* matrix2, + const DPMatrix3x3* matrix3, + const DPMatrix3x3* matrix4, + DPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1); + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2); + sum->r1c2 = (matrix1->r1c3 + matrix2->r1c3) + (matrix3->r1c3 + matrix4->r1c3); + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1); + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2); + sum->r2c3 = (matrix1->r2c3 + matrix2->r2c3) + (matrix3->r2c3 + matrix4->r2c3); + + sum->r3c1 = (matrix1->r3c1 + matrix2->r3c1) + (matrix3->r3c1 + matrix4->r3c1); + sum->r3c2 = (matrix1->r3c2 + matrix2->r3c2) + (matrix3->r3c2 + matrix4->r3c2); + sum->r3c3 = (matrix1->r3c3 + matrix2->r3c3) + (matrix3->r3c3 + matrix4->r3c3); +} + +// ==================== Add5 ==================== // + +static inline void sp_matrix3x3_add5( + const SPMatrix3x3* matrix1, + const SPMatrix3x3* matrix2, + const SPMatrix3x3* matrix3, + const SPMatrix3x3* matrix4, + const SPMatrix3x3* matrix5, + SPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1) + matrix5->r1c1; + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2) + matrix5->r1c2; + sum->r1c3 = (matrix1->r1c3 + matrix2->r1c3) + (matrix3->r1c3 + matrix4->r1c3) + matrix5->r1c3; + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1) + matrix5->r2c1; + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2) + matrix5->r2c2; + sum->r2c3 = (matrix1->r2c3 + matrix2->r2c3) + (matrix3->r2c3 + matrix4->r2c3) + matrix5->r2c3; + + sum->r3c1 = (matrix1->r3c1 + matrix2->r3c1) + (matrix3->r3c1 + matrix4->r3c1) + matrix5->r3c1; + sum->r3c2 = (matrix1->r3c2 + matrix2->r3c2) + (matrix3->r3c2 + matrix4->r3c2) + matrix5->r3c2; + sum->r3c3 = (matrix1->r3c3 + matrix2->r3c3) + (matrix3->r3c3 + matrix4->r3c3) + matrix5->r3c3; +} + +static inline void dp_matrix3x3_add5( + const DPMatrix3x3* matrix1, + const DPMatrix3x3* matrix2, + const DPMatrix3x3* matrix3, + const DPMatrix3x3* matrix4, + const DPMatrix3x3* matrix5, + DPMatrix3x3* sum +) +{ + sum->r1c1 = (matrix1->r1c1 + matrix2->r1c1) + (matrix3->r1c1 + matrix4->r1c1) + matrix5->r1c1; + sum->r1c2 = (matrix1->r1c2 + matrix2->r1c2) + (matrix3->r1c2 + matrix4->r1c2) + matrix5->r1c2; + sum->r1c3 = (matrix1->r1c3 + matrix2->r1c3) + (matrix3->r1c3 + matrix4->r1c3) + matrix5->r1c3; + + sum->r2c1 = (matrix1->r2c1 + matrix2->r2c1) + (matrix3->r2c1 + matrix4->r2c1) + matrix5->r2c1; + sum->r2c2 = (matrix1->r2c2 + matrix2->r2c2) + (matrix3->r2c2 + matrix4->r2c2) + matrix5->r2c2; + sum->r2c3 = (matrix1->r2c3 + matrix2->r2c3) + (matrix3->r2c3 + matrix4->r2c3) + matrix5->r2c3; + + sum->r3c1 = (matrix1->r3c1 + matrix2->r3c1) + (matrix3->r3c1 + matrix4->r3c1) + matrix5->r3c1; + sum->r3c2 = (matrix1->r3c2 + matrix2->r3c2) + (matrix3->r3c2 + matrix4->r3c2) + matrix5->r3c2; + sum->r3c3 = (matrix1->r3c3 + matrix2->r3c3) + (matrix3->r3c3 + matrix4->r3c3) + matrix5->r3c3; +} + +// ================ Subtraction ================= // + +static inline void sp_matrix3x3_subtract(const SPMatrix3x3* minuend, const SPMatrix3x3* subtrahend, SPMatrix3x3* difference) +{ + difference->r1c1 = minuend->r1c1 - subtrahend->r1c1; + difference->r1c2 = minuend->r1c2 - subtrahend->r1c2; + difference->r1c3 = minuend->r1c3 - subtrahend->r1c3; + + difference->r2c1 = minuend->r2c1 - subtrahend->r2c1; + difference->r2c2 = minuend->r2c2 - subtrahend->r2c2; + difference->r2c3 = minuend->r2c3 - subtrahend->r2c3; + + difference->r3c1 = minuend->r3c1 - subtrahend->r3c1; + difference->r3c2 = minuend->r3c2 - subtrahend->r3c2; + difference->r3c3 = minuend->r3c3 - subtrahend->r3c3; +} + +static inline void dp_matrix3x3_subtract(const DPMatrix3x3* minuend, const DPMatrix3x3* subtrahend, DPMatrix3x3* difference) +{ + difference->r1c1 = minuend->r1c1 - subtrahend->r1c1; + difference->r1c2 = minuend->r1c2 - subtrahend->r1c2; + difference->r1c3 = minuend->r1c3 - subtrahend->r1c3; + + difference->r2c1 = minuend->r2c1 - subtrahend->r2c1; + difference->r2c2 = minuend->r2c2 - subtrahend->r2c2; + difference->r2c3 = minuend->r2c3 - subtrahend->r2c3; + + difference->r3c1 = minuend->r3c1 - subtrahend->r3c1; + difference->r3c2 = minuend->r3c2 - subtrahend->r3c2; + difference->r3c3 = minuend->r3c3 - subtrahend->r3c3; +} + +// ============= Weighed Sum of two ============= // + +void sp_matrix3x3_get_weighted_sum2( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + SPMatrix3x3* sum +); + +void dp_matrix3x3_get_weighted_sum2( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + DPMatrix3x3* sum +); + +// ============ Weighed Sum of three ============ // + +void sp_matrix3x3_get_weighted_sum3( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + SPMatrix3x3* sum +); + +void dp_matrix3x3_get_weighted_sum3( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + DPMatrix3x3* sum +); + +// ============ Weighed Sum of four ============= // + +void sp_matrix3x3_get_weighted_sum4( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + const float weight4, const SPMatrix3x3* matrix4, + SPMatrix3x3* sum +); + +void dp_matrix3x3_get_weighted_sum4( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + const double weight4, const DPMatrix3x3* matrix4, + DPMatrix3x3* sum +); + +// ============ Weighed Sum of five ============= // + +void sp_matrix3x3_get_weighted_sum5( + const float weight1, const SPMatrix3x3* matrix1, + const float weight2, const SPMatrix3x3* matrix2, + const float weight3, const SPMatrix3x3* matrix3, + const float weight4, const SPMatrix3x3* matrix4, + const float weight5, const SPMatrix3x3* matrix5, + SPMatrix3x3* sum +); + +void dp_matrix3x3_get_weighted_sum5( + const double weight1, const DPMatrix3x3* matrix1, + const double weight2, const DPMatrix3x3* matrix2, + const double weight3, const DPMatrix3x3* matrix3, + const double weight4, const DPMatrix3x3* matrix4, + const double weight5, const DPMatrix3x3* matrix5, + DPMatrix3x3* sum +); + +// =============== Multiplication =============== // + +static inline void sp_matrix3x3_multiply(const SPMatrix3x3* multiplicand, const float multiplier, SPMatrix3x3* product) +{ + product->r1c1 = multiplicand->r1c1 * multiplier; + product->r1c2 = multiplicand->r1c2 * multiplier; + product->r1c3 = multiplicand->r1c3 * multiplier; + + product->r2c1 = multiplicand->r2c1 * multiplier; + product->r2c2 = multiplicand->r2c2 * multiplier; + product->r2c3 = multiplicand->r2c3 * multiplier; + + product->r3c1 = multiplicand->r3c1 * multiplier; + product->r3c2 = multiplicand->r3c2 * multiplier; + product->r3c3 = multiplicand->r3c3 * multiplier; +} + +static inline void dp_matrix3x3_multiply(const DPMatrix3x3* multiplicand, const double multiplier, DPMatrix3x3* product) +{ + product->r1c1 = multiplicand->r1c1 * multiplier; + product->r1c2 = multiplicand->r1c2 * multiplier; + product->r1c3 = multiplicand->r1c3 * multiplier; + + product->r2c1 = multiplicand->r2c1 * multiplier; + product->r2c2 = multiplicand->r2c2 * multiplier; + product->r2c3 = multiplicand->r2c3 * multiplier; + + product->r3c1 = multiplicand->r3c1 * multiplier; + product->r3c2 = multiplicand->r3c2 * multiplier; + product->r3c3 = multiplicand->r3c3 * multiplier; +} + +// ================== Division ================== // + +static inline void sp_matrix3x3_divide(const SPMatrix3x3* dividend, const float divisor, SPMatrix3x3* quotient) +{ + quotient->r1c1 = dividend->r1c1 / divisor; + quotient->r1c2 = dividend->r1c2 / divisor; + quotient->r1c3 = dividend->r1c3 / divisor; + + quotient->r2c1 = dividend->r2c1 / divisor; + quotient->r2c2 = dividend->r2c2 / divisor; + quotient->r2c3 = dividend->r2c3 / divisor; + + quotient->r3c1 = dividend->r3c1 / divisor; + quotient->r3c2 = dividend->r3c2 / divisor; + quotient->r3c3 = dividend->r3c3 / divisor; +} + +static inline void dp_matrix3x3_divide(const DPMatrix3x3* dividend, const double divisor, DPMatrix3x3* quotient) +{ + quotient->r1c1 = dividend->r1c1 / divisor; + quotient->r1c2 = dividend->r1c2 / divisor; + quotient->r1c3 = dividend->r1c3 / divisor; + + quotient->r2c1 = dividend->r2c1 / divisor; + quotient->r2c2 = dividend->r2c2 / divisor; + quotient->r2c3 = dividend->r2c3 / divisor; + + quotient->r3c1 = dividend->r3c1 / divisor; + quotient->r3c2 = dividend->r3c2 / divisor; + quotient->r3c3 = dividend->r3c3 / divisor; +} + +// ============ Left Vector Product ============= // + +static inline void sp_matrix3x3_left_product(const SPVector3* vector, const SPMatrix3x3* matrix, SPVector3* result) +{ + sp_vector3_set( + vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, + vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, + vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3, + result + ); +} + +static inline void dp_matrix3x3_left_product(const DPVector3* vector, const DPMatrix3x3* matrix, DPVector3* result) +{ + dp_vector3_set( + vector->x1 * matrix->r1c1 + vector->x2 * matrix->r2c1 + vector->x3 * matrix->r3c1, + vector->x1 * matrix->r1c2 + vector->x2 * matrix->r2c2 + vector->x3 * matrix->r3c2, + vector->x1 * matrix->r1c3 + vector->x2 * matrix->r2c3 + vector->x3 * matrix->r3c3, + result + ); +} + +// ============ Right Vector Product ============ // + +static inline void sp_matrix3x3_right_product(const SPMatrix3x3* matrix, const SPVector3* vector, SPVector3* result) +{ + sp_vector3_set( + matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, + matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, + matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3, + result + ); +} + +static inline void dp_matrix3x3_right_product(const DPMatrix3x3* matrix, const DPVector3* vector, DPVector3* result) +{ + dp_vector3_set( + matrix->r1c1 * vector->x1 + matrix->r1c2 * vector->x2 + matrix->r1c3 * vector->x3, + matrix->r2c1 * vector->x1 + matrix->r2c2 * vector->x2 + matrix->r2c3 * vector->x3, + matrix->r3c1 * vector->x1 + matrix->r3c2 * vector->x2 + matrix->r3c3 * vector->x3, + result + ); +} + +// =============== Matrix Product =============== // + +void sp_matrix3x3_matrix_product(const SPMatrix3x3* matrix1, const SPMatrix3x3* matrix2, SPMatrix3x3* result); + +void dp_matrix3x3_matrix_product(const DPMatrix3x3* matrix1, const DPMatrix3x3* matrix2, DPMatrix3x3* result); + +#endif diff --git a/src/quaternion.c b/src/quaternion.c new file mode 100644 index 0000000..1e2bd25 --- /dev/null +++ b/src/quaternion.c @@ -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); +} diff --git a/src/quaternion.h b/src/quaternion.h new file mode 100644 index 0000000..37b590f --- /dev/null +++ b/src/quaternion.h @@ -0,0 +1,199 @@ +#ifndef _GEOMETRY_QUATERNION_H_ +#define _GEOMETRY_QUATERNION_H_ + +#include + +#include "basis.h" +#include "matrix3x3.h" + +typedef struct { + float s0, x1, x2, x3; +} SPQuaternion; + +typedef struct { + double s0, x1, x2, x3; +} DPQuaternion; + +// ==================== Reset =================== // + +static inline void sp_quaternion_reset(SPQuaternion * quaternion) +{ + quaternion->s0 = 0.0f; + quaternion->x1 = 0.0f; + quaternion->x2 = 0.0f; + quaternion->x3 = 0.0f; +} + +static inline void dp_quaternion_reset(DPQuaternion * quaternion) +{ + quaternion->s0 = 0.0; + quaternion->x1 = 0.0; + quaternion->x2 = 0.0; + quaternion->x3 = 0.0; +} + +// ==================== Set ===================== // + +static inline void sp_quaternion_set(const float s0, const float x1, const float x2, const float x3, SPQuaternion * quaternion) +{ + quaternion->s0 = s0; + quaternion->x1 = x1; + quaternion->x2 = x2; + quaternion->x3 = x3; +} + +static inline void dp_quaternion_set(const double s0, const double x1, const double x2, const double x3, DPQuaternion * quaternion) +{ + quaternion->s0 = s0; + quaternion->x1 = x1; + quaternion->x2 = x2; + quaternion->x3 = x3; +} + +// ==================== Copy ==================== // + +static inline void sp_quaternion_copy(const SPQuaternion* from, SPQuaternion* to) +{ + to->s0 = from->s0; + to->x1 = from->x1; + to->x2 = from->x2; + to->x3 = from->x3; +} + +static inline void dp_quaternion_copy(const DPQuaternion* from, DPQuaternion* to) +{ + to->s0 = from->s0; + to->x1 = from->x1; + to->x2 = from->x2; + to->x3 = from->x3; +} + +// ============= Copy to twin type ============== // + +static inline void sp_quaternion_copy_to_double(const SPQuaternion* versor, DPQuaternion* result) +{ + result->s0 = (double)versor->s0; + result->x1 = (double)versor->x1; + result->x2 = (double)versor->x2; + result->x3 = (double)versor->x3; +} + +static inline void dp_quaternion_copy_to_single(const DPQuaternion* versor, SPQuaternion* result) +{ + result->s0 = (float)versor->s0; + result->x1 = (float)versor->x1; + result->x2 = (float)versor->x2; + result->x3 = (float)versor->x3; +} + +// ================= Inversion ================== // + +static inline void sp_quaternion_conjugate(SPQuaternion* versor) +{ + versor->x1 = -versor->x1; + versor->x2 = -versor->x2; + versor->x3 = -versor->x3; +} + +static inline void dp_quaternion_conjugate(DPQuaternion* versor) +{ + versor->x1 = -versor->x1; + versor->x2 = -versor->x2; + versor->x3 = -versor->x3; +} + +// ================ Get Inverted ================ // + +static inline void sp_quaternion_get_conjugate(const SPQuaternion* versor, SPQuaternion* result) +{ + result->s0 = versor->s0; + result->x1 = -versor->x1; + result->x2 = -versor->x2; + result->x3 = -versor->x3; +} + +static inline void dp_quaternion_get_conjugate(const DPQuaternion* versor, DPQuaternion* result) +{ + result->s0 = versor->s0; + result->x1 = -versor->x1; + result->x2 = -versor->x2; + result->x3 = -versor->x3; +} + +// ============ Make Rotation Matrix ============ // + +void sp_quaternion_make_matrix(const SPQuaternion* quaternion, SPMatrix3x3* matrix); + +void dp_quaternion_make_matrix(const DPQuaternion* quaternion, DPMatrix3x3* matrix); + +// ============ Make Reverse Matrix ============= // + +void sp_quaternion_make_reverse_matrix(const SPQuaternion* quaternion, SPMatrix3x3* matrix); + +void dp_quaternion_make_reverse_matrix(const DPQuaternion* quaternion, DPMatrix3x3* matrix); + +// ==================== Add ===================== // + +static inline void sp_quaternion_add(const SPQuaternion * quaternion1, const SPQuaternion * quaternion2, SPQuaternion * result) +{ + result->s0 = quaternion1->s0 + quaternion2->s0; + result->x1 = quaternion1->x1 + quaternion2->x1; + result->x2 = quaternion1->x2 + quaternion2->x2; + result->x3 = quaternion1->x3 + quaternion2->x3; +} + +static inline void dp_quaternion_add(const DPQuaternion * quaternion1, const DPQuaternion * quaternion2, DPQuaternion * result) +{ + result->s0 = quaternion1->s0 + quaternion2->s0; + result->x1 = quaternion1->x1 + quaternion2->x1; + result->x2 = quaternion1->x2 + quaternion2->x2; + result->x3 = quaternion1->x3 + quaternion2->x3; +} + +// ================== Subtract ================== // + +static inline void sp_quaternion_subtract(const SPQuaternion * minuend, const SPQuaternion * subtrahend, SPQuaternion * result) +{ + result->s0 = minuend->s0 - subtrahend->s0; + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; + result->x3 = minuend->x3 - subtrahend->x3; +} + +static inline void dp_quaternion_subtract(const DPQuaternion * minuend, const DPQuaternion * subtrahend, DPQuaternion * result) +{ + result->s0 = minuend->s0 - subtrahend->s0; + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; + result->x3 = minuend->x3 - subtrahend->x3; +} + +// ================ Combination ================= // + +static inline void sp_quaternion_multiply(const SPQuaternion* left, const SPQuaternion* right, SPQuaternion* result) +{ + 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); + + result->s0 = s0; + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + +static inline void dp_quaternion_multiply(const DPQuaternion* left, const DPQuaternion* right, DPQuaternion* result) +{ + 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); + + result->s0 = s0; + result->x1 = x1; + result->x2 = x2; + result->x3 = x3; +} + +#endif // _GEOMETRY_QUATERNION_H_ diff --git a/src/rotation3.c b/src/rotation3.c new file mode 100644 index 0000000..261c814 --- /dev/null +++ b/src/rotation3.c @@ -0,0 +1,5 @@ +#include "rotation3.h" + +const SPRotation3 SP_IDLE_ROTATION3 = { {0.0f, 0.0f, 0.0f}, 0.0f}; + +const DPRotation3 DP_IDLE_ROTATION3 = { {0.0, 0.0, 0.0}, 0.0}; diff --git a/src/rotation3.h b/src/rotation3.h new file mode 100644 index 0000000..1d3cafc --- /dev/null +++ b/src/rotation3.h @@ -0,0 +1,101 @@ +#ifndef _GEOMETRY_ROTATION3_H_ +#define _GEOMETRY_ROTATION3_H_ + +#include "basis.h" +#include "angle.h" +#include "vector3.h" + +typedef struct { + SPVector3 axis; + float radians; +} SPRotation3; + +typedef struct { + DPVector3 axis; + double radians; +} DPRotation3; + +extern const SPRotation3 SP_IDLE_ROTATION3; + +extern const DPRotation3 DP_IDLE_ROTATION3; + +// =================== Reset ==================== // + +static inline void sp_rotation_reset(SPRotation3* rotation) +{ + rotation->axis.x1 = 0.0f; + rotation->axis.x2 = 0.0f; + rotation->axis.x3 = 0.0f; + + rotation->radians = 0.0f; +} + +static inline void dp_rotation_reset(DPRotation3* rotation) +{ + rotation->axis.x1 = 0.0; + rotation->axis.x2 = 0.0; + rotation->axis.x3 = 0.0; + + rotation->radians = 0.0; +} + +// ==================== Make ==================== // + +static inline void sp_rotation_make(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, SPRotation3* rotation) +{ + rotation->axis.x1 = x1; + rotation->axis.x2 = x2; + rotation->axis.x3 = x3; + + if (sp_vector3_normalize(&rotation->axis)) { + rotation->radians = sp_convert_to_radians(angle, unit); + } + else { + rotation->radians = 0.0f; + } +} + + +static inline void dp_rotation_make(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, DPRotation3* rotation) +{ + rotation->axis.x1 = x1; + rotation->axis.x2 = x2; + rotation->axis.x3 = x3; + + if (dp_vector3_normalize(&rotation->axis)) { + rotation->radians = dp_convert_to_radians(angle, unit); + } + else { + rotation->radians = 0.0; + } +} + +static inline void sp_rotation_make_with_vector(const SPVector3* axis, const float angle, const angle_unit_t unit, SPRotation3* rotation) +{ + rotation->axis.x1 = axis->x1; + rotation->axis.x2 = axis->x2; + rotation->axis.x3 = axis->x3; + + if (sp_vector3_normalize(&rotation->axis)) { + rotation->radians = sp_convert_to_radians(angle, unit); + } + else { + rotation->radians = 0.0f; + } +} + +static inline void dp_rotation_make_with_vector(const DPVector3* axis, const double angle, const angle_unit_t unit, DPRotation3* rotation) +{ + rotation->axis.x1 = axis->x1; + rotation->axis.x2 = axis->x2; + rotation->axis.x3 = axis->x3; + + if (dp_vector3_normalize(&rotation->axis)) { + rotation->radians = dp_convert_to_radians(angle, unit); + } + else { + rotation->radians = 0.0; + } +} + +#endif diff --git a/src/vector2.c b/src/vector2.c new file mode 100644 index 0000000..3a53b70 --- /dev/null +++ b/src/vector2.c @@ -0,0 +1,73 @@ +#include "vector2.h" + +const SPVector2 SP_ZERO_VECTOR2 = { 0.0f, 0.0f }; +const SPVector2 SP_X1_UNIT_VECTOR2 = { 1.0f, 0.0f }; +const SPVector2 SP_X2_UNIT_VECTOR2 = { 0.0f, 1.0f }; + +const DPVector2 DP_ZERO_VECTOR2 = { 0.0, 0.0 }; +const DPVector2 DP_X1_UNIT_VECTOR2 = { 1.0, 0.0 }; +const DPVector2 DP_X2_UNIT_VECTOR2 = { 0.0, 1.0 }; + +// =================== Angle ==================== // + +float sp_vector2_angle(const SPVector2* vector1, const SPVector2* vector2, const angle_unit_t unit) +{ + if (vector1 == 0 || vector2 == 0) { + return 0.0f; + } + + const float square_module1 = sp_vector2_get_square_module(vector1); + + if (square_module1 <= SP_SQUARE_EPSYLON) { + return 0.0f; + } + + const float square_module2 = sp_vector2_get_square_module(vector2); + + if (square_module2 <= SP_SQUARE_EPSYLON) { + return 0.0f; + } + + const float cosine = sp_vector2_scalar(vector1, vector2) / sqrtf(square_module1 * square_module2); + + if (cosine >= 1.0f - SP_EPSYLON) { + return 0.0f; + } + + if (cosine <= -1.0f + SP_EPSYLON) { + return sp_get_half_circle(unit); + } + + return sp_convert_from_radians(acosf(cosine), unit); +} + +double dp_vector2_angle(const DPVector2* vector1, const DPVector2* vector2, const angle_unit_t unit) +{ + if (vector1 == 0 || vector2 == 0) { + return 0.0; + } + + const double square_module1 = dp_vector2_get_square_module(vector1); + + if (square_module1 <= DP_SQUARE_EPSYLON) { + return 0.0; + } + + const double square_module2 = dp_vector2_get_square_module(vector2); + + if (square_module2 <= DP_SQUARE_EPSYLON) { + return 0.0; + } + + const double cosine = dp_vector2_scalar(vector1, vector2) / sqrt(square_module1 * square_module2); + + if (cosine >= 1.0 - DP_EPSYLON) { + return 0.0; + } + + if (cosine <= -1.0 + DP_EPSYLON) { + return dp_get_half_circle(unit); + } + + return dp_convert_from_radians(acos(cosine), unit); +} diff --git a/src/vector2.h b/src/vector2.h new file mode 100644 index 0000000..e1c4a1c --- /dev/null +++ b/src/vector2.h @@ -0,0 +1,633 @@ +#ifndef _GEOMETRY_VECTOR2_H_ +#define _GEOMETRY_VECTOR2_H_ + +#include "basis.h" +#include "angle.h" + +#include + +typedef struct +{ + float x1, x2; +} SPVector2; + +typedef struct +{ + double x1, x2; +} DPVector2; + +extern const SPVector2 SP_ZERO_VECTOR2; +extern const SPVector2 SP_X1_UNIT_VECTOR2; +extern const SPVector2 SP_X2_UNIT_VECTOR2; + +extern const DPVector2 DP_ZERO_VECTOR2; +extern const DPVector2 DP_X1_UNIT_VECTOR2; +extern const DPVector2 DP_X2_UNIT_VECTOR2; + +// =================== Reset ==================== // + +static inline void sp_vector2_reset(SPVector2* vector) +{ + vector->x1 = 0.0f; + vector->x2 = 0.0f; +} + +static inline void dp_vector2_reset(DPVector2* vector) +{ + vector->x1 = 0.0; + vector->x2 = 0.0; +} + +// ==================== Copy ==================== // + +static inline void sp_vector2_copy(const SPVector2* from, SPVector2* to) +{ + to->x1 = from->x1; + to->x2 = from->x2; +} + +static inline void dp_vector2_copy(const DPVector2* from, DPVector2* to) +{ + to->x1 = from->x1; + to->x2 = from->x2; +} + +// ==================== Set ===================== // + +static inline void sp_vector2_set(const float x1, const float x2, SPVector2* to) +{ + to->x1 = x1; + to->x2 = x2; +} + +static inline void dp_vector2_set(const double x1, const double x2, DPVector2* to) +{ + to->x1 = x1; + to->x2 = x2; +} + +// =================== Module =================== // + +static inline float sp_vector2_get_square_module(const SPVector2* vector) +{ + return vector->x1 * vector->x1 + vector->x2 * vector->x2; +} + +static inline double dp_vector2_get_square_module(const DPVector2* vector) +{ + return vector->x1 * vector->x1 + vector->x2 * vector->x2; +} + +static inline float sp_vector2_get_module(const SPVector2* vector) +{ + return sqrtf(sp_vector2_get_square_module(vector)); +} + +static inline double dp_vector2_get_module(const DPVector2* vector) +{ + return sqrt(dp_vector2_get_square_module(vector)); +} + +// ================= Comparison ================= // + +static inline int sp_vector2_is_zero(const SPVector2* vector) +{ + return sp_vector2_get_square_module(vector) <= SP_SQUARE_EPSYLON; +} + +static inline int dp_vector2_is_zero(const DPVector2* vector) +{ + return dp_vector2_get_square_module(vector) <= DP_SQUARE_EPSYLON; +} + +static inline int sp_vector2_is_unit(const SPVector2* vector) +{ + const float square_module = sp_vector2_get_square_module(vector); + + return 1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON; +} + +static inline int dp_vector2_is_unit(const DPVector2* vector) +{ + const double square_module = dp_vector2_get_square_module(vector); + + return 1.0f - DP_TWO_EPSYLON <= square_module && square_module <= 1.0f + DP_TWO_EPSYLON; +} + +// ============= Copy to twin type ============== // + +static inline void sp_vector2_copy_to_double(const SPVector2* from, DPVector2* to) +{ + to->x1 = (double)from->x1; + to->x2 = (double)from->x2; +} + +static inline void dp_vector2_copy_to_single(const DPVector2* from, SPVector2* to) +{ + to->x1 = (float)from->x1; + to->x2 = (float)from->x2; +} + +// ================= Inversion ================== // + +static inline void sp_vector2_invert(SPVector2* vector) +{ + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; +} + +static inline void dp_vector2_invert(DPVector2* vector) +{ + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; +} + +// ================ Get Inverted ================ // + +static inline void sp_vector2_get_inverted(const SPVector2* vector, SPVector2* result) +{ + result->x1 = -vector->x1; + result->x2 = -vector->x2; +} + +static inline void dp_vector2_get_inverted(const DPVector2* vector, DPVector2* result) +{ + result->x1 = -vector->x1; + result->x2 = -vector->x2; +} + +// ==================== Add ===================== // + +static inline void sp_vector2_add(const SPVector2* vector1, const SPVector2* vector2, SPVector2* result) +{ + result->x1 = vector1->x1 + vector2->x1; + result->x2 = vector1->x2 + vector2->x2; +} + +static inline void dp_vector2_add(const DPVector2* vector1, const DPVector2* vector2, DPVector2* result) +{ + result->x1 = vector1->x1 + vector2->x1; + result->x2 = vector1->x2 + vector2->x2; +} + +// ==================== Add3 ==================== // + +static inline void sp_vector2_add3( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + SPVector2* result +) +{ + result->x1 = vector1->x1 + vector2->x1 + vector3->x1; + result->x2 = vector1->x2 + vector2->x2 + vector3->x2; +} + +static inline void dp_vector2_add3( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + DPVector2* result +) +{ + result->x1 = vector1->x1 + vector2->x1 + vector3->x1; + result->x2 = vector1->x2 + vector2->x2 + vector3->x2; +} + +// ==================== Add4 ==================== // + +static inline void sp_vector2_add4( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + const SPVector2* vector4, + SPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1); + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2); +} + +static inline void dp_vector2_add4( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + const DPVector2* vector4, + DPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1); + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2); +} + +// ==================== Add5 ==================== // + +static inline void sp_vector2_add5( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + const SPVector2* vector4, + const SPVector2* vector5, + SPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1; + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2; +} + +static inline void dp_vector2_add5( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + const DPVector2* vector4, + const DPVector2* vector5, + DPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1; + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2; +} + +// ================ Subtraction ================= // + +static inline void sp_vector2_subtract(const SPVector2* minuend, const SPVector2* subtrahend, SPVector2* result) +{ + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; +} + +static inline void dp_vector2_subtract(const DPVector2* minuend, const DPVector2* subtrahend, DPVector2* result) +{ + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; +} + +// ============= Weighed Sum of two ============= // + +static inline void sp_vector2_get_weighted_sum2( + const float weight1, const SPVector2* vector1, + const float weight2, const SPVector2* vector2, + SPVector2* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2; +} + +static inline void dp_vector2_get_weighted_sum2( + const double weight1, const DPVector2* vector1, + const double weight2, const DPVector2* vector2, + DPVector2* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2; +} + +// ============ Weighed Sum of three ============ // + +static inline void sp_vector2_get_weighted_sum3( + const float weight1, const SPVector2* vector1, + const float weight2, const SPVector2* vector2, + const float weight3, const SPVector2* vector3, + SPVector2* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2 + vector3->x1 * weight3; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2 + vector3->x2 * weight3; +} + +static inline void dp_vector2_get_weighted_sum3( + const double weight1, const DPVector2* vector1, + const double weight2, const DPVector2* vector2, + const double weight3, const DPVector2* vector3, + DPVector2* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2 + vector3->x1 * weight3; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2 + vector3->x2 * weight3; +} + +// ============ Weighed Sum of four ============= // + +static inline void sp_vector2_get_weighted_sum4( + const float weight1, const SPVector2* vector1, + const float weight2, const SPVector2* vector2, + const float weight3, const SPVector2* vector3, + const float weight4, const SPVector2* vector4, + SPVector2* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4); + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4); +} + +static inline void dp_vector2_get_weighted_sum4( + const double weight1, const DPVector2* vector1, + const double weight2, const DPVector2* vector2, + const double weight3, const DPVector2* vector3, + const double weight4, const DPVector2* vector4, + DPVector2* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4); + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4); +} + +// ============ Weighed Sum of five ============= // + +static inline void sp_vector2_get_weighted_sum5( + const float weight1, const SPVector2* vector1, + const float weight2, const SPVector2* vector2, + const float weight3, const SPVector2* vector3, + const float weight4, const SPVector2* vector4, + const float weight5, const SPVector2* vector5, + SPVector2* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4) + vector5->x1 * weight5; + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4) + vector5->x2 * weight5; +} + +static inline void dp_vector2_get_weighted_sum5( + const double weight1, const DPVector2* vector1, + const double weight2, const DPVector2* vector2, + const double weight3, const DPVector2* vector3, + const double weight4, const DPVector2* vector4, + const double weight5, const DPVector2* vector5, + DPVector2* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4) + vector5->x1 * weight5; + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4) + vector5->x2 * weight5; +} + +// =============== Multiplication =============== // + +static inline void sp_vector2_multiply(const SPVector2* multiplicand, const float multiplier, SPVector2* result) +{ + result->x1 = multiplicand->x1 * multiplier; + result->x2 = multiplicand->x2 * multiplier; +} + +static inline void dp_vector2_multiply(const DPVector2* multiplicand, const double multiplier, DPVector2* result) +{ + result->x1 = multiplicand->x1 * multiplier; + result->x2 = multiplicand->x2 * multiplier; +} + +// ================== Division ================== // + +static inline void sp_vector2_divide(const SPVector2* dividend, const float divisor, SPVector2* result) +{ + result->x1 = dividend->x1 / divisor; + result->x2 = dividend->x2 / divisor; +} + +static inline void dp_vector2_divide(const DPVector2* dividend, const double divisor, DPVector2* result) +{ + result->x1 = dividend->x1 / divisor; + result->x2 = dividend->x2 / divisor; +} + +// ================== Average2 ================== // + +static inline void sp_vector2_get_mean2(const SPVector2* vector1, const SPVector2* vector2, SPVector2* result) +{ + result->x1 = (vector1->x1 + vector2->x1) * 0.5f; + result->x2 = (vector1->x2 + vector2->x2) * 0.5f; +} + +static inline void dp_vector2_get_mean2(const DPVector2* vector1, const DPVector2* vector2, DPVector2* result) +{ + result->x1 = (vector1->x1 + vector2->x1) * 0.5; + result->x2 = (vector1->x2 + vector2->x2) * 0.5; +} + +// ================== Average3 ================== // + +static inline void sp_vector2_get_mean3( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + SPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * SP_ONE_THIRD; + result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * SP_ONE_THIRD; +} + +static inline void dp_vector2_get_mean3( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + DPVector2* result +) +{ + result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * DP_ONE_THIRD; + result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * DP_ONE_THIRD; +} + +// ================== Average4 ================== // + +static inline void sp_vector2_get_mean4( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + const SPVector2* vector4, + SPVector2* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1)) * 0.25f; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2)) * 0.25f; +} + +static inline void dp_vector2_get_mean4( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + const DPVector2* vector4, + DPVector2* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1)) * 0.25; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2)) * 0.25; +} + +// ================== Average5 ================== // + +static inline void sp_vector2_get_mean5( + const SPVector2* vector1, + const SPVector2* vector2, + const SPVector2* vector3, + const SPVector2* vector4, + const SPVector2* vector5, + SPVector2* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1) * 0.2f; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2) * 0.2f; +} + +static inline void dp_vector2_get_mean5( + const DPVector2* vector1, + const DPVector2* vector2, + const DPVector2* vector3, + const DPVector2* vector4, + const DPVector2* vector5, + DPVector2* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1) * 0.2; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2) * 0.2; +} + +// =============== Scalar Product =============== // + +static inline float sp_vector2_scalar(const SPVector2* vector1, const SPVector2* vector2) +{ + return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; +} + +static inline double dp_vector2_scalar(const DPVector2* vector1, const DPVector2* vector2) +{ + return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2; +} + +// =============== Cross Product ================ // + +static inline float sp_vector2_cross(const SPVector2* vector1, const SPVector2* vector2) +{ + return vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; +} + +static inline double dp_vector2_cross(const DPVector2* vector1, const DPVector2* vector2, DPVector2* result) +{ + return vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1; +} + +// =============== Normalization ================ // + +static inline int sp_vector2_normalize(SPVector2* vector) +{ + const float square_module = sp_vector2_get_square_module(vector); + + if (1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON) { + return 1; + } + + if (square_module <= SP_SQUARE_EPSYLON) { + sp_vector2_reset(vector); + return 0; + } + + sp_vector2_divide(vector, sqrtf(square_module), vector); + return 1; +} + +static inline int dp_vector2_normalize(DPVector2* vector) +{ + const double square_module = dp_vector2_get_square_module(vector); + + if (1.0 - DP_TWO_EPSYLON <= square_module && square_module <= 1.0 + DP_TWO_EPSYLON) { + return 1; + } + + if (square_module <= DP_SQUARE_EPSYLON) { + dp_vector2_reset(vector); + return 0; + } + + dp_vector2_divide(vector, sqrt(square_module), vector); + return 1; +} + +// =============== Get Normalized =============== // + +static inline int sp_vector2_get_normalized(const SPVector2* vector, SPVector2* result) +{ + sp_vector2_copy(vector, result); + return sp_vector2_normalize(result); +} + +static inline int dp_vector2_get_normalized(const DPVector2* vector, DPVector2* result) +{ + dp_vector2_copy(vector, result); + return dp_vector2_normalize(result); +} + +// =================== Angle ==================== // + +float sp_vector2_angle(const SPVector2* vector1, const SPVector2* vector2, const angle_unit_t unit); + +double dp_vector2_angle(const DPVector2* vector1, const DPVector2* vector2, const angle_unit_t unit); + +// =============== Square Distance ============== // + +static inline float sp_vector2_get_square_distance(const SPVector2* vector1, const SPVector2* vector2) +{ + const float dx1 = (vector1->x1 - vector2->x1); + const float dx2 = (vector1->x2 - vector2->x2); + + return dx1 * dx1 + dx2 * dx2; +} + +static inline double dp_vector2_get_square_distance(const DPVector2* vector1, const DPVector2* vector2) +{ + const double dx1 = (vector1->x1 - vector2->x1); + const double dx2 = (vector1->x2 - vector2->x2); + + return dx1 * dx1 + dx2 * dx2; +} + +// ================== Distance ================== // + +static inline float sp_vector2_get_distance(const SPVector2* vector1, const SPVector2* vector2) +{ + return sqrtf(sp_vector2_get_square_distance(vector1, vector2)); +} + +static inline double dp_vector2_get_distance(const DPVector2* vector1, const DPVector2* vector2) +{ + return sqrt(dp_vector2_get_square_distance(vector1, vector2)); +} + +// ================== Are Equal ================= // + +static inline int sp_vector2_are_equal(const SPVector2* vector1, const SPVector2* vector2) +{ + const float square_module1 = sp_vector2_get_square_module(vector1); + const float square_module2 = sp_vector2_get_square_module(vector2); + const float square_module3 = sp_vector2_get_square_distance(vector1, vector2); + + // 2.0f means dimension amount + if (square_module1 < SP_EPSYLON_EFFECTIVENESS_LIMIT || square_module2 < SP_EPSYLON_EFFECTIVENESS_LIMIT) { + return square_module3 < (2.0f * SP_SQUARE_EPSYLON); + } + + if (square_module1 <= square_module2) { + return square_module3 <= (2.0f * SP_SQUARE_EPSYLON) * square_module2; + } + + return square_module3 <= (2.0f * SP_SQUARE_EPSYLON) * square_module1; +} + +static inline int dp_vector2_are_equal(const DPVector2* vector1, const DPVector2* vector2) +{ + const double square_module1 = dp_vector2_get_square_module(vector1); + const double square_module2 = dp_vector2_get_square_module(vector2); + const double square_module3 = dp_vector2_get_square_distance(vector1, vector2); + + // 2.0 means dimension amount + if (square_module1 < DP_EPSYLON_EFFECTIVENESS_LIMIT || square_module2 < DP_EPSYLON_EFFECTIVENESS_LIMIT) { + return square_module3 < (2.0 * DP_SQUARE_EPSYLON); + } + + if (square_module1 <= square_module2) { + return square_module3 <= (2.0 * DP_SQUARE_EPSYLON) * square_module2; + } + + return square_module3 <= (2.0 * DP_SQUARE_EPSYLON) * square_module1; +} + +#endif diff --git a/src/vector3.c b/src/vector3.c new file mode 100644 index 0000000..ffd3c49 --- /dev/null +++ b/src/vector3.c @@ -0,0 +1,75 @@ +#include "vector3.h" + +const SPVector3 SP_ZERO_VECTOR3 = { 0.0f, 0.0f, 0.0f }; +const SPVector3 SP_X1_UNIT_VECTOR3 = { 1.0f, 0.0f, 0.0f }; +const SPVector3 SP_X2_UNIT_VECTOR3 = { 0.0f, 1.0f, 0.0f }; +const SPVector3 SP_X3_UNIT_VECTOR3 = { 0.0f, 0.0f, 1.0f }; + +const DPVector3 DP_ZERO_VECTOR3 = { 0.0, 0.0, 0.0 }; +const DPVector3 DP_X1_UNIT_VECTOR3 = { 1.0, 0.0, 0.0 }; +const DPVector3 DP_X2_UNIT_VECTOR3 = { 0.0, 1.0, 0.0 }; +const DPVector3 DP_X3_UNIT_VECTOR3 = { 0.0, 0.0, 1.0 }; + +// =================== Angle ==================== // + +float sp_vector3_angle(const SPVector3* vector1, const SPVector3* vector2, const angle_unit_t unit) +{ + if (vector1 == 0 || vector2 == 0) { + return 0.0f; + } + + const float square_module1 = sp_vector3_get_square_module(vector1); + + if (square_module1 <= SP_SQUARE_EPSYLON) { + return 0.0f; + } + + const float square_module2 = sp_vector3_get_square_module(vector2); + + if (square_module2 <= SP_SQUARE_EPSYLON) { + return 0.0f; + } + + const float cosine = sp_vector3_scalar(vector1, vector2) / sqrtf(square_module1 * square_module2); + + if (cosine >= 1.0f - SP_EPSYLON) { + return 0.0f; + } + + if (cosine <= -1.0f + SP_EPSYLON) { + return sp_get_half_circle(unit); + } + + return sp_convert_from_radians(acosf(cosine), unit); +} + +double dp_vector3_angle(const DPVector3* vector1, const DPVector3* vector2, const angle_unit_t unit) +{ + if (vector1 == 0 || vector2 == 0) { + return 0.0; + } + + const double square_module1 = dp_vector3_get_square_module(vector1); + + if (square_module1 <= DP_SQUARE_EPSYLON) { + return 0.0; + } + + const double square_module2 = dp_vector3_get_square_module(vector2); + + if (square_module2 <= DP_SQUARE_EPSYLON) { + return 0.0; + } + + const double cosine = dp_vector3_scalar(vector1, vector2) / sqrt(square_module1 * square_module2); + + if (cosine >= 1.0 - DP_EPSYLON) { + return 0.0; + } + + if (cosine <= -1.0 + DP_EPSYLON) { + return dp_get_half_circle(unit); + } + + return dp_convert_from_radians(acos(cosine), unit); +} diff --git a/src/vector3.h b/src/vector3.h new file mode 100644 index 0000000..c907832 --- /dev/null +++ b/src/vector3.h @@ -0,0 +1,729 @@ +#ifndef _GEOMETRY_VECTOR3_H_ +#define _GEOMETRY_VECTOR3_H_ + +#include "basis.h" +#include "angle.h" + +#include + +// ================== Vector3 =================== // + +typedef struct +{ + float x1, x2, x3; +} SPVector3; + +typedef struct +{ + double x1, x2, x3; +} DPVector3; + +extern const SPVector3 SP_ZERO_VECTOR3; +extern const SPVector3 SP_X1_UNIT_VECTOR3; +extern const SPVector3 SP_X2_UNIT_VECTOR3; +extern const SPVector3 SP_X3_UNIT_VECTOR3; + +extern const DPVector3 DP_ZERO_VECTOR3; +extern const DPVector3 DP_X1_UNIT_VECTOR3; +extern const DPVector3 DP_X2_UNIT_VECTOR3; +extern const DPVector3 DP_X3_UNIT_VECTOR3; + +// =================== Reset ==================== // + +static inline void sp_vector3_reset(SPVector3* vector) +{ + vector->x1 = 0.0f; + vector->x2 = 0.0f; + vector->x3 = 0.0f; +} + +static inline void dp_vector3_reset(DPVector3* vector) +{ + vector->x1 = 0.0; + vector->x2 = 0.0; + vector->x3 = 0.0; +} + +// ==================== Copy ==================== // + +static inline void sp_vector3_copy(const SPVector3* from, SPVector3* to) +{ + to->x1 = from->x1; + to->x2 = from->x2; + to->x3 = from->x3; +} + +static inline void dp_vector3_copy(const DPVector3* from, DPVector3* to) +{ + to->x1 = from->x1; + to->x2 = from->x2; + to->x3 = from->x3; +} + +// ==================== Set ===================== // + +static inline void sp_vector3_set(const float x1, const float x2, const float x3, SPVector3* to) +{ + to->x1 = x1; + to->x2 = x2; + to->x3 = x3; +} + +static inline void dp_vector3_set(const double x1, const double x2, const double x3, DPVector3* to) +{ + to->x1 = x1; + to->x2 = x2; + to->x3 = x3; +} + +// =================== Module =================== // + +static inline float sp_vector3_get_square_module(const SPVector3* vector) +{ + return vector->x1 * vector->x1 + vector->x2 * vector->x2 + vector->x3 * vector->x3; +} + +static inline double dp_vector3_get_square_module(const DPVector3* vector) +{ + return vector->x1 * vector->x1 + vector->x2 * vector->x2 + vector->x3 * vector->x3; +} + +static inline float sp_vector3_get_module(const SPVector3* vector) +{ + return sqrtf(sp_vector3_get_square_module(vector)); +} + +static inline double dp_vector3_get_module(const DPVector3* vector) +{ + return sqrt(dp_vector3_get_square_module(vector)); +} + +// ================= Comparison ================= // + +static inline int sp_vector3_is_zero(const SPVector3* vector) +{ + return sp_vector3_get_square_module(vector) <= SP_SQUARE_EPSYLON; +} + +static inline int dp_vector3_is_zero(const DPVector3* vector) +{ + return dp_vector3_get_square_module(vector) <= DP_SQUARE_EPSYLON; +} + +static inline int sp_vector3_is_unit(const SPVector3* vector) +{ + const float square_module = sp_vector3_get_square_module(vector); + + return 1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON; +} + +static inline int dp_vector3_is_unit(const DPVector3* vector) +{ + const double square_module = dp_vector3_get_square_module(vector); + + return 1.0f - DP_TWO_EPSYLON <= square_module && square_module <= 1.0f + DP_TWO_EPSYLON; +} + +// ============= Copy to twin type ============== // + +static inline void sp_vector3_copy_to_double(const SPVector3* from, DPVector3* to) +{ + to->x1 = (double)from->x1; + to->x2 = (double)from->x2; + to->x3 = (double)from->x3; +} + +static inline void dp_vector3_copy_to_single(const DPVector3* from, SPVector3* to) +{ + to->x1 = (float)from->x1; + to->x2 = (float)from->x2; + to->x3 = (float)from->x3; +} + +// ================= Inversion ================== // + +static inline void sp_vector3_invert(SPVector3* vector) +{ + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; + vector->x3 = -vector->x3; +} + +static inline void dp_vector3_invert(DPVector3* vector) +{ + vector->x1 = -vector->x1; + vector->x2 = -vector->x2; + vector->x3 = -vector->x3; +} + +// ================ Get Inverted ================ // + +static inline void sp_vector3_get_inverted(const SPVector3* vector, SPVector3* result) +{ + result->x1 = -vector->x1; + result->x2 = -vector->x2; + result->x3 = -vector->x3; +} + +static inline void dp_vector3_get_inverted(const DPVector3* vector, DPVector3* result) +{ + result->x1 = -vector->x1; + result->x2 = -vector->x2; + result->x3 = -vector->x3; +} + +// ==================== Add ===================== // + +static inline void sp_vector3_add(const SPVector3* vector1, const SPVector3* vector2, SPVector3* result) +{ + result->x1 = vector1->x1 + vector2->x1; + result->x2 = vector1->x2 + vector2->x2; + result->x3 = vector1->x3 + vector2->x3; +} + +static inline void dp_vector3_add(const DPVector3* vector1, const DPVector3* vector2, DPVector3* result) +{ + result->x1 = vector1->x1 + vector2->x1; + result->x2 = vector1->x2 + vector2->x2; + result->x3 = vector1->x3 + vector2->x3; +} + +// ==================== Add3 ==================== // + +static inline void sp_vector3_add3( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + SPVector3* result +) +{ + result->x1 = vector1->x1 + vector2->x1 + vector3->x1; + result->x2 = vector1->x2 + vector2->x2 + vector3->x2; + result->x3 = vector1->x3 + vector2->x3 + vector3->x3; +} + +static inline void dp_vector3_add3( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + DPVector3* result +) +{ + result->x1 = vector1->x1 + vector2->x1 + vector3->x1; + result->x2 = vector1->x2 + vector2->x2 + vector3->x2; + result->x3 = vector1->x3 + vector2->x3 + vector3->x3; +} + +// ==================== Add4 ==================== // + +static inline void sp_vector3_add4( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + const SPVector3* vector4, + SPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1); + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2); + result->x3 = (vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3); +} + +static inline void dp_vector3_add4( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + const DPVector3* vector4, + DPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1); + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2); + result->x3 = (vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3); +} + +// ==================== Add5 ==================== // + +static inline void sp_vector3_add5( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + const SPVector3* vector4, + const SPVector3* vector5, + SPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1; + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2; + result->x3 = (vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3) + vector5->x3; +} + +static inline void dp_vector3_add5( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + const DPVector3* vector4, + const DPVector3* vector5, + DPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1; + result->x2 = (vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2; + result->x3 = (vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3) + vector5->x3; +} + +// ================ Subtraction ================= // + +static inline void sp_vector3_subtract(const SPVector3* minuend, const SPVector3* subtrahend, SPVector3* result) +{ + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; + result->x3 = minuend->x3 - subtrahend->x3; +} + +static inline void dp_vector3_subtract(const DPVector3* minuend, const DPVector3* subtrahend, DPVector3* result) +{ + result->x1 = minuend->x1 - subtrahend->x1; + result->x2 = minuend->x2 - subtrahend->x2; + result->x3 = minuend->x3 - subtrahend->x3; +} + +// ============= Weighed Sum of two ============= // + +static inline void sp_vector3_get_weighted_sum2( + const float weight1, const SPVector3* vector1, + const float weight2, const SPVector3* vector2, + SPVector3* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2; + result->x3 = vector1->x3 * weight1 + vector2->x3 * weight2; +} + +static inline void dp_vector3_get_weighted_sum2( + const double weight1, const DPVector3* vector1, + const double weight2, const DPVector3* vector2, + DPVector3* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2; + result->x3 = vector1->x3 * weight1 + vector2->x3 * weight2; +} + +// ============ Weighed Sum of three ============ // + +static inline void sp_vector3_get_weighted_sum3( + const float weight1, const SPVector3* vector1, + const float weight2, const SPVector3* vector2, + const float weight3, const SPVector3* vector3, + SPVector3* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2 + vector3->x1 * weight3; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2 + vector3->x2 * weight3; + result->x3 = vector1->x3 * weight1 + vector2->x3 * weight2 + vector3->x3 * weight3; +} + +static inline void dp_vector3_get_weighted_sum3( + const double weight1, const DPVector3* vector1, + const double weight2, const DPVector3* vector2, + const double weight3, const DPVector3* vector3, + DPVector3* result +) +{ + result->x1 = vector1->x1 * weight1 + vector2->x1 * weight2 + vector3->x1 * weight3; + result->x2 = vector1->x2 * weight1 + vector2->x2 * weight2 + vector3->x2 * weight3; + result->x3 = vector1->x3 * weight1 + vector2->x3 * weight2 + vector3->x3 * weight3; +} + +// ============ Weighed Sum of four ============= // + +static inline void sp_vector3_get_weighted_sum4( + const float weight1, const SPVector3* vector1, + const float weight2, const SPVector3* vector2, + const float weight3, const SPVector3* vector3, + const float weight4, const SPVector3* vector4, + SPVector3* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4); + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4); + result->x3 = (vector1->x3 * weight1 + vector2->x3 * weight2) + (vector3->x3 * weight3 + vector4->x3 * weight4); +} + +static inline void dp_vector3_get_weighted_sum4( + const double weight1, const DPVector3* vector1, + const double weight2, const DPVector3* vector2, + const double weight3, const DPVector3* vector3, + const double weight4, const DPVector3* vector4, + DPVector3* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4); + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4); + result->x3 = (vector1->x3 * weight1 + vector2->x3 * weight2) + (vector3->x3 * weight3 + vector4->x3 * weight4); +} + +// ============ Weighed Sum of five ============= // + +static inline void sp_vector3_get_weighted_sum5( + const float weight1, const SPVector3* vector1, + const float weight2, const SPVector3* vector2, + const float weight3, const SPVector3* vector3, + const float weight4, const SPVector3* vector4, + const float weight5, const SPVector3* vector5, + SPVector3* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4) + vector5->x1 * weight5; + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4) + vector5->x2 * weight5; + result->x3 = (vector1->x3 * weight1 + vector2->x3 * weight2) + (vector3->x3 * weight3 + vector4->x3 * weight4) + vector5->x3 * weight5; +} + +static inline void dp_vector3_get_weighted_sum5( + const double weight1, const DPVector3* vector1, + const double weight2, const DPVector3* vector2, + const double weight3, const DPVector3* vector3, + const double weight4, const DPVector3* vector4, + const double weight5, const DPVector3* vector5, + DPVector3* result +) +{ + result->x1 = (vector1->x1 * weight1 + vector2->x1 * weight2) + (vector3->x1 * weight3 + vector4->x1 * weight4) + vector5->x1 * weight5; + result->x2 = (vector1->x2 * weight1 + vector2->x2 * weight2) + (vector3->x2 * weight3 + vector4->x2 * weight4) + vector5->x2 * weight5; + result->x3 = (vector1->x3 * weight1 + vector2->x3 * weight2) + (vector3->x3 * weight3 + vector4->x3 * weight4) + vector5->x3 * weight5; +} + +// =============== Multiplication =============== // + +static inline void sp_vector3_multiply(const SPVector3* multiplicand, const float multiplier, SPVector3* result) +{ + result->x1 = multiplicand->x1 * multiplier; + result->x2 = multiplicand->x2 * multiplier; + result->x3 = multiplicand->x3 * multiplier; +} + +static inline void dp_vector3_multiply(const DPVector3* multiplicand, const double multiplier, DPVector3* result) +{ + result->x1 = multiplicand->x1 * multiplier; + result->x2 = multiplicand->x2 * multiplier; + result->x3 = multiplicand->x3 * multiplier; +} + +// ================== Division ================== // + +static inline void sp_vector3_divide(const SPVector3* dividend, const float divisor, SPVector3* result) +{ + result->x1 = dividend->x1 / divisor; + result->x2 = dividend->x2 / divisor; + result->x3 = dividend->x3 / divisor; +} + +static inline void dp_vector3_divide(const DPVector3* dividend, const double divisor, DPVector3* result) +{ + result->x1 = dividend->x1 / divisor; + result->x2 = dividend->x2 / divisor; + result->x3 = dividend->x3 / divisor; +} + +// ================== Average2 ================== // + +static inline void sp_vector3_get_mean2(const SPVector3* vector1, const SPVector3* vector2, SPVector3* result) +{ + result->x1 = (vector1->x1 + vector2->x1) * 0.5f; + result->x2 = (vector1->x2 + vector2->x2) * 0.5f; + result->x3 = (vector1->x3 + vector2->x3) * 0.5f; +} + +static inline void dp_vector3_get_mean2(const DPVector3* vector1, const DPVector3* vector2, DPVector3* result) +{ + result->x1 = (vector1->x1 + vector2->x1) * 0.5; + result->x2 = (vector1->x2 + vector2->x2) * 0.5; + result->x3 = (vector1->x3 + vector2->x3) * 0.5; +} + +// ================== Average3 ================== // + +static inline void sp_vector3_get_mean3( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + SPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * SP_ONE_THIRD; + result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * SP_ONE_THIRD; + result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * SP_ONE_THIRD; +} + +static inline void dp_vector3_get_mean3( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + DPVector3* result +) +{ + result->x1 = (vector1->x1 + vector2->x1 + vector3->x1) * DP_ONE_THIRD; + result->x2 = (vector1->x2 + vector2->x2 + vector3->x2) * DP_ONE_THIRD; + result->x3 = (vector1->x3 + vector2->x3 + vector3->x3) * DP_ONE_THIRD; +} + +// ================== Average4 ================== // + +static inline void sp_vector3_get_mean4( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + const SPVector3* vector4, + SPVector3* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1)) * 0.25f; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2)) * 0.25f; + result->x3 = ((vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3)) * 0.25f; +} + +static inline void dp_vector3_get_mean4( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + const DPVector3* vector4, + DPVector3* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1)) * 0.25; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2)) * 0.25; + result->x3 = ((vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3)) * 0.25; +} + +// ================== Average5 ================== // + +static inline void sp_vector3_get_mean5( + const SPVector3* vector1, + const SPVector3* vector2, + const SPVector3* vector3, + const SPVector3* vector4, + const SPVector3* vector5, + SPVector3* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1) * 0.2f; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2) * 0.2f; + result->x3 = ((vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3) + vector5->x3) * 0.2f; +} + +static inline void dp_vector3_get_mean5( + const DPVector3* vector1, + const DPVector3* vector2, + const DPVector3* vector3, + const DPVector3* vector4, + const DPVector3* vector5, + DPVector3* result +) +{ + result->x1 = ((vector1->x1 + vector2->x1) + (vector3->x1 + vector4->x1) + vector5->x1) * 0.2; + result->x2 = ((vector1->x2 + vector2->x2) + (vector3->x2 + vector4->x2) + vector5->x2) * 0.2; + result->x3 = ((vector1->x3 + vector2->x3) + (vector3->x3 + vector4->x3) + vector5->x3) * 0.2; +} + +// =============== Scalar Product =============== // + +static inline float sp_vector3_scalar(const SPVector3* vector1, const SPVector3* vector2) +{ + return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; +} + +static inline double dp_vector3_scalar(const DPVector3* vector1, const DPVector3* vector2) +{ + return vector1->x1 * vector2->x1 + vector1->x2 * vector2->x2 + vector1->x3 * vector2->x3; +} + +// =============== Triple Product =============== // + +static inline float sp_vector3_triple(const SPVector3* vector1, const SPVector3* vector2, const SPVector3* vector3) +{ + return vector1->x1 * (vector2->x2 * vector3->x3 - vector2->x3 * vector3->x2) + + vector1->x2 * (vector2->x3 * vector3->x1 - vector2->x1 * vector3->x3) + + vector1->x3 * (vector2->x1 * vector3->x2 - vector2->x2 * vector3->x1); +} + +static inline double dp_vector3_triple(const DPVector3* vector1, const DPVector3* vector2, const DPVector3* vector3) +{ + return vector1->x1 * (vector2->x2 * vector3->x3 - vector2->x3 * vector3->x2) + + vector1->x2 * (vector2->x3 * vector3->x1 - vector2->x1 * vector3->x3) + + vector1->x3 * (vector2->x1 * vector3->x2 - vector2->x2 * vector3->x1); +} + +// =============== Cross Product ================ // + +static inline void sp_vector3_cross(const SPVector3* vector1, const SPVector3* vector2, SPVector3* result) +{ + sp_vector3_set( + vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, + vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, + vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1, + result + ); +} + +static inline void dp_vector3_cross(const DPVector3* vector1, const DPVector3* vector2, DPVector3* result) +{ + dp_vector3_set( + vector1->x2 * vector2->x3 - vector1->x3 * vector2->x2, + vector1->x3 * vector2->x1 - vector1->x1 * vector2->x3, + vector1->x1 * vector2->x2 - vector1->x2 * vector2->x1, + result + ); +} + +// ============ Double Cross Product ============ // + +static inline void sp_vector3_double_cross(const SPVector3* vector1, const SPVector3* vector2, const SPVector3* vector3, SPVector3* result) +{ + const float ac = sp_vector3_scalar(vector1, vector3); + const float ab = sp_vector3_scalar(vector1, vector2); + + result->x1 = vector2->x1 * ac - vector3->x1 * ab; + result->x2 = vector2->x2 * ac - vector3->x2 * ab; + result->x3 = vector2->x3 * ac - vector3->x3 * ab; +} + +static inline void dp_vector3_double_cross(const DPVector3* vector1, const DPVector3* vector2, const DPVector3* vector3, DPVector3* result) +{ + const double ac = dp_vector3_scalar(vector1, vector3); + const double ab = dp_vector3_scalar(vector1, vector2); + + result->x1 = vector2->x1 * ac - vector3->x1 * ab; + result->x2 = vector2->x2 * ac - vector3->x2 * ab; + result->x3 = vector2->x3 * ac - vector3->x3 * ab; +} + +// =============== Normalization ================ // + +static inline int sp_vector3_normalize(SPVector3* vector) +{ + const float square_module = sp_vector3_get_square_module(vector); + + if (1.0f - SP_TWO_EPSYLON <= square_module && square_module <= 1.0f + SP_TWO_EPSYLON) { + return 1; + } + + if (square_module <= SP_SQUARE_EPSYLON) { + sp_vector3_reset(vector); + return 0; + } + + sp_vector3_divide(vector, sqrtf(square_module), vector); + return 1; +} + +static inline int dp_vector3_normalize(DPVector3* vector) +{ + const double square_module = dp_vector3_get_square_module(vector); + + if (1.0 - DP_TWO_EPSYLON <= square_module && square_module <= 1.0 + DP_TWO_EPSYLON) { + return 1; + } + + if (square_module <= DP_SQUARE_EPSYLON) { + dp_vector3_reset(vector); + return 0; + } + + dp_vector3_divide(vector, sqrt(square_module), vector); + return 1; +} + +// =============== Get Normalized =============== // + +static inline int sp_vector3_get_normalized(const SPVector3* vector, SPVector3* result) +{ + sp_vector3_copy(vector, result); + return sp_vector3_normalize(result); +} + +static inline int dp_vector3_get_normalized(const DPVector3* vector, DPVector3* result) +{ + dp_vector3_copy(vector, result); + return dp_vector3_normalize(result); +} + +// =================== Angle ==================== // + +float sp_vector3_angle(const SPVector3* vector1, const SPVector3* vector2, const angle_unit_t unit); + +double dp_vector3_angle(const DPVector3* vector1, const DPVector3* vector2, const angle_unit_t unit); + +// =============== Square Distance ============== // + +static inline float sp_vector3_get_square_distance(const SPVector3* vector1, const SPVector3* vector2) +{ + const float dx1 = (vector1->x1 - vector2->x1); + const float dx2 = (vector1->x2 - vector2->x2); + const float dx3 = (vector1->x3 - vector2->x3); + + return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; +} + +static inline double dp_vector3_get_square_distance(const DPVector3* vector1, const DPVector3* vector2) +{ + const double dx1 = (vector1->x1 - vector2->x1); + const double dx2 = (vector1->x2 - vector2->x2); + const double dx3 = (vector1->x3 - vector2->x3); + + return dx1 * dx1 + dx2 * dx2 + dx3 * dx3; +} + +// ================== Distance ================== // + +static inline float sp_vector3_get_distance(const SPVector3* vector1, const SPVector3* vector2) +{ + return sqrtf(sp_vector3_get_square_distance(vector1, vector2)); +} + +static inline double dp_vector3_get_distance(const DPVector3* vector1, const DPVector3* vector2) +{ + return sqrt(dp_vector3_get_square_distance(vector1, vector2)); +} + +// ================== Are Equal ================= // + +static inline int sp_vector3_are_equal(const SPVector3* vector1, const SPVector3* vector2) +{ + const float square_module1 = sp_vector3_get_square_module(vector1); + const float square_module2 = sp_vector3_get_square_module(vector2); + const float square_module3 = sp_vector3_get_square_distance(vector1, vector2); + + // 3.0f means dimension amount + if (square_module1 < SP_EPSYLON_EFFECTIVENESS_LIMIT || square_module2 < SP_EPSYLON_EFFECTIVENESS_LIMIT) { + return square_module3 < (3.0f * SP_SQUARE_EPSYLON); + } + + if (square_module1 <= square_module2) { + return square_module3 <= (3.0f * SP_SQUARE_EPSYLON) * square_module2; + } + + return square_module3 <= (3.0f * SP_SQUARE_EPSYLON) * square_module1; +} + +static inline int dp_vector3_are_equal(const DPVector3* vector1, const DPVector3* vector2) +{ + const double square_module1 = dp_vector3_get_square_module(vector1); + const double square_module2 = dp_vector3_get_square_module(vector2); + const double square_module3 = dp_vector3_get_square_distance(vector1, vector2); + + // 3.0 means dimension amount + if (square_module1 < DP_EPSYLON_EFFECTIVENESS_LIMIT || square_module2 < DP_EPSYLON_EFFECTIVENESS_LIMIT) { + return square_module3 < (3.0 * DP_SQUARE_EPSYLON); + } + + if (square_module1 <= square_module2) { + return square_module3 <= (3.0 * DP_SQUARE_EPSYLON) * square_module2; + } + + return square_module3 <= (3.0 * DP_SQUARE_EPSYLON) * square_module1; +} + +#endif diff --git a/src/versor.c b/src/versor.c new file mode 100644 index 0000000..14e106b --- /dev/null +++ b/src/versor.c @@ -0,0 +1,162 @@ +#include + +#include "angle.h" +#include "versor.h" + +const uint16_t GEOMETRY_VERSOR_COUNTER_LIMIT = 64; + +const SPVersor SP_IDLE_VERSOR = { 1.0f, 1.0f, 0.0f, 0.0f, 0.0f }; + +const DPVersor DP_IDLE_VERSOR = { 1.0, 1.0, 0.0, 0.0, 0.0 }; + +void __sp_versor_normalize(const float square_module, SPVersor* versor) +{ + if (square_module <= SP_SQUARE_EPSYLON) { + sp_versor_reset(versor); + return; + } + + if ((versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3) <= SP_SQUARE_EPSYLON * square_module) { + sp_versor_reset(versor); + return; + } + + const float module = sqrtf(square_module); + + versor->_s0 /= module; + versor->_x1 /= module; + versor->_x2 /= module; + versor->_x3 /= module; + + versor->_corrector = 2.0f - (versor->_s0 * versor->_s0 + versor->_x1 * versor->_x1) - (versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3); +} + +void __dp_versor_normalize(const double square_module, DPVersor* versor) +{ + if (square_module <= DP_SQUARE_EPSYLON) { + dp_versor_reset(versor); + return; + } + + const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; + + if (square_vector <= DP_SQUARE_EPSYLON * square_module) { + dp_versor_reset(versor); + return; + } + + const double module = sqrt(square_module); + + versor->_s0 /= module; + versor->_x1 /= module; + versor->_x2 /= module; + versor->_x3 /= module; + + versor->_corrector = 2.0 - (versor->_s0 * versor->_s0 + versor->_x1 * versor->_x1) - (versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3); +} + +// ==================== Make ==================== // + +void sp_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, SPVersor* result) +{ + const float square_vector = x1 * x1 + x2 * x2 + x3 * x3; + + if (square_vector <= SP_SQUARE_EPSYLON) { + sp_versor_reset(result); + return; + } + + const float half_angle = sp_convert_to_radians(0.5f * angle, unit); + + const float sine = sinf(half_angle); + + if (-SP_EPSYLON <= sine && sine <= SP_EPSYLON) { + sp_versor_reset(result); + return; + } + + const float multiplier = sine / sqrtf(square_vector); + + sp_versor_set( + cosf(half_angle), + x1 * multiplier, + x2 * multiplier, + x3 * multiplier, + result + ); +} + +void dp_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, DPVersor* result) +{ + const double square_vector = x1 * x1 + x2 * x2 + x3 * x3; + + if (square_vector <= DP_SQUARE_EPSYLON) { + dp_versor_reset(result); + return; + } + + const double half_angle = dp_convert_to_radians(0.5 * angle, unit); + + const double sine = sin(half_angle); + + if (-DP_EPSYLON <= sine && sine <= DP_EPSYLON) { + dp_versor_reset(result); + return; + } + + const double multiplier = sine / sqrt(square_vector); + + dp_versor_set( + cos(half_angle), + x1 * multiplier, + x2 * multiplier, + x3 * multiplier, + result + ); +} + +// ================= Rotation3 ================== // + +void sp_versor_get_rotation(const SPVersor* versor, SPRotation3* result) +{ + if (versor == 0 || result == 0) { + return; + } + + if (versor->_s0 <= -(1.0f - SP_EPSYLON) || 1.0f - SP_EPSYLON <= versor->_s0) { + sp_rotation_reset(result); + return; + } + + const float square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; + + result->radians = 2.0f * acosf(versor->_s0 / sqrtf(versor->_s0 * versor->_s0 + square_vector)); + + const float vector_module = sqrtf(square_vector); + + result->axis.x1 = versor->_x1 / vector_module; + result->axis.x2 = versor->_x2 / vector_module; + result->axis.x3 = versor->_x3 / vector_module; +} + +void dp_versor_get_rotation(const DPVersor* versor, DPRotation3* result) +{ + if (versor == 0 || result == 0) { + return; + } + + if (versor->_s0 <= -(1.0 - DP_EPSYLON) || 1.0 - DP_EPSYLON <= versor->_s0) { + dp_rotation_reset(result); + return; + } + + const double square_vector = versor->_x1 * versor->_x1 + versor->_x2 * versor->_x2 + versor->_x3 * versor->_x3; + + result->radians = 2.0 * acos(versor->_s0 / sqrt(versor->_s0 * versor->_s0 + square_vector)); + + const double vector_module = sqrt(square_vector); + + result->axis.x1 = versor->_x1 / vector_module; + result->axis.x2 = versor->_x2 / vector_module; + result->axis.x3 = versor->_x3 / vector_module; +} diff --git a/src/versor.h b/src/versor.h new file mode 100644 index 0000000..a450cd8 --- /dev/null +++ b/src/versor.h @@ -0,0 +1,550 @@ +#ifndef _GEOMETRY_VERSOR_H_ +#define _GEOMETRY_VERSOR_H_ + +#include + +#include "basis.h" +#include "vector3.h" +#include "rotation3.h" +#include "matrix3x3.h" + +typedef struct { + float _corrector, _s0, _x1, _x2, _x3; +} SPVersor; + +typedef struct { + double _corrector, _s0, _x1, _x2, _x3; +} DPVersor; + +extern const SPVersor SP_IDLE_VERSOR; +extern const DPVersor DP_IDLE_VERSOR; + +// =================== Reset ==================== // + +static inline void sp_versor_reset(SPVersor* versor) +{ + versor->_corrector = 1.0f; + versor->_s0 = 1.0f; + versor->_x1 = 0.0f; + versor->_x2 = 0.0f; + versor->_x3 = 0.0f; +} + +static inline void dp_versor_reset(DPVersor* versor) +{ + versor->_corrector = 1.0; + versor->_s0 = 1.0; + versor->_x1 = 0.0; + versor->_x2 = 0.0; + versor->_x3 = 0.0; +} + +// ============== Get Scalar Part =============== // + +static inline float sp_get_scalar_part(const SPVersor* versor) +{ + return versor->_s0; +} + +static inline double dp_get_scalar_part(const DPVersor* versor) +{ + return versor->_s0; +} + +// ============== Get Vector Part =============== // + +static inline void sp_get_vector_part(const SPVersor* versor, SPVector3 * result) +{ + result->x1 = versor->_x1; + result->x2 = versor->_x2; + result->x3 = versor->_x3; +} + +static inline void dp_get_vector_part(const DPVersor* versor, DPVector3 * result) +{ + result->x1 = versor->_x1; + result->x2 = versor->_x2; + result->x3 = versor->_x3; +} + +// =================== Get x1 =================== // + +static inline float sp_get_x1(const SPVersor* versor) +{ + return versor->_x1; +} + +static inline double dp_get_x1(const DPVersor* versor) +{ + return versor->_x1; +} + +// =================== Get x2 =================== // + +static inline float sp_get_x2(const SPVersor* versor) +{ + return versor->_x2; +} + +static inline double dp_get_x2(const DPVersor* versor) +{ + return versor->_x2; +} + +// =================== Get x3 =================== // + + +static inline float sp_get_x3(const SPVersor* versor) +{ + return versor->_x3; +} + +static inline double dp_get_x3(const DPVersor* versor) +{ + return versor->_x3; +} + +// ================ Get Corrector =============== // + +static inline float sp_get_corrector(const SPVersor* versor) +{ + return versor->_corrector; +} + +static inline double dp_get_corrector(const DPVersor* versor) +{ + return versor->_corrector; +} + +// ==================== Set ===================== // + +void __sp_versor_normalize(const float square_module, SPVersor* versor); + +void __dp_versor_normalize(const double square_module, DPVersor* versor); + +static inline void sp_versor_set(const float s0, const float x1, const float x2, const float x3, SPVersor* result) +{ + result->_s0 = s0; + result->_x1 = x1; + result->_x2 = x2; + result->_x3 = x3; + + const float square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (square_module < 1.0f - SP_TWO_EPSYLON || 1.0f + SP_TWO_EPSYLON < square_module) { + __sp_versor_normalize(square_module, result); + return; + } + + if (-1.0f + SP_EPSYLON < s0 && s0 < 1.0f - SP_EPSYLON) { + result->_corrector = 2.0f - square_module; + return; + } + + sp_versor_reset(result); +} + +static inline void dp_versor_set(const double s0, const double x1, const double x2, const double x3, DPVersor* result) +{ + result->_s0 = s0; + result->_x1 = x1; + result->_x2 = x2; + result->_x3 = x3; + + const double square_module = (s0 * s0 + x1 * x1) + (x2 * x2 + x3 * x3); + + if (square_module < 1.0 - DP_TWO_EPSYLON || 1.0 + DP_TWO_EPSYLON < square_module) { + __dp_versor_normalize(square_module, result); + return; + } + + if (s0 < -1.0 + DP_EPSYLON || 1.0 - DP_EPSYLON < s0) { + dp_versor_reset(result); + return; + } + + result->_corrector = 2.0 - square_module; +} + +// ==================== Copy ==================== // + +static inline void sp_versor_copy(const SPVersor* from, SPVersor* to) +{ + to->_corrector = from->_corrector; + to->_s0 = from->_s0; + to->_x1 = from->_x1; + to->_x2 = from->_x2; + to->_x3 = from->_x3; +} + +static inline void dp_versor_copy(const DPVersor* from, DPVersor* to) +{ + to->_corrector = from->_corrector; + to->_s0 = from->_s0; + to->_x1 = from->_x1; + to->_x2 = from->_x2; + to->_x3 = from->_x3; +} + +// ==================== Make ==================== // + +void sp_versor_make_from_crude_turn(const float x1, const float x2, const float x3, const float angle, const angle_unit_t unit, SPVersor* result); + +void dp_versor_make_from_crude_turn(const double x1, const double x2, const double x3, const double angle, const angle_unit_t unit, DPVersor* result); + +static inline void sp_versor_make_from_turn(const SPVector3* axis, const float angle, const angle_unit_t unit, SPVersor* result) +{ + sp_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); +} + +static inline void dp_versor_make_from_turn(const DPVector3* axis, const double angle, const angle_unit_t unit, DPVersor* result) +{ + dp_versor_make_from_crude_turn(axis->x1, axis->x2, axis->x3, angle, unit, result); +} + +static inline void sp_versor_make_from_rotation(const SPRotation3* rotation, SPVersor* result) +{ + sp_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, ANGLE_UNIT_RADIANS, result); +} + +static inline void dp_versor_make_from_rotation(const DPRotation3* rotation, DPVersor* result) +{ + dp_versor_make_from_crude_turn(rotation->axis.x1, rotation->axis.x2, rotation->axis.x3, rotation->radians, ANGLE_UNIT_RADIANS, result); +} + +// ================= Comparison ================= // + +static inline int sp_versor_is_idle(const SPVersor* versor) +{ + return 1.0f - SP_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - SP_EPSYLON); +} + +static inline int dp_versor_is_idle(const DPVersor* versor) +{ + return 1.0 - DP_EPSYLON <= versor->_s0 || versor->_s0 <= -(1.0 - DP_EPSYLON); +} + +// ============= Copy to twin type ============== // + +static inline void sp_versor_copy_to_double(const SPVersor* versor, DPVersor* result) +{ + dp_versor_set( + (double)versor->_s0, + (double)versor->_x1, + (double)versor->_x2, + (double)versor->_x3, + result + ); +} + +static inline void dp_versor_copy_to_single(const DPVersor* versor, SPVersor* result) +{ + sp_versor_set( + (float)versor->_s0, + (float)versor->_x1, + (float)versor->_x2, + (float)versor->_x3, + result + ); +} + +// ================= Inversion ================== // + +static inline void sp_versor_invert(SPVersor* versor) +{ + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; +} + +static inline void dp_versor_invert(DPVersor* versor) +{ + versor->_x1 = -versor->_x1; + versor->_x2 = -versor->_x2; + versor->_x3 = -versor->_x3; +} + +// ================ Make Inverted =============== // + +static inline void sp_versor_make_inverted(const SPVersor* versor, SPVersor* result) +{ + result->_corrector = versor->_corrector; + result->_s0 = versor->_s0; + result->_x1 = -versor->_x1; + result->_x2 = -versor->_x2; + result->_x3 = -versor->_x3; +} + +static inline void dp_versor_make_inverted(const DPVersor* versor, DPVersor* result) +{ + result->_corrector = versor->_corrector; + result->_s0 = versor->_s0; + result->_x1 = -versor->_x1; + result->_x2 = -versor->_x2; + result->_x3 = -versor->_x3; +} + +// ================ Combination ================= // + +static inline void sp_versor_combine(const SPVersor* second, const SPVersor* first, SPVersor* result) +{ + const float s0s0 = second->_s0 * first->_s0; + const float x1s0 = second->_x1 * first->_s0; + const float x2s0 = second->_x2 * first->_s0; + const float x3s0 = second->_x3 * first->_s0; + + const float s0x1 = second->_s0 * first->_x1; + const float x1x1 = second->_x1 * first->_x1; + const float x2x1 = second->_x2 * first->_x1; + const float x3x1 = second->_x3 * first->_x1; + + const float s0x2 = second->_s0 * first->_x2; + const float x1x2 = second->_x1 * first->_x2; + const float x2x2 = second->_x2 * first->_x2; + const float x3x2 = second->_x3 * first->_x2; + + const float s0x3 = second->_s0 * first->_x3; + const float x1x3 = second->_x1 * first->_x3; + const float x2x3 = second->_x2 * first->_x3; + const float x3x3 = second->_x3 * first->_x3; + + const float s0b = (x2x2 + x3x3); + const float x1a = (x1s0 + s0x1); + const float x2a = (x2s0 + s0x2); + const float x3a = (x3s0 + s0x3); + + const float s0a = (s0s0 - x1x1); + const float x1b = (x3x2 - x2x3); + const float x2b = (x1x3 - x3x1); + const float x3b = (x2x1 - x1x2); + + sp_versor_set( + s0a - s0b, + x1a - x1b, + x2a - x2b, + x3a - x3b, + result + ); +} + +static inline void sp_versor_combine2(const SPVersor* second, const SPVersor* first, SPVersor* result) +{ + sp_versor_set( + ((second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3)), + ((second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3)), + ((second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1)), + ((second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2)), + result + ); +} + +static inline void dp_versor_combine(const DPVersor* second, const DPVersor* first, DPVersor* result) +{ + dp_versor_set( + (second->_s0 * first->_s0 - second->_x1 * first->_x1) - (second->_x2 * first->_x2 + second->_x3 * first->_x3), + (second->_x1 * first->_s0 + second->_s0 * first->_x1) - (second->_x3 * first->_x2 - second->_x2 * first->_x3), + (second->_x2 * first->_s0 + second->_s0 * first->_x2) - (second->_x1 * first->_x3 - second->_x3 * first->_x1), + (second->_x3 * first->_s0 + second->_s0 * first->_x3) - (second->_x2 * first->_x1 - second->_x1 * first->_x2), + result + ); +} + +// ================= Rotation3 ================== // + +void sp_versor_get_rotation(const SPVersor* versor, SPRotation3* result); + +void dp_versor_get_rotation(const DPVersor* versor, DPRotation3* result); + +// =========== Make Rotation Matrix3x3 ========== // + +static inline void sp_versor_make_matrix(const SPVersor* versor, SPMatrix3x3* 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 = versor->_s0 * versor->_x1; + const float s0x2 = versor->_s0 * versor->_x2; + const float s0x3 = versor->_s0 * versor->_x3; + + const float x1x2 = versor->_x1 * versor->_x2; + const float x1x3 = versor->_x1 * versor->_x3; + const float x2x3 = versor->_x2 * versor->_x3; + + const float corrector2 = 2.0f * versor->_corrector; + + matrix->r1c1 = versor->_corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix->r2c2 = versor->_corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix->r3c3 = versor->_corrector * ((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 dp_versor_make_matrix(const DPVersor* versor, DPMatrix3x3* 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 = versor->_s0 * versor->_x1; + const double s0x2 = versor->_s0 * versor->_x2; + const double s0x3 = versor->_s0 * versor->_x3; + + const double x1x2 = versor->_x1 * versor->_x2; + const double x1x3 = versor->_x1 * versor->_x3; + const double x2x3 = versor->_x2 * versor->_x3; + + const double corrector2 = 2.0 * versor->_corrector; + + matrix->r1c1 = versor->_corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix->r2c2 = versor->_corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix->r3c3 = versor->_corrector * ((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 Matrix3x3 =========== // + +static inline void sp_versor_make_reverse_matrix(const SPVersor* versor, SPMatrix3x3* 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 = versor->_s0 * versor->_x1; + const float s0x2 = versor->_s0 * versor->_x2; + const float s0x3 = versor->_s0 * versor->_x3; + + const float x1x2 = versor->_x1 * versor->_x2; + const float x1x3 = versor->_x1 * versor->_x3; + const float x2x3 = versor->_x2 * versor->_x3; + + const float corrector2 = 2.0f * versor->_corrector; + + matrix->r1c1 = versor->_corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix->r2c2 = versor->_corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix->r3c3 = versor->_corrector * ((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 dp_versor_make_reverse_matrix(const DPVersor* versor, DPMatrix3x3* 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 = versor->_s0 * versor->_x1; + const double s0x2 = versor->_s0 * versor->_x2; + const double s0x3 = versor->_s0 * versor->_x3; + + const double x1x2 = versor->_x1 * versor->_x2; + const double x1x3 = versor->_x1 * versor->_x3; + const double x2x3 = versor->_x2 * versor->_x3; + + const double corrector2 = 2.0 * versor->_corrector; + + matrix->r1c1 = versor->_corrector * ((s0s0 + x1x1) - (x2x2 + x3x3)); + matrix->r2c2 = versor->_corrector * ((s0s0 + x2x2) - (x1x1 + x3x3)); + matrix->r3c3 = versor->_corrector * ((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); +} + +// ================ Turn Vector ================= // + +static inline void sp_versor_turn(const SPVersor* versor, const SPVector3* vector, SPVector3* result) +{ + const float multiplier = 2.0f * versor->_corrector; + const float tx1 = multiplier * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const float tx2 = multiplier * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const float tx3 = multiplier * (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; +} + +static inline void dp_versor_turn(const DPVersor* versor, const DPVector3* vector, DPVector3* result) +{ + const double multiplier = 2.0 * versor->_corrector; + const double tx1 = multiplier * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const double tx2 = multiplier * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const double tx3 = multiplier * (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 ============== // + +static inline void sp_versor_turn_back(const SPVersor* versor, const SPVector3* vector, SPVector3* result) +{ + const float multiplier = 2.0f * versor->_corrector; + const float tx1 = multiplier * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const float tx2 = multiplier * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const float tx3 = multiplier * (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; +} + +static inline void dp_versor_turn_back(const DPVersor* versor, const DPVector3* vector, DPVector3* result) +{ + const double multiplier = 2.0 * versor->_corrector; + const double tx1 = multiplier * (versor->_x2 * vector->x3 - versor->_x3 * vector->x2); + const double tx2 = multiplier * (versor->_x3 * vector->x1 - versor->_x1 * vector->x3); + const double tx3 = multiplier * (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 diff --git a/test/geometry-test.cbp b/test/geometry-test.cbp new file mode 100644 index 0000000..9747ee3 --- /dev/null +++ b/test/geometry-test.cbp @@ -0,0 +1,58 @@ + + + + + + diff --git a/test/geometry-test.layout b/test/geometry-test.layout new file mode 100644 index 0000000..856595e --- /dev/null +++ b/test/geometry-test.layout @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/test/geometry-test.vcxproj b/test/geometry-test.vcxproj new file mode 100644 index 0000000..6ca000b --- /dev/null +++ b/test/geometry-test.vcxproj @@ -0,0 +1,162 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {48dae315-715f-4044-adf5-0308483b887c} + geometry-test + 10.0 + + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + + + false + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(SolutionDir)src;%(AdditionalIncludeDirectories) + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(SolutionDir)src;%(AdditionalIncludeDirectories) + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(SolutionDir)src;%(AdditionalIncludeDirectories) + + + Console + true + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + $(SolutionDir)src;%(AdditionalIncludeDirectories) + + + Console + true + true + true + + + + + {40ca6fb4-135f-4d54-a8d9-7338ba56e6a7} + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/test/geometry-test.vcxproj.filters b/test/geometry-test.vcxproj.filters new file mode 100644 index 0000000..1e57359 --- /dev/null +++ b/test/geometry-test.vcxproj.filters @@ -0,0 +1,36 @@ + + + + + {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/test/geometry-test.vcxproj.user b/test/geometry-test.vcxproj.user new file mode 100644 index 0000000..88a5509 --- /dev/null +++ b/test/geometry-test.vcxproj.user @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/test/geometry_test.c b/test/geometry_test.c new file mode 100644 index 0000000..b6f5de5 --- /dev/null +++ b/test/geometry_test.c @@ -0,0 +1,23 @@ +#include "geometry_test.h" + +#include + +void print_test_section(const char * name) +{ + printf("================ %s ================\n", name); +} + +void print_test_name(const char * name) +{ + printf(" Testing of %s: ", name); +} + +void print_test_success() +{ + printf("[ \x1b[32mSuccess\x1b[0m ]\n"); +} + +void print_test_failed() +{ + printf("[ \x1b[31mFailed\x1b[0m ]\n"); +} diff --git a/test/geometry_test.h b/test/geometry_test.h new file mode 100644 index 0000000..f022a75 --- /dev/null +++ b/test/geometry_test.h @@ -0,0 +1,17 @@ +#ifndef __GEOMETRY_TEST_H__ +#define __GEOMETRY_TEST_H__ + +#include + +#define TEST_RESULT_SUCCES 0 +#define TEST_RESULT_FAILED 100 + +void print_test_section(const char * name); + +void print_test_name(const char * name); + +void print_test_success(); + +void print_test_failed(); + +#endif diff --git a/test/main.c b/test/main.c new file mode 100644 index 0000000..6231098 --- /dev/null +++ b/test/main.c @@ -0,0 +1,17 @@ +#include +#include + +#include "geometry_test.h" +#include "sp_vector2_test.h" + +#define PROGRAM_RESULT_SUCCESS 0 +#define PROGRAM_RESULT_FAILED 1 + +int main() +{ + if (test_sp_vector2() == TEST_RESULT_FAILED) { + return PROGRAM_RESULT_FAILED; + } + + return PROGRAM_RESULT_SUCCESS; +} diff --git a/test/sp_vector2_test.c b/test/sp_vector2_test.c new file mode 100644 index 0000000..9c121a8 --- /dev/null +++ b/test/sp_vector2_test.c @@ -0,0 +1,16 @@ +#include "sp_vector2_test.h" + +int _test_sp_vector2_adding() +{ + return TEST_RESULT_SUCCES; +} + + +int test_sp_vector2() +{ + print_test_section("SPVector2"); + + + + return TEST_RESULT_SUCCES; +} diff --git a/test/sp_vector2_test.h b/test/sp_vector2_test.h new file mode 100644 index 0000000..1a61289 --- /dev/null +++ b/test/sp_vector2_test.h @@ -0,0 +1,8 @@ +#ifndef __GEOMETRY_VECTOR2_FLOAT_TEST_H__ +#define __GEOMETRY_VECTOR2_FLOAT_TEST_H__ + +#include "geometry_test.h" + +int test_sp_vector2(); + +#endif