533 lines
19 KiB
C++
533 lines
19 KiB
C++
|
/*
|
||
|
|
* Software License Agreement (BSD License)
|
||
|
|
*
|
||
|
|
* Copyright (c) 2018. Toyota Research Institute
|
||
|
|
* All rights reserved.
|
||
|
|
*
|
||
|
|
* Redistribution and use in source and binary forms, with or without
|
||
|
|
* modification, are permitted provided that the following conditions
|
||
|
|
* are met:
|
||
|
|
*
|
||
|
|
* * Redistributions of source code must retain the above copyright
|
||
|
|
* notice, this list of conditions and the following disclaimer.
|
||
|
|
* * Redistributions in binary form must reproduce the above
|
||
|
|
* copyright notice, this list of conditions and the following
|
||
|
|
* disclaimer in the documentation and/or other materials provided
|
||
|
|
* with the distribution.
|
||
|
|
* * Neither the name of CNRS-LAAS and AIST nor the names of its
|
||
|
|
* contributors may be used to endorse or promote products derived
|
||
|
|
* from this software without specific prior written permission.
|
||
|
|
*
|
||
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
||
|
|
*/
|
||
|
|
|
||
|
|
/** @author Sean Curtis (sean@tri.global) (2018) */
|
||
|
|
|
||
|
|
// Tests the implementation of a convex polytope geometry.
|
||
|
|
|
||
|
|
#include "fcl/geometry/shape/convex.h"
|
||
|
|
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include <Eigen/StdVector>
|
||
|
|
#include <gtest/gtest.h>
|
||
|
|
|
||
|
|
#include "eigen_matrix_compare.h"
|
||
|
|
#include "fcl/common/types.h"
|
||
|
|
|
||
|
|
namespace fcl {
|
||
|
|
namespace {
|
||
|
|
|
||
|
|
using std::max;
|
||
|
|
|
||
|
|
// Necessary to satisfy Eigen's alignment requirements. See
|
||
|
|
// http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html#StlContainers_vector
|
||
|
|
template <typename S>
|
||
|
|
using PoseVector = std::vector<Transform3<S>,
|
||
|
|
Eigen::aligned_allocator<Transform3<S>>>;
|
||
|
|
|
||
|
|
// Utilities to print scalar type in error messages.
|
||
|
|
template <typename S>
|
||
|
|
struct ScalarString {
|
||
|
|
static std::string value() { return "unknown"; }
|
||
|
|
};
|
||
|
|
|
||
|
|
template <>
|
||
|
|
struct ScalarString<double> {
|
||
|
|
static std::string value() { return "double"; }
|
||
|
|
};
|
||
|
|
|
||
|
|
template <>
|
||
|
|
struct ScalarString<float> {
|
||
|
|
static std::string value() { return "float"; }
|
||
|
|
};
|
||
|
|
|
||
|
|
// Base definition of a "unit" convex polytope. Specific instances should define
|
||
|
|
// faces, vertices, and quantities such as volume, center of mass, and moment of
|
||
|
|
// inertia in terms of a scale factor.
|
||
|
|
template <typename S>
|
||
|
|
class Polytope {
|
||
|
|
public:
|
||
|
|
explicit Polytope(S scale)
|
||
|
|
: vertices_(std::make_shared<std::vector<Vector3<S>>>()),
|
||
|
|
polygons_(std::make_shared<std::vector<int>>()), scale_(scale) {}
|
||
|
|
|
||
|
|
Polytope(const Polytope &other)
|
||
|
|
: vertices_(std::make_shared<std::vector<Vector3<S>>>(*(other.vertices_))),
|
||
|
|
polygons_(std::make_shared<std::vector<int>>(*(other.polygons_))),
|
||
|
|
scale_(other.scale_) {}
|
||
|
|
|
||
|
|
virtual int face_count() const = 0;
|
||
|
|
virtual int vertex_count() const = 0;
|
||
|
|
virtual S volume() const = 0;
|
||
|
|
virtual Vector3<S> com() const = 0;
|
||
|
|
virtual Matrix3<S> principal_inertia_tensor() const = 0;
|
||
|
|
virtual std::string description() const = 0;
|
||
|
|
|
||
|
|
// The scale of the polytope to use with test tolerances.
|
||
|
|
S scale() const { return scale_; }
|
||
|
|
std::shared_ptr<const std::vector<Vector3<S>>> points() const {
|
||
|
|
return vertices_;
|
||
|
|
}
|
||
|
|
std::shared_ptr<const std::vector<int>> polygons() const {
|
||
|
|
return polygons_;
|
||
|
|
}
|
||
|
|
Convex<S> MakeConvex() const {
|
||
|
|
// The Polytope class makes the pointers to vertices and faces const access.
|
||
|
|
// The Convex class calls for non-const pointers. Temporarily const-casting
|
||
|
|
// them to make it compatible.
|
||
|
|
return Convex<S>(points(), face_count(), polygons());
|
||
|
|
}
|
||
|
|
Vector3<S> min_point() const {
|
||
|
|
Vector3<S> m;
|
||
|
|
m.setConstant(std::numeric_limits<S>::max());
|
||
|
|
for (const auto& v : *vertices_) {
|
||
|
|
for (int i = 0; i < 3; ++i) {
|
||
|
|
if (v(i) < m(i)) m(i) = v(i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return m;
|
||
|
|
}
|
||
|
|
Vector3<S> max_point() const {
|
||
|
|
Vector3<S> m;
|
||
|
|
m.setConstant(-std::numeric_limits<S>::max());
|
||
|
|
for (const auto& v : *vertices_) {
|
||
|
|
for (int i = 0; i < 3; ++i) {
|
||
|
|
if (v(i) > m(i)) m(i) = v(i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return m;
|
||
|
|
}
|
||
|
|
Vector3<S> aabb_center() const {
|
||
|
|
return (max_point() + min_point()) / 2;
|
||
|
|
}
|
||
|
|
S aabb_radius() const { return (min_point() - aabb_center()).norm(); }
|
||
|
|
void SetPose(const Transform3<S>& X_WP) {
|
||
|
|
for (auto& v : *vertices_) {
|
||
|
|
v = X_WP * v;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
void add_vertex(const Vector3<S>& vertex) { vertices_->push_back(vertex); }
|
||
|
|
void add_face(std::initializer_list<int> indices) {
|
||
|
|
polygons_->push_back(static_cast<int>(indices.size()));
|
||
|
|
polygons_->insert(polygons_->end(), indices);
|
||
|
|
}
|
||
|
|
// Confirms the number of vertices and number of polygons matches the counts
|
||
|
|
// implied by vertex_count() and face_count(), respectively.
|
||
|
|
void confirm_data() {
|
||
|
|
// Confirm point count.
|
||
|
|
GTEST_ASSERT_EQ(vertex_count(), static_cast<int>(vertices_->size()));
|
||
|
|
|
||
|
|
// Confirm face count.
|
||
|
|
// Count the number of faces encoded in polygons_;
|
||
|
|
int count = 0;
|
||
|
|
int i = 0;
|
||
|
|
while (i < static_cast<int>(polygons_->size())) {
|
||
|
|
++count;
|
||
|
|
i += (*polygons_)[i] + 1;
|
||
|
|
}
|
||
|
|
GTEST_ASSERT_EQ(i, static_cast<int>(polygons_->size()))
|
||
|
|
<< "Badly defined polygons";
|
||
|
|
GTEST_ASSERT_EQ(face_count(), count);
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::shared_ptr<std::vector<Vector3<S>>> vertices_;
|
||
|
|
std::shared_ptr<std::vector<int>> polygons_;
|
||
|
|
S scale_{};
|
||
|
|
};
|
||
|
|
|
||
|
|
// A simple regular tetrahedron with edges of length `scale` centered on the
|
||
|
|
// origin.
|
||
|
|
template <typename S>
|
||
|
|
class EquilateralTetrahedron : public Polytope<S> {
|
||
|
|
public:
|
||
|
|
// Constructs the tetrahedron (of edge length `s`).
|
||
|
|
explicit EquilateralTetrahedron(S scale) : Polytope<S>(scale), scale_(scale) {
|
||
|
|
// Tetrahedron vertices in the tet's canonical frame T. The tet is
|
||
|
|
// "centered" on the origin so that it's center of mass is simple [0, 0, 0].
|
||
|
|
const S z_base = -1 / S(2 * sqrt(6.));
|
||
|
|
Vector3<S> points_T[] = {{S(0.5), S(-0.5 / sqrt(3.)), z_base},
|
||
|
|
{S(-0.5), S(-0.5 / sqrt(3.)), z_base},
|
||
|
|
{S(0), S(1. / sqrt(3.)), z_base},
|
||
|
|
{S(0), S(0), S(sqrt(3. / 8))}};
|
||
|
|
for (const auto& v : points_T) {
|
||
|
|
this->add_vertex(scale * v);
|
||
|
|
};
|
||
|
|
|
||
|
|
// Now add the polygons
|
||
|
|
this->add_face({0, 1, 2});
|
||
|
|
this->add_face({1, 0, 3});
|
||
|
|
this->add_face({0, 2, 3});
|
||
|
|
this->add_face({2, 1, 3});
|
||
|
|
|
||
|
|
this->confirm_data();
|
||
|
|
}
|
||
|
|
// Properties of the polytope.
|
||
|
|
int face_count() const final { return 4; }
|
||
|
|
int vertex_count() const final { return 4; }
|
||
|
|
virtual S volume() const final {
|
||
|
|
// This assumes unit mass.
|
||
|
|
S s = this->scale();
|
||
|
|
return S(sqrt(2) / 12) * s * s * s;
|
||
|
|
}
|
||
|
|
virtual Vector3<S> com() const final { return Vector3<S>::Zero(); }
|
||
|
|
virtual Matrix3<S> principal_inertia_tensor() const {
|
||
|
|
// TODO(SeanCurtis-TRI): Replace this with a legitimate tensor.
|
||
|
|
throw std::logic_error("Not implemented yet");
|
||
|
|
};
|
||
|
|
std::string description() const final {
|
||
|
|
return "Tetrahedron with scale: " + std::to_string(this->scale());
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
S scale_{0};
|
||
|
|
};
|
||
|
|
|
||
|
|
// A simple cube with sides of length `scale`.
|
||
|
|
template <typename S>
|
||
|
|
class Cube : public Polytope<S> {
|
||
|
|
public:
|
||
|
|
Cube(S scale) : Polytope<S>(scale) {
|
||
|
|
// Cube vertices in the cube's canonical frame C.
|
||
|
|
Vector3<S> points_C[] = {{S(-0.5), S(-0.5), S(-0.5)}, // v0
|
||
|
|
{S(0.5), S(-0.5), S(-0.5)}, // v1
|
||
|
|
{S(-0.5), S(0.5), S(-0.5)}, // v2
|
||
|
|
{S(0.5), S(0.5), S(-0.5)}, // v3
|
||
|
|
{S(-0.5), S(-0.5), S(0.5)}, // v4
|
||
|
|
{S(0.5), S(-0.5), S(0.5)}, // v5
|
||
|
|
{S(-0.5), S(0.5), S(0.5)}, // v6
|
||
|
|
{S(0.5), S(0.5), S(0.5)}}; // v7
|
||
|
|
for (const auto& v : points_C) {
|
||
|
|
this->add_vertex(scale * v);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Now add the polygons
|
||
|
|
this->add_face({1, 3, 7, 5}); // +x
|
||
|
|
this->add_face({0, 4, 6, 2}); // -x
|
||
|
|
this->add_face({4, 5, 7, 6}); // +y
|
||
|
|
this->add_face({0, 2, 3, 1}); // -y
|
||
|
|
this->add_face({6, 7, 3, 2}); // +z
|
||
|
|
this->add_face({0, 1, 5, 4}); // -z
|
||
|
|
|
||
|
|
this->confirm_data();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Polytope properties
|
||
|
|
int face_count() const final { return 6; }
|
||
|
|
int vertex_count() const final { return 8; }
|
||
|
|
virtual S volume() const final {
|
||
|
|
S s = this->scale();
|
||
|
|
return s * s * s;
|
||
|
|
}
|
||
|
|
virtual Vector3<S> com() const final { return Vector3<S>::Zero(); }
|
||
|
|
virtual Matrix3<S> principal_inertia_tensor() const {
|
||
|
|
S scale_sqd = this->scale() * this->scale();
|
||
|
|
// This assumes unit mass.
|
||
|
|
return Eigen::DiagonalMatrix<S, 3>(1. / 6., 1. / 6., 1. / 6.) * scale_sqd;
|
||
|
|
};
|
||
|
|
std::string description() const final {
|
||
|
|
return "Cube with scale: " + std::to_string(this->scale());
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
void testConvexConstruction() {
|
||
|
|
Cube<double> cube{1};
|
||
|
|
// Set the cube at some other location to make sure that the interior point
|
||
|
|
// test/ doesn't pass just because it initialized to zero.
|
||
|
|
Vector3<double> p_WB(1, 2, 3);
|
||
|
|
cube.SetPose(Transform3<double>(Eigen::Translation3d(p_WB)));
|
||
|
|
Convex<double> convex = cube.MakeConvex();
|
||
|
|
|
||
|
|
// This doesn't depend on the correct logic in the constructor. But this is
|
||
|
|
// as convenient a time as any to test that it reports the right node type.
|
||
|
|
EXPECT_EQ(convex.getNodeType(), GEOM_CONVEX);
|
||
|
|
|
||
|
|
// The constructor computes the interior point.
|
||
|
|
EXPECT_TRUE(CompareMatrices(convex.getInteriorPoint(), p_WB));
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testAABBComputation(const Shape<S>& model, const Transform3<S>& X_WS) {
|
||
|
|
Shape<S> shape(model);
|
||
|
|
shape.SetPose(X_WS);
|
||
|
|
Convex<S> convex = shape.MakeConvex();
|
||
|
|
convex.computeLocalAABB();
|
||
|
|
|
||
|
|
typename constants<S>::Real eps = constants<S>::eps();
|
||
|
|
EXPECT_NEAR(shape.aabb_radius(), convex.aabb_radius, eps);
|
||
|
|
EXPECT_TRUE(CompareMatrices(shape.aabb_center(), convex.aabb_center, eps));
|
||
|
|
EXPECT_TRUE(CompareMatrices(shape.min_point(), convex.aabb_local.min_, eps));
|
||
|
|
EXPECT_TRUE(CompareMatrices(shape.max_point(), convex.aabb_local.max_, eps));
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testVolume(const Shape<S>& model, const Transform3<S>& X_WS,
|
||
|
|
int bits_lost) {
|
||
|
|
// If we're losing more than 10 bits, then we have a major problem.
|
||
|
|
GTEST_ASSERT_LE(bits_lost, 10);
|
||
|
|
|
||
|
|
Shape<S> shape(model);
|
||
|
|
shape.SetPose(X_WS);
|
||
|
|
Convex<S> convex = shape.MakeConvex();
|
||
|
|
|
||
|
|
// We want the basic tolerance to be near machine precision. The invocation
|
||
|
|
// of this function indicates how many bits of precision are expected to be
|
||
|
|
// lost and the machine epsilon is modified to account for this.
|
||
|
|
typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
|
||
|
|
|
||
|
|
// We want to do a *relative* comparison. We scale our eps by the volume so
|
||
|
|
// that large volumes have tolerances proportional to the actual true value.
|
||
|
|
S scale = max(shape.volume(), S(1));
|
||
|
|
EXPECT_NEAR(shape.volume(), convex.computeVolume(), eps * scale)
|
||
|
|
<< shape.description() << " at\n" << X_WS.matrix()
|
||
|
|
<< "\nusing scalar: " << ScalarString<S>::value();
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testCenterOfMass(const Shape<S>& model, const Transform3<S>& X_WS,
|
||
|
|
int bits_lost) {
|
||
|
|
// If we're losing more than 10 bits, then we have a major problem.
|
||
|
|
GTEST_ASSERT_LE(bits_lost, 10);
|
||
|
|
|
||
|
|
Shape<S> shape(model);
|
||
|
|
shape.SetPose(X_WS);
|
||
|
|
Convex<S> convex = shape.MakeConvex();
|
||
|
|
|
||
|
|
// We want the basic tolerance to be near machine precision. The invocation
|
||
|
|
// of this function indicates how many bits of precision are expected to be
|
||
|
|
// lost and the machine epsilon is modified to account for this.
|
||
|
|
typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
|
||
|
|
|
||
|
|
// We want to do a *relative* comparison. The center-of-mass calculation is a
|
||
|
|
// volume-weighted calculation. So, the relative tolerance should scale with
|
||
|
|
// volume.
|
||
|
|
S scale = max(shape.volume(), S(1));
|
||
|
|
EXPECT_TRUE(
|
||
|
|
CompareMatrices(X_WS * shape.com(), convex.computeCOM(), eps * scale))
|
||
|
|
<< shape.description() << " at\n" << X_WS.matrix()
|
||
|
|
<< "\nusing scalar: " << ScalarString<S>::value();
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testMomentOfInertia(const Shape<S>& model, const Transform3<S>& X_WS,
|
||
|
|
int bits_lost) {
|
||
|
|
// If we're losing more than 10 bits, then we have a major problem.
|
||
|
|
GTEST_ASSERT_LE(bits_lost, 10);
|
||
|
|
|
||
|
|
Shape<S> shape(model);
|
||
|
|
shape.SetPose(X_WS);
|
||
|
|
Convex<S> convex = shape.MakeConvex();
|
||
|
|
|
||
|
|
// We want the basic tolerance to be near machine precision. The invocation
|
||
|
|
// of this function indicates how many bits of precision are expected to be
|
||
|
|
// lost and the machine epsilon is modified to account for this.
|
||
|
|
typename constants<S>::Real eps = (1 << bits_lost) * constants<S>::eps();
|
||
|
|
|
||
|
|
// We want to do a *relative* comparison. The inertia calculation is a
|
||
|
|
// volume-weighted calculation. So, the relative tolerance should scale with
|
||
|
|
// volume.
|
||
|
|
S scale = max(shape.volume(), S(1));
|
||
|
|
EXPECT_TRUE(
|
||
|
|
CompareMatrices(X_WS.linear().transpose() *
|
||
|
|
shape.principal_inertia_tensor() * X_WS.linear(),
|
||
|
|
convex.computeMomentofInertiaRelatedToCOM(), eps * scale))
|
||
|
|
<< shape.description() << " at\n" << X_WS.matrix()
|
||
|
|
<< "\nusing scalar: " << ScalarString<S>::value();
|
||
|
|
}
|
||
|
|
|
||
|
|
template <typename S>
|
||
|
|
PoseVector<S> GetPoses() {
|
||
|
|
PoseVector<S> poses;
|
||
|
|
// Identity.
|
||
|
|
poses.push_back(Transform3<S>::Identity());
|
||
|
|
|
||
|
|
Transform3<S> X_WS;
|
||
|
|
// 90-degree rotation around each axis, in turn.
|
||
|
|
for (int i = 0; i < 3; ++i) {
|
||
|
|
X_WS = Transform3<S>::Identity();
|
||
|
|
X_WS.linear() = AngleAxis<S>(constants<S>::pi() / 2,
|
||
|
|
Vector3<S>::Unit(i)).matrix();
|
||
|
|
poses.push_back(X_WS);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Small angle away from identity.
|
||
|
|
X_WS.linear() = AngleAxis<S>(S(1e-5), Vector3<S>{1, 2, 3}.normalized())
|
||
|
|
.matrix();
|
||
|
|
poses.push_back(X_WS);
|
||
|
|
|
||
|
|
// 45-degree angle to move away from axis-aligned as much as possible.
|
||
|
|
X_WS.linear() = AngleAxis<S>(constants<S>::pi() / 4,
|
||
|
|
Vector3<S>{1, 2, 3}.normalized()).matrix();
|
||
|
|
poses.push_back(X_WS);
|
||
|
|
|
||
|
|
// We don't test translation because that would imply the geometry is
|
||
|
|
// defined away from its own frame's origin. And that's just a recklessly
|
||
|
|
// stupid thing to do. Given the *current* algorithms, this will degrade
|
||
|
|
// the answers based on the *distance* to the origin.
|
||
|
|
// TODO(SeanCurtis-TRI): When the algorithms are no longer sensitive to vertex
|
||
|
|
// position relative to the origin, add tests that show that.
|
||
|
|
|
||
|
|
return poses;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::vector<double> get_test_scales() {
|
||
|
|
return std::vector<double>{0.001, 1, 1000.};
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testLocalAABBComputation(const Shape<S>& shape) {
|
||
|
|
for (const auto& X_WP : GetPoses<S>()) {
|
||
|
|
testAABBComputation<Shape>(shape, X_WP);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testVolumeComputation(const Shape<S>& shape, int bits_lost) {
|
||
|
|
for (const auto& X_WP : GetPoses<S>()) {
|
||
|
|
testVolume<Shape>(shape, X_WP, bits_lost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testCenterOfMassComputation(const Shape<S>& shape, int bits_lost) {
|
||
|
|
for (const auto& X_WP : GetPoses<S>()) {
|
||
|
|
testCenterOfMass<Shape>(shape, X_WP, bits_lost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template <template <typename> class Shape, typename S>
|
||
|
|
void testMomentOfInertiaComputation(const Shape<S>& shape, int bits_lost) {
|
||
|
|
for (const auto& X_WP : GetPoses<S>()) {
|
||
|
|
testMomentOfInertia<Shape>(shape, X_WP, bits_lost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, Constructor) {
|
||
|
|
testConvexConstruction();
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, LocalAABBComputation_Cube) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
Cube<double> cube_d(scale);
|
||
|
|
testLocalAABBComputation(cube_d);
|
||
|
|
Cube<float> cube_f(static_cast<float>(scale));
|
||
|
|
testLocalAABBComputation(cube_f);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, Volume_Cube) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
Cube<double> cube_d(scale);
|
||
|
|
testVolumeComputation(cube_d, 0);
|
||
|
|
Cube<float> cube_f(static_cast<float>(scale));
|
||
|
|
// Apparently, no bits of precision are lost (relative to machine precision)
|
||
|
|
// on the cube volume *except* for the *large* cube in single precision.
|
||
|
|
// The reason for this isn't obvious, but probably a coincidental artifact
|
||
|
|
// of the particular configuration.
|
||
|
|
const int bits_lost = scale > 1 ? 2 : 0;
|
||
|
|
testVolumeComputation(cube_f, bits_lost);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, CenterOfMass_Cube) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
Cube<double> cube_d(scale);
|
||
|
|
testCenterOfMassComputation(cube_d, 0);
|
||
|
|
Cube<float> cube_f(static_cast<float>(scale));
|
||
|
|
testCenterOfMassComputation(cube_f, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, MomentOfInertia_Cube) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
Cube<double> cube_d(scale);
|
||
|
|
testMomentOfInertiaComputation(cube_d, 0);
|
||
|
|
Cube<float> cube_f(static_cast<float>(scale));
|
||
|
|
testMomentOfInertiaComputation(cube_f, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, LocalAABBComputation_Tetrahedron) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
EquilateralTetrahedron<double> tet_d(scale);
|
||
|
|
testLocalAABBComputation(tet_d);
|
||
|
|
EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
|
||
|
|
testLocalAABBComputation(tet_f);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, Volume_Tetrahedron) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
EquilateralTetrahedron<double> tet_d(scale);
|
||
|
|
// Apparently, no bits of precision are lost (relative to machine precision)
|
||
|
|
// on the tet volume *except* for the *large* test in double precision.
|
||
|
|
// The reason for this isn't obvious, but probably a coincidental artifact
|
||
|
|
// of the particular configuration.
|
||
|
|
const int bits_lost = scale > 1 ? 1 : 0;
|
||
|
|
testVolumeComputation(tet_d, bits_lost);
|
||
|
|
EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
|
||
|
|
testVolumeComputation(tet_f, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
GTEST_TEST(ConvexGeometry, CenterOfMass_Tetrahedron) {
|
||
|
|
for (double scale : get_test_scales()) {
|
||
|
|
EquilateralTetrahedron<double> tet_d(scale);
|
||
|
|
testCenterOfMassComputation(tet_d, 0);
|
||
|
|
EquilateralTetrahedron<float> tet_f(static_cast<float>(scale));
|
||
|
|
testCenterOfMassComputation(tet_f, 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// TODO(SeanCurtis-TRI): Add Tetrahedron inertia unit test.
|
||
|
|
|
||
|
|
// TODO(SeanCurtis-TRI): Extend the moment of inertia test.
|
||
|
|
// Tesselate smooth geometries (sphere, ellipsoid, cone, etc) which have
|
||
|
|
// well-known closed-form values for the tensor product. Confirm that as
|
||
|
|
// the tesselation gets finer, that the answer converges to the reference
|
||
|
|
// solution.
|
||
|
|
|
||
|
|
} // namespace
|
||
|
|
} // namespace fcl
|
||
|
|
|
||
|
|
//==============================================================================
|
||
|
|
int main(int argc, char *argv[]) {
|
||
|
|
::testing::InitGoogleTest(&argc, argv);
|
||
|
|
return RUN_ALL_TESTS();
|
||
|
|
}
|
||
|
|
|