You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

128 lines
4.2 KiB

2 years ago
#include "collision_generator.hpp"
#include <iomanip>
#include <stdlib.h> /* rand */
namespace ipc::rigid {
namespace unittests {
std::ostream& operator<<(std::ostream& o, const TestImpact& p)
{
// clang-format off
return o << std::setprecision(10)
<< " Vi << " << p.Vi(0) << ", " << p.Vi(1) << ";\n"
<< " Vj << " << p.Vj(0) << ", " << p.Vj(1) << ";\n"
<< " Vk << " << p.Vk(0) << ", " << p.Vk(1) << ";\n"
<< " Ui << " << p.Ui(0) << ", " << p.Ui(1) << ";\n"
<< " Uj << " << p.Uj(0) << ", " << p.Uj(1) << ";\n"
<< " Uk << " << p.Uj(0) << ", " << p.Uk(1) << ";\n"
<< " toi = " << p.toi << ";\n"
<< " alpha = " << p.alpha << ";\n"
<< std::endl;
// clang-format on
}
TestImpactGenerator::TestImpactGenerator(size_t value, bool rigid)
: rigid(rigid)
, current_i(0)
, max_i(value)
{
current = generate_random_impact(rigid);
}
bool TestImpactGenerator::next()
{
current = generate_random_impact(rigid);
return ++current_i < max_i;
}
TestImpact const& TestImpactGenerator::get() const { return current; }
Catch::Generators::GeneratorWrapper<TestImpact>
random_impacts(size_t value, bool rigid)
{
return Catch::Generators::GeneratorWrapper<TestImpact>(
std::make_unique<TestImpactGenerator>(value, rigid));
}
TestImpact generate_random_impact(const bool rigid)
{
static const double kEPSILON = 1E-8;
static const double kSCALE = 10.0;
TestImpact impact;
// set first edge random positions keeping in larger than kEPSILON
impact.Vi.setRandom(2);
impact.Vj.setRandom(2);
impact.Vi *= kSCALE;
impact.Vk *= kSCALE;
double len_ij = (impact.Vj - impact.Vi).norm();
if (len_ij < kEPSILON) {
impact.Vj += (impact.Vj - impact.Vi) / len_ij * kEPSILON;
assert((impact.Vj - impact.Vi).norm() >= kEPSILON);
}
// set first edge random i.Velocities keeping in larger than kEPSILON
// note that this doest NOT ensure its length is > 0 oi.Ver the full
// time.
impact.Ui.setRandom(2);
impact.Ui *= kSCALE;
if (rigid) {
impact.Uj = impact.Ui;
} else {
impact.Uj.setRandom();
len_ij = (impact.Vj + impact.Uj - impact.Vi - impact.Ui).norm();
if (len_ij < kEPSILON) {
impact.Uj += (impact.Vj + impact.Uj - impact.Vi - impact.Ui)
/ len_ij * kEPSILON;
assert(
(impact.Vj + impact.Uj - impact.Vi - impact.Ui).norm()
>= kEPSILON);
}
}
// Generate a time and position for the impact.
// How do we ensure it is the first time of impact?
double toi = double(rand()) / RAND_MAX;
double alpha = double(rand()) / RAND_MAX;
Eigen::Vector2d Ve, Vi_toi, Vj_toi;
Vi_toi = impact.Vi + impact.Ui * toi;
Vj_toi = impact.Vj + impact.Uj * toi;
Ve = Vi_toi + alpha * (Vj_toi - Vi_toi);
// Ve = Vk + Uk * toi --> (Ve - Vk)/toi = Uk
impact.Vk.setRandom(2);
impact.Vk *= kSCALE;
impact.Uk = (Ve - impact.Vk) / toi;
impact.Vl.setRandom(2);
impact.Vl *= kSCALE;
double len_kl = (impact.Vl - impact.Vk).norm();
if (len_kl < kEPSILON) {
impact.Vl += (impact.Vl - impact.Vk) / len_kl * kEPSILON;
assert((impact.Vl - impact.Vk).norm() >= kEPSILON);
}
if (rigid) {
impact.Ul = impact.Uk;
} else {
impact.Ul.setRandom();
len_kl = (impact.Vl + impact.Ul - impact.Vk - impact.Uk).norm();
if (len_kl < kEPSILON) {
impact.Ul += (impact.Vl + impact.Ul - impact.Vk - impact.Uk)
/ len_kl * kEPSILON;
assert(
(impact.Vl + impact.Ul - impact.Vk - impact.Uk).norm()
>= kEPSILON);
}
}
impact.toi = toi;
impact.alpha = alpha;
return impact;
}
} // namespace unittests
} // namespace ipc::rigid