void initializeWithRandom(std::mt19937* randomGenerator) {
std::uniform_real_distribution<float> randomDistribution(-1.0f, +1.0f);
for (auto& row : data) {
for (auto& value : row) {
value = randomDistribution(*randomGenerator);
}
}
}
inline static void initRandom()
{
/*
발생되는 랜덤의 분포는 균일분포를 따르게 되는데
이 분포를 초기화함
*/
randomDistribution() = std::uniform_real_distribution<double>(0.0, 1.0);
};
int
main(int argc, char** argv)
{
std::cout.precision(15);
std::cerr.precision(15);
if (argc < 4)
{
std::cout << "Usage: rlCollisionTest SCENEFILE KINEMATICSFILE JOINT1 ... JOINTn (in degrees)" << std::endl;
return EXIT_FAILURE;
}
rl::mdl::XmlFactory modelFactory;
std::shared_ptr<rl::mdl::Kinematic> kinematics = std::dynamic_pointer_cast<rl::mdl::Kinematic>(modelFactory.create(argv[2]));
std::size_t dof = kinematics->getDof();
rl::math::Vector q(kinematics->getDof());
for (std::size_t i = 0; i < kinematics->getDof(); ++i)
{
q(i) = boost::lexical_cast<rl::math::Real>(argv[i + 3]) * rl::math::DEG2RAD;
}
std::cout << "Set joint angle [rad] " << q.transpose() << std::endl;
kinematics->setPosition(q);
kinematics->forwardPosition();
std::vector<rl::sg::SimpleScene*> scenes;
std::vector<std::string> sceneNames;
#ifdef RL_SG_BULLET
scenes.push_back(new rl::sg::bullet::Scene);
sceneNames.push_back("bullet");
#endif // RL_SG_BULLET
#ifdef RL_SG_FCL
scenes.push_back(new rl::sg::fcl::Scene);
sceneNames.push_back("fcl");
#endif // RL_SG_FCL
#ifdef RL_SG_ODE
scenes.push_back(new rl::sg::ode::Scene);
sceneNames.push_back("ode");
#endif // RL_SG_ODE
#ifdef RL_SG_PQP
scenes.push_back(new rl::sg::pqp::Scene);
sceneNames.push_back("pqp");
#endif // RL_SG_PQP
#ifdef RL_SG_SOLID
scenes.push_back(new rl::sg::solid::Scene);
sceneNames.push_back("solid");
#endif // RL_SG_SOLID
rl::sg::XmlFactory sceneFactory;
for (std::size_t i = 0; i < scenes.size(); ++i)
{
std::cout << "Loading " << sceneNames[i] << " scene" << std::endl;
sceneFactory.load(argv[1], scenes[i]);
assert(scenes[i]->getModel(0)->getNumBodies() == kinematics->getBodies());
for (std::size_t b = 0; b < kinematics->getBodies(); ++b)
{
scenes[i]->getModel(0)->getBody(b)->setFrame(kinematics->getBodyFrame(b));
}
}
std::cout << "Loading done." << std::endl;
for (std::size_t i = 0; i < scenes.size(); ++i)
{
std::cout << "Testing SimpleScene::isColliding() in " << sceneNames[i] << ": ";
std::cout << (collides(scenes[i], kinematics.get()) ? "true" : "false") << std::endl;
}
std::mt19937 randomGenerator(0);
std::uniform_real_distribution<rl::math::Real> randomDistribution(-180 * rl::math::DEG2RAD, 180 * rl::math::DEG2RAD);
std::size_t j;
for (j = 0; j < 10; ++j)
{
rl::math::Vector q(dof);
for (std::size_t i = 0; i < dof; ++i)
{
q(i) = randomDistribution(randomGenerator);
}
kinematics->setPosition(q);
kinematics->forwardPosition();
rl::math::Vector results(scenes.size());
for (std::size_t i = 0; i < scenes.size(); ++i)
{
for (std::size_t b = 0; b < kinematics->getBodies(); ++b)
{
scenes[i]->getModel(0)->getBody(b)->setFrame(kinematics->getBodyFrame(b));
}
results[i] = collides(scenes[i], kinematics.get());
}
if ((results.array() == false).any() && (results.array() == true).any())
{
std::cerr << "Error: Counterexample " << j << ": SimpleScene::isColliding() Joint angle [rad] " << q.transpose();
std::cerr << " [deg] " << q.transpose() * rl::math::RAD2DEG << " ";
for (std::size_t i = 0; i < scenes.size(); ++i)
{
std::cerr << " " << sceneNames[i] << "=" << results[i];
}
std::cerr << std::endl;
return EXIT_FAILURE;
}
}
std::cout << "Tested " << j << " random poses, no differences between the collision libraries." << std::endl;
return EXIT_SUCCESS;
}
int main(int argc, char **argv) {
#ifdef USE_MPI
MPI_Init(&argc, &argv);
#endif
if (argc != 5) {
LOG_ERROR("I want width, height, zombies, iterations.\n");
#ifdef USE_MPI
MPI_Finalize();
#endif
exit(1);
}
int width = atoi(argv[1]);
int height = atoi(argv[2]);
int people = (int) (width * height * INITIAL_DENSITY);
int zombies = atoi(argv[3]);
int iters = atoi(argv[4]);
initRandom(0);
WorldPtr input, output;
double ratio = divideWorld(&width, &height, &input, &output);
// there should not be any output prior to this point
#ifdef REDIRECT
initRedirectToFiles(input);
#endif
LOG_DEBUG("World size is %d x %d at position [%d, %d] of %d x %d\n",
input->localWidth, input->localHeight, input->globalX,
input->globalY, input->globalColumns, input->globalRows);
if (input->globalX == 0 && input->globalY == 0) {
randomDistribution(input, people * ratio, zombies, 0);
} else {
// no zombies elsewhere
randomDistribution(input, people * ratio, 0, 0);
}
#ifndef NIMAGES
printWorld(input, false);
#endif
Timer timer = startTimer();
Stats cumulative = NO_STATS;
for (int i = 0; i < iters; i++) {
simulateStep(input, output);
output->stats.clock = cumulative.clock = output->clock;
Stats stats = output->stats;
mergeStats(&cumulative, stats, false);
printStatistics(output, cumulative);
WorldPtr temp = input;
input = output;
output = temp;
input->stats = stats;
}
double elapsedTime = getElapsedTime(timer);
#ifdef _OPENMP
int numThreads = omp_get_max_threads();
#else
int numThreads = 1;
#endif
LOG_TIME("Simulation took %f milliseconds with %d threads\n", elapsedTime,
numThreads);
// this is a clean up
// we destroy both worlds
destroyWorld(input);
destroyWorld(output);
destroyRandom();
#ifdef REDIRECT
finishRedirectToFiles();
#endif
#ifdef USE_MPI
MPI_Finalize();
#endif
}
void RandomDistributionDialog::ok() {
// set qvector params
switch (distribution) {
case Gaussian: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case Exponential: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Laplace: {
ui_->parameter1DbleSpinBox->show();
ui_->parameter1Label->show();
ui_->parameter2DbleSpinBox->show();
ui_->parameter2Label->show();
ui_->parameter3DbleSpinBox->hide();
ui_->parameter3Label->hide();
ui_->parameter1Label->setText(QString::fromUtf8("μ = "));
ui_->parameter2Label->setText("a = ");
ui_->parameter1DbleSpinBox->setValue(0.0);
ui_->parameter2DbleSpinBox->setValue(1.0);
parameters.clear();
parameters << 0.0 << 1.0 << nan("null");
} break;
case ExponentialPower: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value()
<< ui_->parameter3DbleSpinBox->value();
} break;
case Cauchy: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Rayleigh: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case RayleighTail: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case Landau: {
parameters.clear();
parameters << nan("null") << nan("null") << nan("null");
} break;
case LevyAlphaStable: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case LevySkewAlphaStable: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value()
<< ui_->parameter3DbleSpinBox->value();
} break;
case Flat: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case Gamma:
case Beta:
case Pareto:
case Weibull:
case Gumbel1:
case Gumbel2: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case Lognormal: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case ChiSquared: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case F: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case t: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Logistic: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Poisson: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Bernoulli:
case Geometric:
case Logarithmic: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value() << nan("null")
<< nan("null");
} break;
case Binomial:
case NegativeBinomial:
case Pascal: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value() << nan("null");
} break;
case Hypergeometric: {
parameters.clear();
parameters << ui_->parameter1DbleSpinBox->value()
<< ui_->parameter2DbleSpinBox->value()
<< ui_->parameter3DbleSpinBox->value();
} break;
}
emit randomDistribution(distribution, parameters);
// Save settings
QSettings settings;
settings.beginGroup("General");
settings.setValue("RandomDistribution", ui_->distComboBox->currentIndex());
settings.setValue("RandomDistributionParam-1", parameters.at(0));
settings.setValue("RandomDistributionParam-2", parameters.at(1));
settings.setValue("RandomDistributionParam-3", parameters.at(2));
settings.endGroup();
QDialog::accept();
}
inline static double random()
{
return randomDistribution()(randomGenerator());
};
