234 lines
7.0 KiB
C++
234 lines
7.0 KiB
C++
#include <cruft/util/vector.hpp>
|
|
|
|
#include <cruft/util/maths.hpp>
|
|
#include <cruft/util/tap.hpp>
|
|
|
|
#include <cruft/util/float.hpp>
|
|
#include <cruft/util/coord/iostream.hpp>
|
|
|
|
using cruft::vector;
|
|
using cruft::vector2f;
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
void
|
|
test_polar (cruft::TAP::logger &tap)
|
|
{
|
|
static const struct {
|
|
cruft::vector2f polar;
|
|
cruft::vector2f cartesian;
|
|
const char *desc;
|
|
} TESTS[] {
|
|
{
|
|
{ 0.f, 0.f },
|
|
{ 0.f, 0.f },
|
|
"all zeroes"
|
|
},
|
|
|
|
{
|
|
{ 1.f, 0.f },
|
|
{ 1.f, 0.f },
|
|
"unit length, unrotated"
|
|
},
|
|
|
|
{
|
|
{ 1.f, cruft::pi<float> / 2.f },
|
|
{ 0.f, 1.f },
|
|
"unit length, rotated"
|
|
},
|
|
|
|
{
|
|
{ 1.f, 2 * cruft::pi<float> },
|
|
{ 1.f, 0.f },
|
|
"full rotation, unit length"
|
|
}
|
|
};
|
|
|
|
for (const auto &t: TESTS) {
|
|
// Compare the difference of cartesian representations. Don't use
|
|
// direct equality comparisons here as the numeric stability can be
|
|
// poor and we have nice whole numbers to start with.
|
|
auto in_cart = t.cartesian;
|
|
auto to_cart = cruft::polar_to_cartesian (t.polar);
|
|
|
|
tap.expect_lt (norm (in_cart - to_cart), 0.00001f, "{:s}", t.desc);
|
|
|
|
// Compare polar representations. Make sure to normalise them first.
|
|
auto in_polar = t.polar;
|
|
auto to_polar = cruft::cartesian_to_polar (t.cartesian);
|
|
|
|
in_polar[1] = std::fmod (in_polar[1], 2 * cruft::pi<float>);
|
|
to_polar[1] = std::fmod (to_polar[1], 2 * cruft::pi<float>);
|
|
|
|
tap.expect_eq (in_polar, to_polar, "{:s}", t.desc);
|
|
}
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
void
|
|
test_euler (cruft::TAP::logger &tap)
|
|
{
|
|
static const struct {
|
|
cruft::vector3f dir;
|
|
cruft::vector2f euler;
|
|
const char *name;
|
|
} TESTS[] = {
|
|
// y-axis
|
|
{ { 0, 0, -1 }, { 0.5f, 0.5f }, "forward" },
|
|
{ { -1, 0, 0 }, { 0.5f, -1.0f }, "left" },
|
|
{ { 0, 0, 1 }, { 0.5f, -0.5f }, "back" },
|
|
{ { 1, 0, 0 }, { 0.5f, 0.0f }, "right" },
|
|
|
|
// x-axis
|
|
{ { 0, 1, 0 }, { 0, 0 }, "up" },
|
|
{ { 0, -1, 0 }, { 1, 0 }, "down" },
|
|
};
|
|
|
|
// check that simple axis rotations look correct
|
|
for (auto i: TESTS) {
|
|
tap.expect_eq (cruft::to_euler (i.dir),
|
|
i.euler * cruft::pi<float>,
|
|
"to euler, {:s}", i.name);
|
|
}
|
|
|
|
// check error in round trip through euler angles
|
|
for (auto i: TESTS) {
|
|
auto trip = cruft::from_euler (cruft::to_euler (i.dir));
|
|
auto diff = i.dir - trip;
|
|
auto mag = norm (diff);
|
|
|
|
// trig functions reduce precision above almost_equal levels, so we
|
|
// hard code a fairly low bound here instead.
|
|
tap.expect_lt (mag, 1e-7, "euler round-trip error, {:s}", i.name);
|
|
}
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
void
|
|
test_spherical (cruft::TAP::logger &tap)
|
|
{
|
|
constexpr auto q = cruft::pi<float> / 2.f;
|
|
|
|
static constexpr struct {
|
|
cruft::vector3f spherical;
|
|
cruft::vector3f cartesian;
|
|
const char *message;
|
|
} S2C [] = {
|
|
{ { 1, 0 * q, 0 * q }, { 0, 0, 1 }, "+zero", },
|
|
{ { 1, 2 * q, 0 * q }, { 0, 0, -1 }, "-zero", },
|
|
|
|
{ { 1, 1 * q, 0 * q }, { 1, 0, 0 }, "90-theta", },
|
|
{ { 1, 2 * q, 0 * q }, { 0, 0, -1 }, "180-theta", },
|
|
{ { 1, 3 * q, 0 * q }, { -1, 0, 0 }, "270-theta", },
|
|
|
|
{ { 1, 0 * q, 1 * q }, { 0, 0, 1 }, "90-phi", },
|
|
{ { 1, 0 * q, 2 * q }, { 0, 0, 1 }, "180-phi", },
|
|
{ { 1, 0 * q, 3 * q }, { 0, 0, 1 }, "270-phi", },
|
|
|
|
{ { 1, 1 * q, 1 * q }, { 0, 1, 0 }, "90-theta, 90-phi" },
|
|
{ { 1, 1 * q, 2 * q }, { -1, 0, 0 }, "90-theta, 180-phi" },
|
|
{ { 1, 1 * q, 3 * q }, { 0, -1, 0 }, "90-theta, 270-phi" },
|
|
};
|
|
|
|
for (const auto t: S2C) {
|
|
tap.expect (
|
|
all (abs (cruft::spherical_to_cartesian (t.spherical) - t.cartesian) < 1e-7f),
|
|
"{:s}, spherical-cartesian",
|
|
t.message
|
|
);
|
|
}
|
|
|
|
|
|
// double check origin points are transformed correctly. the zeros and
|
|
// multiple representations need to be accounted for.
|
|
tap.expect_eq (
|
|
cruft::cartesian_to_spherical (cruft::vector3f{0}).x,
|
|
0,
|
|
"origin, cartesian-spherical"
|
|
);
|
|
|
|
tap.expect_eq (
|
|
cruft::spherical_to_cartesian (cruft::vector3f{0,1,-1}),
|
|
cruft::vector3f{0},
|
|
"origin, cartesian-spherical"
|
|
);
|
|
|
|
|
|
//-------------------------------------------------------------------------
|
|
// dedicated cartesian-spherical test cases. it's hard to use the
|
|
// spherical-cartesian cases from above because. some of the tests don't
|
|
// have unique representations in spherical space,
|
|
// eg, {1,0,z} is always {0,0,1})
|
|
// so the reverse transform isn't usable as a test.
|
|
static constexpr struct {
|
|
cruft::vector3f cartesian;
|
|
cruft::vector3f spherical;
|
|
const char *message;
|
|
} C2S[] = {
|
|
{ { 0, 0, 1 }, { 1, 0, 0 }, "+z" },
|
|
{ { 0, 0, -1 }, { 1, q*2, 0 }, "-z" },
|
|
|
|
{ { 0, 1, 0 }, { 1, q, q }, "+y" },
|
|
{ { 0, -1, 0 }, { 1, q, -q }, "-y" },
|
|
|
|
{ { 1, 0, 0 }, { 1, q, 0 }, "+x" },
|
|
{ { -1, 0, 0 }, { 1, q, q*2 }, "-x" },
|
|
|
|
{ { 1, 1, 1 }, { 1.732f, 0.95539f, q/2 }, "+ve" },
|
|
{ { -1, -1, -1 }, { 1.732f, 2.18619942f, -2.35619f}, "-ve" },
|
|
|
|
{ { 9, 9, 9 }, { 15.5885f, 0.955317f, q/2 }, "+9ve" },
|
|
};
|
|
|
|
|
|
for (const auto &t: C2S) {
|
|
const auto s0 = cruft::canonical_spherical (cruft::cartesian_to_spherical (t.cartesian));
|
|
const auto s1 = cruft::canonical_spherical (t.spherical);
|
|
tap.expect (
|
|
all (abs (s0 - s1) < 1e-4f),
|
|
"{:s}, cartesian-spherical",
|
|
t.message
|
|
);
|
|
}
|
|
//auto s = cruft::cartesian_to_spherical (t.cartesian);
|
|
//std::clog << s << " == " << t.spherical << '\n';
|
|
//tap.expect_eq (
|
|
// cruft::cartesian_to_spherical (t.cartesian),
|
|
// t.spherical,
|
|
// "{:s}, cartesian-spherical",
|
|
// t.message
|
|
//);
|
|
|
|
|
|
{
|
|
//cruft::vector3f s { 1, .5f, 2/3.f };
|
|
//cruft::vector3f c { 0.35f, 0.61f, 0.71f };
|
|
//tap.expect_eq
|
|
}
|
|
};
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
int
|
|
main ()
|
|
{
|
|
cruft::TAP::logger tap;
|
|
|
|
test_polar (tap);
|
|
test_euler (tap);
|
|
test_spherical (tap);
|
|
|
|
tap.expect (!is_normalised (cruft::vector3f::zeros ()), "zeros isn't normalised");
|
|
tap.expect (!is_normalised (cruft::vector3f::ones ()), "ones isn't normalised");
|
|
|
|
tap.expect_eq (
|
|
cruft::hypot (cruft::vector3f{0,1,2} - cruft::vector3f{3,2,4}),
|
|
std::sqrt (14.f),
|
|
"vector3f hypot"
|
|
);
|
|
|
|
return tap.status ();
|
|
}
|