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

.gitignore

@@ -47,8 +47,14 @@
 *.mod*
 *.cmd
 .tmp_versions/
+.vs
+x64
+x86
 modules.order
 Module.symvers
 Mkfile.old
 dkms.conf
-
+bin
+obj
+dev
+logs

Geometry.workspace

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_workspace_file>
+	<Workspace title="Workspace">
+		<Project filename="src/geometry.cbp" />
+		<Project filename="dev/geometry-dev.cbp">
+			<Depends filename="src/geometry.cbp" />
+		</Project>
+		<Project filename="test/geometry-test.cbp">
+			<Depends filename="src/geometry.cbp" />
+		</Project>
+	</Workspace>
+</CodeBlocks_workspace_file>

Geometry.workspace.layout

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_workspace_layout_file>
+	<FileVersion major="1" minor="0" />
+	<ActiveProject path="dev/geometry-dev.cbp" />
+</CodeBlocks_workspace_layout_file>

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

src/angle.c

@@ -0,0 +1,3 @@
+#include "basis.h"
+#include "angle.h"
+

src/angle.h

@@ -0,0 +1,549 @@
+#ifndef _GEOMETRY_ANGLE_H_
+#define _GEOMETRY_ANGLE_H_
+
+#include <math.h>
+#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

src/basis.c

@@ -0,0 +1,2 @@
+#include "basis.h"
+

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

src/geometry.cbp

@@ -0,0 +1,87 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="geometry" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/geometry" prefix_auto="1" extension_auto="1" />
+				<Option working_dir="" />
+				<Option object_output="obj/Debug/" />
+				<Option type="2" />
+				<Option compiler="gcc" />
+				<Option createDefFile="1" />
+				<Compiler>
+					<Add option="-Wall" />
+					<Add option="-g" />
+				</Compiler>
+				<Linker>
+					<Add library="c" />
+					<Add library="m" />
+					<LinkerExe value="CCompiler" />
+				</Linker>
+			</Target>
+			<Target title="Release">
+				<Option output="bin/Release/geometry" prefix_auto="1" extension_auto="1" />
+				<Option working_dir="" />
+				<Option object_output="obj/Release/" />
+				<Option type="2" />
+				<Option compiler="gcc" />
+				<Option createDefFile="1" />
+				<Compiler>
+					<Add option="-march=corei7-avx" />
+					<Add option="-O3" />
+					<Add option="-Wall" />
+				</Compiler>
+				<Linker>
+					<Add option="-O3" />
+					<Add option="-s" />
+					<Add option="-static" />
+					<Add library="c" />
+					<Add library="m" />
+					<LinkerExe value="CCompiler" />
+				</Linker>
+			</Target>
+		</Build>
+		<Unit filename="angle.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="angle.h" />
+		<Unit filename="basis.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="basis.h" />
+		<Unit filename="geometry.h" />
+		<Unit filename="matrix2x2.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="matrix2x2.h" />
+		<Unit filename="matrix3x3.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="matrix3x3.h" />
+		<Unit filename="quaternion.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="quaternion.h" />
+		<Unit filename="rotation3.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="rotation3.h" />
+		<Unit filename="vector2.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="vector2.h" />
+		<Unit filename="vector3.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="vector3.h" />
+		<Unit filename="versor.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="versor.h" />
+		<Extensions />
+	</Project>
+</CodeBlocks_project_file>

src/geometry.depend

@@ -0,0 +1 @@
+# depslib dependency file v1.0

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

src/geometry.layout

@@ -0,0 +1,100 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_layout_file>
+	<FileVersion major="1" minor="0" />
+	<ActiveTarget name="Debug" />
+	<File name="matrix3x3.c" open="1" top="1" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="3207" topLine="198" />
+		</Cursor>
+	</File>
+	<File name="angle.c" open="0" top="0" tabpos="7" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="0" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="matrix3x3.h" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="6363" topLine="266" />
+		</Cursor>
+	</File>
+	<File name="vector2.c" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="1940" topLine="25" />
+		</Cursor>
+	</File>
+	<File name="vector3.h" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="0" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="quaternion.c" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="6789" topLine="145" />
+		</Cursor>
+	</File>
+	<File name="rotation3.c" open="0" top="0" tabpos="5" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="154" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="matrix2x2.c" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="305" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="geometry.h" open="0" top="0" tabpos="8" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="315" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="quaternion.h" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="3387" topLine="111" />
+		</Cursor>
+	</File>
+	<File name="rotation3.h" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="305" topLine="29" />
+		</Cursor>
+	</File>
+	<File name="angle.h" open="0" top="0" tabpos="6" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="1082" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="vector2.h" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="0" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="basis.h" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="103" topLine="8" />
+		</Cursor>
+	</File>
+	<File name="matrix2x2.h" open="1" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="6700" topLine="238" />
+		</Cursor>
+	</File>
+	<File name="versor.h" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="3718" topLine="116" />
+		</Cursor>
+	</File>
+	<File name="basis.c" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="20" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="vector3.c" open="0" top="0" tabpos="4" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="1436" topLine="15" />
+		</Cursor>
+	</File>
+	<File name="versor.c" open="0" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="504" topLine="0" />
+		</Cursor>
+	</File>
+</CodeBlocks_layout_file>

