1 /****************************************************************************** 2 * @file quaternion_math_functions.h 3 * @brief Public header file for CMSIS DSP Library 4 * @version V1.9.0 5 * @date 23 April 2021 6 * 7 * Target Processor: Cortex-M and Cortex-A cores 8 ******************************************************************************/ 9 /* 10 * Copyright (c) 2010-2021 Arm Limited or its affiliates. All rights reserved. 11 * 12 * SPDX-License-Identifier: Apache-2.0 13 * 14 * Licensed under the Apache License, Version 2.0 (the License); you may 15 * not use this file except in compliance with the License. 16 * You may obtain a copy of the License at 17 * 18 * www.apache.org/licenses/LICENSE-2.0 19 * 20 * Unless required by applicable law or agreed to in writing, software 21 * distributed under the License is distributed on an AS IS BASIS, WITHOUT 22 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 * See the License for the specific language governing permissions and 24 * limitations under the License. 25 */ 26 27 28 #ifndef _QUATERNION_MATH_FUNCTIONS_H_ 29 #define _QUATERNION_MATH_FUNCTIONS_H_ 30 31 #include "arm_math_types.h" 32 #include "arm_math_memory.h" 33 34 #include "dsp/none.h" 35 #include "dsp/utils.h" 36 37 38 #ifdef __cplusplus 39 extern "C" 40 { 41 #endif 42 43 /** 44 * @defgroup groupQuaternionMath Quaternion Math Functions 45 * Functions to operates on quaternions and convert between a 46 * rotation and quaternion representation. 47 */ 48 49 50 /** 51 @brief Floating-point quaternion Norm. 52 @param[in] pInputQuaternions points to the input vector of quaternions 53 @param[out] pNorms points to the output vector of norms 54 @param[in] nbQuaternions number of quaternions in each vector 55 @return none 56 */ 57 58 59 60 void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 61 float32_t *pNorms, 62 uint32_t nbQuaternions); 63 64 65 /** 66 @brief Floating-point quaternion inverse. 67 @param[in] pInputQuaternions points to the input vector of quaternions 68 @param[out] pInverseQuaternions points to the output vector of inverse quaternions 69 @param[in] nbQuaternions number of quaternions in each vector 70 @return none 71 */ 72 73 void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 74 float32_t *pInverseQuaternions, 75 uint32_t nbQuaternions); 76 77 /** 78 @brief Floating-point quaternion conjugates. 79 @param[in] pInputQuaternions points to the input vector of quaternions 80 @param[out] pConjugateQuaternions points to the output vector of conjugate quaternions 81 @param[in] nbQuaternions number of quaternions in each vector 82 @return none 83 */ 84 void arm_quaternion_conjugate_f32(const float32_t *inputQuaternions, 85 float32_t *pConjugateQuaternions, 86 uint32_t nbQuaternions); 87 88 /** 89 @brief Floating-point normalization of quaternions. 90 @param[in] pInputQuaternions points to the input vector of quaternions 91 @param[out] pNormalizedQuaternions points to the output vector of normalized quaternions 92 @param[in] nbQuaternions number of quaternions in each vector 93 @return none 94 */ 95 void arm_quaternion_normalize_f32(const float32_t *inputQuaternions, 96 float32_t *pNormalizedQuaternions, 97 uint32_t nbQuaternions); 98 99 100 /** 101 @brief Floating-point product of two quaternions. 102 @param[in] qa First quaternion 103 @param[in] qb Second quaternion 104 @param[out] r Product of two quaternions 105 @return none 106 */ 107 void arm_quaternion_product_single_f32(const float32_t *qa, 108 const float32_t *qb, 109 float32_t *r); 110 111 /** 112 @brief Floating-point elementwise product two quaternions. 113 @param[in] qa First array of quaternions 114 @param[in] qb Second array of quaternions 115 @param[out] r Elementwise product of quaternions 116 @param[in] nbQuaternions Number of quaternions in the array 117 @return none 118 */ 119 void arm_quaternion_product_f32(const float32_t *qa, 120 const float32_t *qb, 121 float32_t *r, 122 uint32_t nbQuaternions); 123 124 /** 125 * @brief Conversion of quaternion to equivalent rotation matrix. 126 * @param[in] pInputQuaternions points to an array of normalized quaternions 127 * @param[out] pOutputRotations points to an array of 3x3 rotations (in row order) 128 * @param[in] nbQuaternions in the array 129 * @return none. 130 * 131 * <b>Format of rotation matrix</b> 132 * \par 133 * The quaternion a + ib + jc + kd is converted into rotation matrix: 134 * a^2 + b^2 - c^2 - d^2 2bc - 2ad 2bd + 2ac 135 * 2bc + 2ad a^2 - b^2 + c^2 - d^2 2cd - 2ab 136 * 2bd - 2ac 2cd + 2ab a^2 - b^2 - c^2 + d^2 137 * 138 * Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22 139 */ 140 void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 141 float32_t *pOutputRotations, 142 uint32_t nbQuaternions); 143 144 /** 145 * @brief Conversion of a rotation matrix to equivalent quaternion. 146 * @param[in] pInputRotations points to an array 3x3 rotation matrix (in row order) 147 * @param[out] pOutputQuaternions points to an array of quaternions 148 * @param[in] nbQuaternions in the array 149 * @return none. 150 */ 151 void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 152 float32_t *pOutputQuaternions, 153 uint32_t nbQuaternions); 154 155 #ifdef __cplusplus 156 } 157 #endif 158 159 #endif /* ifndef _QUATERNION_MATH_FUNCTIONS_H_ */ 160
