1 /* 2 * Copyright (C) 2021 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 #pragma once 17 18 #include <gtest/gtest.h> 19 20 #include "media/Pose.h" 21 #include "media/Twist.h" 22 23 namespace { 24 25 constexpr float kPoseComparisonPrecision = 1e-5; 26 27 } // namespace 28 29 // These specializations make {EXPECT,ASSERT}_{EQ,NE} work correctly for Pose3f, Twist3f, Vector3f 30 // and Quaternionf. 31 namespace testing { 32 namespace internal { 33 34 template <> 35 inline AssertionResult CmpHelperEQ<android::media::Pose3f, android::media::Pose3f>( 36 const char* lhs_expression, const char* rhs_expression, const android::media::Pose3f& lhs, 37 const android::media::Pose3f& rhs) { 38 if (lhs.isApprox(rhs, kPoseComparisonPrecision)) { 39 return AssertionSuccess(); 40 } 41 42 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 43 } 44 45 template <> 46 inline AssertionResult CmpHelperNE<android::media::Pose3f, android::media::Pose3f>( 47 const char* lhs_expression, const char* rhs_expression, const android::media::Pose3f& lhs, 48 const android::media::Pose3f& rhs) { 49 if (!lhs.isApprox(rhs, kPoseComparisonPrecision)) { 50 return AssertionSuccess(); 51 } 52 53 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 54 } 55 56 template <> 57 inline AssertionResult CmpHelperEQ<android::media::Twist3f, android::media::Twist3f>( 58 const char* lhs_expression, const char* rhs_expression, const android::media::Twist3f& lhs, 59 const android::media::Twist3f& rhs) { 60 if (lhs.isApprox(rhs, kPoseComparisonPrecision)) { 61 return AssertionSuccess(); 62 } 63 64 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 65 } 66 67 template <> 68 inline AssertionResult CmpHelperNE<android::media::Twist3f, android::media::Twist3f>( 69 const char* lhs_expression, const char* rhs_expression, const android::media::Twist3f& lhs, 70 const android::media::Twist3f& rhs) { 71 if (!lhs.isApprox(rhs, kPoseComparisonPrecision)) { 72 return AssertionSuccess(); 73 } 74 75 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 76 } 77 78 template <> 79 inline AssertionResult CmpHelperEQ<Eigen::Vector3f, Eigen::Vector3f>(const char* lhs_expression, 80 const char* rhs_expression, 81 const Eigen::Vector3f& lhs, 82 const Eigen::Vector3f& rhs) { 83 if (lhs.isApprox(rhs)) { 84 return AssertionSuccess(); 85 } 86 87 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 88 } 89 90 template <> 91 inline AssertionResult CmpHelperNE<Eigen::Vector3f, Eigen::Vector3f>(const char* lhs_expression, 92 const char* rhs_expression, 93 const Eigen::Vector3f& lhs, 94 const Eigen::Vector3f& rhs) { 95 if (!lhs.isApprox(rhs)) { 96 return AssertionSuccess(); 97 } 98 99 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 100 } 101 102 template <> 103 inline AssertionResult CmpHelperEQ<Eigen::Quaternionf, Eigen::Quaternionf>( 104 const char* lhs_expression, const char* rhs_expression, const Eigen::Quaternionf& lhs, 105 const Eigen::Quaternionf& rhs) { 106 // Negating the coefs results in an equivalent quaternion. 107 if (lhs.isApprox(rhs) || lhs.isApprox(Eigen::Quaternionf(-rhs.coeffs()))) { 108 return AssertionSuccess(); 109 } 110 111 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 112 } 113 114 template <> 115 inline AssertionResult CmpHelperNE<Eigen::Quaternionf, Eigen::Quaternionf>( 116 const char* lhs_expression, const char* rhs_expression, const Eigen::Quaternionf& lhs, 117 const Eigen::Quaternionf& rhs) { 118 // Negating the coefs results in an equivalent quaternion. 119 if (!(lhs.isApprox(rhs) || lhs.isApprox(Eigen::Quaternionf(-rhs.coeffs())))) { 120 return AssertionSuccess(); 121 } 122 123 return CmpHelperEQFailure(lhs_expression, rhs_expression, lhs, rhs); 124 } 125 126 } // namespace internal 127 } // namespace testing 128