mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
34c852255e
* Changed M_PI to M_PI_F in the matrix library since M_PI is non-standard. * Added a new M_PI_PRECISE constant definition to px4_platform_common/defines.h to be used in places when M_PI is desired but shouldn't be used because it is not C standard. * Added the px4_platform_common/defines.h include to the matrix library math.hpp header to pull in some non-standard M_PI constants and updated the test files to use those constants. * Fixed PI constants in matrix helper test to prevent test failure
126 lines
4.6 KiB
C++
126 lines
4.6 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <matrix/math.hpp>
|
|
|
|
using namespace matrix;
|
|
|
|
TEST(MatrixHelperTest, SignFloat)
|
|
{
|
|
EXPECT_FLOAT_EQ(sign(-100.f), -1.f);
|
|
EXPECT_FLOAT_EQ(sign(-FLT_EPSILON), -1.f);
|
|
EXPECT_FLOAT_EQ(sign(0.f), 0.f);
|
|
EXPECT_FLOAT_EQ(sign(FLT_EPSILON), 1.f);
|
|
EXPECT_FLOAT_EQ(sign(100.f), 1.f);
|
|
}
|
|
|
|
TEST(MatrixHelperTest, SignInt)
|
|
{
|
|
EXPECT_FLOAT_EQ(sign(-100), -1);
|
|
EXPECT_FLOAT_EQ(sign(-1), -1);
|
|
EXPECT_FLOAT_EQ(sign(0), 0);
|
|
EXPECT_FLOAT_EQ(sign(1), 1);
|
|
EXPECT_FLOAT_EQ(sign(100), 1);
|
|
}
|
|
|
|
TEST(MatrixHelperTest, Helper)
|
|
{
|
|
// general wraps
|
|
EXPECT_FLOAT_EQ(wrap(4., 0., 10.), 4.);
|
|
EXPECT_FLOAT_EQ(wrap(4., 0., 1.), 0.);
|
|
EXPECT_FLOAT_EQ(wrap(-4., 0., 10.), 6.);
|
|
EXPECT_FLOAT_EQ(wrap(-18., 0., 10.), 2.);
|
|
EXPECT_FLOAT_EQ(wrap(-1.5, 3., 5.), 4.5);
|
|
EXPECT_FLOAT_EQ(wrap(15.5, 3., 5.), 3.5);
|
|
EXPECT_FLOAT_EQ(wrap(-1., 30., 40.), 39.);
|
|
EXPECT_FLOAT_EQ(wrap(-8000., -555., 1.), -216.);
|
|
EXPECT_FLOAT_EQ(wrap(0., 0., 360.), 0.);
|
|
EXPECT_FLOAT_EQ(wrap(0. - FLT_EPSILON, 0., 360.), 360. - FLT_EPSILON);
|
|
EXPECT_FLOAT_EQ(wrap(0. + FLT_EPSILON, 0., 360.), FLT_EPSILON);
|
|
EXPECT_FLOAT_EQ(wrap(360., 0., 360.), 0.);
|
|
EXPECT_FLOAT_EQ(wrap(360. - FLT_EPSILON, 0., 360.), 360. - FLT_EPSILON);
|
|
EXPECT_FLOAT_EQ(wrap(360. + FLT_EPSILON, 0., 360.), FLT_EPSILON);
|
|
|
|
// integer wraps
|
|
EXPECT_EQ(wrap(-10, 0, 10), 0);
|
|
EXPECT_EQ(wrap(-4, 0, 10), 6);
|
|
EXPECT_EQ(wrap(0, 0, 10), 0);
|
|
EXPECT_EQ(wrap(4, 0, 10), 4);
|
|
EXPECT_EQ(wrap(10, 0, 10), 0);
|
|
|
|
// wrap pi
|
|
EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
|
|
EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE)));
|
|
EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE)));
|
|
EXPECT_FLOAT_EQ(wrap_pi(3.), 3.);
|
|
EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
|
|
EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
|
|
EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE));
|
|
EXPECT_FALSE(std::isfinite(wrap_pi(NAN)));
|
|
|
|
// wrap 2pi
|
|
EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.);
|
|
EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE));
|
|
EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.));
|
|
EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE)));
|
|
EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
|
|
EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
|
|
|
|
// Equality checks
|
|
EXPECT_TRUE(isEqualF(1., 1.));
|
|
EXPECT_FALSE(isEqualF(1., 2.));
|
|
EXPECT_FALSE(isEqualF(NAN, 1.f));
|
|
EXPECT_FALSE(isEqualF(1.f, NAN));
|
|
EXPECT_FALSE(isEqualF(INFINITY, 1.f));
|
|
EXPECT_FALSE(isEqualF(1.f, INFINITY));
|
|
EXPECT_TRUE(isEqualF(NAN, NAN));
|
|
EXPECT_TRUE(isEqualF(NAN, -NAN));
|
|
EXPECT_TRUE(isEqualF(-NAN, NAN));
|
|
EXPECT_TRUE(isEqualF(INFINITY, INFINITY));
|
|
EXPECT_FALSE(isEqualF(INFINITY, -INFINITY));
|
|
EXPECT_FALSE(isEqualF(-INFINITY, INFINITY));
|
|
EXPECT_TRUE(isEqualF(-INFINITY, -INFINITY));
|
|
|
|
Vector3f a(1, 2, 3);
|
|
Vector3f b(4, 5, 6);
|
|
EXPECT_FALSE(isEqual(a, b));
|
|
EXPECT_TRUE(isEqual(a, a));
|
|
|
|
Vector3f c(1, 2, 3);
|
|
Vector3f d(1, 2, NAN);
|
|
EXPECT_FALSE(isEqual(c, d));
|
|
EXPECT_TRUE(isEqual(c, c));
|
|
EXPECT_TRUE(isEqual(d, d));
|
|
}
|