1  /* ----------------------------------------------------------------------
  2   * Project:      CMSIS DSP Library
  3   * Title:        arm_quaternion_product_single_f32.c
  4   * Description:  Floating-point quaternion product
  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  /**
 34    @ingroup QuatProd
 35   */
 36  
 37  /**
 38    @defgroup QuatProdSingle Quaternion Product
 39  
 40    Compute the  product of two quaternions.
 41   */
 42  
 43  /**
 44    @addtogroup QuatProdSingle
 45    @{
 46   */
 47  
 48  /**
 49    @brief         Floating-point product of two quaternions.
 50    @param[in]     qa       first quaternion
 51    @param[in]     qb       second quaternion
 52    @param[out]    qr       product of two quaternions
 53    @return        none
 54   */
 55  
 56  #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE)
 57  
 58  #include "arm_helium_utils.h"
 59  void arm_quaternion_product_single_f32(const float32_t *qa, 
 60      const float32_t *qb, 
 61      float32_t *qr)
 62  {
 63      static uint32_t patternA[4] = { 0, 1, 0, 1 };
 64      static uint32_t patternB[4] = { 3, 2, 3, 2 };
 65      static uint32_t patternC[4] = { 3, 2, 1, 0 };
 66      static float32_t signA[4] = { -1, -1, 1, 1 };
 67  
 68      uint32x4_t vecA = vld1q_u32(patternA);
 69      uint32x4_t vecB = vld1q_u32(patternB);
 70      uint32x4_t vecC = vld1q_u32(patternC);
 71      f32x4_t vecSignA = vld1q_f32(signA);
 72  
 73  
 74      f32x4_t vecTmpA, vecTmpB, vecAcc;
 75  
 76      vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecA);
 77      vecTmpB = vld1q_f32(qb);
 78  
 79      vecAcc = vcmulq_f32(vecTmpA, vecTmpB);
 80      vecAcc = vcmlaq_rot90_f32(vecAcc, vecTmpA, vecTmpB);
 81  
 82      vecTmpA = vldrwq_gather_shifted_offset_f32(qa, vecB);
 83      vecTmpB = vldrwq_gather_shifted_offset_f32(qb, vecC);
 84  
 85      vecTmpB = vecTmpB * vecSignA;
 86  
 87      vecAcc = vcmlaq_rot270_f32(vecAcc, vecTmpA, vecTmpB);
 88      vecAcc = vcmlaq_f32(vecAcc, vecTmpA, vecTmpB);
 89  
 90      vst1q_f32(qr, vecAcc);
 91  }
 92  
 93  #else
 94  void arm_quaternion_product_single_f32(const float32_t *qa, 
 95      const float32_t *qb, 
 96      float32_t *qr)
 97  {
 98      qr[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
 99      qr[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2];
100      qr[2] = qa[0] * qb[2] + qa[2] * qb[0] + qa[3] * qb[1] - qa[1] * qb[3];
101      qr[3] = qa[0] * qb[3] + qa[3] * qb[0] + qa[1] * qb[2] - qa[2] * qb[1];
102  }
103  #endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */
104  
105  /**
106    @} end of QuatProdSingle group
107   */