void Map::generate(const unsigned int city_count,
const unsigned int width,
const unsigned int height,
const unsigned int seed)
{
cities.resize(city_count);
std::default_random_engine generator(seed);
std::uniform_int_distribution<unsigned int> width_distribution(0, width);
std::uniform_int_distribution<unsigned int> height_distribution(0, height);
std::uniform_int_distribution<unsigned int> city_distribution(0, city_count - 1);
for (size_t c = 0; c < city_count; c++)
{
cities[c].x = width_distribution(generator);
cities[c].y = height_distribution(generator);
}
start = city_distribution(generator);
end = city_distribution(generator);
m_adaptor = MapAdaptor(cities);
city_tree_t city_tree(2, m_adaptor,
nanoflann::KDTreeSingleIndexAdaptorParams(10));
city_tree.buildIndex();
for (size_t c = 0; c < cities.size(); c++)
{
// Set index of city and generate connections
cities[c].index = c; // Do we need this?
// Consider 5 closest
std::vector<size_t> ret_index(CITY_SEARCH + 1);
std::vector<unsigned int>dist_sqr(CITY_SEARCH + 1);
city_tree.knnSearch(
&cities[c].x,
CITY_SEARCH + 1,
&ret_index[0],
&dist_sqr[0]);
for (size_t i = 1; i < ret_index.size(); i++)
{
// Do we include you or not?
// Bernoulli, do your job buddy!
std::bernoulli_distribution city_test_distribution(CITY_RATIO);
if (city_test_distribution(generator))
{
// Update our information
cities[c].neighbors.insert(
std::make_pair(ret_index[i],
std::sqrt(dist_sqr[i])));
cities[ret_index[i]].neighbors.insert(
std::make_pair(c,
std::sqrt(dist_sqr[i])));
}
}
}
}
void CFruchtermanReingold::GenerateRandomCoordinates() {
std::default_random_engine generator;
std::uniform_int_distribution<int> height_distribution(vgc_nodeRadius, vgc_areaHeight - vgc_nodeRadius);
std::uniform_int_distribution<int> width_distribution(vgc_nodeRadius, vgc_areaWidth - vgc_nodeRadius);
for(int i =0; i < vgc_nodes_num; ++i) {
vgc_vertices[i].v_coordinates.setX(width_distribution(generator));
vgc_vertices[i].v_coordinates.setY(height_distribution(generator));
}
}
/* Main function */
int main(int argc, char** argv)
{
#if THREE_D
FILE* fp;
fp = fopen(FILENAME, "w");
if (fp == NULL) {
return -1;
}
/* Print header */
fprintf(fp, "%d %d %d\n", WIDTH, HEIGHT, DEPTH);
/* Print image */
for (int d = 0; d < DEPTH; d++) {
for (int i = 0; i < HEIGHT; i++) {
for (int j = 0; j < WIDTH; j++) {
float freq1 = 1.0 / 32.0;
float freq2 = 1.0 / 8.0;
float freq3 = 1.0 / 1.0;
float x1 = (float)j * freq1;
float y1 = (float)i * freq1;
float z1 = (float)d * freq1;
float x2 = (float)j * freq2;
float y2 = (float)i * freq2;
float z2 = (float)d * freq2;
float x3 = (float)j * freq3;
float y3 = (float)i * freq3;
float z3 = (float)d * freq3;
int g = (int)((fbm_perlin(x1, y1, z1, 5, (int)(128 * freq1)) * 0.5 + 0.5) * 255) * height_distribution((float)d / DEPTH, (float)i / HEIGHT, (float)j / WIDTH);
int r = (int)((fbm_worley(x1, y1, z1, 5, (int)(128 * freq1)) * 0.5 + 0.5) * 255) * height_distribution((float)d / DEPTH, (float)i / HEIGHT, (float)j / WIDTH);;
int b = (int)((fbm_worley(x2, y2, z2, 5, (int)(128 * freq2)) * 0.5 + 0.5) * 255);
int a = (int)((fbm_worley(x3, y3, z3, 5, (int)(128 * freq3)) * 0.5 + 0.5) * 255);
uint32_t pixel = 0;
pixel |= r << 24;
pixel |= g << 16;
pixel |= b << 8;
pixel |= a << 0;
fprintf(fp, "%u ", pixel);
}
fprintf(fp, "\n");
}
printf("\b\b\b\b\b\b%3.1f %%", 100 * (float)d / (float)DEPTH);
}
#else
FILE* fp;
fp = fopen(FILENAME, "w");
if (fp == NULL) {
return -1;
}
/* Print header */
fprintf(fp, "%d %d\n", WIDTH, HEIGHT);
/* Print image */
for (int i = 0; i < HEIGHT; i++) {
for (int j = 0; j < WIDTH; j++) {
int noise1 = (int)((fbm_value(i, j, 2, 0.5, 2.0, 1.0, 0.5) * 0.5 + 1.0) * 255);
int noise2 = (int)((fbm_value(i, j, 3, 10.0, 2.0, 1.0, 0.5) * 0.5 + 1.0) * 255);
int noise3 = (int)((fbm_value(i, j, 6, 50.0, 2.0, 1.0, 0.5) * 0.5 + 1.0) * 255);
int noise4 = 0;
uint32_t pixel = 0;
pixel |= noise1 << 24;
pixel |= noise2 << 16;
pixel |= noise3 << 8;
pixel |= noise4 << 0;
fprintf(fp, "%u ", pixel);
}
fprintf(fp, "\n");
}
#endif
return 0;
}
int main() {
// Height of the capillary interface for the grid
CpuPtr_2D height_distribution(nx, ny, 0, true);
computeRandomHeights(0, H, height_distribution);
// Density difference between brine and CO2
float delta_rho = 500;
// Gravitational acceleration
float g = 9.87;
// Non-dimensional constant that scales the strength of the capillary forces
float c_cap = 1.0/6.0;
// Permeability data (In real simulations this will be a table based on rock data, here we use a random distribution )
float k_data[10] = {0.9352, 1.0444, 0.9947, 0.9305, 0.9682, 1.0215, 0.9383, 1.0477, 0.9486, 1.0835};
float k_heights[10] = {10, 20, 25, 100, 155, 193, 205, 245, 267, 300};
//Inside Kernel
// Converting the permeability data into a table of even subintervals in the z-directions
//float k_values[n+1];
//kDistribution(dz, h, k_heights, k_data, k_values);
// MOBILITY
// The mobility is a function of the saturation, which is directly related to the capillary pressure
// Pressure at capillary interface, which is known
float p_ci = 1;
// Table of capillary pressure values for our subintervals along the z-axis ranging from 0 to h
float resolution = 0.01;
int size = 1/resolution + 1;
float p_cap_ref_table[size];
float s_b_ref_table[size];
createReferenceTable(g, H, delta_rho, c_cap, resolution, p_cap_ref_table, s_b_ref_table);
// Set block and grid sizes and initialize gpu pointer
dim3 grid;
dim3 block;
computeGridBlock(dim3& grid, dim3& block, nx, ny, block_x, block_y);
// Allocate and set data on the GPU
GpuPtr_2D Lambda_device(nx, ny, 0, NULL);
GpuPtr_1D k_data_device(10, k_data);
GpuPtr_1D k_heights_device(10, k_heights);
GpuPtr_1D p_cap_ref_table_device(size, p_cap_ref_table);
GpuPtr_1D s_b_ref_table_device(size, s_b_ref_table);
cudaHostAlloc(&args, sizeof(CoarsePermIntegrationKernelArgs), cudaHostAllocWriteCombined);
// Set arguments and run coarse integration kernel
CoarsePermIntegrationArgs coarse_perm_int_args;
setCoarsePermIntegrationArgs(coarse_perm_int_args,\
Lambda_device.getRawPtr(),\
k_data_device.getRawPtr(),\
k_heights_device.getRawPtr(),\
p_cap_ref_table_device.getRawPtr(),\
s_b_ref_table_device.getRawPtr(),\
nx, ny, 0);
callCoarseIntegrationKernel(grid, block, coarse_perm_int_args);
float p_cap_values[n+1];
computeCapillaryPressure(p_ci, g, delta_rho, h, dz, n, p_cap_values);
float s_b_values[n+1];
inverseCapillaryPressure(n, g, h, delta_rho, c_cap, p_cap_values, s_b_values);
printArray(n+1, s_b_values);
// End point mobility lambda'_b, a known quantity
float lambda_end_point = 1;
float lambda_values[n+1];
computeMobility(n, s_b_values, lambda_end_point, lambda_values);
// Multiply permeability values with lambda values
float f_values[n+1];
multiply(n+1, lambda_values, k_values, f_values);
//Numerical integral with trapezoidal
float K = trapezoidal(dz, n, k_values);
float L = trapezoidal(dz, n, f_values)/K;
printf("Value of integral K. %.4f", K);
printf("Value of integral L. %.4f", L);
}
void ObstaclePointCloud::broadcast() {
if (q_obstacles_.size() < 1) {
return;
}
const auto sim_obstacles = q_obstacles_.front();
using Cell = std::pair<float, float>;
std::vector<Cell> all_cells;
for (const auto& line : sim_obstacles->obstacles) {
const auto cells = pedsim::LineObstacleToCells(line.start.x, line.start.y,
line.end.x, line.end.y);
std::copy(cells.begin(), cells.end(), std::back_inserter(all_cells));
}
constexpr int point_density = 100;
const int num_points = all_cells.size() * point_density;
std::default_random_engine generator;
// \todo - Read params from config file.
std::uniform_int_distribution<int> color_distribution(1, 255);
std::uniform_real_distribution<float> height_distribution(0, 1);
std::uniform_real_distribution<float> width_distribution(-0.5, 0.5);
sensor_msgs::PointCloud pcd_global;
pcd_global.header.stamp = ros::Time::now();
pcd_global.header.frame_id = sim_obstacles->header.frame_id;
pcd_global.points.resize(num_points);
pcd_global.channels.resize(1);
pcd_global.channels[0].name = "intensities";
pcd_global.channels[0].values.resize(num_points);
sensor_msgs::PointCloud pcd_local;
pcd_local.header.stamp = ros::Time::now();
pcd_local.header.frame_id = robot_odom_.header.frame_id;
pcd_local.points.resize(num_points);
pcd_local.channels.resize(1);
pcd_local.channels[0].name = "intensities";
pcd_local.channels[0].values.resize(num_points);
// prepare the transform to robot odom frame.
tf::StampedTransform robot_transform;
try {
transform_listener_->lookupTransform(robot_odom_.header.frame_id,
sim_obstacles->header.frame_id,
ros::Time(0), robot_transform);
} catch (tf::TransformException& e) {
ROS_WARN_STREAM_THROTTLE(5.0, "TFP lookup from ["
<< sim_obstacles->header.frame_id
<< "] to [" << robot_odom_.header.frame_id
<< "] failed. Reason: " << e.what());
return;
}
size_t index = 0;
for (const auto& cell : all_cells) {
const int cell_color = color_distribution(generator);
for (size_t j = 0; j < point_density; ++j) {
if (fov_->inside(cell.first, cell.second)) {
const tf::Vector3 point(cell.first + width_distribution(generator),
cell.second + width_distribution(generator),
0.);
const auto transformed_point = transformPoint(robot_transform, point);
pcd_local.points[index].x = transformed_point.getOrigin().x();
pcd_local.points[index].y = transformed_point.getOrigin().y();
pcd_local.points[index].z = height_distribution(generator);
pcd_local.channels[0].values[index] = cell_color;
// Global observations.
pcd_global.points[index].x = cell.first + width_distribution(generator);
pcd_global.points[index].y =
cell.second + width_distribution(generator);
pcd_global.points[index].z = height_distribution(generator);
pcd_global.channels[0].values[index] = cell_color;
}
index++;
}
}
if (pcd_local.channels[0].values.size() > 1) {
pub_signals_local_.publish(pcd_local);
}
if (pcd_global.channels[0].values.size() > 1) {
pub_signals_global_.publish(pcd_global);
}
q_obstacles_.pop();
};
