{

const int num_tests = int(1e5);

const double mu = .4;

const CoulombFrictionCone cone(mu);

const DualCoulombFrictionCone dual_cone = cone.dual();

BOOST_CHECK(cone.isInside(Eigen::Vector3d::Zero()));

BOOST_CHECK(cone.project(Eigen::Vector3d::Zero()) == Eigen::Vector3d::Zero());

BOOST_CHECK(dual_cone.isInside(Eigen::Vector3d::Zero()));

BOOST_CHECK(dual_cone.project(Eigen::Vector3d::Zero()) == Eigen::Vector3d::Zero());

for (int k = 0; k < num_tests; k)

{

const Eigen::Vector3d x = Eigen::Vector3d::Random();

// Cone

const Eigen::Vector3d proj_x = cone.project(x);

const Eigen::Vector3d proj_proj_x = cone.project(proj_x);

BOOST_CHECK(cone.isInside(proj_x, 1e-12));

BOOST_CHECK(cone.isInside(proj_proj_x, 1e-12));

BOOST_CHECK(proj_x.isApprox(proj_proj_x));

if (cone.isInside(x))

BOOST_CHECK(x == proj_x);

BOOST_CHECK(fabs((x - proj_x).dot(proj_x)) <= 1e-12); // orthogonal projection

// Dual cone

const Eigen::Vector3d dual_proj_x = dual_cone.project(x);

const Eigen::Vector3d dual_proj_proj_x = dual_cone.project(dual_proj_x);

BOOST_CHECK(dual_cone.isInside(dual_proj_x, 1e-12));

BOOST_CHECK(dual_cone.isInside(dual_proj_proj_x, 1e-12));

BOOST_CHECK(dual_proj_x.isApprox(dual_proj_proj_x));

if (dual_cone.isInside(x))

BOOST_CHECK(x == dual_proj_x);

BOOST_CHECK(fabs((x - dual_proj_x).dot(dual_proj_x)) <= 1e-12); // orthogonal projection

// Radial projection

{

const Eigen::Vector3d radial_proj_x = cone.computeRadialProjection(x);

const Eigen::Vector3d radial_proj_radial_proj_x = cone.computeRadialProjection(radial_proj_x);

BOOST_CHECK(cone.isInside(radial_proj_x, 1e-12));

BOOST_CHECK(radial_proj_x[2] == x[2] || radial_proj_x[2] == 0.);

if (radial_proj_x[2] == x[2])

BOOST_CHECK(

std::fabs(radial_proj_x.head<2>().normalized().dot(x.head<2>().normalized()) - 1.)

<= 1e-6);

else

BOOST_CHECK(radial_proj_x.head<2>().isZero());

BOOST_CHECK(radial_proj_radial_proj_x.isApprox(radial_proj_radial_proj_x));

}

}