Skip to content

Commit

Permalink
[DELETE ME] Disable all tests except the failing test
Browse files Browse the repository at this point in the history 
This includes *only* the one test configuration that is failing CI. Once
I figure out what the difference is, kill this commit. It logs a *ton* of
data, so I've disconnected the primitive test from the solver so it only
prints out as part of the local test.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed Jul 25, 2018
1 parent 5b61e8b commit d36c4a2
Show file tree
Hide file tree
Showing 4 changed files with 114 additions and 81 deletions.
70 changes: 35 additions & 35 deletions include/fcl/narrowphase/detail/gjk_solver_indep-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,7 +247,7 @@ FCL_GJK_INDEP_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)
//FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
FCL_GJK_INDEP_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
Expand Down Expand Up @@ -693,40 +693,40 @@ bool GJKSolver_indep<S>::shapeSignedDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Sphere<S>, Box<S>>
{
static bool run(
const GJKSolver_indep<S>& /*gjkSolver*/,
const Sphere<S>& s1,
const Transform3<S>& tf1,
const Box<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceIndepImpl<S, Box<S>, Sphere<S>>
{
static bool run(
const GJKSolver_indep<S>& /*gjkSolver*/,
const Box<S>& s1,
const Transform3<S>& tf1,
const Sphere<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1);
}
};
//template<typename S>
//struct ShapeDistanceIndepImpl<S, Sphere<S>, Box<S>>
//{
// static bool run(
// const GJKSolver_indep<S>& /*gjkSolver*/,
// const Sphere<S>& s1,
// const Transform3<S>& tf1,
// const Box<S>& s2,
// const Transform3<S>& tf2,
// S* dist,
// Vector3<S>* p1,
// Vector3<S>* p2)
// {
// return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2);
// }
//};
//
////==============================================================================
//template<typename S>
//struct ShapeDistanceIndepImpl<S, Box<S>, Sphere<S>>
//{
// static bool run(
// const GJKSolver_indep<S>& /*gjkSolver*/,
// const Box<S>& s1,
// const Transform3<S>& tf1,
// const Sphere<S>& s2,
// const Transform3<S>& tf2,
// S* dist,
// Vector3<S>* p1,
// Vector3<S>* p2)
// {
// return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1);
// }
//};

//==============================================================================
template<typename S>
Expand Down
70 changes: 35 additions & 35 deletions include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ FCL_GJK_LIBCCD_SHAPE_INTERSECT(Box, detail::boxBoxIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Capsule, detail::sphereCapsuleIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)
//FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Box, detail::sphereBoxIntersect)

FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Sphere, Halfspace, detail::sphereHalfspaceIntersect)
FCL_GJK_LIBCCD_SHAPE_SHAPE_INTERSECT(Ellipsoid, Halfspace, detail::ellipsoidHalfspaceIntersect)
Expand Down Expand Up @@ -674,40 +674,40 @@ bool GJKSolver_libccd<S>::shapeDistance(
// +------------+-----+--------+-----------+---------+------+----------+-------+------------+----------+

//==============================================================================
template<typename S>
struct ShapeDistanceLibccdImpl<S, Sphere<S>, Box<S>>
{
static bool run(
const GJKSolver_libccd<S>& /*gjkSolver*/,
const Sphere<S>& s1,
const Transform3<S>& tf1,
const Box<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2);
}
};

//==============================================================================
template<typename S>
struct ShapeDistanceLibccdImpl<S, Box<S>, Sphere<S>>
{
static bool run(
const GJKSolver_libccd<S>& /*gjkSolver*/,
const Box<S>& s1,
const Transform3<S>& tf1,
const Sphere<S>& s2,
const Transform3<S>& tf2,
S* dist,
Vector3<S>* p1,
Vector3<S>* p2)
{
return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1);
}
};
//template<typename S>
//struct ShapeDistanceLibccdImpl<S, Sphere<S>, Box<S>>
//{
// static bool run(
// const GJKSolver_libccd<S>& /*gjkSolver*/,
// const Sphere<S>& s1,
// const Transform3<S>& tf1,
// const Box<S>& s2,
// const Transform3<S>& tf2,
// S* dist,
// Vector3<S>* p1,
// Vector3<S>* p2)
// {
// return detail::sphereBoxDistance(s1, tf1, s2, tf2, dist, p1, p2);
// }
//};
//
////==============================================================================
//template<typename S>
//struct ShapeDistanceLibccdImpl<S, Box<S>, Sphere<S>>
//{
// static bool run(
// const GJKSolver_libccd<S>& /*gjkSolver*/,
// const Box<S>& s1,
// const Transform3<S>& tf1,
// const Sphere<S>& s2,
// const Transform3<S>& tf2,
// S* dist,
// Vector3<S>* p1,
// Vector3<S>* p2)
// {
// return detail::sphereBoxDistance(s2, tf2, s1, tf1, dist, p2, p1);
// }
//};

