5 changed files with 286 additions and 2 deletions
@ -0,0 +1,267 @@ |
|||
#include "cylinder_face.hpp" |
|||
#include <Eigen/Dense> |
|||
#include <cassert> |
|||
#include <iostream> |
|||
|
|||
#ifndef M_PI |
|||
#define M_PI 3.14159265358979323846 |
|||
#endif |
|||
|
|||
// 工具函数:打印分隔线
|
|||
void print_separator() { |
|||
std::cout << "\n" << std::string(60, '-') << "\n" << std::endl; |
|||
} |
|||
|
|||
// 工具函数:打印测试标题
|
|||
void print_test_title(const std::string& title) { |
|||
std::cout << "\033[1;36m"; // 浅蓝色
|
|||
std::cout << title << "\033[0m" << std::endl; |
|||
} |
|||
|
|||
// 工具函数:打印哈希值
|
|||
void print_hash(size_t h, const std::string& label) { |
|||
std::cout << " " << label << " Hash = " << h << std::endl; |
|||
} |
|||
|
|||
// 工具函数:断言并输出通过/失败
|
|||
void assert_equal_hashes(size_t h1, size_t h2, const std::string& msg) { |
|||
if (h1 == h2) { |
|||
std::cout << " \033[32mPASSED\033[0m: " << msg << std::endl; |
|||
} else { |
|||
std::cout << " \033[31mFAILED\033[0m: " << msg << std::endl; |
|||
std::cout << " Hash1: " << h1 << "\n Hash2: " << h2 << std::endl; |
|||
assert(false); |
|||
} |
|||
} |
|||
|
|||
void test_cylinder_hasher_equivalence() { |
|||
print_test_title("Test 1: Same Cylinder, Different Scale and Offset"); |
|||
std::cout |
|||
<< "Purpose: Verify hash is invariant under:\n" |
|||
<< " - Uniform scaling in xy-plane\n" |
|||
<< " - Translation along z-axis\n" |
|||
<< " - Combined transformations\n" |
|||
<< "Cylinder 1: Identity (unit radius, axis = Z)\n" |
|||
<< "Cylinder 2: Scaled by 2 in xy, rotated -30 degree around Z, shifted by (0,0,5)\n" |
|||
<< "Expected: Same hash\n"; |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
// Cylinder 1: Identity
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
mat1->world_to_local = Eigen::Affine3d::Identity(); |
|||
|
|||
// Cylinder 2: Scaled, rotated, translated
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Affine3d t = Eigen::Affine3d::Identity(); |
|||
t.linear() = Eigen::AngleAxisd(-M_PI/6, Eigen::Vector3d::UnitZ()).toRotationMatrix() |
|||
* Eigen::DiagonalMatrix<double,3>(2.0, 2.0, 1.0); |
|||
t.translation() = Eigen::Vector3d(0, 0, 5); // only z-translation matters
|
|||
mat2->world_to_local = t; |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Cylinder 1"); |
|||
print_hash(h2, "Cylinder 2"); |
|||
assert_equal_hashes(h1, h2, "Scaled/rotated/shifted cylinder should hash the same"); |
|||
print_separator(); |
|||
} |
|||
|
|||
void test_cylinder_hasher_rotation_invariance() { |
|||
print_test_title("Test 2: Invariance Under Local Frame Rotation"); |
|||
std::cout |
|||
<< "Purpose: Verify hash is invariant under rotation of local x/y axes\n" |
|||
<< " (around cylinder axis)\n" |
|||
<< "Cylinder 1: Identity\n" |
|||
<< "Cylinder 2: Rotated by 90 degree around Z-axis\n" |
|||
<< "Expected: Same hash (rotation symmetry)\n"; |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
mat1->world_to_local = Eigen::Affine3d::Identity(); |
|||
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Affine3d t = Eigen::Affine3d::Identity(); |
|||
t.linear() = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()).toRotationMatrix(); |
|||
mat2->world_to_local = t; |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Cylinder 1 (0 degree)"); |
|||
print_hash(h2, "Cylinder 2 (90 degree)"); |
|||
assert_equal_hashes(h1, h2, "Rotation around axis should not change hash"); |
|||
print_separator(); |
|||
} |
|||
|
|||
void test_cylinder_hasher_different_axis() { |
|||
print_test_title("Test 3: Different Cylinder Axis"); |
|||
std::cout |
|||
<< "Purpose: Verify hash changes when cylinder axis changes\n" |
|||
<< "Cylinder 1: Axis = Z\n" |
|||
<< "Cylinder 2: Axis = X\n" |
|||
<< "Expected: Different hashes\n"; |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
mat1->world_to_local = Eigen::Affine3d::Identity(); |
|||
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Affine3d t = Eigen::Affine3d::Identity(); |
|||
t.linear() = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()).toRotationMatrix(); // Z -> X
|
|||
mat2->world_to_local = t; |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Axis = Z"); |
|||
print_hash(h2, "Axis = X"); |
|||
if (h1 != h2) { |
|||
std::cout << " \033[32mPASSED\033[0m: Different axes produce different hashes" << std::endl; |
|||
} else { |
|||
std::cout << " \033[31mFAILED\033[0m: Different axes should have different hashes" << std::endl; |
|||
assert(false); |
|||
} |
|||
print_separator(); |
|||
} |
|||
|
|||
void test_cylinder_hasher_different_radius() { |
|||
print_test_title("Test 4: Different Radius"); |
|||
std::cout |
|||
<< "Purpose: Verify hash changes with radius\n" |
|||
<< "Cylinder 1: Radius = 1.0\n" |
|||
<< "Cylinder 2: Radius = 3.0\n" |
|||
<< "Expected: Different hashes\n"; |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
mat1->world_to_local = Eigen::Affine3d::Identity(); |
|||
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Affine3d t = Eigen::Affine3d::Identity(); |
|||
t.linear() = Eigen::DiagonalMatrix<double,3>(3.0, 3.0, 1.0) * t.linear(); // scale xy by 3
|
|||
mat2->world_to_local = t; |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Radius = 1.0"); |
|||
print_hash(h2, "Radius = 3.0"); |
|||
if (h1 != h2) { |
|||
std::cout << " \033[32mPASSED\033[0m: Different radii produce different hashes" << std::endl; |
|||
} else { |
|||
std::cout << " \033[31mFAILED\033[0m: Different radii should have different hashes" << std::endl; |
|||
assert(false); |
|||
} |
|||
print_separator(); |
|||
} |
|||
|
|||
void test_cylinder_hasher_translation_along_axis() { |
|||
print_test_title("Test 5: Translation Along Cylinder Axis"); |
|||
std::cout |
|||
<< "Purpose: Verify hash is invariant under translation along the cylinder's axis\n" |
|||
<< " (i.e., sliding the cylinder along its own length)\n" |
|||
<< "Cylinder 1: Centered at origin\n" |
|||
<< "Cylinder 2: Translated by (0, 0, 100) — far along Z-axis\n" |
|||
<< "Expected: Same hash (axis-aligned translation should not affect equivalence)\n"; |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
// Cylinder 1: Identity (centered at origin)
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
mat1->world_to_local = Eigen::Affine3d::Identity(); |
|||
|
|||
// Cylinder 2: Same orientation and scale, but translated along Z-axis
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
mat2->world_to_local.translation() = Eigen::Vector3d(0, 0, 100); |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Cylinder 1 (origin)"); |
|||
print_hash(h2, "Cylinder 2 (translated by (0,0,100))"); |
|||
|
|||
assert_equal_hashes(h1, h2, "Translation along axis should not change hash"); |
|||
print_separator(); |
|||
} |
|||
|
|||
void test_cylinder_hasher_with_raw_matrix() { |
|||
print_test_title("Test: Construct Cylinder from Raw Matrix Data"); |
|||
|
|||
using namespace internal; |
|||
detail::hasher<cylinder_paired_model_matrix> hasher; |
|||
|
|||
// 构造第一个圆柱
|
|||
auto mat1 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Matrix<double, 3, 4> m1; |
|||
// 用你复制的 array 数据填充 m1
|
|||
m1 << 1.0, 0.0, 0.0, -0.5, |
|||
0.0, 1.0, 0.0, 0.0, |
|||
0.0, 0.0, 1.0, 0.0; |
|||
mat1->world_to_local.matrix() = m1; |
|||
|
|||
// 构造第二个圆柱(可以用不同数据或同样数据做等价性测试)
|
|||
auto mat2 = std::make_unique<paired_model_matrix>(); |
|||
Eigen::Matrix<double, 3, 4> m2; |
|||
m2 << 1.0, 0.0, 0.0, -0.5, |
|||
0.0, 1.0, 0.0, 0.0, |
|||
0.0, 0.0, 1.0, -1.0; |
|||
mat2->world_to_local.matrix() = m2; |
|||
|
|||
cylinder_paired_model_matrix block1, block2; |
|||
block1.data = mat1.get(); |
|||
block2.data = mat2.get(); |
|||
|
|||
size_t h1 = hasher(block1); |
|||
size_t h2 = hasher(block2); |
|||
|
|||
print_hash(h1, "Cylinder 1 (raw matrix)"); |
|||
print_hash(h2, "Cylinder 2 (raw matrix)"); |
|||
|
|||
assert_equal_hashes(h1, h2, "Raw matrix cylinders should hash the same"); |
|||
print_separator(); |
|||
} |
|||
|
|||
int main() { |
|||
std::cout << "\033[1;35m Starting Cylinder Hasher Tests\033[0m\n" |
|||
<< std::string(60, '=') << "\n" << std::endl; |
|||
|
|||
test_cylinder_hasher_equivalence(); |
|||
test_cylinder_hasher_rotation_invariance(); |
|||
test_cylinder_hasher_different_axis(); |
|||
//test_cylinder_hasher_different_radius();
|
|||
test_cylinder_hasher_translation_along_axis(); |
|||
test_cylinder_hasher_with_raw_matrix(); |
|||
|
|||
std::cout << "\033[1;32m All tests completed successfully!\033[0m" << std::endl; |
|||
return 0; |
|||
} |
Loading…
Reference in new issue