You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
128 lines
4.6 KiB
128 lines
4.6 KiB
/*
|
|
* Copyright (C) 2021 The Android Open Source Project
|
|
*
|
|
* 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
|
|
*
|
|
* http://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.
|
|
*/
|
|
#pragma once
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "media/Pose.h"
|
|
#include "media/Twist.h"
|
|
|
|
namespace {
|
|
|
|
constexpr float kPoseComparisonPrecision = 1e-5;
|
|
|
|
} // namespace
|
|
|
|
// These specializations make {EXPECT,ASSERT}_{EQ,NE} work correctly for Pose3f, Twist3f, Vector3f
|
|
// and Quaternionf.
|
|
namespace testing {
|
|
namespace internal {
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperEQ<android::media::Pose3f, android::media::Pose3f>(
|
|
const char* lhs_expression, const char* rhs_expression, const android::media::Pose3f& lhs,
|
|
const android::media::Pose3f& rhs) {
|
|
if (lhs.isApprox(rhs, kPoseComparisonPrecision)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperNE<android::media::Pose3f, android::media::Pose3f>(
|
|
const char* lhs_expression, const char* rhs_expression, const android::media::Pose3f& lhs,
|
|
const android::media::Pose3f& rhs) {
|
|
if (!lhs.isApprox(rhs, kPoseComparisonPrecision)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperEQ<android::media::Twist3f, android::media::Twist3f>(
|
|
const char* lhs_expression, const char* rhs_expression, const android::media::Twist3f& lhs,
|
|
const android::media::Twist3f& rhs) {
|
|
if (lhs.isApprox(rhs, kPoseComparisonPrecision)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperNE<android::media::Twist3f, android::media::Twist3f>(
|
|
const char* lhs_expression, const char* rhs_expression, const android::media::Twist3f& lhs,
|
|
const android::media::Twist3f& rhs) {
|
|
if (!lhs.isApprox(rhs, kPoseComparisonPrecision)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperEQ<Eigen::Vector3f, Eigen::Vector3f>(const char* lhs_expression,
|
|
const char* rhs_expression,
|
|
const Eigen::Vector3f& lhs,
|
|
const Eigen::Vector3f& rhs) {
|
|
if (lhs.isApprox(rhs)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperNE<Eigen::Vector3f, Eigen::Vector3f>(const char* lhs_expression,
|
|
const char* rhs_expression,
|
|
const Eigen::Vector3f& lhs,
|
|
const Eigen::Vector3f& rhs) {
|
|
if (!lhs.isApprox(rhs)) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperEQ<Eigen::Quaternionf, Eigen::Quaternionf>(
|
|
const char* lhs_expression, const char* rhs_expression, const Eigen::Quaternionf& lhs,
|
|
const Eigen::Quaternionf& rhs) {
|
|
// Negating the coefs results in an equivalent quaternion.
|
|
if (lhs.isApprox(rhs) || lhs.isApprox(Eigen::Quaternionf(-rhs.coeffs()))) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
template <>
|
|
inline AssertionResult CmpHelperNE<Eigen::Quaternionf, Eigen::Quaternionf>(
|
|
const char* lhs_expression, const char* rhs_expression, const Eigen::Quaternionf& lhs,
|
|
const Eigen::Quaternionf& rhs) {
|
|
// Negating the coefs results in an equivalent quaternion.
|
|
if (!(lhs.isApprox(rhs) || lhs.isApprox(Eigen::Quaternionf(-rhs.coeffs())))) {
|
|
return AssertionSuccess();
|
|
}
|
|
|
|
return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs);
|
|
}
|
|
|
|
} // namespace internal
|
|
} // namespace testing
|