//==============================================================================
template<typename S>
Expand Down
27 changes: 22 additions & 5 deletions include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@

#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h"

#include <iomanip>

namespace fcl {
namespace detail {

Expand Down Expand Up @@ -176,10 +178,23 @@ FCL_EXPORT bool sphereBoxDistance(const Sphere<S>& sphere,
const Vector3<S> p_BC = X_BS.translation();
const S r = sphere.radius;

const Transform3<S> I = X_FB.inverse() * X_FB;

std::cout << "sphereBoxDistance\n";
std::cout << std::setprecision(20);
std::cout << " X_FS:\n" << X_FS.matrix() << "\n";
std::cout << " X_FB:\n" << X_FB.matrix() << "\n";
std::cout << " X_FB^-1\n" << X_FB.inverse().matrix() << "\n";
std::cout << " |X_FB^-1 . X_FB|: " << I.matrix().cwiseAbs().maxCoeff() << "\n";
std::cout << " X_BS:\n" << X_BS.matrix() << "\n";
std::cout << " p_BC: " << p_BC.transpose() << "\n";
std::cout << " r: " << r << "\n";

// Find N, the nearest point *inside* the box to the sphere center C (measured
// and expressed in frame B)
Vector3<S> p_BN;
bool N_is_not_C = nearestPointInBox(box.side, p_BC, p_BN);
std::cout << " p_BN: " << p_BN.transpose() << "\n";

if (N_is_not_C) {
// If N is not C, we know the sphere center is *outside* the box (but we
Expand All @@ -188,17 +203,19 @@ FCL_EXPORT bool sphereBoxDistance(const Sphere<S>& sphere,
// Compute the position vector from the nearest point N to the sphere center
// C in the frame B.
Vector3<S> p_NC_B = p_BC - p_BN;
std::cout << " p_NC_B: " << p_NC_B.transpose() << "\n";
S squared_distance = p_NC_B.squaredNorm();
std::cout << " dist^2: " << squared_distance << "\n";
if (squared_distance > r * r) {
// The distance to the nearest point is greater than the radius, we have
// proven separation.
S d{-1};
if (distance || p_FBs || p_FSb)
if (distance || p_FBs || p_FSb) {
d = sqrt(squared_distance);
if (distance != nullptr)
*distance = d - r;
if (p_FBs != nullptr)
*p_FBs = X_FB * p_BN;
std::cout << " |p_NC_B|: " << d << "\n";
}
if (distance != nullptr) *distance = d - r;
if (p_FBs != nullptr) *p_FBs = X_FB * p_BN;
if (p_FSb != nullptr) {
const Vector3<S> p_BSb = (p_NC_B / d) * (d - r) + p_BN;
*p_FSb = X_FB * p_BSb;
Expand Down
28 changes: 22 additions & 6 deletions test/narrowphase/detail/primitive_shape_algorithm/test_sphere_box.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@

#include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h"

#include <iomanip>
#include <string>

#include <gtest/gtest.h>
Expand Down Expand Up @@ -378,7 +379,7 @@ std::vector<TestConfiguration<S>> GetUniformConfigurations() {
template <typename S>
std::vector<TestConfiguration<S>> GetNonUniformConfigurations() {
std::vector<TestConfiguration<S>> configurations;

#if 0
{
// Case: long "skinny" box and tiny sphere. Nearest feature is the +z face.
const Vector3<S> half_size(15, 1, 1);
Expand Down Expand Up @@ -410,13 +411,14 @@ std::vector<TestConfiguration<S>> GetNonUniformConfigurations() {
config.expected_p_BBs << p_BS(0), p_BS(1), half_size(2);
}
}

#endif
{
// Case: Large sphere collides with small box. Nearest feature is the +x,
// +y, +z corner.
const Vector3<S> half_size(0.1, 0.15, 0.2);
const S r = 10;
const Vector3<S> n_SB = Vector3<S>{-1, -2, -3}.normalized();
#if 0
{
// Subcase: colliding.
S target_depth = half_size.minCoeff() * 0.5;
Expand All @@ -428,6 +430,7 @@ std::vector<TestConfiguration<S>> GetNonUniformConfigurations() {
config.expected_depth = target_depth;
config.expected_pos = half_size + n_SB * (target_depth * 0.5);
}
#endif
{
// Subcase: not colliding.
S distance = half_size.minCoeff() * 0.1;
Expand Down Expand Up @@ -527,6 +530,13 @@ void EvalDistanceForTestConfiguration(const TestConfiguration<S>& config,
sphereBoxDistance<S>(sphere, X_WS, box, X_WB, &distance, &p_WSb, &p_WBs);
EXPECT_NE(separated, config.expected_colliding) << config.name;
if (!config.expected_colliding) {
std::cout << "DISTANCE: " << config.name << "\n";
std::cout << std::setprecision(20);
std::cout << " X_WB:\n" << X_WB.matrix() << "\n";
std::cout << " X_WS:\n" << X_WS.matrix() << "\n";
std::cout << " distance: " << distance << "\n";
std::cout << " p_WSb: " << p_WSb.transpose() << "\n";
std::cout << " p_WBs: " << p_WBs.transpose() << "\n";
EXPECT_NEAR(distance, config.expected_distance, eps)
<< config.name;
EXPECT_TRUE(CompareMatrices(p_WSb,
Expand Down Expand Up @@ -562,10 +572,13 @@ void QueryWithVaryingWorldFrames(

// Frame F is the box frame.
Transform3<S> X_FB = Transform3<S>::Identity();
#if 0
evaluate_all(AppendLabel(configurations, "X_FB = I"), X_FB);

// Simple arbitrary translation away from the origin.
#endif
X_FB.translation() << 1.3, 2.7, 6.5;
#if 0
evaluate_all(AppendLabel(configurations, "X_FB is translation"), X_FB);

std::string axis_name[] = {"x", "y", "z"};
Expand All @@ -576,7 +589,7 @@ void QueryWithVaryingWorldFrames(
X_FB.linear() << angle_axis.matrix();
evaluate_all(AppendLabel(configurations, label), X_FB);
}

#endif
// Arbitrary orientation.
{
AngleAxis<S> angle_axis{constants<S>::pi() / 3,
Expand All @@ -585,14 +598,15 @@ void QueryWithVaryingWorldFrames(
evaluate_all(AppendLabel(configurations, "X_FB is arbitrary rotation"),
X_FB);
}

#if 0
// Near axis aligned.
{
AngleAxis<S> angle_axis{constants<S>::eps_12(), Vector3<S>::UnitX()};
X_FB.linear() << angle_axis.matrix();
evaluate_all(AppendLabel(configurations, "X_FB is near identity"),
X_FB);
}
#endif
}

// Runs all test configurations across multiple poses in the world frame --
Expand Down Expand Up @@ -632,7 +646,7 @@ void QueryWithOrientedSphere(
}

//======================================================================

#if 0
// Tests the helper function that finds the closest point in the box.
GTEST_TEST(SphereBoxPrimitiveTest, NearestPointInBox) {
NearestPointInBox<float>();
Expand Down Expand Up @@ -684,15 +698,17 @@ GTEST_TEST(SphereBoxPrimitiveTest, DistanceWithSphereRotations) {
QueryWithOrientedSphere<double>(GetUniformConfigurations<double>(),
EvalDistanceForTestConfiguration<double>);
}

#endif
// Evaluates distance on a small set of configurations where the box and scale
// are of radically different scales - evaluation across multiple poses in the
// world frame.
GTEST_TEST(SphereBoxPrimitiveTest, DistanceIncompatibleScales) {
QueryWithVaryingWorldFrames<float>(GetNonUniformConfigurations<float>(),
EvalDistanceForTestConfiguration<float>);
#if 0
QueryWithVaryingWorldFrames<double>(GetNonUniformConfigurations<double>(),
EvalDistanceForTestConfiguration<double>);
#endif
}

} // namespace
Expand Down

0 comments on commit d36c4a2

Please sign in to comment.