void World::randomize(uint32_t width, uint32_t height, uint32_t nbObstacles, uint32_t seed)
{
if(seed == 0)
{
seed = static_cast<uint32_t>(std::time(0));
}
std::mt19937 rng(seed);
std::uniform_int_distribution<uint32_t> heightDistribution(0, height);
std::uniform_int_distribution<uint32_t> widthDistribution(0, width);
std::uniform_int_distribution<uint32_t> angleDistribution(0, 359);
std::normal_distribution<float32> sizeDistribution(10.0, 5.0);
for(uint32_t i = 0; i < nbObstacles; ++i)
{
std::shared_ptr<StaticBox> box = std::make_shared<StaticBox>(
b2Vec2(widthDistribution(rng), heightDistribution(rng)),
angleDistribution(rng),
std::abs(sizeDistribution(rng)),
std::abs(sizeDistribution(rng))
);
addDrawable(box);
}
}
std::vector<sv::Real3> generatePoints(size_t count) {
std::vector<sv::Real3> points;
std::random_device rd;
std::mt19937 rng(rd());
std::uniform_real_distribution<> heightDistribution(-1, 1);
std::uniform_real_distribution<> angleDistribution(0, 2 * M_PI);
for (size_t i = 0; i < count; i++) {
double z = heightDistribution(rng);
double phi = angleDistribution(rng);
double theta = asin(z);
double x = cos(theta) * cos(phi);
double y = cos(theta) * sin(phi);
points.push_back(sv::Real3(x, y, z));
}
return points;
}
Ped::Tvector ShoppingPlanner::createRandomOffset() const
{
const double radiusStd = 4;
std::normal_distribution<double> radiusDistribution ( 0, radiusStd );
double radius = radiusDistribution ( RNG() );
std::discrete_distribution<int> angleDistribution {0,45,90,135,180,225,270,315,360};
double angle = angleDistribution ( RNG() );
Ped::Tvector randomOffset = Ped::Tvector::fromPolar ( Ped::Tangle::fromDegree ( angle ), radius );
// only update for significant shopping idea change
if (randomOffset.lengthSquared() < 2.0)
return Ped::Tvector(0, 0, 0);
return randomOffset;
}
bool RealLidar2D::measure(const nanosecond& time, const Point2D& pose, Measure &valueSensed, Measure &valueIdeal) noexcept {
if (time - timeLastMeasure_ < samplingTime_)
return false;
auto applyRes = [](double const &value, double const &resolution) -> double {
if (resolution == .0) return value;
return resolution*int(value/resolution);
};
auto rangeLimit = [&](double const &value) -> double {
if (value < rangeMin_) return rangeMin_;
if (rangeMax_ < value) return rangeMax_;
return value;
};
auto angleLimit = [&](double const &value) -> double {
return value<.0? 2.*pi+value : std::fmod(value,2.*pi) ;
};
double const rangeNoise{rangeDistribution(generator)};
double const angleNoise{angleDistribution(generator)};
valueIdeal.m[1] = angleLimit(ns2s(time)*scanSpeed_);
if (scanAngle_ == .0 || valueIdeal.m[1] <= scanAngle_ || 2.*pi-scanAngle_ <= valueIdeal.m[1]) {
Lidar2D const *lidar = this;
valueIdeal.m[0] = lidar->measure(valueIdeal.m[1],pose);
valueSensed.m[0] = applyRes(rangeLimit(valueIdeal.m[0] + rangeNoise),rangeResolution_);
valueSensed.m[1] = applyRes(angleLimit(valueIdeal.m[1] + angleNoise),angleResolution_);
return true;
}
valueIdeal.m[0] = .0;
valueSensed.m[0] = applyRes(rangeLimit(rangeNoise),rangeResolution_);
valueSensed.m[1] = applyRes(angleLimit(valueIdeal.m[1] + angleNoise),angleResolution_);
return true;
}
