#include "StdAfx.h"
#include "MA_Voxel_Coding.h"
#include "MA_vc_Flags.h"
// ----------------------------------------------------------------------------------------
//
// Description -
// ----------------------------------------------------------------------------------------
void MA_VoxelCoding::traceShortestPaths()
{
// Progress tracking setup
unsigned short step_size = (unsigned short)( (number_of_local_max_clusters + number_of_merging_clusters)/100);
if ( step_size == 0 ) step_size = 1;
unsigned int paths_so_far_total = 0;
progress_bar_ptr->SetRange(0, 100);
progress_bar_ptr->SetPos(0);
// An array to keep track of what shortest path travels over what cluster.
// (index is cluster id, contents is path number in medial_path_volumes[cur_volume].medial_paths_list list, starting at 1)
bool *traces_in_clusters = new bool [number_of_clusters];
// Pointer to current shortest path
LinkedListNode<LinkedList<PathNode>*> *cur_shortest_path_ptr;
LinkedListNode<PathNode> *cur_path_node_ptr;
LinkedListNode<cl_t> *smaller_nodes_list_ptr;
LinkedListNode<cl_t> *cur_clt_node_ptr, *final_clt_node_ptr;
Vect3usi cur_coord;
Vect3usi nxt_coord;
Vect3usi tmp_coord;
cl_t cid, neigh_cid;
char i;
unsigned int largest_cluster;
v_id cur_volume;
unsigned int paths_branch_cnt;
bool path_had_a_branch, first_dividing_cluster_encountered;
// Clear trace array
memset( traces_in_clusters, 0, sizeof(bool)*number_of_clusters );
write_to_log("MA_VoxelCoding::traceShortestPaths - Tracing shortest paths to RP points.");
progress_win_ptr->SetWindowText( "Tracing shortests paths..." );
for ( cur_volume = 0; cur_volume < number_of_volumes; cur_volume++ )
{
// For each medial axis starting point...
cur_shortest_path_ptr = medial_path_volumes[cur_volume].medial_paths_list->getHeadPtr();
while ( cur_shortest_path_ptr )
{
cur_path_node_ptr = cur_shortest_path_ptr->item->getHeadPtr();
path_had_a_branch = false;
// If this is a path linked from a branch, skip the initial marking node
if ( cur_path_node_ptr->item.type == LINK_NODE)
{
cur_path_node_ptr = cur_path_node_ptr->next;
}
cur_coord = cur_path_node_ptr->item.coordinate;
nxt_coord = cur_path_node_ptr->item.coordinate;
// Mark current cluster with current path number
traces_in_clusters[CL_cube->datac(cur_coord) - 1] = true;
// Now travel all the way to the RP point (1 in SS cube)
while ( SS_cube->datac(cur_coord) > 1 )
{
// If current node is a merging cluster (and hasn't been traveled yet), add new start coordinates to medial axis list
cid = CL_cube->datac(cur_coord) - 1;
if ( (cluster_nodes_array[cid].cluster_type & CLUSTER_TYPE_MERGING) )
{
path_had_a_branch = true;
// First find largest cluster, this will be the continuation of current path
largest_cluster = 0;
smaller_nodes_list_ptr = cluster_nodes_array[cid].s_list_ptr->getHeadPtr();
while ( smaller_nodes_list_ptr )
{
if ( (cluster_nodes_array[smaller_nodes_list_ptr->item-1].SS_code == (cluster_nodes_array[cid].SS_code-1)) &&
(cluster_nodes_array[smaller_nodes_list_ptr->item-1].size > largest_cluster) )
{
largest_cluster = cluster_nodes_array[smaller_nodes_list_ptr->item-1].size;
nxt_coord = cluster_nodes_array[smaller_nodes_list_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
}
// Go on to the next smaller neighbor
smaller_nodes_list_ptr = smaller_nodes_list_ptr->next;
}
if ( largest_cluster == 0 )
{
write_to_log("MA_VoxelCoding::traceShortestPaths - FATAL ERROR - could not find a neighboring cluster");
exit( 1 );
}
// Now add all other neighbors as new paths starting points
smaller_nodes_list_ptr = cluster_nodes_array[cid].s_list_ptr->getHeadPtr();
while ( smaller_nodes_list_ptr )
{
// The following must be true for an addition:
// 1) It isn't the same cluster we just found (nxt_coord)
// 2) It is one smaller than current clusters SS code
if ( (smaller_nodes_list_ptr->item != CL_cube->datac(nxt_coord)) &&
(cluster_nodes_array[smaller_nodes_list_ptr->item-1].SS_code == (cluster_nodes_array[cid].SS_code-1)) )
{
// We want to insert this branch path right after current path. This way the branching paths closest
// to RP will be done first, insuring other paths are as short as possible.
medial_path_volumes[cur_volume].medial_paths_list->insertAfter( cur_shortest_path_ptr, medial_path_volumes[cur_volume].medial_paths_list->getNewNode() );
cur_shortest_path_ptr->next->item = new LinkedList<PathNode>( 0 );
cur_shortest_path_ptr->next->item->insertAtStart( cur_shortest_path_ptr->next->item->getNewNode() );
cur_shortest_path_ptr->next->item->getHeadPtr()->item.type = LINK_NODE;
cur_shortest_path_ptr->next->item->getHeadPtr()->item.from_branch_cluster_id = cid+1;
// If cluster has more than one LM, take the one nearest to current one
findNearestCoordinateInList( cur_shortest_path_ptr->item->getTailPtr()->item.coordinate,
cluster_nodes_array[smaller_nodes_list_ptr->item-1].max_coord_list_ptr,
tmp_coord );
// Now insert the coordinate
cur_shortest_path_ptr->next->item->insertAtEnd( cur_shortest_path_ptr->next->item->getNewNode() );
cur_shortest_path_ptr->next->item->getTailPtr()->item.coordinate = tmp_coord;
}
// Go on to the next smaller neighbor
smaller_nodes_list_ptr = smaller_nodes_list_ptr->next;
}
}
// Otherwise just find next lower SS cluster
else
{
for ( i = 0; i < 7; i++ )
{
if ( get_zyx_neigh_6dir(i, nxt_coord, boundary_cube->dimensions) )
{
// Make sure next spot is closer to the RP point than current
if ( (!boundary_cube->get_spot(nxt_coord)) &&
(SS_cube->datac(nxt_coord) < SS_cube->datac(cur_coord)) )
{
// Check if we have run into a cluster that has already been traveled on,
// this indicates we have hit a dividing cluster (since we are traveling backwards)
// However, make sure we have at least two coordinates in list already
if ( traces_in_clusters[CL_cube->datac(nxt_coord) - 1] )
{
cur_shortest_path_ptr->item->insertAtEnd( cur_shortest_path_ptr->item->getNewNode() );
cur_shortest_path_ptr->item->getTailPtr()->item.type = LINK_NODE;
cur_shortest_path_ptr->item->getTailPtr()->item.from_branch_cluster_id = CL_cube->datac(nxt_coord);
goto end_of_cur_trace;
}
// We found a valid spot closer to the RP, break out
break;
}
}
}
}
// If next spot is same as current, something broke!!
if ( cur_coord == nxt_coord )
{
write_to_log( "MA_VoxelCoding::traceShortestPaths - FATAL ERROR - SP %d stuck at %v!",
medial_path_volumes[cur_volume].medial_paths_list->getSize(), &cur_coord );
exit( 0 );
}
// Add next coord to current trace list
cur_shortest_path_ptr->item->insertAtEnd( cur_shortest_path_ptr->item->getNewNode() );
cur_shortest_path_ptr->item->getTailPtr()->item.coordinate = nxt_coord;
// Go on to next coordinate, and mark cluster as belonging to current path count
cur_coord = nxt_coord;
traces_in_clusters[ CL_cube->datac(cur_coord)-1 ] = true;
} // While NOT at RP point
end_of_cur_trace:
// If the SP is short (and is an original starting), it is likely from noise, so remove it
if ( (path_had_a_branch == false) &&
(cur_shortest_path_ptr->item->getSize() < delete_sp_below_size) &&
(cur_shortest_path_ptr->item->getHeadPtr()->item.type != LINK_NODE) &&
(medial_path_volumes[cur_volume].medial_paths_list->getSize() > 1) )
{
first_dividing_cluster_encountered = false;
if ( run_options & MA_LOG_DELETED_PATHS )
{
write_to_log("MA_VoxelCoding::traceShortestPaths - Removed SP starting at %v, length was %d.",
cur_shortest_path_ptr->item->getHeadPtr()->item.coordinate, cur_shortest_path_ptr->item->getSize() );
}
// First release the traveled clusters, and all branches associated with this path
// in the cluster graph.
cur_path_node_ptr = cur_shortest_path_ptr->item->getHeadPtr();
while ( cur_path_node_ptr )
{
if ( cur_path_node_ptr->item.type == COORD_NODE )
{
cid = CL_cube->datac( cur_path_node_ptr->item.coordinate );
traces_in_clusters[cid-1] = false;
// Remove the very first dividing cluster we encounter along the way. From that
// point on, any dividing clusters should be kept for other possible paths
if ( (cluster_nodes_array[cid-1].cluster_type == CLUSTER_TYPE_DIVIDING) &&
(first_dividing_cluster_encountered == false) )
{
first_dividing_cluster_encountered = true;
removeFromDividingCluster( CL_cube->datac(cur_path_node_ptr->prev->item.coordinate) );
}
// If ending in a dividing cluster, remove this cluster id from all smaller neighbors
if ( (cur_path_node_ptr->next) && (cur_path_node_ptr->next->item.type == LINK_NODE) &&
(first_dividing_cluster_encountered == false) )
{
removeFromDividingCluster( cid );
}
}
cur_path_node_ptr = cur_path_node_ptr->next;
}
delete cur_shortest_path_ptr->item;
cur_shortest_path_ptr->item = NULL;
if ( cur_shortest_path_ptr->next )
{
cur_shortest_path_ptr = cur_shortest_path_ptr->next;
medial_path_volumes[cur_volume].medial_paths_list->removeNode( cur_shortest_path_ptr->prev );
}
else
{
medial_path_volumes[cur_volume].medial_paths_list->removeNode( cur_shortest_path_ptr );
cur_shortest_path_ptr = NULL;
}
}
// Otherwise just go on to the next path
else
{
cur_shortest_path_ptr = cur_shortest_path_ptr->next;
}
// Update progress
progress_bar_ptr->SetPos( ++paths_so_far_total/step_size );
}
// Now we have a set number of paths, convert them to be stored in an array rather than linked list for efficiency
medial_path_volumes[cur_volume].number_of_medial_paths = medial_path_volumes[cur_volume].medial_paths_list->getSize( );
medial_path_volumes[cur_volume].medial_paths_array = medial_path_volumes[cur_volume].medial_paths_list->convertToArray( );
number_of_medial_paths_total += medial_path_volumes[cur_volume].number_of_medial_paths;
// No longer need list form of paths
delete medial_path_volumes[cur_volume].medial_paths_list;
medial_path_volumes[cur_volume].medial_paths_list = NULL;
}
delete[] traces_in_clusters;
write_to_log("MA_VoxelCoding::traceShortestPaths - Determining wall off clusters.");
progress_win_ptr->SetWindowText( "Determining wall off clusters..." );
// Determine what to wall off for each point that starts/ends in a branch
for ( cur_volume = 0; cur_volume < number_of_volumes; cur_volume++ )
{
if ( medial_path_volumes[cur_volume].number_of_medial_paths > 0 )
{
medial_path_volumes[cur_volume].walls_to_destroy_at_start_array =
new LinkedList<Vect3usi> [medial_path_volumes[cur_volume].number_of_medial_paths];
medial_path_volumes[cur_volume].wall_off_start_array =
new LinkedList<Vect3usi> [medial_path_volumes[cur_volume].number_of_medial_paths];
medial_path_volumes[cur_volume].wall_off_end_array =
new LinkedList<Vect3usi> [medial_path_volumes[cur_volume].number_of_medial_paths];
medial_path_volumes[cur_volume].wall_off_between_array =
new LinkedList<Vect3usi> [medial_path_volumes[cur_volume].number_of_medial_paths];
for ( unsigned int cur_path = 0; cur_path < medial_path_volumes[cur_volume].number_of_medial_paths; cur_path++ )
{
if ( (medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getHeadPtr()->item.type == LINK_NODE) &&
(medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->item.type == LINK_NODE) &&
(medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getSize() == 3) )
{
// Special case: If we have a Link->Coord->Link, we will need to create walls at coordinates out of this path
// First link back to higher SS cluster... so make all smaller ones walls
cur_coord = medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getHeadPtr()->next->item.coordinate;
cid = CL_cube->datac(cur_coord) - 1;
cur_clt_node_ptr = cluster_nodes_array[cid].s_list_ptr->getHeadPtr();
while ( cur_clt_node_ptr )
{
// If smaller cluster, mark it as to be walled off at start
if ( cluster_nodes_array[cur_clt_node_ptr->item-1].SS_code == (SS_cube->datac(cur_coord)-1) )
{
tmp_coord = cluster_nodes_array[cur_clt_node_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].insertAtStart( tmp_coord );
}
cur_clt_node_ptr = cur_clt_node_ptr->next;
}
// Now wall of all larger SS clusters for linking at end.
cur_clt_node_ptr = cluster_nodes_array[cid].l_list_ptr->getHeadPtr();
while ( cur_clt_node_ptr )
{
// If larger cluster, mark it as to be walled off at end
if ( cluster_nodes_array[cur_clt_node_ptr->item-1].SS_code == (SS_cube->datac(cur_coord)+1) )
{
tmp_coord = cluster_nodes_array[cur_clt_node_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].wall_off_end_array[cur_path].insertAtStart( tmp_coord );
}
cur_clt_node_ptr = cur_clt_node_ptr->next;
}
}
else if ( medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->item.type == LINK_NODE )
{
final_clt_node_ptr = NULL;
// Insert prev to end coordinate as wall off for start/end linking separation
cur_coord = medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->prev->prev->item.coordinate;
medial_path_volumes[cur_volume].wall_off_end_array[cur_path].insertAtStart( cur_coord );
// We cannot share the previous coord for start/end if path is only 2 coordinate nodes
if ( medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getSize() == 4 )
{
cur_coord = medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->prev->item.coordinate;
}
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].insertAtStart( cur_coord );
// Since this path ends with a branch, block off other possible paths to prevent accidental
// loop formations back towards the start of current path.
cur_coord = medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->prev->item.coordinate;
cid = CL_cube->datac(cur_coord) - 1;
cur_clt_node_ptr = cluster_nodes_array[cid].s_list_ptr->getHeadPtr();
while ( cur_clt_node_ptr )
{
// If the branch cluster has more than one lower neighbor, we will want to wall off
// everything but the path to which we are merging
if ( cluster_nodes_array[cur_clt_node_ptr->item-1].SS_code == (SS_cube->datac(cur_coord)-1) )
{
if ( cur_clt_node_ptr->item != medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getTailPtr()->item.from_branch_cluster_id )
{
tmp_coord = cluster_nodes_array[cur_clt_node_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].wall_off_between_array[cur_path].insertAtStart( tmp_coord );
}
else
{
final_clt_node_ptr = cur_clt_node_ptr;
}
}
cur_clt_node_ptr = cur_clt_node_ptr->next;
}
// Now add all BUT current cluster as wall off points
cur_clt_node_ptr = cluster_nodes_array[final_clt_node_ptr->item-1].l_list_ptr->getHeadPtr();
while ( cur_clt_node_ptr )
{
if ( (cluster_nodes_array[cur_clt_node_ptr->item-1].SS_code == SS_cube->datac(cur_coord)) &&
(cluster_nodes_array[cur_clt_node_ptr->item-1].ID != (cid+1)) )
{
tmp_coord = cluster_nodes_array[cur_clt_node_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].wall_off_between_array[cur_path].insertAtStart( tmp_coord );
}
cur_clt_node_ptr = cur_clt_node_ptr->next;
}
}
// Now go through entire path, and add any additional walls at branches
cur_path_node_ptr = medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getHeadPtr();
paths_branch_cnt = (cur_path_node_ptr->item.type == LINK_NODE) ? (1) : (0);
path_had_a_branch = false;
while ( cur_path_node_ptr )
{
if ( (cur_path_node_ptr->item.type == COORD_NODE) )
{
// If next item in list is a link to, we want to wall current cluster until this path is active
if ( (paths_branch_cnt) && (cur_path_node_ptr->next) && (cur_path_node_ptr->next->item.type == LINK_NODE) )
{
-- paths_branch_cnt;
if ( medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getSize() > 4 )
{
cid = CL_cube->datac(cur_path_node_ptr->prev->item.coordinate) - 1;
tmp_coord = cluster_nodes_array[cid].max_coord_list_ptr->getHeadPtr()->item;
}
else
{
cid = CL_cube->datac(cur_path_node_ptr->item.coordinate) - 1;
tmp_coord = cluster_nodes_array[cid].max_coord_list_ptr->getHeadPtr()->item;
}
// Only wall off if not branching cluster type
if ( !(cluster_nodes_array[cid].cluster_type & CLUSTER_TYPE_BRANCHING) )
{
medial_path_volumes[cur_volume].walls_to_destroy_at_start_array[cur_path].insertAtStart( tmp_coord );
// Also remove this clusters from any previous paths wall off lists
for ( unsigned int tmp_path = 0; tmp_path < cur_path; tmp_path++ )
{
removeCoordinateFromList( &medial_path_volumes[cur_volume].wall_off_between_array[tmp_path], tmp_coord );
}
}
}
// If we hit a branch cluster, need to move the start wall up if path began from a link to another path.
cid = CL_cube->datac(cur_path_node_ptr->item.coordinate) - 1;
if ( (medial_path_volumes[cur_volume].medial_paths_array[cur_path]->getHeadPtr()->item.type == LINK_NODE) &&
(cluster_nodes_array[cid].cluster_type & CLUSTER_TYPE_BRANCHING) && (path_had_a_branch == false) )
{
// Try to go back one cluster, since branch one is likely larger
if ( (cur_path_node_ptr->prev) && (cur_path_node_ptr->prev->prev) &&
(cur_path_node_ptr->prev->prev->item.type == COORD_NODE) )
{
path_had_a_branch = true;
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].removeAllNodes();
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].insertAtStart( cur_path_node_ptr->prev->item.coordinate );
}
else if ( (cur_path_node_ptr->prev) && (cur_path_node_ptr->prev->item.type == COORD_NODE) )
{
path_had_a_branch = true;
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].removeAllNodes();
medial_path_volumes[cur_volume].wall_off_start_array[cur_path].insertAtStart( cur_path_node_ptr->item.coordinate );
}
}
// If cluster is dividing and we are post a merging point, add all larger neighbors that are not part of path
// to wall off list.
if ( (cluster_nodes_array[cid].cluster_type & CLUSTER_TYPE_DIVIDING) && (cur_path_node_ptr->prev) &&
(cur_path_node_ptr->prev->item.type == COORD_NODE) && (paths_branch_cnt > 0) )
{
-- paths_branch_cnt;
neigh_cid = CL_cube->datac(cur_path_node_ptr->prev->item.coordinate);
// Wall off the previous spot until path active (unless branching type
if ( !(cluster_nodes_array[neigh_cid-1].cluster_type & CLUSTER_TYPE_BRANCHING) )
{
tmp_coord = cluster_nodes_array[neigh_cid-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].walls_to_destroy_at_start_array[cur_path].insertAtStart( tmp_coord );
// Also remove this clusters from any previous paths wall off lists
for ( unsigned int tmp_path = 0; tmp_path < cur_path; tmp_path++ )
{
removeCoordinateFromList( &medial_path_volumes[cur_volume].wall_off_between_array[tmp_path], tmp_coord );
}
}
// Wall off other paths to prevent accidental linking over them rather than through current path
cur_clt_node_ptr = cluster_nodes_array[cid].l_list_ptr->getHeadPtr();
while ( cur_clt_node_ptr )
{
if ( (cluster_nodes_array[cur_clt_node_ptr->item-1].SS_code == (cluster_nodes_array[cid].SS_code+1) ) &&
(cluster_nodes_array[cur_clt_node_ptr->item-1].ID != neigh_cid) )
{
tmp_coord = cluster_nodes_array[cur_clt_node_ptr->item-1].max_coord_list_ptr->getHeadPtr()->item;
medial_path_volumes[cur_volume].wall_off_between_array[cur_path].insertAtStart( tmp_coord );
}
cur_clt_node_ptr = cur_clt_node_ptr->next;
}
}
// Keep track of how many merging branches we encounter, so then we can
// create a wall for corresponding dividing clusters
if ( cluster_nodes_array[cid].cluster_type & CLUSTER_TYPE_MERGING )
{
++ paths_branch_cnt;
}
}
cur_path_node_ptr = cur_path_node_ptr->next;
}
}
// For the first path, we do not need to bother walling off the branch paths
medial_path_volumes[cur_volume].walls_to_destroy_at_start_array[0].removeAllNodes();
}
}
}
// ----------------------------------------------------------------------------------------
//
// Description - Finds nearest coordinate in list, and if there are others that are close
// to it, returns max BS of those, as well as number of near neighbors.
// Moves the other near ones to start of max list.
// ----------------------------------------------------------------------------------------
int MA_VoxelCoding::findNearestCoordinateInList( Vect3usi &coord_near_to, LinkedList<Vect3usi> *max_list_ptr, Vect3usi &max_nearest )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findNearestCoordinateInList) - nearest to %v, BS value %d, list size %d",
trace_text_buffer, &coord_near_to, BS_cube->datac(coord_near_to), max_list_ptr->getSize() );
}
#endif
if ( max_list_ptr->getSize() > 1 )
{
LinkedListNode<Vect3usi> *cur_node_ptr = max_list_ptr->getHeadPtr();
LinkedListNode<Vect3usi> *tmp_node_ptr;
unsigned short dist_min = 0xFFFF;
unsigned short dist_tmp;
int ret_val = -1;
bs_t max_bs=0;
// First find closest coordinate
while ( cur_node_ptr )
{
if ( SS_cube->datac(cur_node_ptr->item) > 0 )
{
dist_tmp = cur_node_ptr->item.get_dist_man_to( coord_near_to );
#if TRACE_COORDINATE_DETAILS
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findNearestCoordinateInList) - => %v, BS value %d, distance %d",
trace_text_buffer, &(cur_node_ptr->item), BS_cube->datac(cur_node_ptr->item), dist_tmp );
}
#endif
if ( dist_tmp < dist_min )
{
dist_min = dist_tmp;
max_nearest = cur_node_ptr->item;
max_bs = BS_cube->datac( max_nearest );
}
}
cur_node_ptr = cur_node_ptr->next;
}
// Now go through again, and find any near the closest, keep the one with highest BS
cur_node_ptr = max_list_ptr->getHeadPtr();
dist_min = max_nearest.get_dist_man_to( coord_near_to );
while ( cur_node_ptr )
{
if ( SS_cube->datac(cur_node_ptr->item) > 0 )
{
dist_tmp = cur_node_ptr->item.get_dist_man_to( coord_near_to );
// Consider to be near if coordinate is either double distance of minimum,
// or closer than 8
if ( ((dist_tmp-dist_min) < dist_min) || (dist_tmp <= BS_cube->datac(cur_node_ptr->item)/6) )
{
// Moves to start of list
tmp_node_ptr = cur_node_ptr->next;
max_list_ptr->unlinkNode( cur_node_ptr );
max_list_ptr->insertAtStart( cur_node_ptr );
// If larger bs, update return coords
if ( max_bs < BS_cube->datac(cur_node_ptr->item) )
{
max_nearest = cur_node_ptr->item;
max_bs = BS_cube->datac( max_nearest );
}
++ ret_val;
cur_node_ptr = tmp_node_ptr;
continue;
}
}
cur_node_ptr = cur_node_ptr->next;
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findNearestCoordinateInList) - final nearest %v, number near %d",
trace_text_buffer, &max_nearest, ret_val );
}
#endif
return ret_val;
}
// There is only one max coordinate in list, return it
else
{
max_nearest = max_list_ptr->getHeadPtr()->item;
return 0;
}
}
// ----------------------------------------------------------------------------------------
//
// Description - Finds max BS coordinate in list, and if there are others that have almost
// the same BS, returns the number of such coordinate in list.
// Moves the other near ones to start of max list.
// ----------------------------------------------------------------------------------------
int MA_VoxelCoding::findMaxBSCoordinateInList( LinkedList<Vect3usi> *max_list_ptr, Vect3usi &max_bs_coord )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findMaxBSCoordinateInList) - list size %d", trace_text_buffer, max_list_ptr->getSize() );
}
#endif
if ( max_list_ptr->getSize() > 1)
{
LinkedListNode<Vect3usi> *cur_node_ptr = max_list_ptr->getHeadPtr();
LinkedListNode<Vect3usi> *tmp_node_ptr;
bs_t bs_max = 0;
bs_t bs_tmp;
int ret_val = -1;
// First find maximum BS coordinate
while ( cur_node_ptr )
{
bs_tmp = BS_cube->datac( cur_node_ptr->item );
#if TRACE_COORDINATE_DETAILS
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findMaxBSCoordinateInList) - => %v, BS value %d",
trace_text_buffer, &(cur_node_ptr->item), BS_cube->datac(cur_node_ptr->item) );
}
#endif
if ( bs_tmp > bs_max )
{
bs_max = bs_tmp;
max_bs_coord = cur_node_ptr->item;
}
cur_node_ptr = cur_node_ptr->next;
}
// Now find how many are close to same BS value
cur_node_ptr = max_list_ptr->getHeadPtr();
bs_max = (bs_t) (0.90*bs_max);
while ( cur_node_ptr )
{
bs_tmp = BS_cube->datac( cur_node_ptr->item );
if ( bs_tmp > bs_max )
{
// Move to start of list
tmp_node_ptr = cur_node_ptr->next;
max_list_ptr->unlinkNode( cur_node_ptr );
max_list_ptr->insertAtStart( cur_node_ptr );
cur_node_ptr = tmp_node_ptr;
++ret_val;
}
else
{
cur_node_ptr = cur_node_ptr->next;
}
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "%sTRACE (findMaxBSCoordinateInList) - final max %v, number near %d",
trace_text_buffer, &max_bs_coord, ret_val );
}
#endif
return ret_val;
}
// There is only one coordinate in the list
else
{
max_bs_coord = max_list_ptr->getHeadPtr()->item;
return 0;
}
}
// ----------------------------------------------------------------------------------------
//
// Description -
// ----------------------------------------------------------------------------------------
void MA_VoxelCoding::editSSClusterWall( LinkedList<Vect3usi> *cluster_coordinates, ma_wall_action action )
{
if ( cluster_coordinates->getSize() == 0 )
{
return;
}
LinkedListNode<Vect3usi> *cur_node_ptr = cluster_coordinates->getHeadPtr();
// Check what action we are taking
if ( action == CREATE )
{
coordinate_queue->insertAtStart( queue_mark_coord );
creating_ss_wall = true;
while ( cur_node_ptr )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (editSSClusterWall) - creating wall at coordinate %v, cluster ID %d",
&(cur_node_ptr->item), CL_cube->datac(cur_node_ptr->item) );
}
#endif
recurseOnAllNeighbors( cur_node_ptr->item, &MA_VoxelCoding::markSSClusterWall );
cur_node_ptr = cur_node_ptr->next;
}
}
else
{
LinkedListNode<Vect3usi> *tmp_node_ptr;
creating_ss_wall = false;
while ( cur_node_ptr )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (editSSClusterWall) - destroying wall at coordinate %v, cluster ID %d",
&(cur_node_ptr->item), CL_cube->datac(cur_node_ptr->item) );
}
#endif
recurseOnAllNeighbors( cur_node_ptr->item, &MA_VoxelCoding::markSSClusterWall );
cur_node_ptr = cur_node_ptr->next;
}
// Now restore all medial axis coordinates
cur_node_ptr = coordinate_queue->getHeadPtr();
while ( cur_node_ptr )
{
if (cur_node_ptr->item == queue_mark_coord )
{
coordinate_queue->removeNode( cur_node_ptr );
break;
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (editSSClusterWall) - restoring medial path coordinate %v", &(cur_node_ptr->item) );
}
#endif
SS_cube->datac( cur_node_ptr->item, ss_medial_path );
tmp_node_ptr = cur_node_ptr->next;
coordinate_queue->removeNode( cur_node_ptr );
cur_node_ptr = tmp_node_ptr;
}
}
}
// ----------------------------------------------------------------------------------------
//
// Description -
// ----------------------------------------------------------------------------------------
bool MA_VoxelCoding::markSSClusterWall( Vect3usi &cur_coord, Vect3usi &nxt_coord )
{
// Make sure we are in the same cluster
if ( CL_cube->datac(cur_coord) == CL_cube->datac(nxt_coord) )
{
// If the wall is being created, SS coordinate must not be zero to proceed
if ( (creating_ss_wall==true) && (SS_cube->datac(nxt_coord) > 0) )
{
// Search spot and it's neighbors for existing MA's, add to queue for later restoration
Vect3usi tmp_coord = nxt_coord;
for ( char i = 0; i <= 6; i++ )
{
if ( ((get_zyx_neigh_6dir(i, tmp_coord, boundary_cube->dimensions)) || (i == 6)) &&
(SS_cube->datac(tmp_coord) == ss_medial_path) )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (markSSClusterWall) - backing up medial path coordinate %v", tmp_coord );
}
#endif
SS_cube->datac( tmp_coord, ((i==6)?(0):(ss_infinity)) );
coordinate_queue->insertAtStart( tmp_coord );
}
}
SS_cube->datac( nxt_coord, 0 );
return true;
}
// If the wall is being destroyed, the SS coordinate must be zero to proceed
else if ( (creating_ss_wall==false) && (SS_cube->datac(nxt_coord) == 0) )
{
SS_cube->datac(nxt_coord, ss_infinity);
return true;
}
}
return false;
}
// ----------------------------------------------------------------------------------------
//
// Description - Looks ahead at the next MA_LINK_LOOK_AHEAD nodes, and picks the nearest
// one to medial_node_ptr coordinates as next one to link to. If final
// nearest has multiple potential, will travel further down list in an attempt
// to find a single max point and returns REVERSE if succeeds, signifying reverse
// link should be attempted. Otherwise returns NORMAL.
// ----------------------------------------------------------------------------------------
LinkedListNode<PathNode>* MA_VoxelCoding::findNextMedialCoordinate( LinkedList<PathNode> *path_ptr,
LinkedListNode<PathNode> *first_node_ptr,
ma_link_type &returned_link_type,
LinkedListNode<PathNode> *reverse_tie_breaker_node_ptr )
{
LinkedListNode<PathNode> *cur_node_ptr = first_node_ptr;
LinkedListNode<PathNode> *closest_node_ptr = first_node_ptr;
LinkedListNode<PathNode> *medial_node_ptr;
medial_node_ptr = (reverse_tie_breaker_node_ptr) ? (first_node_ptr->next) : (first_node_ptr->prev);
LinkedList<Vect3usi> *max_list_ptr;
Vect3usi closest_coord, medial_coord = medial_node_ptr->item.coordinate;
cl_t cluster_id;
unsigned int closest_distance;
int num_other_options;
// If we are not linking in reverse, travel down path until first branch.
// If the branch is merging type, we will try going back words from there
if ( reverse_tie_breaker_node_ptr == NULL )
{
while ( cur_node_ptr->next )
{
// If current spot is a dividing cluster, or if next spot is a LINK to another path or close to RP point,
// we need to link normally.
if ( (cluster_nodes_array[CL_cube->datac(cur_node_ptr->item.coordinate) - 1].cluster_type == CLUSTER_TYPE_DIVIDING) ||
(cur_node_ptr->next->item.type == LINK_NODE) ||
(cluster_nodes_array[CL_cube->datac(cur_node_ptr->next->item.coordinate) - 1].SS_code < 4) )
{
break;
}
// If current cluster is a merging type, merge backwards from next ones max point
if ( (cluster_nodes_array[CL_cube->datac(cur_node_ptr->item.coordinate) - 1].cluster_type == CLUSTER_TYPE_MERGING) &&
(cur_node_ptr->next->item.type != LINK_NODE) )
{
cur_node_ptr = cur_node_ptr->next;
cluster_id = CL_cube->datac(cur_node_ptr->item.coordinate) - 1;
max_list_ptr = cluster_nodes_array[cluster_id].max_coord_list_ptr;
findMaxBSCoordinateInList( max_list_ptr, cur_node_ptr->item.coordinate );
returned_link_type = REVERSE;
return cur_node_ptr;
}
cur_node_ptr = cur_node_ptr->next;
}
}
cur_node_ptr = first_node_ptr;
returned_link_type = NORMAL;
// First clump together the max coordinate lists
for ( int i = 0; i < MA_LINK_LOOK_AHEAD; i++ )
{
if ( (cur_node_ptr == NULL) || (cur_node_ptr->item.type == LINK_NODE) )
{
break;
}
else if ( cur_node_ptr == reverse_tie_breaker_node_ptr )
{
// Consider final coordinate as a candidate
max_points_list->insertAtStart( cur_node_ptr->item.coordinate );
break;
}
cluster_id = CL_cube->datac( cur_node_ptr->item.coordinate ) - 1;
max_points_list->copyFromOtherListAfter( max_points_list->getTailPtr(),
cluster_nodes_array[cluster_id].max_coord_list_ptr );
cur_node_ptr = (reverse_tie_breaker_node_ptr) ? (cur_node_ptr->prev) : (cur_node_ptr->next);
}
// Now find all candidates for next MA node
num_other_options = findNearestCoordinateInList( medial_coord, max_points_list, closest_coord );
// We are going in reverse, and there are multiple options, pick closest to goal
if ( (reverse_tie_breaker_node_ptr != NULL) && (num_other_options > 0) )
{
LinkedListNode<Vect3usi> *cur_coord_node_ptr;
closest_distance = 0xFFFFFFFF;
cur_coord_node_ptr = max_points_list->getHeadPtr();
do
{
if ( cur_coord_node_ptr->item.get_dist_2_to(reverse_tie_breaker_node_ptr->item.coordinate) < closest_distance )
{
closest_distance = cur_coord_node_ptr->item.get_dist_2_to(reverse_tie_breaker_node_ptr->item.coordinate);
closest_coord = cur_coord_node_ptr->item;
}
cur_coord_node_ptr = cur_coord_node_ptr->next;
-- num_other_options;
} while ( num_other_options >= 0 );
}
// No longer need max coord list
max_points_list->removeAllNodes( );
// Find the node which corresponds to the coordinate
cur_node_ptr = first_node_ptr;
for ( int i = 0; i < MA_LINK_LOOK_AHEAD; i++ )
{
if ( CL_cube->datac(cur_node_ptr->item.coordinate) == CL_cube->datac(closest_coord) )
{
closest_node_ptr = cur_node_ptr;
closest_node_ptr->item.coordinate = closest_coord;
break;
}
cur_node_ptr = (reverse_tie_breaker_node_ptr) ? (cur_node_ptr->prev) : (cur_node_ptr->next);
}
// Remove all nodes in between, and return
if ( reverse_tie_breaker_node_ptr )
{
path_ptr->removeAllNodesBetween( closest_node_ptr, medial_node_ptr );
}
else
{
path_ptr->removeAllNodesBetween( medial_node_ptr, closest_node_ptr );
}
return closest_node_ptr;
}
// ----------------------------------------------------------------------------------------
//
// Description -
// ----------------------------------------------------------------------------------------
void MA_VoxelCoding::centerPathInReverse(LinkedList<PathNode> *path_ptr,
LinkedListNode<PathNode> *path_end_ptr,
LinkedListNode<PathNode> *path_start_ptr)
{
LinkedListNode<PathNode> *cur_node_ptr = path_end_ptr;
ma_link_type returned_link_type;
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (centerPathInReverse) - furthest in list [start from] %v, nearest [goal point] %v",
&(path_end_ptr->item.coordinate), &(path_start_ptr->item.coordinate) );
}
#endif
// Link current with next until we reach start node
while ( cur_node_ptr->item.coordinate != path_start_ptr->item.coordinate )
{
cur_node_ptr = cur_node_ptr->prev;
// If more than one cluster BS max, find nearest to previous (next since going backwards) spot
cur_node_ptr = findNextMedialCoordinate( path_ptr, cur_node_ptr, returned_link_type, path_start_ptr );
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (centerPathInReverse) - linking %v to %v",
&(cur_node_ptr->item.coordinate), &(cur_node_ptr->next->item.coordinate) );
}
#endif
// Link with previous coordinate (link looks at next spot)
if ( linkPointToPoint( path_ptr, cur_node_ptr ) == NULL )
{
path_ptr->removeAllNodes();
return;
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
// Visually separate each coordinate linking
write_to_log_empty_lines( 1 );
}
#endif
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (centerPathInReverse) - Done with reverse linking" );
}
#endif
}
// ----------------------------------------------------------------------------------------
//
// Description -
// ----------------------------------------------------------------------------------------
void MA_VoxelCoding::traceMedialAxis()
{
// Setup progress control
char progress_text[50];
float step_size = (float)number_of_medial_paths_total/100.0f;
unsigned int paths_so_far_total = 0;
progress_bar_ptr->SetRange(0, 100);
progress_bar_ptr->SetPos(0);
Vect3usi tmp_coord, max_coord;
LinkedListNode<PathNode> *cur_path_node_ptr, *prev_path_node_ptr;
// Reset SS cube
write_to_log("MA_VoxelCoding::traceMedialAxis - Initializing SS and MA cubes for path centering.");
SS_Xfrm->initXfrmCube( SS_cube );
// MARK cubes need to be clear for finding additional cluster max points
MARK_cubes[IN_QUEUE]->clear();
MARK_cubes[NOT_LM]->clear();
// Go through all shortest paths, centering and linking gaps
write_to_log( "MA_VoxelCoding::traceMedialAxis - Now centering %d paths across %d volumes.",
number_of_medial_paths_total, number_of_volumes );
for ( current_volume_num = 0; current_volume_num < number_of_volumes; current_volume_num++ )
{
#if TRACE_SPECIFIC_VOLUME
if ( current_volume_num == 14 )
{
full_trace_enabled= true;
}
else
{
full_trace_enabled= false;
}
#endif
// Create all walls (note that first path is empty from Shortest Paths stage)
for ( current_medial_path_num = medial_path_volumes[current_volume_num].number_of_medial_paths; current_medial_path_num > 0; current_medial_path_num-- )
{
editSSClusterWall( &medial_path_volumes[current_volume_num].walls_to_destroy_at_start_array[current_medial_path_num-1], CREATE );
}
for ( current_medial_path_num = 0; current_medial_path_num < medial_path_volumes[current_volume_num].number_of_medial_paths; current_medial_path_num++ )
{
#if TRACE_SPECIFIC_VOLUME
if ( (current_volume_num == 31)
&& ( (current_medial_path_num == 1)
// || (current_medial_path_num == 34)
)
)
{
full_trace_enabled= true;
}
else
{
full_trace_enabled= false;
}
#endif
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (traceMedialAxis)\n\t ============== Starting path %d of volume %d ==============",
current_medial_path_num, current_volume_num );
}
#endif
sprintf( progress_text, "Centering path %d [out of %d] in volume %d [out of %d]...",
current_medial_path_num+1, medial_path_volumes[current_volume_num].number_of_medial_paths, current_volume_num+1, number_of_volumes );
progress_win_ptr->SetWindowText( progress_text );
// Destroy walls created to reserve this path
editSSClusterWall( &medial_path_volumes[current_volume_num].walls_to_destroy_at_start_array[current_medial_path_num], DESTROY );
// Grab head pointer of current path
cur_path_node_ptr = medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num]->getHeadPtr();
// Create all walls main walls for this path
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_between_array[current_medial_path_num], CREATE );
// This path started from a merging cluster, link back to it
if ( cur_path_node_ptr->item.type == LINK_NODE )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (traceMedialAxis) - volume %d, path %d, cluster id [%d] to point %v link",
current_volume_num, current_medial_path_num, cur_path_node_ptr->item.from_branch_cluster_id,
&(cur_path_node_ptr->next->item.coordinate) );
}
#endif
// Move to first valid coordinate and remove marker node
cur_path_node_ptr = cur_path_node_ptr->next;
medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num]->removeNode( cur_path_node_ptr->prev );
// Create wall behind coordinate to prevent going in wrong direction
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_start_array[current_medial_path_num], CREATE );
cur_path_node_ptr = linkPointToPath( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], cur_path_node_ptr, true );
// Destroy the wall if it was created
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_start_array[current_medial_path_num], DESTROY );
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
// Visually separate each coordinate linking
write_to_log_empty_lines( 1 );
}
#endif
}
// Start from head again, if previous link to path failed it will be NULL
prev_path_node_ptr = cur_path_node_ptr;
if ( cur_path_node_ptr )
{
cur_path_node_ptr = cur_path_node_ptr->next;
}
// Now travel down path linking with previous coordinate,
// until either RP is reached, or merge with another path occurs.
while ( cur_path_node_ptr )
{
// We have reached an indicator to link with another existing Path
if ( cur_path_node_ptr->item.type == LINK_NODE )
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (traceMedialAxis) - volume %d, path %d, point %v to cluster id [%d] link",
current_volume_num, current_medial_path_num, &(cur_path_node_ptr->prev->item.coordinate),
cur_path_node_ptr->item.from_branch_cluster_id );
}
#endif
// Delete marker node
cur_path_node_ptr = cur_path_node_ptr->prev;
medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num]->removeNode( cur_path_node_ptr->next );
// Create wall behind coordinate to prevent going in wrong direction
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_end_array[current_medial_path_num], CREATE );
linkPointToPath( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], cur_path_node_ptr, false );
// Destroy the wall if it was created
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_end_array[current_medial_path_num], DESTROY );
// We are done with this path
break;
}
// Continue linking central medial axis nodes...
else
{
ma_link_type returned_link_type;
cur_path_node_ptr = findNextMedialCoordinate( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], cur_path_node_ptr, returned_link_type, NULL );
if ( returned_link_type == REVERSE )
{
centerPathInReverse( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], cur_path_node_ptr, prev_path_node_ptr );
if ( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num]->getSize() == 0 )
{
cur_path_node_ptr = NULL;
}
}
// Link normally
else
{
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
write_to_log( "TRACE (traceMedialAxis) - volume %d, path %d, point %v to point %v",
current_volume_num, current_medial_path_num, &(prev_path_node_ptr->item.coordinate),
&(cur_path_node_ptr->item.coordinate) );
}
#endif
// Note that cur_path_node_ptr->item.coordinate was set to midpoint in the above 'if'
if ( linkPointToPoint( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], prev_path_node_ptr ) == NULL )
{
break;
}
}
}
#if MA_DO_FULL_TRACE
if ( full_trace_enabled )
{
// Visually separate each coordinate linking
write_to_log_empty_lines( 1 );
}
#endif
prev_path_node_ptr = cur_path_node_ptr;
if ( cur_path_node_ptr )
{
cur_path_node_ptr = cur_path_node_ptr->next;
}
}
// Destroy all walls for this path
editSSClusterWall( &medial_path_volumes[current_volume_num].wall_off_between_array[current_medial_path_num], DESTROY );
// Smooth path to be 26 direction linked, rather than 6.
if ( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num]->getSize() > 0 )
{
convertPathTo26Linked( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num] );
}
// Mark it in SS cube for future paths to link to.
markPathInSSCube( medial_path_volumes[current_volume_num].medial_paths_array[current_medial_path_num], ss_medial_path );
// Update progress
progress_bar_ptr->SetPos( (int)(((float)(++paths_so_far_total))/step_size) );
}
}
}