Skip to content

Commit ce26303

Browse files
ayaankhan98ayaankhan98
authored
Merge pull request #594 from kvedala/quaternions
[feature] added 3d geometry operations
2 parents 5d0972a + dd19e03 commit ce26303

File tree

6 files changed

+552
-0
lines changed

6 files changed

+552
-0
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ add_subdirectory(hash)
5252
add_subdirectory(misc)
5353
add_subdirectory(games)
5454
add_subdirectory(sorting)
55+
add_subdirectory(geometry)
5556
add_subdirectory(graphics)
5657
add_subdirectory(searching)
5758
add_subdirectory(conversions)

‎DIRECTORY.md

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,11 @@
112112
## Games
113113
* [Tic Tac Toe](https://github.com/TheAlgorithms/C/blob/master/games/tic_tac_toe.c)
114114

115+
## Geometry
116+
* [Geometry Datatypes](https://github.com/TheAlgorithms/C/blob/master/geometry/geometry_datatypes.h)
117+
* [Quaternions](https://github.com/TheAlgorithms/C/blob/master/geometry/quaternions.c)
118+
* [Vectors 3D](https://github.com/TheAlgorithms/C/blob/master/geometry/vectors_3d.c)
119+
115120
## Graphics
116121
* [Spirograph](https://github.com/TheAlgorithms/C/blob/master/graphics/spirograph.c)
117122

‎geometry/CMakeLists.txt

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
# If necessary, use the RELATIVE flag, otherwise each source file may be listed
2+
# with full pathname. RELATIVE may makes it easier to extract an executable name
3+
# automatically.
4+
file( GLOB APP_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.c)
5+
# file( GLOB APP_SOURCES ${CMAKE_SOURCE_DIR}/*.c )
6+
# AUX_SOURCE_DIRECTORY(${CMAKE_CURRENT_SOURCE_DIR} APP_SOURCES)
7+
foreach( testsourcefile ${APP_SOURCES} )
8+
# I used a simple string replace, to cut off .cpp.
9+
string( REPLACE ".c" "" testname ${testsourcefile} )
10+
add_executable( ${testname} ${testsourcefile} )
11+
12+
if(OpenMP_C_FOUND)
13+
target_link_libraries(${testname} OpenMP::OpenMP_C)
14+
endif()
15+
if(MATH_LIBRARY)
16+
target_link_libraries(${testname} ${MATH_LIBRARY})
17+
endif()
18+
install(TARGETS ${testname} DESTINATION "bin/geometry")
19+
20+
endforeach( testsourcefile ${APP_SOURCES} )

‎geometry/geometry_datatypes.h

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
/**
2+
* @addtogroup quaternions Library for 3D Vectors & Quaternions
3+
* @{
4+
* @file
5+
* @brief Generic header that provides data types for 3D vectors and quaternions
6+
* @author Krishna Vedala
7+
*/
8+
9+
#ifndef __LIBQUAT_H_
10+
#define __LIBQUAT_H_
11+
12+
/** Minimum recognizable value. Any value less than this is considered to be
13+
* @f$=0@f$ */
14+
#define EPSILON 1e-9
15+
16+
/**
17+
* @addtogroup vec_3d 3D Vector operations
18+
* @{
19+
*/
20+
/** 3D vector type */
21+
typedef struct vec_3d_
22+
{
23+
float x; /**< X co-ordinate */
24+
float y; /**< Y co-ordinate */
25+
float z; /**< Z co-ordinate */
26+
} vec_3d;
27+
/** @} */
28+
29+
/**
30+
* @addtogroup matrix Matrix operations
31+
* @{
32+
*/
33+
/** A 3x3 Matrix type definition */
34+
typedef struct mat_3x3_
35+
{
36+
union
37+
{ /**< 3 element row 1 */
38+
float row1[3];
39+
vec_3d vec1;
40+
};
41+
union
42+
{ /**< 3 element row 2 */
43+
float row2[3];
44+
vec_3d vec2;
45+
};
46+
union
47+
{ /**< 3 element row 3 */
48+
float row3[3];
49+
vec_3d vec3;
50+
};
51+
} mat_3x3;
52+
/** @} */
53+
54+
/** @addtogroup quats 3D Quaternion operations
55+
* @{
56+
*/
57+
/** a Quaternion type represented using a scalar \f$w\f$ or \f$q_0\f$ and a
58+
* 3D vector \f$\left(q_1,q_2,q_3\right)\f$
59+
*/
60+
typedef struct quaternion_
61+
{
62+
union
63+
{
64+
float w; /**< real part of quaternion */
65+
float q0; /**< real part of quaternion */
66+
};
67+
/**< dual part of quaternion */
68+
union
69+
{
70+
vec_3d dual; /**< can be a 3D vector */
71+
/** or individual values */
72+
struct
73+
{
74+
float q1, q2, q3;
75+
};
76+
};
77+
} quaternion;
78+
79+
/** 3D Euler or Tait-Bryan angles (in radian) */
80+
typedef struct euler_
81+
{
82+
union
83+
{
84+
float roll; /**< or bank \f$\phi\f$ = rotation about X axis */
85+
float bank; /**< or roll \f$\phi\f$ = rotation about X axis */
86+
};
87+
union
88+
{
89+
float pitch; /**< or elevation \f$\theta\f$ = rotation about Y axis */
90+
float elevation; /**< or pitch \f$\theta\f$ = rotation about Y axis */
91+
};
92+
union
93+
{
94+
float yaw; /**< or heading \f$\psi\f$ = rotation about Z axis */
95+
float heading; /**< or yaw \f$\psi\f$ = rotation about Z axis */
96+
};
97+
} euler;
98+
99+
/** @} */
100+
101+
/** @addtogroup dual_quats 3D Dual-Quaternion operations
102+
* @{
103+
*/
104+
/** a dual quaternion type */
105+
typedef struct dual_quat_
106+
{
107+
quaternion real; /**< real part of dual quaternion */
108+
quaternion dual; /**< dual part of dual quaternion */
109+
} dual_quat;
110+
111+
/** @} */
112+
113+
#endif // __LIBQUAT_H_
114+
115+
/** @} */

‎geometry/quaternions.c

Lines changed: 173 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,173 @@
1+
/**
2+
* @file
3+
* @brief Functions related to 3D quaternions and Euler angles.
4+
* @author Krishna Vedala
5+
*/
6+
7+
#include <stdio.h>
8+
#ifdef __arm__ // if compiling for ARM-Cortex processors
9+
#define LIBQUAT_ARM
10+
#include <arm_math.h>
11+
#else
12+
#include <math.h>
13+
#endif
14+
#include <assert.h>
15+
16+
#include "geometry_datatypes.h"
17+
18+
/**
19+
* @addtogroup quats 3D Quaternion operations
20+
* @{
21+
*/
22+
23+
/**
24+
* Function to convert given Euler angles to a quaternion.
25+
* \f{eqnarray*}{
26+
* q_{0} & =
27+
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
28+
* +
29+
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
30+
* q_{1} & =
31+
* &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
32+
* -
33+
* \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
34+
* q_{2} & =
35+
* &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
36+
* +
37+
* \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
38+
* q_{3} & =
39+
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
40+
* -
41+
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
42+
* \f}
43+
*
44+
* @param [in] in_euler input Euler angles instance
45+
* @returns converted quaternion
46+
*/
47+
quaternion quat_from_euler(const euler *in_euler)
48+
{
49+
quaternion out_quat;
50+
51+
if (!in_euler) // if null
52+
{
53+
fprintf(stderr, "%s: Invalid input.", __func__);
54+
return out_quat;
55+
}
56+
57+
quaternion temp;
58+
59+
float cy = cosf(in_euler->yaw * 0.5f);
60+
float sy = sinf(in_euler->yaw * 0.5f);
61+
float cp = cosf(in_euler->pitch * 0.5f);
62+
float sp = sinf(in_euler->pitch * 0.5f);
63+
float cr = cosf(in_euler->roll * 0.5f);
64+
float sr = sinf(in_euler->roll * 0.5f);
65+
66+
temp.w = cr * cp * cy + sr * sp * sy;
67+
temp.q1 = sr * cp * cy - cr * sp * sy;
68+
temp.q2 = cr * sp * cy + sr * cp * sy;
69+
temp.q3 = cr * cp * sy - sr * sp * cy;
70+
71+
return temp;
72+
}
73+
74+
/**
75+
* Function to convert given quaternion to Euler angles.
76+
* \f{eqnarray*}{
77+
* \phi & = &
78+
* \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
79+
* \theta & =
80+
* &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
81+
* \psi & = &
82+
* \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
83+
* \f}
84+
*
85+
* @param [in] in_quat input quaternion instance
86+
* @returns converted euler angles
87+
*/
88+
euler euler_from_quat(const quaternion *in_quat)
89+
{
90+
euler out_euler;
91+
if (!in_quat) // if null
92+
{
93+
fprintf(stderr, "%s: Invalid input.", __func__);
94+
return out_euler;
95+
}
96+
97+
out_euler.roll = atan2f(
98+
2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
99+
1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
100+
out_euler.pitch =
101+
asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
102+
out_euler.yaw = atan2f(
103+
2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
104+
1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
105+
106+
return out_euler;
107+
}
108+
109+
/**
110+
* Function to multiply two quaternions.
111+
* \f{eqnarray*}{
112+
* \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
113+
* & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
114+
* a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
115+
* b_{3}\end{bmatrix}\\
116+
* & = &
117+
* \begin{bmatrix}
118+
* a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
119+
* a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
120+
* a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
121+
* a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
122+
* \end{bmatrix}^{T}
123+
* \f}
124+
*
125+
* @param [in] in_quat1 first input quaternion instance
126+
* @param [in] in_quat2 second input quaternion instance
127+
* @returns resultant quaternion
128+
*/
129+
quaternion quaternion_multiply(const quaternion *in_quat1,
130+
const quaternion *in_quat2)
131+
{
132+
quaternion out_quat;
133+
if (!in_quat1 || !in_quat2) // if null
134+
{
135+
fprintf(stderr, "%s: Invalid input.", __func__);
136+
return out_quat;
137+
}
138+
139+
out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
140+
in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
141+
out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
142+
in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
143+
out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
144+
in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
145+
out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
146+
in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
147+
148+
return out_quat;
149+
}
150+
151+
/** @} */
152+
153+
static void test()
154+
{
155+
quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
156+
euler eul = euler_from_quat(&quat);
157+
printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);
158+
159+
quaternion test_quat = quat_from_euler(&eul);
160+
printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
161+
test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);
162+
163+
assert(fabsf(test_quat.w - quat.w) < .01);
164+
assert(fabsf(test_quat.q1 - quat.q1) < .01);
165+
assert(fabsf(test_quat.q2 - quat.q2) < .01);
166+
assert(fabsf(test_quat.q3 - quat.q3) < .01);
167+
}
168+
169+
int main()
170+
{
171+
test();
172+
return 0;
173+
}

0 commit comments

Comments
 (0)