bool experimental_findInitialGuess(const P3* points, int n, const P3** p0, const P3** p1, const P3** p2) {
bool found_in_quadrant[4] = {false, false, false, false};
const P3* quadrant_points[4] = {NULL, NULL, NULL, NULL};
int quadrants_found = 0;
for (int i=0; i<n && quadrants_found < 4; i++) {
int q = find_quadrant(points[i]);
if (q != -1 && !found_in_quadrant[q]) {
found_in_quadrant[q] = true;
quadrant_points[q] = &points[i];
quadrants_found++;
}
}
if (quadrants_found == 4) {
if (set_points_if_inDaZone(*quadrant_points[0], *quadrant_points[1], *quadrant_points[2], p0, p1, p2))
return true;
if (set_points_if_inDaZone(*quadrant_points[0], *quadrant_points[1], *quadrant_points[3], p0, p1, p2))
return true;
if (set_points_if_inDaZone(*quadrant_points[0], *quadrant_points[2], *quadrant_points[3], p0, p1, p2))
return true;
*p0 = quadrant_points[1];
*p1 = quadrant_points[2];
*p2 = quadrant_points[3];
return true;
}
return false;
}
void
mark_mapsumpoint( QUAD_TREE *tree, MAP2 *map, int x, int y )
{
tree->inuse=TRUE;
if ((tree->level)>0) {
mark_mapsumpoint( tree->elem[find_quadrant( tree->center, x, y )],
map, x, y );
} else {
map->mapsum[tree->center.x/2][tree->center.y/2]++;
}
}
void
mark_maphitpoint( QUAD_TREE *tree, MAP2 *map, int x, int y, float value )
{
tree->inuse=TRUE;
if ((tree->level)>0) {
mark_maphitpoint( tree->elem[find_quadrant( tree->center, x, y )],
map, x, y, value );
} else {
map->maphit[tree->center.x/2][tree->center.y/2] += value;
}
}
void
mark_mapsumpoint( carmen_vascocore_quad_tree_t *tree,
carmen_vascocore_map_t *map, int x, int y )
{
tree->inuse=TRUE;
if ((tree->level)>0) {
mark_mapsumpoint( tree->elem[find_quadrant( tree->center, x, y )],
map, x, y );
} else {
map->mapsum[tree->center.x/2][tree->center.y/2]++;
}
}
void
mark_maphitpoint( carmen_vascocore_quad_tree_t *tree,
carmen_vascocore_map_t *map, int x, int y, float value )
{
tree->inuse=TRUE;
if ((tree->level)>0) {
mark_maphitpoint( tree->elem[find_quadrant( tree->center, x, y )],
map, x, y, value );
} else {
map->maphit[tree->center.x/2][tree->center.y/2] += value;
}
}
