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