CMSIS-DSP: Added scalar f32 quaternion functions.
Some correction for RFFT Fast f32 in Python wrapperpull/19/head
Christophe Favergeon
5 years ago
parent
25a524baad
commit
4357c9e947
@ -0,0 +1,155 @@
|
||||
/******************************************************************************
|
||||
* @file quaternion_math_functions.h
|
||||
* @brief Public header file for CMSIS DSP Library
|
||||
******************************************************************************/
|
||||
/*
|
||||
* Copyright (c) 2010-2021 Arm Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
|
||||
#ifndef _QUATERNION_MATH_FUNCTIONS_H_
|
||||
#define _QUATERNION_MATH_FUNCTIONS_H_
|
||||
|
||||
#include "arm_math_types.h"
|
||||
#include "arm_math_memory.h"
|
||||
|
||||
#include "dsp/none.h"
|
||||
#include "dsp/utils.h"
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @defgroup groupQuaternionMath Quaternion Math Functions
|
||||
* Functions to operates on quaternions and convert between a
|
||||
* rotation and quaternion representation.
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion Norm.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pNorms points to the output vector of norms
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
|
||||
|
||||
|
||||
void arm_quaternion_norm_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pNorms,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion inverse.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pInverseQuaternions points to the output vector of inverse quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
|
||||
void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pInverseQuaternions,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion conjugates.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pConjugateQuaternions points to the output vector of conjugate quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_conjugate_f32(const float32_t *inputQuaternions,
|
||||
float32_t *pConjugateQuaternions,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
/**
|
||||
@brief Floating-point normalization of quaternions.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pNormalizedQuaternions points to the output vector of normalized quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_normalize_f32(const float32_t *inputQuaternions,
|
||||
float32_t *pNormalizedQuaternions,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
|
||||
/**
|
||||
@brief Floating-point product of two quaternions.
|
||||
@param[in] qa First quaternion
|
||||
@param[in] qb Second quaternion
|
||||
@param[out] r Product of two quaternions
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_product_single_f32(const float32_t *qa,
|
||||
const float32_t *qb,
|
||||
float32_t *r);
|
||||
|
||||
/**
|
||||
@brief Floating-point elementwise product two quaternions.
|
||||
@param[in] qa First array of quaternions
|
||||
@param[in] qb Second array of quaternions
|
||||
@param[out] r Elementwise product of quaternions
|
||||
@param[in] nbQuaternions Number of quaternions in the array
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_product_f32(const float32_t *qa,
|
||||
const float32_t *qb,
|
||||
float32_t *r,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
/**
|
||||
* @brief Conversion of quaternion to equivalent rotation matrix.
|
||||
* @param[in] pInputQuaternions points to an array of normalized quaternions
|
||||
* @param[out] pOutputRotations points to an array of 3x3 rotations (in row order)
|
||||
* @param[in] nbQuaternions in the array
|
||||
* @return none.
|
||||
*
|
||||
* <b>Format of rotation matrix</b>
|
||||
* \par
|
||||
* The quaternion a + ib + jc + kd is converted into rotation matrix:
|
||||
* a^2 + b^2 - c^2 - d^2 2bc - 2ad 2bd + 2ac
|
||||
* 2bc + 2ad a^2 - b^2 + c^2 - d^2 2cd - 2ab
|
||||
* 2bd - 2ac 2cd + 2ab a^2 - b^2 - c^2 + d^2
|
||||
*
|
||||
* Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22
|
||||
*/
|
||||
void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pOutputRotations,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
/**
|
||||
* @brief Conversion of a rotation matrix to equivalent quaternion.
|
||||
* @param[in] pInputRotations points to an array 3x3 rotation matrix (in row order)
|
||||
* @param[out] pOutputQuaternions points to an array of quaternions
|
||||
* @param[in] nbQuaternions in the array
|
||||
* @return none.
|
||||
*/
|
||||
void arm_rotation2quaternion_f32(const float32_t *pInputRotations,
|
||||
float32_t *pOutputQuaternions,
|
||||
uint32_t nbQuaternions);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* ifndef _QUATERNION_MATH_FUNCTIONS_H_ */
|
||||
@ -0,0 +1,27 @@
|
||||
import cmsisdsp as dsp
|
||||
import numpy as np
|
||||
from scipy import signal
|
||||
import matplotlib.pyplot as plt
|
||||
import scipy.fft
|
||||
|
||||
|
||||
def chop(A, eps = 1e-6):
|
||||
B = np.copy(A)
|
||||
B[np.abs(A) < eps] = 0
|
||||
return B
|
||||
|
||||
nb = 32
|
||||
signal = np.cos(2 * np.pi * np.arange(nb) / nb)*np.cos(0.2*2 * np.pi * np.arange(nb) / nb)
|
||||
|
||||
#print("{")
|
||||
#for x in signal:
|
||||
# print("%f," % x)
|
||||
#print("}")
|
||||
|
||||
result1=scipy.fft.rfft(signal)
|
||||
print(chop(result1))
|
||||
rfftf32=dsp.arm_rfft_fast_instance_f32()
|
||||
status=dsp.arm_rfft_fast_init_f32(rfftf32,nb)
|
||||
print(status)
|
||||
resultI = dsp.arm_rfft_fast_f32(rfftf32,signal,0)
|
||||
print(chop(resultI))
|
||||
@ -0,0 +1,31 @@
|
||||
cmake_minimum_required (VERSION 3.14)
|
||||
|
||||
project(CMSISDSPQuaternionMath)
|
||||
|
||||
include(configLib)
|
||||
include(configDsp)
|
||||
|
||||
|
||||
|
||||
add_library(CMSISDSPQuaternionMath STATIC arm_quaternion_norm_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_inverse_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_conjugate_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_normalize_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_product_single_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion_product_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_quaternion2rotation_f32.c)
|
||||
target_sources(CMSISDSPQuaternionMath PRIVATE arm_rotation2quaternion_f32.c)
|
||||
|
||||
|
||||
if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16))
|
||||
endif()
|
||||
|
||||
|
||||
configLib(CMSISDSPQuaternionMath ${ROOT})
|
||||
configDsp(CMSISDSPQuaternionMath ${ROOT})
|
||||
|
||||
### Includes
|
||||
target_include_directories(CMSISDSPQuaternionMath PUBLIC "${DSP}/Include")
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,34 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: QuaternionMathFunctions.c
|
||||
* Description: Combination of all quaternion math function source files.
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2019-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "arm_quaternion_norm_f32.c"
|
||||
#include "arm_quaternion_inverse_f32.c"
|
||||
#include "arm_quaternion_conjugate_f32.c"
|
||||
#include "arm_quaternion_normalize_f32.c"
|
||||
#include "arm_quaternion_product_single_f32.c"
|
||||
#include "arm_quaternion_product_f32.c"
|
||||
#include "arm_quaternion2rotation_f32.c"
|
||||
#include "arm_rotation2quaternion_f32.c"
|
||||
@ -0,0 +1,109 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion2rotation_f32.c
|
||||
* Description: Floating-point quaternion 2 rotation conversion
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatConv Quaternion conversions
|
||||
|
||||
Conversions between quaternion and rotation representations.
|
||||
*/
|
||||
|
||||
/**
|
||||
@ingroup QuatConv
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatRot Quaternion to Rotation
|
||||
|
||||
Conversions from quaternion to rotation.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatRot
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Conversion of quaternion to equivalent rotation matrix.
|
||||
@param[in] pInputQuaternions points to an array of normalized quaternions
|
||||
@param[out] pOutputRotations points to an array of 3x3 rotations (in row order)
|
||||
@param[in] nbQuaternions number of quaternions in the array
|
||||
@return none.
|
||||
|
||||
@par
|
||||
Format of rotation matrix
|
||||
|
||||
|
||||
The quaternion a + ib + jc + kd is converted into rotation matrix:
|
||||
<pre>
|
||||
a^2 + b^2 - c^2 - d^2 2bc - 2ad 2bd + 2ac
|
||||
2bc + 2ad a^2 - b^2 + c^2 - d^2 2cd - 2ab
|
||||
2bd - 2ac 2cd + 2ab a^2 - b^2 - c^2 + d^2
|
||||
</pre>
|
||||
Rotation matrix is saved in row order : R00 R01 R02 R10 R11 R12 R20 R21 R22
|
||||
*/
|
||||
void arm_quaternion2rotation_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pOutputRotations,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
for(uint32_t nb=0; nb < nbQuaternions; nb++)
|
||||
{
|
||||
float32_t q00 = SQ(pInputQuaternions[0 + nb * 4]);
|
||||
float32_t q11 = SQ(pInputQuaternions[1 + nb * 4]);
|
||||
float32_t q22 = SQ(pInputQuaternions[2 + nb * 4]);
|
||||
float32_t q33 = SQ(pInputQuaternions[3 + nb * 4]);
|
||||
float32_t q01 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[1 + nb * 4];
|
||||
float32_t q02 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[2 + nb * 4];
|
||||
float32_t q03 = pInputQuaternions[0 + nb * 4]*pInputQuaternions[3 + nb * 4];
|
||||
float32_t q12 = pInputQuaternions[1 + nb * 4]*pInputQuaternions[2 + nb * 4];
|
||||
float32_t q13 = pInputQuaternions[1 + nb * 4]*pInputQuaternions[3 + nb * 4];
|
||||
float32_t q23 = pInputQuaternions[2 + nb * 4]*pInputQuaternions[3 + nb * 4];
|
||||
|
||||
float32_t xx = q00 + q11 - q22 - q33;
|
||||
float32_t yy = q00 - q11 + q22 - q33;
|
||||
float32_t zz = q00 - q11 - q22 + q33;
|
||||
float32_t xy = 2*(q12 - q03);
|
||||
float32_t xz = 2*(q13 + q02);
|
||||
float32_t yx = 2*(q12 + q03);
|
||||
float32_t yz = 2*(q23 - q01);
|
||||
float32_t zx = 2*(q13 - q02);
|
||||
float32_t zy = 2*(q23 + q01);
|
||||
|
||||
pOutputRotations[0 + nb * 9] = xx; pOutputRotations[1 + nb * 9] = xy; pOutputRotations[2 + nb * 9] = xz;
|
||||
pOutputRotations[3 + nb * 9] = yx; pOutputRotations[4 + nb * 9] = yy; pOutputRotations[5 + nb * 9] = yz;
|
||||
pOutputRotations[6 + nb * 9] = zx; pOutputRotations[7 + nb * 9] = zy; pOutputRotations[8 + nb * 9] = zz;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatRot group
|
||||
*/
|
||||
@ -0,0 +1,68 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_conjugate_f32.c
|
||||
* Description: Floating-point quaternion conjugate
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatConjugate Quaternion Conjugate
|
||||
|
||||
Compute the conjugate of a quaternion.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatConjugate
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion conjugates.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pConjugateQuaternions points to the output vector of conjugate quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_conjugate_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pConjugateQuaternions,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
for(uint32_t i=0; i < nbQuaternions; i++)
|
||||
{
|
||||
|
||||
pConjugateQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0];
|
||||
pConjugateQuaternions[4 * i + 1] = -pInputQuaternions[4 * i + 1];
|
||||
pConjugateQuaternions[4 * i + 2] = -pInputQuaternions[4 * i + 2];
|
||||
pConjugateQuaternions[4 * i + 3] = -pInputQuaternions[4 * i + 3];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatConjugate group
|
||||
*/
|
||||
@ -0,0 +1,78 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_inverse_f32.c
|
||||
* Description: Floating-point quaternion inverse
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatInverse Quaternion Inverse
|
||||
|
||||
Compute the inverse of a quaternion.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatInverse
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion inverse.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pInverseQuaternions points to the output vector of inverse quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
|
||||
|
||||
|
||||
void arm_quaternion_inverse_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pInverseQuaternions,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
float32_t temp;
|
||||
|
||||
for(uint32_t i=0; i < nbQuaternions; i++)
|
||||
{
|
||||
|
||||
temp = SQ(pInputQuaternions[4 * i + 0]) +
|
||||
SQ(pInputQuaternions[4 * i + 1]) +
|
||||
SQ(pInputQuaternions[4 * i + 2]) +
|
||||
SQ(pInputQuaternions[4 * i + 3]);
|
||||
|
||||
pInverseQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0] / temp;
|
||||
pInverseQuaternions[4 * i + 1] = -pInputQuaternions[4 * i + 1] / temp;
|
||||
pInverseQuaternions[4 * i + 2] = -pInputQuaternions[4 * i + 2] / temp;
|
||||
pInverseQuaternions[4 * i + 3] = -pInputQuaternions[4 * i + 3] / temp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatInverse group
|
||||
*/
|
||||
@ -0,0 +1,73 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_norm_f32.c
|
||||
* Description: Floating-point quaternion Norm
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatNorm Quaternion Norm
|
||||
|
||||
Compute the norm of a quaternion.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatNorm
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point quaternion Norm.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pNorms points to the output vector of norms
|
||||
@param[in] nbQuaternions number of quaternions in the input vector
|
||||
@return none
|
||||
*/
|
||||
|
||||
|
||||
|
||||
void arm_quaternion_norm_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pNorms,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
float32_t temp;
|
||||
|
||||
for(uint32_t i=0; i < nbQuaternions; i++)
|
||||
{
|
||||
temp = SQ(pInputQuaternions[4 * i + 0]) +
|
||||
SQ(pInputQuaternions[4 * i + 1]) +
|
||||
SQ(pInputQuaternions[4 * i + 2]) +
|
||||
SQ(pInputQuaternions[4 * i + 3]);
|
||||
pNorms[i] = sqrtf(temp);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatNorm group
|
||||
*/
|
||||
@ -0,0 +1,75 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_normalize_f32.c
|
||||
* Description: Floating-point quaternion normalization
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatNormalized Quaternion normalization
|
||||
|
||||
Compute a normalized quaternion.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatNormalized
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point normalization of quaternions.
|
||||
@param[in] pInputQuaternions points to the input vector of quaternions
|
||||
@param[out] pNormalizedQuaternions points to the output vector of normalized quaternions
|
||||
@param[in] nbQuaternions number of quaternions in each vector
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_normalize_f32(const float32_t *pInputQuaternions,
|
||||
float32_t *pNormalizedQuaternions,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
float32_t temp;
|
||||
|
||||
for(uint32_t i=0; i < nbQuaternions; i++)
|
||||
{
|
||||
temp = SQ(pInputQuaternions[4 * i + 0]) +
|
||||
SQ(pInputQuaternions[4 * i + 1]) +
|
||||
SQ(pInputQuaternions[4 * i + 2]) +
|
||||
SQ(pInputQuaternions[4 * i + 3]);
|
||||
temp = sqrtf(temp);
|
||||
|
||||
pNormalizedQuaternions[4 * i + 0] = pInputQuaternions[4 * i + 0] / temp;
|
||||
pNormalizedQuaternions[4 * i + 1] = pInputQuaternions[4 * i + 1] / temp;
|
||||
pNormalizedQuaternions[4 * i + 2] = pInputQuaternions[4 * i + 2] / temp;
|
||||
pNormalizedQuaternions[4 * i + 3] = pInputQuaternions[4 * i + 3] / temp;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatNormalized group
|
||||
*/
|
||||
@ -0,0 +1,70 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_product_f32.c
|
||||
* Description: Floating-point quaternion product
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatProd Quaternion Product
|
||||
|
||||
Compute the product of quaternions.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatProd
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point elementwise product two quaternions.
|
||||
@param[in] qa first array of quaternions
|
||||
@param[in] qb second array of quaternions
|
||||
@param[out] r elementwise product of quaternions
|
||||
@param[in] nbQuaternions number of quaternions in the array
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_product_f32(const float32_t *qa,
|
||||
const float32_t *qb,
|
||||
float32_t *r,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
for(uint32_t i=0; i < nbQuaternions; i++)
|
||||
{
|
||||
arm_quaternion_product_single_f32(qa, qb, r);
|
||||
|
||||
qa += 4;
|
||||
qb += 4;
|
||||
r += 4;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatProd group
|
||||
*/
|
||||
@ -0,0 +1,64 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_quaternion_product_single_f32.c
|
||||
* Description: Floating-point quaternion product
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
/**
|
||||
@ingroup groupQuaternionMath
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup QuatProd Quaternion Product
|
||||
|
||||
Compute the product of quaternions.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup QuatProdSingle
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
@brief Floating-point product of two quaternions.
|
||||
@param[in] qa first quaternion
|
||||
@param[in] qb second quaternion
|
||||
@param[out] r product of two quaternions
|
||||
@return none
|
||||
*/
|
||||
void arm_quaternion_product_single_f32(const float32_t *qa,
|
||||
const float32_t *qb,
|
||||
float32_t *r)
|
||||
{
|
||||
r[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
|
||||
r[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2];
|
||||
r[2] = qa[0] * qb[2] + qa[2] * qb[0] + qa[3] * qb[1] - qa[1] * qb[3];
|
||||
r[3] = qa[0] * qb[3] + qa[3] * qb[0] + qa[1] * qb[2] - qa[2] * qb[1];
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of QuatProdSingle group
|
||||
*/
|
||||
@ -0,0 +1,117 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_rotation2quaternion_f32.c
|
||||
* Description: Floating-point rotation to quaternion conversion
|
||||
*
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
#include <math.h>
|
||||
|
||||
#define RI(x,y) r[(3*(x) + (y))]
|
||||
|
||||
|
||||
/**
|
||||
@ingroup QuatConv
|
||||
*/
|
||||
|
||||
/**
|
||||
@defgroup RotQuat Rotation to Quaternion
|
||||
|
||||
Conversions from rotation to quaternion.
|
||||
*/
|
||||
|
||||
/**
|
||||
@addtogroup RotQuat
|
||||
@{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Conversion of a rotation matrix to an equivalent quaternion.
|
||||
* @param[in] pInputRotations points to an array 3x3 rotation matrix (in row order)
|
||||
* @param[out] pOutputQuaternions points to an array quaternions
|
||||
* @param[in] nbQuaternions number of quaternions in the array
|
||||
* @return none.
|
||||
*
|
||||
* q and -q are representing the same rotation. This ambiguity must be taken into
|
||||
* account when using the output of this function.
|
||||
*
|
||||
*/
|
||||
void arm_rotation2quaternion_f32(const float32_t *pInputRotations,
|
||||
float32_t *pOutputQuaternions,
|
||||
uint32_t nbQuaternions)
|
||||
{
|
||||
for(uint32_t nb=0; nb < nbQuaternions; nb++)
|
||||
{
|
||||
const float32_t *r=&pInputRotations[nb*9];
|
||||
float32_t *q=&pOutputQuaternions[nb*4];
|
||||
|
||||
float32_t trace = RI(0,0) + RI(1,1) + RI(2,2);
|
||||
|
||||
float32_t doubler;
|
||||
float32_t s;
|
||||
|
||||
|
||||
|
||||
if (trace > 0)
|
||||
{
|
||||
doubler = sqrtf(trace + 1.0) * 2; // invs=4*qw
|
||||
s = 1.0 / doubler;
|
||||
q[0] = 0.25 * doubler;
|
||||
q[1] = (RI(2,1) - RI(1,2)) * s;
|
||||
q[2] = (RI(0,2) - RI(2,0)) * s;
|
||||
q[3] = (RI(1,0) - RI(0,1)) * s;
|
||||
}
|
||||
else if ((RI(0,0) > RI(1,1)) && (RI(0,0) > RI(2,2)) )
|
||||
{
|
||||
doubler = sqrtf(1.0 + RI(0,0) - RI(1,1) - RI(2,2)) * 2; // invs=4*qx
|
||||
s = 1.0 / doubler;
|
||||
q[0] = (RI(2,1) - RI(1,2)) * s;
|
||||
q[1] = 0.25 * doubler;
|
||||
q[2] = (RI(0,1) + RI(1,0)) * s;
|
||||
q[3] = (RI(0,2) + RI(2,0)) * s;
|
||||
}
|
||||
else if (RI(1,1) > RI(2,2))
|
||||
{
|
||||
doubler = sqrtf(1.0 + RI(1,1) - RI(0,0) - RI(2,2)) * 2; // invs=4*qy
|
||||
s = 1.0 / doubler;
|
||||
q[0] = (RI(0,2) - RI(2,0)) * s;
|
||||
q[1] = (RI(0,1) + RI(1,0)) * s;
|
||||
q[2] = 0.25 * doubler;
|
||||
q[3] = (RI(1,2) + RI(2,1)) * s;
|
||||
}
|
||||
else
|
||||
{
|
||||
doubler = sqrtf(1.0 + RI(2,2) - RI(0,0) - RI(1,1)) * 2; // invs=4*qz
|
||||
s = 1.0 / doubler;
|
||||
q[0] = (RI(1,0) - RI(0,1)) * s;
|
||||
q[1] = (RI(0,2) + RI(2,0)) * s;
|
||||
q[2] = (RI(1,2) + RI(2,1)) * s;
|
||||
q[3] = 0.25 * doubler;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@} end of RotQuat group
|
||||
*/
|
||||
@ -0,0 +1,20 @@
|
||||
#include "Test.h"
|
||||
#include "Pattern.h"
|
||||
|
||||
#include "dsp/quaternion_math_functions.h"
|
||||
|
||||
class QuaternionTestsF32:public Client::Suite
|
||||
{
|
||||
public:
|
||||
QuaternionTestsF32(Testing::testID_t id);
|
||||
virtual void setUp(Testing::testID_t,std::vector<Testing::param_t>& params,Client::PatternMgr *mgr);
|
||||
virtual void tearDown(Testing::testID_t,Client::PatternMgr *mgr);
|
||||
private:
|
||||
#include "QuaternionTestsF32_decl.h"
|
||||
|
||||
Client::Pattern<float32_t> input1;
|
||||
Client::Pattern<float32_t> input2;
|
||||
Client::LocalPattern<float32_t> output;
|
||||
// Reference patterns are not loaded when we are in dump mode
|
||||
Client::RefPattern<float32_t> ref;
|
||||
};
|
||||
@ -0,0 +1,82 @@
|
||||
81
|
||||
1
|
||||
1
|
||||
1
|
||||
1
|
||||
1
|
||||
3
|
||||
1
|
||||
1
|
||||
5
|
||||
1
|
||||
3
|
||||
1
|
||||
1
|
||||
3
|
||||
3
|
||||
1
|
||||
3
|
||||
5
|
||||
1
|
||||
5
|
||||
1
|
||||
1
|
||||
5
|
||||
3
|
||||
1
|
||||
5
|
||||
5
|
||||
3
|
||||
1
|
||||
1
|
||||
3
|
||||
1
|
||||
3
|
||||
3
|
||||
1
|
||||
5
|
||||
3
|
||||
3
|
||||
1
|
||||
3
|
||||
3
|
||||
3
|
||||
3
|
||||
3
|
||||
5
|
||||
3
|
||||
5
|
||||
1
|
||||
3
|
||||
5
|
||||
3
|
||||
3
|
||||
5
|
||||
5
|
||||
5
|
||||
1
|
||||
1
|
||||
5
|
||||
1
|
||||
3
|
||||
5
|
||||
1
|
||||
5
|
||||
5
|
||||
3
|
||||
1
|
||||
5
|
||||
3
|
||||
3
|
||||
5
|
||||
3
|
||||
5
|
||||
5
|
||||
5
|
||||
1
|
||||
5
|
||||
5
|
||||
3
|
||||
5
|
||||
5
|
||||
5
|
||||
@ -0,0 +1,79 @@
|
||||
import os.path
|
||||
import numpy as np
|
||||
import itertools
|
||||
import Tools
|
||||
from pyquaternion import Quaternion
|
||||
|
||||
# mult, multvec, inverse, conjugate, normalize rot2quat, quat2rot , norm
|
||||
def flattenQuat(l):
|
||||
return(np.array([list(x) for x in l]).reshape(4*len(l)))
|
||||
|
||||
def flattenRot(l):
|
||||
return(np.array([list(x) for x in l]).reshape(9*len(l)))
|
||||
|
||||
# q and -q are representing the same rotation.
|
||||
# So there is an ambiguity for the tests.
|
||||
# We force the real part of be positive.
|
||||
def mkQuaternion(mat):
|
||||
q=Quaternion(matrix=mat)
|
||||
if q.scalar < 0:
|
||||
return(-q)
|
||||
else:
|
||||
return(q)
|
||||
|
||||
def writeTests(config,format):
|
||||
NBSAMPLES=128
|
||||
|
||||
a=[Quaternion.random() for x in range(NBSAMPLES)]
|
||||
b=[Quaternion.random() for x in range(NBSAMPLES)]
|
||||
|
||||
config.writeInput(1, flattenQuat(a))
|
||||
config.writeInput(2, flattenQuat(b))
|
||||
|
||||
normTest = [x.norm for x in a]
|
||||
config.writeReference(1, normTest)
|
||||
|
||||
inverseTest = [x.inverse for x in a]
|
||||
config.writeReference(2, flattenQuat(inverseTest))
|
||||
|
||||
conjugateTest = [x.conjugate for x in a]
|
||||
config.writeReference(3, flattenQuat(conjugateTest))
|
||||
|
||||
normalizeTest = [x.normalised for x in a]
|
||||
config.writeReference(4, flattenQuat(normalizeTest))
|
||||
|
||||
multTest = [a[i] * b[i] for i in range(NBSAMPLES)]
|
||||
config.writeReference(5, flattenQuat(multTest))
|
||||
|
||||
quat2RotTest = [x.rotation_matrix for x in a]
|
||||
config.writeReference(6, flattenRot(quat2RotTest))
|
||||
|
||||
config.writeInput(7, flattenRot(quat2RotTest))
|
||||
rot2QuatTest = [mkQuaternion(x) for x in quat2RotTest]
|
||||
config.writeReference(7, flattenQuat(rot2QuatTest))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
def generatePatterns():
|
||||
PATTERNDIR = os.path.join("Patterns","DSP","QuaternionMaths","QuaternionMaths")
|
||||
PARAMDIR = os.path.join("Parameters","DSP","QuaternionMaths","QuaternionMaths")
|
||||
|
||||
configf32=Tools.Config(PATTERNDIR,PARAMDIR,"f32")
|
||||
configf16=Tools.Config(PATTERNDIR,PARAMDIR,"f16")
|
||||
|
||||
|
||||
writeTests(configf32,0)
|
||||
writeTests(configf16,16)
|
||||
|
||||
|
||||
# Params just as example
|
||||
someLists=[[1,3,5],[1,3,5],[1,3,5]]
|
||||
|
||||
r=np.array([element for element in itertools.product(*someLists)])
|
||||
configf32.writeParam(1, r.reshape(81))
|
||||
|
||||
if __name__ == '__main__':
|
||||
generatePatterns()
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,258 @@
|
||||
H
|
||||
128
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
// 1.000000
|
||||
0x3c00
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,258 @@
|
||||
W
|
||||
128
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
// 1.000000
|
||||
0x3f800000
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,234 @@
|
||||
#include "QuaternionTestsF32.h"
|
||||
#include <stdio.h>
|
||||
#include "Error.h"
|
||||
|
||||
#define SNR_THRESHOLD 120
|
||||
|
||||
/*
|
||||
|
||||
Reference patterns are generated with
|
||||
a double precision computation.
|
||||
|
||||
*/
|
||||
#define REL_ERROR (1.0e-6)
|
||||
#define ABS_ERROR (1.0e-7)
|
||||
|
||||
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_norm_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion_norm_f32(inp1,outp,output.nbSamples());
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_inverse_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion_inverse_f32(inp1,outp,input1.nbSamples() >> 2);
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_conjugate_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion_conjugate_f32(inp1,outp,input1.nbSamples() >> 2);
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_normalize_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion_normalize_f32(inp1,outp,input1.nbSamples() >> 2);
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_prod_single_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
const float32_t *inp2=input2.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
for(uint32_t i=0; i < input1.nbSamples() >> 2; i++)
|
||||
{
|
||||
arm_quaternion_product_single_f32(inp1,inp2,outp);
|
||||
outp += 4;
|
||||
inp1 += 4;
|
||||
inp2 += 4;
|
||||
}
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion_product_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
const float32_t *inp2=input2.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion_product_f32(inp1,inp2,outp,input1.nbSamples() >> 2);
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_quaternion2rotation_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
arm_quaternion2rotation_f32(inp1,outp,input1.nbSamples() >> 2);
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::test_rotation2quaternion_f32()
|
||||
{
|
||||
const float32_t *inp1=input1.ptr();
|
||||
float32_t *outp=output.ptr();
|
||||
|
||||
/*
|
||||
|
||||
q and -q are representing the same rotation.
|
||||
To remove the ambiguity we force the real part ot be positive.
|
||||
Same convention followed in Python script.
|
||||
|
||||
*/
|
||||
|
||||
arm_rotation2quaternion_f32(inp1,outp,output.nbSamples() >> 2);
|
||||
|
||||
/* Remove ambiguity */
|
||||
for(uint32_t i=0; i < output.nbSamples() >> 2 ; i++)
|
||||
{
|
||||
if (outp[0] < 0.0f)
|
||||
{
|
||||
outp[0] = -outp[0];
|
||||
outp[1] = -outp[1];
|
||||
outp[2] = -outp[2];
|
||||
outp[3] = -outp[3];
|
||||
}
|
||||
|
||||
outp += 4;
|
||||
}
|
||||
|
||||
ASSERT_EMPTY_TAIL(output);
|
||||
|
||||
ASSERT_SNR(output,ref,(float32_t)SNR_THRESHOLD);
|
||||
|
||||
ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void QuaternionTestsF32::setUp(Testing::testID_t id,std::vector<Testing::param_t>& params,Client::PatternMgr *mgr)
|
||||
{
|
||||
|
||||
(void)params;
|
||||
|
||||
Testing::nbSamples_t nb=MAX_NB_SAMPLES;
|
||||
|
||||
|
||||
switch(id)
|
||||
{
|
||||
case QuaternionTestsF32::TEST_QUATERNION_NORM_F32_1:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_NORM_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION_INVERSE_F32_2:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_INVERSE_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION_CONJUGATE_F32_3:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_CONJUGATE_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION_NORMALIZE_F32_4:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_NORMALIZE_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION_PROD_SINGLE_F32_5:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
input2.reload(QuaternionTestsF32::INPUT2_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_MULT_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION_PRODUCT_F32_6:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
input2.reload(QuaternionTestsF32::INPUT2_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_MULT_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_QUATERNION2ROTATION_F32_7:
|
||||
input1.reload(QuaternionTestsF32::INPUT1_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_QUAT2ROT_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
case QuaternionTestsF32::TEST_ROTATION2QUATERNION_F32_8:
|
||||
input1.reload(QuaternionTestsF32::INPUT7_F32_ID,mgr,nb);
|
||||
ref.reload(QuaternionTestsF32::REF_ROT2QUAT_F32_ID,mgr,nb);
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
output.create(ref.nbSamples(),QuaternionTestsF32::OUT_SAMPLES_F32_ID,mgr);
|
||||
}
|
||||
|
||||
void QuaternionTestsF32::tearDown(Testing::testID_t id,Client::PatternMgr *mgr)
|
||||
{
|
||||
(void)id;
|
||||
output.dump(mgr);
|
||||
}
|
||||
Loading…
Reference in New Issue