mirror of
https://github.moeyy.xyz/https://github.com/TheAlgorithms/C.git
synced 2023-10-11 15:56:24 +08:00
Merge pull request #594 from kvedala/quaternions
[feature] added 3d geometry operations
This commit is contained in:
Ayaan Khan
GitHub
commit
ce263033f8
@ -52,6 +52,7 @@ add_subdirectory(hash)
|
||||
add_subdirectory(misc)
|
||||
add_subdirectory(games)
|
||||
add_subdirectory(sorting)
|
||||
add_subdirectory(geometry)
|
||||
add_subdirectory(graphics)
|
||||
add_subdirectory(searching)
|
||||
add_subdirectory(conversions)
|
||||
|
||||
@ -112,6 +112,11 @@
|
||||
## Games
|
||||
* [Tic Tac Toe](https://github.com/TheAlgorithms/C/blob/master/games/tic_tac_toe.c)
|
||||
|
||||
## Geometry
|
||||
* [Geometry Datatypes](https://github.com/TheAlgorithms/C/blob/master/geometry/geometry_datatypes.h)
|
||||
* [Quaternions](https://github.com/TheAlgorithms/C/blob/master/geometry/quaternions.c)
|
||||
* [Vectors 3D](https://github.com/TheAlgorithms/C/blob/master/geometry/vectors_3d.c)
|
||||
|
||||
## Graphics
|
||||
* [Spirograph](https://github.com/TheAlgorithms/C/blob/master/graphics/spirograph.c)
|
||||
|
||||
|
||||
20
geometry/CMakeLists.txt
Normal file
20
geometry/CMakeLists.txt
Normal file
@ -0,0 +1,20 @@
|
||||
# If necessary, use the RELATIVE flag, otherwise each source file may be listed
|
||||
# with full pathname. RELATIVE may makes it easier to extract an executable name
|
||||
# automatically.
|
||||
file( GLOB APP_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.c)
|
||||
# file( GLOB APP_SOURCES ${CMAKE_SOURCE_DIR}/*.c )
|
||||
# AUX_SOURCE_DIRECTORY(${CMAKE_CURRENT_SOURCE_DIR} APP_SOURCES)
|
||||
foreach( testsourcefile ${APP_SOURCES} )
|
||||
# I used a simple string replace, to cut off .cpp.
|
||||
string( REPLACE ".c" "" testname ${testsourcefile} )
|
||||
add_executable( ${testname} ${testsourcefile} )
|
||||
|
||||
if(OpenMP_C_FOUND)
|
||||
target_link_libraries(${testname} OpenMP::OpenMP_C)
|
||||
endif()
|
||||
if(MATH_LIBRARY)
|
||||
target_link_libraries(${testname} ${MATH_LIBRARY})
|
||||
endif()
|
||||
install(TARGETS ${testname} DESTINATION "bin/geometry")
|
||||
|
||||
endforeach( testsourcefile ${APP_SOURCES} )
|
||||
115
geometry/geometry_datatypes.h
Normal file
115
geometry/geometry_datatypes.h
Normal file
@ -0,0 +1,115 @@
|
||||
/**
|
||||
* @addtogroup quaternions Library for 3D Vectors & Quaternions
|
||||
* @{
|
||||
* @file
|
||||
* @brief Generic header that provides data types for 3D vectors and quaternions
|
||||
* @author Krishna Vedala
|
||||
*/
|
||||
|
||||
#ifndef __LIBQUAT_H_
|
||||
#define __LIBQUAT_H_
|
||||
|
||||
/** Minimum recognizable value. Any value less than this is considered to be
|
||||
* @f$=0@f$ */
|
||||
#define EPSILON 1e-9
|
||||
|
||||
/**
|
||||
* @addtogroup vec_3d 3D Vector operations
|
||||
* @{
|
||||
*/
|
||||
/** 3D vector type */
|
||||
typedef struct vec_3d_
|
||||
{
|
||||
float x; /**< X co-ordinate */
|
||||
float y; /**< Y co-ordinate */
|
||||
float z; /**< Z co-ordinate */
|
||||
} vec_3d;
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @addtogroup matrix Matrix operations
|
||||
* @{
|
||||
*/
|
||||
/** A 3x3 Matrix type definition */
|
||||
typedef struct mat_3x3_
|
||||
{
|
||||
union
|
||||
{ /**< 3 element row 1 */
|
||||
float row1[3];
|
||||
vec_3d vec1;
|
||||
};
|
||||
union
|
||||
{ /**< 3 element row 2 */
|
||||
float row2[3];
|
||||
vec_3d vec2;
|
||||
};
|
||||
union
|
||||
{ /**< 3 element row 3 */
|
||||
float row3[3];
|
||||
vec_3d vec3;
|
||||
};
|
||||
} mat_3x3;
|
||||
/** @} */
|
||||
|
||||
/** @addtogroup quats 3D Quaternion operations
|
||||
* @{
|
||||
*/
|
||||
/** a Quaternion type represented using a scalar \f$w\f$ or \f$q_0\f$ and a
|
||||
* 3D vector \f$\left(q_1,q_2,q_3\right)\f$
|
||||
*/
|
||||
typedef struct quaternion_
|
||||
{
|
||||
union
|
||||
{
|
||||
float w; /**< real part of quaternion */
|
||||
float q0; /**< real part of quaternion */
|
||||
};
|
||||
/**< dual part of quaternion */
|
||||
union
|
||||
{
|
||||
vec_3d dual; /**< can be a 3D vector */
|
||||
/** or individual values */
|
||||
struct
|
||||
{
|
||||
float q1, q2, q3;
|
||||
};
|
||||
};
|
||||
} quaternion;
|
||||
|
||||
/** 3D Euler or Tait-Bryan angles (in radian) */
|
||||
typedef struct euler_
|
||||
{
|
||||
union
|
||||
{
|
||||
float roll; /**< or bank \f$\phi\f$ = rotation about X axis */
|
||||
float bank; /**< or roll \f$\phi\f$ = rotation about X axis */
|
||||
};
|
||||
union
|
||||
{
|
||||
float pitch; /**< or elevation \f$\theta\f$ = rotation about Y axis */
|
||||
float elevation; /**< or pitch \f$\theta\f$ = rotation about Y axis */
|
||||
};
|
||||
union
|
||||
{
|
||||
float yaw; /**< or heading \f$\psi\f$ = rotation about Z axis */
|
||||
float heading; /**< or yaw \f$\psi\f$ = rotation about Z axis */
|
||||
};
|
||||
} euler;
|
||||
|
||||
/** @} */
|
||||
|
||||
/** @addtogroup dual_quats 3D Dual-Quaternion operations
|
||||
* @{
|
||||
*/
|
||||
/** a dual quaternion type */
|
||||
typedef struct dual_quat_
|
||||
{
|
||||
quaternion real; /**< real part of dual quaternion */
|
||||
quaternion dual; /**< dual part of dual quaternion */
|
||||
} dual_quat;
|
||||
|
||||
/** @} */
|
||||
|
||||
#endif // __LIBQUAT_H_
|
||||
|
||||
/** @} */
|
||||
173
geometry/quaternions.c
Normal file
173
geometry/quaternions.c
Normal file
@ -0,0 +1,173 @@
|
||||
/**
|
||||
* @file
|
||||
* @brief Functions related to 3D quaternions and Euler angles.
|
||||
* @author Krishna Vedala
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#ifdef __arm__ // if compiling for ARM-Cortex processors
|
||||
#define LIBQUAT_ARM
|
||||
#include <arm_math.h>
|
||||
#else
|
||||
#include <math.h>
|
||||
#endif
|
||||
#include <assert.h>
|
||||
|
||||
#include "geometry_datatypes.h"
|
||||
|
||||
/**
|
||||
* @addtogroup quats 3D Quaternion operations
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Function to convert given Euler angles to a quaternion.
|
||||
* \f{eqnarray*}{
|
||||
* q_{0} & =
|
||||
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
|
||||
* +
|
||||
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
|
||||
* q_{1} & =
|
||||
* &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
|
||||
* -
|
||||
* \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
|
||||
* q_{2} & =
|
||||
* &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
|
||||
* +
|
||||
* \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
|
||||
* q_{3} & =
|
||||
* &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
|
||||
* -
|
||||
* \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
|
||||
* \f}
|
||||
*
|
||||
* @param [in] in_euler input Euler angles instance
|
||||
* @returns converted quaternion
|
||||
*/
|
||||
quaternion quat_from_euler(const euler *in_euler)
|
||||
{
|
||||
quaternion out_quat;
|
||||
|
||||
if (!in_euler) // if null
|
||||
{
|
||||
fprintf(stderr, "%s: Invalid input.", __func__);
|
||||
return out_quat;
|
||||
}
|
||||
|
||||
quaternion temp;
|
||||
|
||||
float cy = cosf(in_euler->yaw * 0.5f);
|
||||
float sy = sinf(in_euler->yaw * 0.5f);
|
||||
float cp = cosf(in_euler->pitch * 0.5f);
|
||||
float sp = sinf(in_euler->pitch * 0.5f);
|
||||
float cr = cosf(in_euler->roll * 0.5f);
|
||||
float sr = sinf(in_euler->roll * 0.5f);
|
||||
|
||||
temp.w = cr * cp * cy + sr * sp * sy;
|
||||
temp.q1 = sr * cp * cy - cr * sp * sy;
|
||||
temp.q2 = cr * sp * cy + sr * cp * sy;
|
||||
temp.q3 = cr * cp * sy - sr * sp * cy;
|
||||
|
||||
return temp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to convert given quaternion to Euler angles.
|
||||
* \f{eqnarray*}{
|
||||
* \phi & = &
|
||||
* \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
|
||||
* \theta & =
|
||||
* &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
|
||||
* \psi & = &
|
||||
* \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
|
||||
* \f}
|
||||
*
|
||||
* @param [in] in_quat input quaternion instance
|
||||
* @returns converted euler angles
|
||||
*/
|
||||
euler euler_from_quat(const quaternion *in_quat)
|
||||
{
|
||||
euler out_euler;
|
||||
if (!in_quat) // if null
|
||||
{
|
||||
fprintf(stderr, "%s: Invalid input.", __func__);
|
||||
return out_euler;
|
||||
}
|
||||
|
||||
out_euler.roll = atan2f(
|
||||
2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
|
||||
1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
|
||||
out_euler.pitch =
|
||||
asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
|
||||
out_euler.yaw = atan2f(
|
||||
2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
|
||||
1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));
|
||||
|
||||
return out_euler;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to multiply two quaternions.
|
||||
* \f{eqnarray*}{
|
||||
* \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
|
||||
* & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
|
||||
* a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
|
||||
* b_{3}\end{bmatrix}\\
|
||||
* & = &
|
||||
* \begin{bmatrix}
|
||||
* a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
|
||||
* a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
|
||||
* a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
|
||||
* a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
|
||||
* \end{bmatrix}^{T}
|
||||
* \f}
|
||||
*
|
||||
* @param [in] in_quat1 first input quaternion instance
|
||||
* @param [in] in_quat2 second input quaternion instance
|
||||
* @returns resultant quaternion
|
||||
*/
|
||||
quaternion quaternion_multiply(const quaternion *in_quat1,
|
||||
const quaternion *in_quat2)
|
||||
{
|
||||
quaternion out_quat;
|
||||
if (!in_quat1 || !in_quat2) // if null
|
||||
{
|
||||
fprintf(stderr, "%s: Invalid input.", __func__);
|
||||
return out_quat;
|
||||
}
|
||||
|
||||
out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
|
||||
in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
|
||||
out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
|
||||
in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
|
||||
out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
|
||||
in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
|
||||
out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
|
||||
in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;
|
||||
|
||||
return out_quat;
|
||||
}
|
||||
|
||||
/** @} */
|
||||
|
||||
static void test()
|
||||
{
|
||||
quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
|
||||
euler eul = euler_from_quat(&quat);
|
||||
printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);
|
||||
|
||||
quaternion test_quat = quat_from_euler(&eul);
|
||||
printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
|
||||
test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);
|
||||
|
||||
assert(fabsf(test_quat.w - quat.w) < .01);
|
||||
assert(fabsf(test_quat.q1 - quat.q1) < .01);
|
||||
assert(fabsf(test_quat.q2 - quat.q2) < .01);
|
||||
assert(fabsf(test_quat.q3 - quat.q3) < .01);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
test();
|
||||
return 0;
|
||||
}
|
||||
238
geometry/vectors_3d.c
Normal file
238
geometry/vectors_3d.c
Normal file
@ -0,0 +1,238 @@
|
||||
/**
|
||||
* @file
|
||||
* @brief Functions related to 3D vector operations.
|
||||
* @author Krishna Vedala
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#ifdef __arm__ // if compiling for ARM-Cortex processors
|
||||
#define LIBQUAT_ARM
|
||||
#include <arm_math.h>
|
||||
#else
|
||||
#include <math.h>
|
||||
#endif
|
||||
#include <assert.h>
|
||||
|
||||
#include "geometry_datatypes.h"
|
||||
|
||||
/**
|
||||
* @addtogroup vec_3d 3D Vector operations
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Subtract one vector from another. @f[
|
||||
* \vec{c}=\vec{a}-\vec{b}=\left(a_x-b_x\right)\hat{i}+
|
||||
* \left(a_y-b_y\right)\hat{j}+\left(a_z-b_z\right)\hat{k}@f]
|
||||
* @param[in] a vector to subtract from
|
||||
* @param[in] b vector to subtract
|
||||
* @returns resultant vector
|
||||
*/
|
||||
vec_3d vector_sub(const vec_3d *a, const vec_3d *b)
|
||||
{
|
||||
vec_3d out;
|
||||
#ifdef LIBQUAT_ARM
|
||||
arm_sub_f32((float *)a, (float *)b, (float *)&out);
|
||||
#else
|
||||
out.x = a->x - b->x;
|
||||
out.y = a->y - b->y;
|
||||
out.z = a->z - b->z;
|
||||
#endif
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add one vector to another. @f[
|
||||
* \vec{c}=\vec{a}+\vec{b}=\left(a_x+b_x\right)\hat{i}+
|
||||
* \left(a_y+b_y\right)\hat{j}+\left(a_z+b_z\right)\hat{k}@f]
|
||||
* @param[in] a vector to add to
|
||||
* @param[in] b vector to add
|
||||
* @returns resultant vector
|
||||
*/
|
||||
vec_3d vector_add(const vec_3d *a, const vec_3d *b)
|
||||
{
|
||||
vec_3d out;
|
||||
#ifdef LIBQUAT_ARM
|
||||
arm_add_f32((float *)a, (float *)b, (float *)&out);
|
||||
#else
|
||||
out.x = a->x + b->x;
|
||||
out.y = a->y + b->y;
|
||||
out.z = a->z + b->z;
|
||||
#endif
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* Obtain the dot product of two 3D vectors.
|
||||
* @f[
|
||||
* \vec{a}\cdot\vec{b}=a_xb_x + a_yb_y + a_zb_z
|
||||
* @f]
|
||||
* @param[in] a first vector
|
||||
* @param[in] b second vector
|
||||
* @returns resulting dot product
|
||||
*/
|
||||
float dot_prod(const vec_3d *a, const vec_3d *b)
|
||||
{
|
||||
float dot;
|
||||
#ifdef LIBQUAT_ARM
|
||||
arm_dot_prod_f32((float *)a, (float *)b, &dot);
|
||||
#else
|
||||
dot = a->x * b->x;
|
||||
dot += a->y * b->y;
|
||||
dot += a->z * b->z;
|
||||
#endif
|
||||
|
||||
return dot;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute the vector product of two 3d vectors.
|
||||
* @f[\begin{align*}
|
||||
* \vec{a}\times\vec{b} &= \begin{vmatrix}
|
||||
* \hat{i} & \hat{j} & \hat{k}\\
|
||||
* a_x & a_y & a_z\\
|
||||
* b_x & b_y & b_z
|
||||
* \end{vmatrix}\\
|
||||
* &= \left(a_yb_z-b_ya_z\right)\hat{i} - \left(a_xb_z-b_xa_z\right)\hat{j}
|
||||
* + \left(a_xb_y-b_xa_y\right)\hat{k} \end{align*}
|
||||
* @f]
|
||||
* @param[in] a first vector @f$\vec{a}@f$
|
||||
* @param[in] b second vector @f$\vec{b}@f$
|
||||
* @returns resultant vector @f$\vec{o}=\vec{a}\times\vec{b}@f$
|
||||
*/
|
||||
vec_3d vector_prod(const vec_3d *a, const vec_3d *b)
|
||||
{
|
||||
vec_3d out; // better this way to avoid copying results to input
|
||||
// vectors themselves
|
||||
out.x = a->y * b->z - a->z * b->y;
|
||||
out.y = -a->x * b->z + a->z * b->x;
|
||||
out.z = a->x * b->y - a->y * b->x;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* Print formatted vector on stdout.
|
||||
* @param[in] a vector to print
|
||||
* @param[in] name name of the vector
|
||||
* @returns string representation of vector
|
||||
*/
|
||||
const char *print_vector(const vec_3d *a, const char *name)
|
||||
{
|
||||
static char vec_str[100]; // static to ensure the string life extends the
|
||||
// life of function
|
||||
|
||||
snprintf(vec_str, 99, "vec(%s) = (%.3g)i + (%.3g)j + (%.3g)k\n", name, a->x,
|
||||
a->y, a->z);
|
||||
return vec_str;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute the norm a vector.
|
||||
* @f[\lVert\vec{a}\rVert = \sqrt{\vec{a}\cdot\vec{a}} @f]
|
||||
* @param[in] a input vector
|
||||
* @returns norm of the given vector
|
||||
*/
|
||||
float vector_norm(const vec_3d *a)
|
||||
{
|
||||
float n = dot_prod(a, a);
|
||||
#ifdef LIBQUAT_ARM
|
||||
arm_sqrt_f32(*n, n);
|
||||
#else
|
||||
n = sqrtf(n);
|
||||
#endif
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
/**
|
||||
* Obtain unit vector in the same direction as given vector.
|
||||
* @f[\hat{a}=\frac{\vec{a}}{\lVert\vec{a}\rVert}@f]
|
||||
* @param[in] a input vector
|
||||
* @returns n unit vector in the direction of @f$\vec{a}@f$
|
||||
*/
|
||||
vec_3d unit_vec(const vec_3d *a)
|
||||
{
|
||||
vec_3d n = {0};
|
||||
|
||||
float norm = vector_norm(a);
|
||||
if (fabsf(norm) < EPSILON)
|
||||
{ // detect possible divide by 0
|
||||
return n;
|
||||
}
|
||||
|
||||
if (norm != 1.F) // perform division only if needed
|
||||
{
|
||||
n.x = a->x / norm;
|
||||
n.y = a->y / norm;
|
||||
n.z = a->z / norm;
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
/**
|
||||
* The cross product of vectors can be represented as a matrix
|
||||
* multiplication operation. This function obtains the `3x3` matrix
|
||||
* of the cross-product operator from the first vector.
|
||||
* @f[\begin{align*}
|
||||
* \left(\vec{a}\times\right)\vec{b} &= \tilde{A}_a\vec{b}\\
|
||||
* \tilde{A}_a &=
|
||||
* \begin{bmatrix}0&-a_z&a_y\\a_z&0&-a_x\\-a_y&a_x&0\end{bmatrix}
|
||||
* \end{align*}@f]
|
||||
* @param[in] a input vector
|
||||
* @returns the `3x3` matrix for the cross product operator
|
||||
* @f$\left(\vec{a}\times\right)@f$
|
||||
*/
|
||||
mat_3x3 get_cross_matrix(const vec_3d *a)
|
||||
{
|
||||
mat_3x3 A = {0., -a->z, a->y, a->z, 0., -a->x, -a->y, a->x, 0.};
|
||||
return A;
|
||||
}
|
||||
|
||||
/** @} */
|
||||
|
||||
/**
|
||||
* @brief Testing function
|
||||
* @returns `void`
|
||||
*/
|
||||
static void test()
|
||||
{
|
||||
vec_3d a = {1., 2., 3.};
|
||||
vec_3d b = {1., 1., 1.};
|
||||
float d;
|
||||
|
||||
// printf("%s", print_vector(&a, "a"));
|
||||
// printf("%s", print_vector(&b, "b"));
|
||||
|
||||
d = vector_norm(&a);
|
||||
// printf("|a| = %.4g\n", d);
|
||||
assert(fabsf(d - 3.742f) < 0.01);
|
||||
d = vector_norm(&b);
|
||||
// printf("|b| = %.4g\n", d);
|
||||
assert(fabsf(d - 1.732f) < 0.01);
|
||||
|
||||
d = dot_prod(&a, &b);
|
||||
// printf("Dot product: %f\n", d);
|
||||
assert(fabsf(d - 6.f) < 0.01);
|
||||
|
||||
vec_3d c = vector_prod(&a, &b);
|
||||
// printf("Vector product ");
|
||||
// printf("%s", print_vector(&c, "c"));
|
||||
assert(fabsf(c.x - (-1.f)) < 0.01);
|
||||
assert(fabsf(c.y - (2.f)) < 0.01);
|
||||
assert(fabsf(c.z - (-1.f)) < 0.01);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main function
|
||||
*
|
||||
* @return 0 on exit
|
||||
*/
|
||||
int main(void)
|
||||
{
|
||||
test();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user