arm_rotation2quaternion_f32.c
1 /* ---------------------------------------------------------------------- 2 * Project: CMSIS DSP Library 3 * Title: arm_rotation2quaternion_f32.c 4 * Description: Floating-point rotation to quaternion 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 #define RI(x,y) r[(3*(x) + (y))] 33 34 35 /** 36 @ingroup QuatConv 37 */ 38 39 /** 40 @defgroup RotQuat Rotation to Quaternion 41 42 Conversions from rotation to quaternion. 43 */ 44 45 /** 46 @addtogroup RotQuat 47 @{ 48 */ 49 50 /** 51 * @brief Conversion of a rotation matrix to an equivalent quaternion. 52 * @param[in] pInputRotations points to an array 3x3 rotation matrix (in row order) 53 * @param[out] pOutputQuaternions points to an array quaternions 54 * @param[in] nbQuaternions number of quaternions in the array 55 * @return none. 56 * 57 * q and -q are representing the same rotation. This ambiguity must be taken into 58 * account when using the output of this function. 59 * 60 */ 61 62 #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) 63 64 #include "arm_helium_utils.h" 65 66 #define R00 vgetq_lane(q1,0) 67 #define R01 vgetq_lane(q1,1) 68 #define R02 vgetq_lane(q1,2) 69 #define R10 vgetq_lane(q1,3) 70 #define R11 vgetq_lane(q2,0) 71 #define R12 vgetq_lane(q2,1) 72 #define R20 vgetq_lane(q2,2) 73 #define R21 vgetq_lane(q2,3) 74 #define R22 ro22 75 76 void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 77 float32_t *pOutputQuaternions, 78 uint32_t nbQuaternions) 79 { 80 float32_t ro22, trace; 81 f32x4_t q1,q2, q; 82 83 float32_t doubler; 84 float32_t s; 85 86 q = vdupq_n_f32(0.0f); 87 88 for(uint32_t nb=0; nb < nbQuaternions; nb++) 89 { 90 q1 = vld1q(pInputRotations); 91 pInputRotations += 4; 92 93 q2 = vld1q(pInputRotations); 94 pInputRotations += 4; 95 96 ro22 = *pInputRotations++; 97 98 trace = R00 + R11 + R22; 99 100 101 if (trace > 0) 102 { 103 (void)arm_sqrt_f32(trace + 1.0f, &doubler) ; // invs=4*qw 104 doubler = 2.0f*doubler; 105 s = 1.0f / doubler; 106 107 q1 = vmulq_n_f32(q1,s); 108 q2 = vmulq_n_f32(q2,s); 109 110 q[0] = 0.25f * doubler; 111 q[1] = R21 - R12; 112 q[2] = R02 - R20; 113 q[3] = R10 - R01; 114 } 115 else if ((R00 > R11) && (R00 > R22) ) 116 { 117 (void)arm_sqrt_f32(1.0f + R00 - R11 - R22,&doubler); // invs=4*qx 118 doubler = 2.0f*doubler; 119 s = 1.0f / doubler; 120 121 q1 = vmulq_n_f32(q1,s); 122 q2 = vmulq_n_f32(q2,s); 123 124 q[0] = R21 - R12; 125 q[1] = 0.25f * doubler; 126 q[2] = R01 + R10; 127 q[3] = R02 + R20; 128 } 129 else if (R11 > R22) 130 { 131 (void)arm_sqrt_f32(1.0f + R11 - R00 - R22,&doubler); // invs=4*qy 132 doubler = 2.0f*doubler; 133 s = 1.0f / doubler; 134 135 q1 = vmulq_n_f32(q1,s); 136 q2 = vmulq_n_f32(q2,s); 137 138 q[0] = R02 - R20; 139 q[1] = R01 + R10; 140 q[2] = 0.25f * doubler; 141 q[3] = R12 + R21; 142 } 143 else 144 { 145 (void)arm_sqrt_f32(1.0f + R22 - R00 - R11,&doubler); // invs=4*qz 146 doubler = 2.0f*doubler; 147 s = 1.0f / doubler; 148 149 q1 = vmulq_n_f32(q1,s); 150 q2 = vmulq_n_f32(q2,s); 151 152 q[0] = R10 - R01; 153 q[1] = R02 + R20; 154 q[2] = R12 + R21; 155 q[3] = 0.25f * doubler; 156 } 157 158 vst1q(pOutputQuaternions, q); 159 pOutputQuaternions += 4; 160 161 } 162 } 163 164 #else 165 void arm_rotation2quaternion_f32(const float32_t *pInputRotations, 166 float32_t *pOutputQuaternions, 167 uint32_t nbQuaternions) 168 { 169 uint32_t nb; 170 for(nb=0; nb < nbQuaternions; nb++) 171 { 172 const float32_t *r=&pInputRotations[nb*9]; 173 float32_t *q=&pOutputQuaternions[nb*4]; 174 175 float32_t trace = RI(0,0) + RI(1,1) + RI(2,2); 176 177 float32_t doubler; 178 float32_t s; 179 180 181 182 if (trace > 0.0f) 183 { 184 doubler = sqrtf(trace + 1.0f) * 2.0f; // invs=4*qw 185 s = 1.0f / doubler; 186 q[0] = 0.25f * doubler; 187 q[1] = (RI(2,1) - RI(1,2)) * s; 188 q[2] = (RI(0,2) - RI(2,0)) * s; 189 q[3] = (RI(1,0) - RI(0,1)) * s; 190 } 191 else if ((RI(0,0) > RI(1,1)) && (RI(0,0) > RI(2,2)) ) 192 { 193 doubler = sqrtf(1.0f + RI(0,0) - RI(1,1) - RI(2,2)) * 2.0f; // invs=4*qx 194 s = 1.0f / doubler; 195 q[0] = (RI(2,1) - RI(1,2)) * s; 196 q[1] = 0.25f * doubler; 197 q[2] = (RI(0,1) + RI(1,0)) * s; 198 q[3] = (RI(0,2) + RI(2,0)) * s; 199 } 200 else if (RI(1,1) > RI(2,2)) 201 { 202 doubler = sqrtf(1.0f + RI(1,1) - RI(0,0) - RI(2,2)) * 2.0f; // invs=4*qy 203 s = 1.0f / doubler; 204 q[0] = (RI(0,2) - RI(2,0)) * s; 205 q[1] = (RI(0,1) + RI(1,0)) * s; 206 q[2] = 0.25f * doubler; 207 q[3] = (RI(1,2) + RI(2,1)) * s; 208 } 209 else 210 { 211 doubler = sqrtf(1.0f + RI(2,2) - RI(0,0) - RI(1,1)) * 2.0f; // invs=4*qz 212 s = 1.0f / doubler; 213 q[0] = (RI(1,0) - RI(0,1)) * s; 214 q[1] = (RI(0,2) + RI(2,0)) * s; 215 q[2] = (RI(1,2) + RI(2,1)) * s; 216 q[3] = 0.25f * doubler; 217 } 218 219 } 220 } 221 #endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ 222 223 /** 224 @} end of RotQuat group 225 */