arm_quaternion2rotation_f32.c
1 /* ---------------------------------------------------------------------- 2 * Project: CMSIS DSP Library 3 * Title: arm_quaternion2rotation_f32.c 4 * Description: Floating-point quaternion 2 rotation conversion 5 * 6 * $Date: 23 April 2021 7 * $Revision: V1.9.0 8 * 9 * Target Processor: Cortex-M and Cortex-A cores 10 * -------------------------------------------------------------------- */ 11 /* 12 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved. 13 * 14 * SPDX-License-Identifier: Apache-2.0 15 * 16 * Licensed under the Apache License, Version 2.0 (the License); you may 17 * not use this file except in compliance with the License. 18 * You may obtain a copy of the License at 19 * 20 * www.apache.org/licenses/LICENSE-2.0 21 * 22 * Unless required by applicable law or agreed to in writing, software 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 25 * See the License for the specific language governing permissions and 26 * limitations under the License. 27 */ 28 29 #include "dsp/quaternion_math_functions.h" 30 #include <math.h> 31 32 /** 33 @ingroup groupQuaternionMath 34 */ 35 36 /** 37 @defgroup QuatConv Quaternion conversions 38 39 Conversions between quaternion and rotation representations. 40 */ 41 42 /** 43 @ingroup QuatConv 44 */ 45 46 /** 47 @defgroup QuatRot Quaternion to Rotation 48 49 Conversions from quaternion to rotation. 50 */ 51 52 /** 53 @addtogroup QuatRot 54 @{ 55 */ 56 57 /** 58 @brief Conversion of quaternion to equivalent rotation matrix. 59 @param[in] pInputQuaternions points to an array of normalized quaternions 60 @param[out] pOutputRotations points to an array of 3x3 rotations (in row order) 61 @param[in] nbQuaternions number of quaternions in the array 62 @return none. 63 64 @par 65 Format of rotation matrix 66 67 68 The quaternion a + ib + jc + kd is converted into rotation matrix: 69 <pre> 70 a^2 + b^2 - c^2 - d^2 2bc - 2ad 2bd + 2ac 71 2bc + 2ad a^2 - b^2 + c^2 - d^2 2cd - 2ab 72 2bd - 2ac 2cd + 2ab a^2 - b^2 - c^2 + d^2 73 </pre> 74 Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22 75 */ 76 77 #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) 78 79 #include "arm_helium_utils.h" 80 81 void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 82 float32_t *pOutputRotations, 83 uint32_t nbQuaternions) 84 { 85 f32x4_t vec0,vec1, vec2 ,vec3; 86 float32_t q2q3, tmp1, tmp2 ; 87 88 for(uint32_t nb=0; nb < nbQuaternions; nb++) 89 { 90 91 // q0 q1 q2 q3 92 vec0 = vld1q(pInputQuaternions); 93 94 // q0^2 q1^2 q2^2 q3^2 95 vec1 = vmulq(vec0,vec0); 96 97 // q0^2 q1q0 q2q0 q3q0 98 vec2 = vmulq_n_f32(vec0, vgetq_lane(vec0,0)); 99 100 // 2 (q0^2 q1q0 q2q0 q3q0) 101 vec2 = vmulq_n_f32(vec2, 2.0f); 102 103 104 // 2 q2q3 105 q2q3 = vgetq_lane(vec0,2) * vgetq_lane(vec0,3); 106 q2q3 = q2q3 * 2.0f; 107 108 // 2 (q0q1 q1^2 q2q1 q3q1) 109 vec3 = vmulq_n_f32(vec0, vgetq_lane(vec0,1)); 110 vec3 = vmulq_n_f32(vec3, 2.0f); 111 112 113 114 vec0 = vsetq_lane(vgetq_lane(vec1,0) + vgetq_lane(vec1,1),vec0,0); 115 vec0 = vsetq_lane(vgetq_lane(vec0,0) - vgetq_lane(vec1,2),vec0,0); 116 vec0 = vsetq_lane(vgetq_lane(vec0,0) - vgetq_lane(vec1,3),vec0,0); 117 vec0 = vsetq_lane(vgetq_lane(vec3,2) - vgetq_lane(vec2,3),vec0,1); 118 vec0 = vsetq_lane(vgetq_lane(vec3,3) + vgetq_lane(vec2,2),vec0,2); 119 vec0 = vsetq_lane(vgetq_lane(vec3,2) + vgetq_lane(vec2,3),vec0,3); 120 121 vst1q(pOutputRotations, vec0); 122 pOutputRotations += 4; 123 124 tmp1 = vgetq_lane(vec1,0) - vgetq_lane(vec1,1); 125 tmp2 = vgetq_lane(vec1,2) - vgetq_lane(vec1,3); 126 127 128 vec0 = vsetq_lane(tmp1 + tmp2,vec0,0); 129 vec0 = vsetq_lane(q2q3 - vgetq_lane(vec2,1) ,vec0,1); 130 vec0 = vsetq_lane(vgetq_lane(vec3,3) - vgetq_lane(vec2,2),vec0,2); 131 vec0 = vsetq_lane(q2q3 + vgetq_lane(vec2,1) ,vec0,3); 132 133 vst1q(pOutputRotations, vec0); 134 pOutputRotations += 4; 135 136 *pOutputRotations = tmp1 - tmp2; 137 pOutputRotations ++; 138 139 pInputQuaternions += 4; 140 } 141 } 142 143 #else 144 void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions, 145 float32_t *pOutputRotations, 146 uint32_t nbQuaternions) 147 { 148 uint32_t nb; 149 for(nb=0; nb < nbQuaternions; nb++) 150 { 151 float32_t q00 = SQ(pInputQuaternions[0 + nb * 4]); 152 float32_t q11 = SQ(pInputQuaternions[1 + nb * 4]); 153 float32_t q22 = SQ(pInputQuaternions[2 + nb * 4]); 154 float32_t q33 = SQ(pInputQuaternions[3 + nb * 4]); 155 float32_t q01 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[1 + nb * 4]; 156 float32_t q02 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[2 + nb * 4]; 157 float32_t q03 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[3 + nb * 4]; 158 float32_t q12 = pInputQuaternions[1 + nb * 4]*pInputQuaternions[2 + nb * 4]; 159 float32_t q13 = pInputQuaternions[1 + nb * 4]*pInputQuaternions[3 + nb * 4]; 160 float32_t q23 = pInputQuaternions[2 + nb * 4]*pInputQuaternions[3 + nb * 4]; 161 162 float32_t xx = q00 + q11 - q22 - q33; 163 float32_t yy = q00 - q11 + q22 - q33; 164 float32_t zz = q00 - q11 - q22 + q33; 165 float32_t xy = 2*(q12 - q03); 166 float32_t xz = 2*(q13 + q02); 167 float32_t yx = 2*(q12 + q03); 168 float32_t yz = 2*(q23 - q01); 169 float32_t zx = 2*(q13 - q02); 170 float32_t zy = 2*(q23 + q01); 171 172 pOutputRotations[0 + nb * 9] = xx; pOutputRotations[1 + nb * 9] = xy; pOutputRotations[2 + nb * 9] = xz; 173 pOutputRotations[3 + nb * 9] = yx; pOutputRotations[4 + nb * 9] = yy; pOutputRotations[5 + nb * 9] = yz; 174 pOutputRotations[6 + nb * 9] = zx; pOutputRotations[7 + nb * 9] = zy; pOutputRotations[8 + nb * 9] = zz; 175 } 176 } 177 #endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ 178 179 /** 180 @} end of QuatRot group 181 */