Compare commits

..

4 Commits

Author SHA1 Message Date
Martin Felis 384293a853 Added another sconv scene 2021-11-16 15:26:01 +01:00
Martin Felis b521474b10 Added missing rbdl test files 2021-11-16 15:01:52 +01:00
Martin Felis 71bed07ed1 sconvcol: querying for support vertex now needs transformation of the hull. 2021-11-16 14:53:46 +01:00
Martin Felis 33f08ff39b SimpleMath: fixed compile warnings when using debug builds 2021-11-16 14:18:15 +01:00
6 changed files with 17776 additions and 21 deletions

View File

@ -819,17 +819,19 @@ struct Storage {
inline size_t cols() const { return NumCols; }
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {
#ifdef NDEBUG
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {}
#else
void resize(int num_rows, int num_cols) {
// Resizing of fixed size matrices not allowed
#ifndef NDEBUG
if (num_rows != NumRows || num_cols != NumCols) {
std::cout << "Error: trying to resize fixed matrix from "
<< NumRows << ", " << NumCols << " to "
<< num_rows << ", " << num_cols << "." << std::endl;
}
#endif
assert (num_rows == NumRows && num_cols == NumCols);
}
#endif
inline ScalarType& coeff(int row_index, int col_index) {
// assert (row_index >= 0 && row_index <= NumRows);

17618
3rdparty/rbdl/tests/catch.hpp vendored Normal file

File diff suppressed because it is too large Load Diff

114
3rdparty/rbdl/tests/rbdl_tests.h vendored Normal file
View File

@ -0,0 +1,114 @@
#ifndef RBDL_RBDL_TESTS_H
#define RBDL_RBDL_TESTS_H
#include <cmath>
#include "catch.hpp"
#include "rbdl/rbdl_math.h"
template <typename T>
struct IsCloseMatcher : Catch::MatcherBase<T> {
IsCloseMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
using namespace Catch::Matchers::Floating;
if (std::abs(v - m_comparator)
> (m_atol + m_rtol * std::abs(m_comparator))) {
return false;
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
IsCloseMatcher<T>
IsClose(T const& comparator, double atol = 1.0e-8, double rtol = 1.0e-5) {
return IsCloseMatcher<T>(comparator, atol, rtol);
}
template <typename T>
struct AllCloseVectorMatcher : Catch::MatcherBase<T> {
AllCloseVectorMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
using namespace Catch::Matchers::Floating;
if (m_comparator.size() != v.size()) {
return false;
}
for (std::size_t i = 0; i < v.size(); ++i) {
if (!IsClose(m_comparator[i], m_atol, m_rtol).match(v[i])) {
return false;
}
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T const& m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
AllCloseVectorMatcher<T> AllCloseVector(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5) {
return AllCloseVectorMatcher<T>(comparator, atol, rtol);
}
template <typename T>
struct AllCloseMatrixMatcher : Catch::MatcherBase<T> {
AllCloseMatrixMatcher(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5)
: m_comparator(comparator), m_atol(atol), m_rtol(rtol) {}
bool match(T const& v) const override {
if ((m_comparator.rows() != v.rows()) && (m_comparator.cols() != v.cols()))
return false;
for (int i = 0; i < v.rows(); ++i) {
for (int j = 0; j < v.cols(); ++j) {
if (!IsClose(m_comparator(i, j), m_atol, m_rtol).match(v(i, j))) {
return false;
}
}
}
return true;
}
std::string describe() const override {
return "is approx: " + ::Catch::Detail::stringify(m_comparator);
}
T const& m_comparator;
double m_atol;
double m_rtol;
};
template <typename T>
AllCloseMatrixMatcher<T> AllCloseMatrix(
T const& comparator,
double atol = 1.0e-8,
double rtol = 1.0e-5) {
return AllCloseMatrixMatcher<T>(comparator, atol, rtol);
}
#endif // RBDL_RBDL_TESTS_H

View File

@ -168,7 +168,7 @@ void sch_hull_calc_plane(const sch_hull* hull, const int index, sch_plane* out_p
sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4f v1);
void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert);
float sch_query_face_directions(
const sch_hull* hull_A,
@ -489,14 +489,17 @@ sch_edge* sch_hull_find_edge (const sch_hull* hull, const simd4f v0, const simd4
return NULL;
}
void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert) {
void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4f* out_vert) {
sch_edge* edge = hull->faces[0].edge;
sch_edge* last_edge = edge;
float normal_dot_edge = -1.;
simd4f n_local;
simd4x4f_inv_ortho_matrix_vector3_mul(trans, &n, &n_local);
do {
simd4f dir = simd4f_normalize3(simd4f_sub (edge->next->vert->p, edge->vert->p));
normal_dot_edge = simd4f_dot3_scalar(dir, normal);
normal_dot_edge = simd4f_dot3_scalar(dir, n_local);
if (normal_dot_edge <= 0.) {
edge = edge->twin->next;
@ -506,7 +509,7 @@ void sch_hull_get_support(const sch_hull* hull, simd4f normal, simd4f* out_vert)
}
} while (last_edge != edge || normal_dot_edge > 0);
*out_vert = edge->vert->p;
simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
}
float sch_query_face_directions(
@ -519,18 +522,14 @@ float sch_query_face_directions(
sch_plane plane;
sch_hull_calc_plane(hull_A, fi, &plane);
simd4f vert_B;
simd4f plane_n_B;
simd4x4f_inv_ortho_matrix_vector3_mul(trans_BtoA, &plane.n, &plane_n_B);
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_n_B), &vert_B);
sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &vert_B);
simd4f vert_A;
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
float distance = sch_plane_distance(&plane, &vert_A);
float distance = sch_plane_distance(&plane, &vert_B);
if (distance > result->dist) {
result->dist = distance;
result->face_idx = fi;
result->plane = plane;
result->vert = vert_A;
result->vert = vert_B;
}
}
@ -572,10 +571,12 @@ float sch_query_edge_directions(
// Note: in Gregorius talk he uses the origin of edge_A. This is wrong as
// there are likely points in hull_A that are further out along the plane
// normal. We therefore use the support point of hull_A along the axis.
sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
simd4x4f identity;
simd4x4f_identity(&identity);
sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p);
simd4f vert_B_B;
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
simd4f vert_B;
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);

View File

@ -624,6 +624,22 @@ struct SconvHullScene {
simd4x4f_matrix_mul(&translation, &rot, &mHullB.transform);
}
void setupRotatedBoxes () {
simd4x4f trans_A;
simd4x4f trans_B;
simd4x4f_axis_rotation (&trans_A, M_PI / 180.0f * 45.f, simd4f_create(0.f, 0.f, 1.f, 1.f));
simd4x4f rot_B;
simd4x4f_axis_rotation (&rot_B, M_PI / 180.0f * 45.f, simd4f_create(0.f, 1.f, 0.f, 1.f));
simd4x4f translation_B;
simd4x4f_translation (&translation_B, sqrt(2.f) + 0.001, 0.f, 0.f);
simd4x4f_matrix_mul (&translation_B, &rot_B, &trans_B);
mHullA.transform = trans_A;
mHullB.transform = trans_B;
}
void init() {
sch_create_unitbox(&mHullA.hull);
mHullA.createMesh();
@ -631,7 +647,8 @@ struct SconvHullScene {
sch_create_unitbox(&mHullB.hull);
mHullB.createMesh();
setupSceneEdgeCollision();
// setupSceneEdgeCollision();
setupRotatedBoxes();
mCurrentHull = &mHullA;
};

View File

@ -261,10 +261,13 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
sch_hull hull;
sch_create_unitbox(&hull);
simd4x4f identity;
simd4x4f_identity(&identity);
WHEN("Querying support for 2, 2, 2") {
simd4f support;
simd4f normal = simd4f_create(2.f, 2.f, 2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("support is 0.5, 0.5, 0.5") {
simd4f reference = simd4f_create (0.5f, 0.5f, 0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
@ -274,7 +277,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
WHEN("Querying support for -2, -2, -2") {
simd4f support;
simd4f normal = simd4f_create(-2.f, -2.f, -2.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("support is -0.5, -0.5, -0.5") {
simd4f reference = simd4f_create (-0.5f, -0.5f, -0.5f, 1.f);
REQUIRE (sch_simd4f_equal(reference, support));
@ -284,7 +287,7 @@ TEST_CASE("UnitCubeSupport", "[sconvcol]") {
WHEN("Querying support for 0, 1, 0") {
simd4f support;
simd4f normal = simd4f_create(0.f, 1.f, 0.f, 1.f);
sch_hull_get_support(&hull, normal, &support);
sch_hull_get_support(&hull, &identity, normal, &support);
THEN ("y component of support is 0.5") {
REQUIRE (simd4f_get_y(support) == 0.5);
}
@ -639,4 +642,4 @@ TEST_CASE ("ClipFaces", "[sconvcol]") {
sch_manifold_free_memory(&manifold);
}
}
}
}