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