TEST(ReachabilitySphere, convertToCapability)
{
Capability capability(CONE, 70.0, 55.0, 25.0);
ReachabilitySphere sphere = createReachabilitySphereFromCapability(capability, 500);
Capability testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CONE);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
ASSERT_EQ(0.0, testCapability.getShapeFitError());
capability = Capability(CYLINDER_1, 70.0, 55.0, 25.0);
sphere = createReachabilitySphereFromCapability(capability, 10000);
testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CYLINDER_1);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
// ASSERT_EQ(0.0, testCapability.getShapeFitError());
// TODO: improve PCA to get correct axis (for now SFE could be 0.0 but is higher)
capability = Capability(CYLINDER_2, 70.0, 55.0, 25.0);
sphere = createReachabilitySphereFromCapability(capability, 10000);
testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CYLINDER_2);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
// ASSERT_EQ(0.0, testCapability.getShapeFitError());
}
