1  /* ----------------------------------------------------------------------
  2   * Project:      CMSIS DSP Library
  3   * Title:        arm_quaternion_normalize_f32.c
  4   * Description:  Floating-point quaternion normalization
  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 QuatNormalized Quaternion normalization
 38  
 39    Compute a normalized quaternion.
 40   */
 41  
 42  /**
 43    @addtogroup QuatNormalized
 44    @{
 45   */
 46  
 47  /**
 48    @brief         Floating-point normalization of quaternions.
 49    @param[in]     pInputQuaternions            points to the input vector of quaternions
 50    @param[out]    pNormalizedQuaternions       points to the output vector of normalized quaternions
 51    @param[in]     nbQuaternions                number of quaternions in each vector
 52    @return        none
 53   */
 54  
 55  #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
 56  
 57  #include "arm_helium_utils.h"
 58  
 59  void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions, 
 60      float32_t *pNormalizedQuaternions, 
 61      uint32_t nbQuaternions)
 62  {
 63     f32x4_t vec1,vec2;
 64     float32_t squaredSum,norm;
 65  
 66     for(uint32_t i=0; i < nbQuaternions; i++)
 67     {
 68        vec1 = vld1q(pInputQuaternions);
 69        vec2 = vmulq(vec1,vec1);
 70        squaredSum = vecAddAcrossF32Mve(vec2);
 71        arm_sqrt_f32(squaredSum,&norm);
 72        vec1 = vmulq_n_f32(vec1, 1.0f / norm);
 73        vst1q(pNormalizedQuaternions, vec1);
 74  
 75        pInputQuaternions += 4;
 76        pNormalizedQuaternions += 4;
 77  
 78     }
 79  }
 80  
 81  #else
 82  void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions, 
 83      float32_t *pNormalizedQuaternions, 
 84      uint32_t nbQuaternions)
 85  {
 86     float32_t temp;
 87  
 88     uint32_t i;
 89     for(i=0; i < nbQuaternions; i++)
 90     {
 91        temp = SQ(pInputQuaternions[4 * i + 0]) +
 92               SQ(pInputQuaternions[4 * i + 1]) +
 93               SQ(pInputQuaternions[4 * i + 2]) +
 94               SQ(pInputQuaternions[4 * i + 3]);
 95        temp = sqrtf(temp);
 96  
 97        pNormalizedQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0] / temp;
 98        pNormalizedQuaternions[4 * i + 1] = pInputQuaternions[4 * i + 1] / temp;
 99        pNormalizedQuaternions[4 * i + 2] = pInputQuaternions[4 * i + 2] / temp;
100        pNormalizedQuaternions[4 * i + 3] = pInputQuaternions[4 * i + 3] / temp;
101     }
102  }
103  #endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
104  
105  /**
106    @} end of QuatNormalized group
107   */