1 /****************************************************************************** 2 * @file quaternion_math_functions.h 3 * @brief Public header file for CMSIS DSP Library 4 * @version V1.10.0 5 * @date 08 July 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 */ 56 void arm_quaternion_norm_f32(const float32_t *pInputQuaternions, 57 float32_t *pNorms, 58 uint32_t nbQuaternions); 59 60 61 /** 62 @brief Floating-point quaternion inverse. 63 @param[in] pInputQuaternions points to the input vector of quaternions 64 @param[out] pInverseQuaternions points to the output vector of inverse quaternions 65 @param[in] nbQuaternions number of quaternions in each vector 66 */ 67 void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions, 68 float32_t *pInverseQuaternions, 69 uint32_t nbQuaternions); 70 71 72 /** 73 @brief Floating-point quaternion conjugates. 74 @param[in] pInputQuaternions points to the input vector of quaternions 75 @param[out] pConjugateQuaternions points to the output vector of conjugate quaternions 76 @param[in] nbQuaternions number of quaternions in each vector 77 */ 78 void arm_quaternion_conjugate_f32(const float32_t *inputQuaternions, 79 float32_t *pConjugateQuaternions, 80 uint32_t nbQuaternions); 81 82 83 /** 84 @brief Floating-point normalization of quaternions. 85 @param[in] pInputQuaternions points to the input vector of quaternions 86 @param[out] pNormalizedQuaternions points to the output vector of normalized quaternions 87 @param[in] nbQuaternions number of quaternions in each vector 88 */ 89 void arm_quaternion_normalize_f32(const float32_t *inputQuaternions, 90 float32_t *pNormalizedQuaternions, 91 uint32_t nbQuaternions); 92 93 94 /** 95 @brief Floating-point product of two quaternions. 96 @param[in] qa First quaternion 97 @param[in] qb Second quaternion 98 @param[out] r Product of two quaternions 99 */ 100 void arm_quaternion_product_single_f32(const float32_t *qa, 101 const float32_t *qb, 102 float32_t *r); 103 104 105 /** 106 @brief Floating-point elementwise product two quaternions. 107 @param[in] qa First array of quaternions 108 @param[in] qb Second array of quaternions 109 @param[out] r Elementwise product of quaternions 110 @param[in] nbQuaternions Number of quaternions in the array 111 */ 112 void arm_quaternion_product_f32(const float32_t *qa, 113 const float32_t *qb, 114 float32_t *r, 115 uint32_t nbQuaternions); 116 117 118 /** 119 * @brief Conversion of quaternion to equivalent rotation matrix. 120 * @param[in] pInputQuaternions points to an array of normalized quaternions 121 * @param[out] pOutputRotations points to an array of 3x3 rotations (in row order) 122 * @param[in] nbQuaternions in the array 123 * 124 * <b>Format of rotation matrix</b> 125 * \par 126 * The quaternion a + ib + jc + kd is converted into rotation matrix: 127 * a^2 + b^2 - c^2 - d^2 2bc - 2ad 2bd + 2ac 128 * 2bc + 2ad a^2 - b^2 + c^2 - d^2 2cd - 2ab 129 * 2bd - 2ac 2cd + 2ab a^2 - b^2 - c^2 + d^2 130 * 131 * Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22 132 */ 133 void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 134 float32_t *pOutputRotations, 135 uint32_t nbQuaternions); 136 137 138 /** 139 * @brief Conversion of a rotation matrix to equivalent quaternion. 140 * @param[in] pInputRotations points to an array 3x3 rotation matrix (in row order) 141 * @param[out] pOutputQuaternions points to an array of quaternions 142 * @param[in] nbQuaternions in the array 143 */ 144 void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 145 float32_t *pOutputQuaternions, 146 uint32_t nbQuaternions); 147 148 149 #ifdef __cplusplus 150 } 151 #endif 152 153 #endif /* ifndef _QUATERNION_MATH_FUNCTIONS_H_ */ 154
