/**
* Raster the color triangle to the screen. Points are in
* NDC coordinates (converted to screen coordinates).
*
* colors correspond to the respective points.
*/
void raster_triangle(point *a, point *b, point *c, color ca,
color cb, color cc, int xres, int yres, color** grid,
float** depth_grid) {
point screen_a = NDC_to_screen(a, xres, yres);
point screen_b = NDC_to_screen(b, xres, yres);
point screen_c = NDC_to_screen(c, xres, yres);
float x_min = min_x(&screen_a, &screen_b, &screen_c);
float x_max = max_x(&screen_a, &screen_b, &screen_c);
float y_min = min_y(&screen_a, &screen_b, &screen_c);
float y_max = max_y(&screen_a, &screen_b, &screen_c);
x_max = x_max > xres ? xres : x_max;
y_max = y_max > yres ? yres : y_max;
for (int x = x_min; x < x_max; x++) {
for (int y = y_min; y < y_max; y++) {
point curr_point = create_point(x, y, 0);
float alpha = compute_alpha(&screen_a, &screen_b, &screen_c, &curr_point);
float beta = compute_beta(&screen_a, &screen_b, &screen_c, &curr_point);
float gamma = compute_gamma(&screen_a, &screen_b, &screen_c, &curr_point);
curr_point.z = alpha * screen_a.z + beta * screen_b.z + gamma * screen_c.z;
if (valid_parameters(alpha, beta, gamma) && depth_grid[x][y] > curr_point.z) {
color col = compute_color(ca, cb, cc, alpha, beta, gamma);
grid[x][y].r = col.r;
grid[x][y].g = col.g;
grid[x][y].b = col.b;
depth_grid[x][y] = curr_point.z;
}
}
}
}
/*!
* Return the named point of the Rect.
* \param c which corner of the rect.
*/
vecN<T, 2>
point(enum corner_t c) const
{
vecN<T, 2> return_value;
return_value.x() = (c & maxx_mask) ? max_x() : min_x();
return_value.y() = (c & maxy_mask) ? max_y() : min_y();
return return_value;
}
/**
* Also has depth buffering and ignores points where normals point away
*
* Points a, b, c are in NDC
*/
void raster_triangle_Phong(point *a, point *b, point *c,
point *world_a, point *world_b, point *world_c,
point *normal_a, point *normal_b, point *normal_c,
object_copy *obj, vector<light> *lights, camera *CAM,
int xres, int yres, color** grid, float** depth_grid,
MatrixXd *perspective, MatrixXd *inv_cam) {
point screen_a = NDC_to_screen(a, xres, yres);
point screen_b = NDC_to_screen(b, xres, yres);
point screen_c = NDC_to_screen(c, xres, yres);
float x_min = min_x(&screen_a, &screen_b, &screen_c);
float x_max = max_x(&screen_a, &screen_b, &screen_c);
float y_min = min_y(&screen_a, &screen_b, &screen_c);
float y_max = max_y(&screen_a, &screen_b, &screen_c);
x_max = x_max > xres ? xres : x_max;
y_max = y_max > yres ? yres : y_max;
x_min = x_min < 0 ? 0 : x_min;
y_min = y_min < 0 ? 0 : y_min;
// TODO: compute colors by normals
for (int x = x_min; x < x_max; x++) {
for (int y = y_min; y < y_max; y++) {
// get alpha/beta/gamma
point curr_point = create_point(x, y, 0);
float alpha = compute_alpha(&screen_a, &screen_b, &screen_c, &curr_point);
float beta = compute_beta(&screen_a, &screen_b, &screen_c, &curr_point);
float gamma = compute_gamma(&screen_a, &screen_b, &screen_c, &curr_point);
curr_point.z = alpha * screen_a.z + beta * screen_b.z + gamma * screen_c.z;
// compute interpolated point (in world view) and normal for the point
point normal = (*normal_a) * alpha + (*normal_b) * beta + (*normal_c) * gamma;
point coordinate = (*world_a) * alpha + (*world_b) * beta + (*world_c) * gamma;
point ndc_coordinate = to_NDC(&coordinate, perspective, inv_cam);
if (is_in_box(&ndc_coordinate) && valid_parameters(alpha, beta, gamma)
&& depth_grid[x][y] > curr_point.z) {
color col = lighting(&coordinate, &normal, obj, lights, CAM);
grid[x][y].r = col.r;
grid[x][y].g = col.g;
grid[x][y].b = col.b;
depth_grid[x][y] = curr_point.z;
}
}
}
}
void extract(std::vector<pcl::PointIndices>& cluster_list, //output
std::vector<char>& mask)
{
std::vector<char>* output = new std::vector<char>(mask.size(), 0);
ConnectedComponents cc(30);
int nLabels = cc.connected(mask.data(), output->data(), W, H,
std::equal_to<unsigned char>(), true);
cluster_list.resize(nLabels);
std::vector<float> min_x(nLabels, 100.0f), min_y(nLabels, 100.0f), min_z(nLabels, 100.0f);
std::vector<float> max_x(nLabels,-100.0f), max_y(nLabels,-100.0f), max_z(nLabels,-100.0f);
for(int i=0; i< mask.size(); ++i)
{
char tag = output->at(i);
cluster_list[tag].indices.push_back(i);
T& pt= cloud->points[i];
if(pt.x < min_x[tag]) min_x[tag] = pt.x;
if(pt.y < min_y[tag]) min_y[tag] = pt.y;
if(pt.z < min_z[tag]) min_z[tag] = pt.z;
if(pt.x > max_x[tag]) max_x[tag] = pt.x;
if(pt.y > max_y[tag]) max_y[tag] = pt.y;
if(pt.z > max_z[tag]) max_z[tag] = pt.z;
}
for(int i=0; i< nLabels; ++i)
{
int s = cluster_list[i].indices.size();
if( s < min_c || s >max_c)
{
cluster_list[i].indices.clear();
printf("Cluster[%d] rejected: size=%d\n", i, s);
}
else
{
float v = (max_x[i]-min_x[i])*(max_y[i]-min_y[i])*(max_z[i]-min_z[i]);
if (v < min_v || v> max_v)
{
cluster_list[i].indices.clear();
printf("Cluster[%d] rejected: volume=%f\n", i, v);
}
printf("Cluster[%d] Added: size=%d volume=%f\n", i, s, v);
}
}
delete output;
}
int findBestQuadrant(TH1D *ahisto) {
Int_t quadrant(0);
Float_t qarea(0.0);
Float_t qqarea[4];
Float_t max_x(0.0), min_x(0.0), tot_x(0.0);
Float_t sfactor(0.0);
Int_t max_bin(0), sep_bin(0), k;
Float_t value = 0;
Float_t maxvalue =0;
Float_t maximum = ahisto->GetMaximum();
max_x=(ahisto->GetXaxis())->GetXmax();
min_x=(ahisto->GetXaxis())->GetXmin();
tot_x= max_x - min_x;
qarea=(tot_x*maximum)/4;
max_bin=ahisto->GetNbinsX();
sep_bin=max_bin/4;
sfactor=tot_x/max_bin;
qqarea[0]=qarea-sfactor*(ahisto->Integral(1,sep_bin));
qqarea[1]=qarea-sfactor*(ahisto->Integral(sep_bin+1,2*sep_bin));
qqarea[2]=qarea-sfactor*(ahisto->Integral(2*sep_bin+1,3*sep_bin));
qqarea[3]=qarea-sfactor*(ahisto->Integral(3*sep_bin+1,max_bin));
//printf("%d Areas: %f %f %f %f \n",i, qqarea[0],qqarea[1],qqarea[2],qqarea[3]);
for(k=0; k<4 ; k++) {
value=qqarea[k];
if(value > maxvalue) {
maxvalue = value;
quadrant=k;
}
}
return quadrant;
}
void MapCamera::makeSureCoordinatesDoNotExceedMapLimits() {
if (y < min_y()) y = min_y();
if (y > max_y()) y = max_y();
if (x < min_x()) x = min_x();
if (x > max_x()) x = max_x();
}
