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.
127 lines
4.2 KiB
127 lines
4.2 KiB
#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
|
|
|