/** * * * @brief Functions for quaternion manipulation. * * All functions have two versions, one that acts on an rc_vector_t of length 4 * and another that acts on normal C arrays, also of length 4. They are * functionally identical and just give the user options depending on how they * represent quaternions in their code. * * @author James Strawson * @date 2016 * * @addtogroup Quaternion * @ingroup Math * @{ */ #ifndef RC_QUATERNION_H #define RC_QUATERNION_H #ifdef __cplusplus extern "C" { #endif #include #include /** * @brief Returns the length of a quaternion vector by finding its 2-norm. * * @param[in] q The quarternion in form of a vector of length 4 * * @return Returns the norm, or prints an error message and returns -1.0f on * error. */ double rc_quaternion_norm(rc_vector_t q); /** * @brief Returns the length of a quaternion vector by finding its 2-norm. * * @param[in] q The quarternion in form of an array of length 4 * * @return Returns the norm, or prints an error message and returns -1.0f on * error. */ double rc_quaternion_norm_array(double q[4]); /** * @brief Normalizes a quaternion in-place to have length 1.0 * * @param q The quarternion in form of a vector of lenth 4 * * @return Returns 0 on success or -1 on failure */ int rc_normalize_quaternion(rc_vector_t* q); /** * @brief Normalizes a quaternion in-place to have length 1.0 * * @param q The quarternion in form of an array of length 4 * * @return Returns 0 on success or -1 on failure */ int rc_normalize_quaternion_array(double q[4]); /** * @brief Calculates 321 Tait Bryan angles in array order XYZ with * operation order 321(yaw-Z, pitch-Y, roll-x). * * If tb is already allocated and of length 3 then the new values are written in * place, otherwise any existing memory is freed and a new vector of length 3 is * allocated for tb. * * @param[in] q The quarternion in form of a vector of lenth 4 * @param tb Output tait-bryan angles * * @return Returns 0 on success or -1 on failure */ int rc_quaternion_to_tb(rc_vector_t q, rc_vector_t* tb); /** * @brief Calculates 321 Tait Bryan angles in array order XYZ with * operation order 321(yaw-Z, pitch-Y, roll-x). * * @param[in] q The quarternion in form of an array of lenth 4 * @param[out] tb Output tait-bryan angles * * @return Returns 0 on success or -1 on failure */ int rc_quaternion_to_tb_array(double q[4], double tb[3]); /** * @brief Calculates quaternion vector q from tait-bryan angles tb. * * If q is already of length 4 then old contents are simply overwritten. * Otherwise q'd existing memory is freed and new memory is allocated to aint * memory leaks. tb angles are 321 Tait Bryan angles in array order XYZ with * operation order 321(yaw-Z, pitch-Y, roll-x). * * @param[in] tb input tait-bryan angles * @param[out] q output quaternion * * @return Returns 0 on success or -1 on failure */ int rc_quaternion_from_tb(rc_vector_t tb, rc_vector_t* q); /** * @brief Calculates quaternion vector q from tait-bryan angles tb. * * tb angles are 321 Tait Bryan angles in array order XYZ with operation order * 321(yaw-Z, pitch-Y, roll-x). * * @param[in] tb input tait-bryan angles * @param[out] q output quaternion * * @return Returns 0 on success or -1 on failure */ int rc_quaternion_from_tb_array(double tb[3], double q[4]); /** * @brief Calculates conjugate of quaternion q. * * Populates quaternion vector c with the conjugate of quaternion q where the 3 * imaginary parts ijk are multiplied by -1. If c is already of length 4 then * the old values are overwritten. Otherwise the old memory in c is freed and * new memory is allocated to help prevent memory leaks. * * @param[in] q The quarter * @param[out] c output conjugate * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_conjugate(rc_vector_t q, rc_vector_t* c); /** * @brief Calculates conjugate of quaternion q and overwrites the old q. * * Populates quaternion vector q with its conjugate where the 3 imaginary parts * ijk are multiplied by -1. * * @param[in] q The quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_conjugate_inplace(rc_vector_t* q); /** * @brief Calculates conjugate of quaternion q. * * Populates quaternion vector c with the conjugate of quaternion q where the 3 * imaginary parts ijk are multiplied by -1. * * @param[in] q The quarter * @param[out] c output conjugate * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_conjugate_array(double q[4], double c[4]); /** * @brief Calculates conjugate of quaternion q and overwrites the old q. * * Populates quaternion vector q with its conjugate where the 3 imaginary parts * ijk are multiplied by -1. * * @param[in] q The quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_conjugate_array_inplace(double q[4]); /** * @brief Populates vector i with the imaginary components ijk of of * quaternion vector q. * * If img is already of length 3 then its original contents are overwritten. * Otherwise the original allocated memory is freed and new memory is allocated. * * @param[in] q The quarternion * @param[out] img imaginary part * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_imaginary_part(rc_vector_t q, rc_vector_t* img); /** * @brief Calculates the quaternion Hamilton product ab=c * * Places the result in vector argument c. If c is already of length 4 then the * old values are overwritten. Otherwise the old memory in c is freed and new * memory is allocated to help prevent memory leaks. * * @param[in] a First input * @param[in] b second input * @param[out] c output * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_multiply(rc_vector_t a, rc_vector_t b, rc_vector_t* c); /** * @brief Calculates the quaternion Hamilton product ab=c * * Places the result in vector argument c. * * @param[in] a First input * @param[in] b second input * @param[out] c output * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_multiply_array(double a[4], double b[4], double c[4]); /** * @brief Rotates the quaternion p by quaternion q with the operation * p'=qpq* * * p is modified in place, q is unmodified. * * @param p quaternion to be rotated * @param[in] q rotation quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_rotate(rc_vector_t* p, rc_vector_t q); /** * @brief Rotates the quaternion p by quaternion q with the operation * p'=qpq* * * p is modified in place, q is unmodified. * * @param p quaternion to be rotated * @param[in] q rotation quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_rotate_array(double p[4], double q[4]); /** * @brief Rotate a 3D vector v in-place about the origin by quaternion q by * converting v to a quaternion and performing the operation p'=qpq* * * v is modified in place, q is unmodified. * * @param v vectpr to be rotated * @param[in] q rotation quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_rotate_vector(rc_vector_t* v, rc_vector_t q); /** * @brief Rotate a 3D vector v in-place about the origin by quaternion q by * converting v to a quaternion and performing the operation p'=qpq* * * v is modified in place, q is unmodified. * * @param v vectpr to be rotated * @param[in] q rotation quarternion * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_rotate_vector_array(double v[3], double q[4]); /** * @brief Converts a normalized quaternion to a 3x3 orthogonal rotation * matrix. * * q must be normalized before calling this! The orthogonal matrix corresponds * to a rotation by the unit quaternion q when post-multiplied with a column * vector as such: v_rotated=mv. * * If m is already 3x3 then its contents are overwritten, otherwise its existing * memory is freed and new memory is allocated. * * @param[in] q The quarter * @param m output 3x3 rotation matrix * * @return Returns 0 on success or -1 on failure. */ int rc_quaternion_to_rotation_matrix(rc_vector_t q, rc_matrix_t* m); #ifdef __cplusplus } #endif #endif // RC_QUATERNION_H /** @} end group math*/