src/geometry.vcxproj

@@ -0,0 +1,175 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="angle.h" />
+    <ClInclude Include="basis.h" />
+    <ClInclude Include="geometry.h" />
+    <ClInclude Include="matrix2x2.h" />
+    <ClInclude Include="matrix3x3.h" />
+    <ClInclude Include="quaternion.h" />
+    <ClInclude Include="rotation3.h" />
+    <ClInclude Include="versor.h" />
+    <ClInclude Include="vector2.h" />
+    <ClInclude Include="vector3.h" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="angle.c" />
+    <ClCompile Include="basis.c" />
+    <ClCompile Include="matrix2x2.c" />
+    <ClCompile Include="matrix3x3.c" />
+    <ClCompile Include="quaternion.c" />
+    <ClCompile Include="rotation3.c" />
+    <ClCompile Include="versor.c" />
+    <ClCompile Include="vector2.c" />
+    <ClCompile Include="vector3.c" />
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{40ca6fb4-135f-4d54-a8d9-7338ba56e6a7}</ProjectGuid>
+    <RootNamespace>geometry</RootNamespace>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>StaticLibrary</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>StaticLibrary</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>StaticLibrary</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>StaticLibrary</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <Link>
+      <SubSystem>
+      </SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <Link>
+      <SubSystem>
+      </SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>_DEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <Link>
+      <SubSystem>
+      </SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>NDEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
+    </ClCompile>
+    <Link>
+      <SubSystem>
+      </SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>

src/geometry.vcxproj.filters

@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup>
+    <Filter Include="Исходные файлы">
+      <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
+      <Extensions>cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
+    </Filter>
+    <Filter Include="Файлы заголовков">
+      <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
+      <Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions>
+    </Filter>
+    <Filter Include="Файлы ресурсов">
+      <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
+      <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
+    </Filter>
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="angle.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="basis.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="geometry.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="matrix2x2.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="matrix3x3.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="rotation3.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="versor.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="vector2.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="vector3.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+    <ClInclude Include="quaternion.h">
+      <Filter>Файлы заголовков</Filter>
+    </ClInclude>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="angle.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="basis.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="matrix2x2.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="matrix3x3.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="versor.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="vector2.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="vector3.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="rotation3.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="quaternion.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+  </ItemGroup>
+</Project>

src/geometry.vcxproj.user

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup />
+</Project>

src/matrix2x2.c

@@ -0,0 +1,24 @@
+#include "matrix2x2.h"
+
+#include <math.h>
+#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
+};

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

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;
+}

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

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);
+}

src/quaternion.h

@@ -0,0 +1,199 @@
+#ifndef _GEOMETRY_QUATERNION_H_
+#define _GEOMETRY_QUATERNION_H_
+
+#include <math.h>
+
+#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_

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};

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

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);
+}

src/vector2.h

@@ -0,0 +1,633 @@
+#ifndef _GEOMETRY_VECTOR2_H_
+#define _GEOMETRY_VECTOR2_H_
+
+#include "basis.h"
+#include "angle.h"
+
+#include <math.h>
+
+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

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);
+}

src/vector3.h

@@ -0,0 +1,729 @@
+#ifndef _GEOMETRY_VECTOR3_H_
+#define _GEOMETRY_VECTOR3_H_
+
+#include "basis.h"
+#include "angle.h"
+
+#include <math.h>
+
+// ================== 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

src/versor.c

@@ -0,0 +1,162 @@
+#include <math.h>
+
+#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;
+}

src/versor.h

@@ -0,0 +1,550 @@
+#ifndef _GEOMETRY_VERSOR_H_
+#define _GEOMETRY_VERSOR_H_
+
+#include <stdint.h>
+
+#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

test/geometry-test.cbp

@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="geometry-test" />
+		<Option pch_mode="2" />
+		<Option compiler="gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/Debug/geometry-test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Debug/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-g" />
+					<Add directory="../src" />
+				</Compiler>
+				<Linker>
+					<Add library="geometry" />
+					<Add library="m" />
+					<Add directory="../src/bin/Debug" />
+				</Linker>
+			</Target>
+			<Target title="Release">
+				<Option output="bin/Release/geometry-test" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/Release/" />
+				<Option type="1" />
+				<Option compiler="gcc" />
+				<Compiler>
+					<Add option="-O3" />
+					<Add directory="../src" />
+				</Compiler>
+				<Linker>
+					<Add option="-O3" />
+					<Add option="-s" />
+					<Add library="geometry" />
+					<Add library="m" />
+					<Add directory="../src/bin/Release" />
+				</Linker>
+			</Target>
+		</Build>
+		<Compiler>
+			<Add option="-Wall" />
+		</Compiler>
+		<Unit filename="geometry_test.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="geometry_test.h" />
+		<Unit filename="main.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="sp_vector2_test.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Unit filename="sp_vector2_test.h" />
+		<Extensions />
+	</Project>
+</CodeBlocks_project_file>

