martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Compare commits

base: martin:384293a853a3ac3f3a10ed35026077c4f194daf6
Branches Tags
martin:master
..
compare: martin:b19a74dcdfde4df2d43d1dbd3ed2b35c066e6145
Branches Tags
martin:master

No commits in common. "384293a853a3ac3f3a10ed35026077c4f194daf6" and "b19a74dcdfde4df2d43d1dbd3ed2b35c066e6145" have entirely different histories.
384293a853 ... b19a74dcdf

6 changed files with 21 additions and 17776 deletions
Show Stats Expand all files Collapse all files

8
3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h vendored
View File

@ -819,19 +819,17 @@ struct Storage {
inline size_t cols() const { return NumCols; }
#ifdef NDEBUG
void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {}
#else
void resize(int num_rows, int num_cols) {
void resize(int UNUSED(num_rows), int UNUSED(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
View File

File diff suppressed because it is too large Load Diff

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

@ -1,114 +0,0 @@
#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

27
src/sconvcol.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, simd4x4f* trans, simd4f n, simd4f* out_vert);
void sch_hull_get_support(const sch_hull* hull, simd4f n, simd4f* out_vert);
float sch_query_face_directions(
const sch_hull* hull_A,
@ -489,17 +489,14 @@ 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, simd4x4f* trans, simd4f n, simd4f* out_vert) {
void sch_hull_get_support(const sch_hull* hull, simd4f normal, 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, n_local);
normal_dot_edge = simd4f_dot3_scalar(dir, normal);
if (normal_dot_edge <= 0.) {
edge = edge->twin->next;
@ -509,7 +506,7 @@ void sch_hull_get_support(const sch_hull* hull, simd4x4f* trans, simd4f n, simd4
}
} while (last_edge != edge || normal_dot_edge > 0);
simd4x4f_matrix_point3_mul(trans, &edge->vert->p, out_vert);
*out_vert = edge->vert->p;
}
float sch_query_face_directions(
@ -522,14 +519,18 @@ float sch_query_face_directions(
sch_plane plane;
sch_hull_calc_plane(hull_A, fi, &plane);
simd4f vert_B;
sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane.n), &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);
float distance = sch_plane_distance(&plane, &vert_B);
simd4f vert_A;
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B, &vert_A);
float distance = sch_plane_distance(&plane, &vert_A);
if (distance > result->dist) {
result->dist = distance;
result->face_idx = fi;
result->plane = plane;
result->vert = vert_B;
result->vert = vert_A;
}
}
@ -571,12 +572,10 @@ 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.
simd4x4f identity;
simd4x4f_identity(&identity);
sch_hull_get_support(hull_A, &identity, plane_A.n, &plane_A.p);
sch_hull_get_support(hull_A, plane_A.n, &plane_A.p);
simd4f vert_B_B;
sch_hull_get_support(hull_B, trans_BtoA, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
sch_hull_get_support(hull_B, simd4f_sub(simd4f_zero(), plane_A.n), &vert_B_B);
simd4f vert_B;
simd4x4f_matrix_point3_mul(trans_BtoA, &vert_B_B, &vert_B);

19
src/vissim.cc
View File

@ -624,22 +624,6 @@ 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();
@ -647,8 +631,7 @@ struct SconvHullScene {
sch_create_unitbox(&mHullB.hull);
mHullB.createMesh();
// setupSceneEdgeCollision();
setupRotatedBoxes();
setupSceneEdgeCollision();
mCurrentHull = &mHullA;
};

9
tests/sconvcolTests.cc
View File

@ -261,13 +261,10 @@ 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, &identity, normal, &support);
sch_hull_get_support(&hull, 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));
@ -277,7 +274,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, &identity, normal, &support);
sch_hull_get_support(&hull, 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));
@ -287,7 +284,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, &identity, normal, &support);
sch_hull_get_support(&hull, normal, &support);
THEN ("y component of support is 0.5") {
REQUIRE (simd4f_get_y(support) == 0.5);
}