37 #include <gtest/gtest.h>
83 const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, p1);
84 EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
99 const S eps =
kEps * S(0.99);
100 const Vector3<S> q0(p0(0) + eps, p0(1), p0(2));
101 const Vector3<S> q1(p1(0) + eps, p1(1), p1(2));
102 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
103 EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
114 const S eps =
kEps * S(1.5);
117 const Vector3<S> q0(p0(0) + eps, p0(1), p0(2));
118 const Vector3<S> q1(p1(0), p1(1) + eps, p1(2));
119 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
120 EXPECT_NE(squared_dist, (p1 - p0).squaredNorm());
135 const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
136 EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
144 const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
145 EXPECT_EQ(squared_dist, (q1 - p0).squaredNorm());
156 const S squared_dist = this->ComputeNearestPoint(p0, p0, p1, q1);
157 EXPECT_EQ(squared_dist, p0.squaredNorm());
173 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
174 EXPECT_EQ(squared_dist, (p1 - p0).squaredNorm());
182 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
183 EXPECT_EQ(squared_dist, (q0 - p1).squaredNorm());
194 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, p1);
195 EXPECT_EQ(squared_dist, p1.squaredNorm());
212 ASSERT_NEAR(dir.dot(dir_perp), 0, eps);
233 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
239 EXPECT_NEAR((this->n0_ - this->n1_).norm(), S(0), eps);
240 const S nearest_dist = this->n0_.norm();
241 EXPECT_NEAR(nearest_dist, dir.dot(this->n0_), eps);
242 EXPECT_LE(nearest_dist, q0.norm());
252 const S expected_dist{0.5};
253 const Vector3<S> offset = dir_perp * expected_dist;
256 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
258 EXPECT_NEAR(squared_dist, expected_dist * expected_dist, eps);
260 EXPECT_NEAR((this->n0_ - this->n1_).norm(), expected_dist, eps);
261 const S nearest_dist = this->n0_.norm();
262 EXPECT_NEAR(nearest_dist, dir.dot(this->n0_), eps);
263 EXPECT_LE(nearest_dist, q0.norm());
272 const S squared_dist = this->ComputeNearestPoint(p0, q0, q0, q1);
287 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
302 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
304 EXPECT_NEAR(squared_dist, (p1 - q0).squaredNorm(), eps);
317 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
332 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
334 EXPECT_NEAR(squared_dist, (p0 - q1).squaredNorm(), eps);
354 GTEST_ASSERT_GE(abs(dir_A.dot(dir_B)), eps);
355 GTEST_ASSERT_LT(abs(dir_A.dot(dir_B)), S(1) - eps);
358 const Vector3<S> perp = dir_A.cross(dir_B).normalized();
363 const Vector3<S> q0 = expected_n0 - S(2) * dir_A;
365 const Vector3<S> q1 = expected_n0 - S(2.5) * dir_B;
366 const S expected_s = 1 / S(3);
367 const S t_expected = 1 / S(3.5);
371 const S squared_dist = this->ComputeNearestPoint(p0, q0, p1, q1);
384 const S expected_dist{1.25};
385 const Vector3<S> offset = expected_dist * perp;
386 const S squared_dist =
387 this->ComputeNearestPoint(p0, q0, p1 + offset, q1 + offset);
389 EXPECT_NEAR(squared_dist, expected_dist * expected_dist, eps);
401 *u = d.dot(Q - A) / d.squaredNorm();
410 const Vector3<S> offset = S(1.1) * (p0 - expected_n0);
415 project_onto_segment(p1_shift, q1_shift, p0, &expected_N1, &t_expected);
416 const S expected_squared_dist = (p0 - expected_N1).squaredNorm();
420 this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
422 EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
431 squared_dist = this->ComputeNearestPoint(p1_shift, q1_shift, p0, q0);
433 EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
442 const Vector3<S> offset = S(1.1) * (q0 - expected_n0);
447 project_onto_segment(p1_shift, q1_shift, q0, &expected_N1, &t_expected);
448 const S expected_squared_dist = (q0 - expected_N1).squaredNorm();
452 this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
454 EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
463 squared_dist = this->ComputeNearestPoint(p1_shift, q1_shift, p0, q0);
465 EXPECT_NEAR(squared_dist, expected_squared_dist, eps);
481 p0_dir.cwiseProduct(
Vector3<S>{S(1.75), S(1.5), S(1.25)});
488 p1_shift + (offset.dot(dir_B) > S(0) ? dir_B :
Vector3<S>{-dir_B});
491 S squared_dist = this->ComputeNearestPoint(p0, q0, p1_shift, q1_shift);
493 EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
500 squared_dist = this->ComputeNearestPoint(p0, q0, q1_shift, p1_shift);
502 EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
509 squared_dist = this->ComputeNearestPoint(q0, p0, p1_shift, q1_shift);
511 EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
518 squared_dist = this->ComputeNearestPoint(q0, p0, q1_shift, p1_shift);
520 EXPECT_NEAR(squared_dist, offset.squaredNorm(), eps);
534 template <
typename S>
537 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
540 : ::testing::Test(), c1_(S(1.5), S(2.5)), c2_(S(2), S(3)) {}
585 ConfigureSeparated();
588 ConfigureIntersecting();
591 ConfigureSingleOverlap();
593 case kMultipleOverlap:
594 ConfigureMultipleOverlap();
603 this->Configure(config);
606 this->X_FC2_, &
distance, &p_FW1, &p_FW2);
609 S error = abs(
distance - this->expected_distance_);
611 return ::testing::AssertionFailure()
612 <<
"Error in reported distances"
614 <<
"\n Expected: " << this->expected_distance_
615 <<
"\n Error: " << error
616 <<
"\n Tolerance: " << eps;
619 if (!result1)
return result1;
624 this->ConfigureViableT(0.25);
628 this->ConfigureViableT(-0.25);
640 expected_distance_ =
max(
distance, -(c1_.radius + c2_.radius));
644 X_TC2.translation() << S(0), S(0),
645 c1_.lz / 2 + c1_.radius + c2_.radius +
distance;
647 Vector3<S> p_TW1{S(0), S(0), c1_.lz / S(2) + c1_.radius};
648 expected_p_FW1_ = X_FT() * p_TW1;
650 expected_p_FW2_ = X_FT() * p_TW2;
652 X_FC1_ = X_FT() * X_TC1;
653 X_FC2_ = X_FT() * X_TC2;
657 expected_distance_ = -(c1_.radius + c2_.radius);
663 X_TC2.translation() << S(0), S(0), c1_.lz / S(4);
672 Vector3<S> p_TW1{S(0), c1_.radius, c1_.lz / S(4)};
673 expected_p_FW1_ = X_FT() * p_TW1;
674 Vector3<S> p_TW2{S(0), -c2_.radius, c1_.lz / S(4)};
675 expected_p_FW2_ = X_FT() * p_TW2;
677 X_FC1_ = X_FT() * X_TC1;
678 X_FC2_ = X_FT() * X_TC2;
682 expected_distance_ = -(c1_.radius + c2_.radius);
688 X_TC2.translation() << S(0), S(0), c2_.lz / S(2);
696 Vector3<S> p_TW1{c1_.radius, S(0), c1_.lz / S(2)};
697 expected_p_FW1_ = X_FT() * p_TW1;
698 Vector3<S> p_TW2{-c2_.radius, S(0), c1_.lz / S(2)};
699 expected_p_FW2_ = X_FT() * p_TW2;
701 X_FC1_ = X_FT() * X_TC1;
702 X_FC2_ = X_FT() * X_TC2;
709 X_FT_.translation() << S(10.5), S(12.75), S(-2.5);
760 int main(
int argc,
char* argv[])
762 ::testing::InitGoogleTest(&argc, argv);
763 return RUN_ALL_TESTS();