test/geometry-test.layout

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_layout_file>
+	<FileVersion major="1" minor="0" />
+	<ActiveTarget name="Debug" />
+	<File name="geometry_test.c" open="0" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="407" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="sp_vector2_test.c" open="0" top="0" tabpos="0" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="177" topLine="4" />
+		</Cursor>
+	</File>
+	<File name="main.c" open="0" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="178" topLine="0" />
+		</Cursor>
+	</File>
+</CodeBlocks_layout_file>

test/geometry-test.vcxproj

@@ -0,0 +1,162 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup Label="ProjectConfigurations">
+    <ProjectConfiguration Include="Debug|Win32">
+      <Configuration>Debug</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|Win32">
+      <Configuration>Release</Configuration>
+      <Platform>Win32</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Debug|x64">
+      <Configuration>Debug</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+    <ProjectConfiguration Include="Release|x64">
+      <Configuration>Release</Configuration>
+      <Platform>x64</Platform>
+    </ProjectConfiguration>
+  </ItemGroup>
+  <PropertyGroup Label="Globals">
+    <VCProjectVersion>16.0</VCProjectVersion>
+    <Keyword>Win32Proj</Keyword>
+    <ProjectGuid>{48dae315-715f-4044-adf5-0308483b887c}</ProjectGuid>
+    <RootNamespace>geometry-test</RootNamespace>
+    <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>true</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
+    <ConfigurationType>Application</ConfigurationType>
+    <UseDebugLibraries>false</UseDebugLibraries>
+    <PlatformToolset>v143</PlatformToolset>
+    <WholeProgramOptimization>true</WholeProgramOptimization>
+    <CharacterSet>Unicode</CharacterSet>
+  </PropertyGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
+  <ImportGroup Label="ExtensionSettings">
+  </ImportGroup>
+  <ImportGroup Label="Shared">
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
+  </ImportGroup>
+  <PropertyGroup Label="UserMacros" />
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <LinkIncremental>true</LinkIncremental>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <LinkIncremental>false</LinkIncremental>
+  </PropertyGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>$(SolutionDir)src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>$(SolutionDir)src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>$(SolutionDir)src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
+    <ClCompile>
+      <WarningLevel>Level3</WarningLevel>
+      <FunctionLevelLinking>true</FunctionLevelLinking>
+      <IntrinsicFunctions>true</IntrinsicFunctions>
+      <SDLCheck>true</SDLCheck>
+      <PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <ConformanceMode>true</ConformanceMode>
+      <AdditionalIncludeDirectories>$(SolutionDir)src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+    </ClCompile>
+    <Link>
+      <SubSystem>Console</SubSystem>
+      <EnableCOMDATFolding>true</EnableCOMDATFolding>
+      <OptimizeReferences>true</OptimizeReferences>
+      <GenerateDebugInformation>true</GenerateDebugInformation>
+    </Link>
+  </ItemDefinitionGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\src\geometry.vcxproj">
+      <Project>{40ca6fb4-135f-4d54-a8d9-7338ba56e6a7}</Project>
+    </ProjectReference>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="geometry_test.c" />
+    <ClCompile Include="main.c" />
+    <ClCompile Include="sp_vector2_test.c" />
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="geometry_test.h" />
+    <ClInclude Include="sp_vector2_test.h" />
+  </ItemGroup>
+  <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
+  <ImportGroup Label="ExtensionTargets">
+  </ImportGroup>
+</Project>

test/geometry-test.vcxproj.filters

@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ItemGroup>
+    <Filter Include="Исходные файлы">
+      <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
+      <Extensions>cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
+    </Filter>
+    <Filter Include="Файлы заголовков">
+      <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
+      <Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions>
+    </Filter>
+    <Filter Include="Файлы ресурсов">
+      <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
+      <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
+    </Filter>
+  </ItemGroup>
+  <ItemGroup>
+    <ClCompile Include="main.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="geometry_test.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+    <ClCompile Include="sp_vector2_test.c">
+      <Filter>Исходные файлы</Filter>
+    </ClCompile>
+  </ItemGroup>
+  <ItemGroup>
+    <ClInclude Include="geometry_test.h">
+      <Filter>Исходные файлы</Filter>
+    </ClInclude>
+    <ClInclude Include="sp_vector2_test.h">
+      <Filter>Исходные файлы</Filter>
+    </ClInclude>
+  </ItemGroup>
+</Project>

test/geometry-test.vcxproj.user

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="Current" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup />
+</Project>

test/geometry_test.c

@@ -0,0 +1,23 @@
+#include "geometry_test.h"
+
+#include <stdio.h>
+
+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");
+}

test/geometry_test.h

@@ -0,0 +1,17 @@
+#ifndef __GEOMETRY_TEST_H__
+#define __GEOMETRY_TEST_H__
+
+#include <geometry.h>
+
+#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

test/main.c

@@ -0,0 +1,17 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#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;
+}

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;
+}

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