#include "StdAfx.h" #include "MA_Voxel_Coding.h" #include "MA_vc_Flags.h" // ---------------------------------------------------------------------------------------- // attemptDirectLinearLink // // Description - Given 'start_node_ptr' node in 'path_ptr' list, checks if a link with // '->next' qualifies for a linear one. If it qualifies, will do required // work to link, and return 'true'. Otherwise returns false. // ---------------------------------------------------------------------------------------- bool MA_VoxelCoding::attemptDirectLinearLink(LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr) { // Obtain distance between the nodes being linked unsigned short distance = start_node_ptr->item.coordinate.get_dist_man_to(start_node_ptr->next->item.coordinate); // If next path node is direct neighbor, link is complete if ( distance < 2 ) { #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (attemptDirectLinearLink) - distance was 1, no link needed", trace_text_buffer ); } #endif return true; } // See if distance is below minimum required for direct linear link if ( (distance <= MA_DIRECT_LINEAR_LINK_DISTANCE) || (current_recursion > MA_FORCE_DIRECT_LINK_AT_RECURSION) ) { if ( (current_recursion > MA_FORCE_DIRECT_LINK_AT_RECURSION) && (distance > MA_DIRECT_LINEAR_LINK_DISTANCE) && (run_options & MA_LOG_FORCED_LINKS) ) { write_to_log( "MA_VoxelCoding::attemptDirectLinearLink - Warning: distance %d linear link %v to %v (path %d, volume %d) due to recursion limit (%d).", distance, start_node_ptr->item.coordinate, start_node_ptr->next->item.coordinate, current_medial_path_num, current_volume_num, current_recursion ); } // There is a chance of running into a fiber trying a direct link, // so save end point in case we need to revert LinkedListNode<PathNode> *end_node_ptr = start_node_ptr->next; if ( linkDirectlyWithNextPathNode( path_ptr, start_node_ptr ) == false ) { // Failed, remove added nodes LinkedListNode<PathNode> *tmp_node_ptr = start_node_ptr->next; while ( tmp_node_ptr != end_node_ptr ) { tmp_node_ptr = tmp_node_ptr->next; path_ptr->removeNode( tmp_node_ptr->prev ); } // If we are at double the recursion limit, link using shorest path (last resort) if ( current_recursion > (2*MA_FORCE_DIRECT_LINK_AT_RECURSION) ) { if ( (distance > 3) || (run_options & MA_LOG_FORCED_LINKS) ) { write_to_log( "MA_VoxelCoding::attemptDirectLinearLink - Warning: distance %d shortest path link forced %v to %v (path %d, volume %d), recursion (%d).", distance, start_node_ptr->item.coordinate, start_node_ptr->next->item.coordinate, current_medial_path_num, current_volume_num, current_recursion ); } linkUsingShortestPathToNextPathNode( path_ptr, start_node_ptr ); return true; } return false; } return true; } // If distance is smaller than the larger BS value, can also do linear link bs_t larger_bs = BS_cube->datac( start_node_ptr->item.coordinate ); if ( larger_bs < BS_cube->datac(start_node_ptr->next->item.coordinate) ) { larger_bs = BS_cube->datac(start_node_ptr->next->item.coordinate); } // Normalize BS value to represent true distance (NOTE: manhattan vs euclidean doesn't matter) larger_bs = larger_bs/3; if ( distance <= larger_bs ) { LinkedListNode<PathNode> *end_node_ptr = start_node_ptr->next; if ( linkDirectlyWithNextPathNode( path_ptr, start_node_ptr ) == false ) { // Failed, remove added nodes LinkedListNode<PathNode> *tmp_node_ptr = start_node_ptr->next; while ( tmp_node_ptr != end_node_ptr ) { tmp_node_ptr = tmp_node_ptr->next; path_ptr->removeNode( tmp_node_ptr->prev ); } return false; } return true; } return false; } // ---------------------------------------------------------------------------------------- // linkUsingShortestPathToNextPathNode // // Description - Given 'start_node_ptr' node in 'path_ptr' list, additional nodes will // be inserted into list after 'start_node_ptr' to create a shortest path 6-way // linked path to 'start_node_ptr->next' // ---------------------------------------------------------------------------------------- void MA_VoxelCoding::linkUsingShortestPathToNextPathNode(LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr) { Vect3usi start_coord = start_node_ptr->next->item.coordinate; // Mark end point and perform SS xfrm SS_cube->datac( start_node_ptr->item.coordinate, ss_medial_path-1 ); SS_Xfrm->changeEndValueForSS( ss_medial_path-1 ); if ( SS_Xfrm->runSS(SS_cube, start_coord, DT_FACE_ONLY|DT_STOP_AT_HIT) != DT_STOPPED_AT_HIT ) { // Something happened during transform and we never reached goal coordinate write_to_log( "MA_VoxelCoding::linkUsingShortestPathToNextPathNode - FATAL ERROR - could not link point %v to %v of path %d (volume %d)", &start_coord, &start_node_ptr->item.coordinate, current_medial_path_num, current_volume_num ); // Mark what we have of the path markPathInSSCube( path_ptr, ss_medial_path - SS_Xfrm->getInfinityValue()/4 ); doFullDump( MA_CRASH_ON_FATAL_ERROR ); } // Do shortest path end coordinate to start coordinate doSingleShortestPath( path_ptr, start_node_ptr, false ); // Erase SS SS_Xfrm->eraseSS( SS_cube, start_coord, ss_medial_path-1, DT_FACE_ONLY ); } // ---------------------------------------------------------------------------------------- // linkDirectlyWithNextPathNode // // Description - Given 'start_node_ptr' node in 'path_ptr' list, additional nodes will // be inserted into list after 'start_node_ptr' to create a direct 6-way // linked path to 'start_node_ptr->next' // Returns 'false' if fiber encountered. // ---------------------------------------------------------------------------------------- bool MA_VoxelCoding::linkDirectlyWithNextPathNode(LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr) { LinkedListNode<PathNode> *tmp_node_ptr; Vect3usi cur_coord = start_node_ptr->item.coordinate; Vect3usi end_coord = start_node_ptr->next->item.coordinate; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkDirectlyWithNextPathNode) - from %v to %v", trace_text_buffer, &cur_coord, &end_coord ); } #endif float dx = (float)end_coord.x - cur_coord.x; float dy = (float)end_coord.y - cur_coord.y; float dz = (float)end_coord.z - cur_coord.z; float mdist = ((dx>=0)?(dx):(-dx)) + ((dy>=0)?(dy):(-dy)) + ((dz>=0)?(dz):(-dz)); dx = dx/mdist; dy = dy/mdist; dz = dz/mdist; float tx = 0, ty = 0, tz = 0; while ( cur_coord != end_coord ) { tx += dx; // Make sure enough difference has been built up to warrant a full voxel progression if ( (tx >= 1.0f) || (tx <= -1.0f) ) { (tx > 0) ? (++cur_coord.x) : (--cur_coord.x); // Make sure we are not next to the goal voxel yet if ( end_coord == cur_coord ) { return true; } else { // Make sure we did not hit a fiber or wall if ( (boundary_cube->get_spot(cur_coord)) || (SS_cube->datac(cur_coord) == 0) ) { return false; } // Fill out a new path node object tmp_node_ptr = path_ptr->getNewNode(); tmp_node_ptr->item.type= COORD_NODE; tmp_node_ptr->item.coordinate = cur_coord; // Insert into list path_ptr->insertAfter( start_node_ptr, tmp_node_ptr ); start_node_ptr = start_node_ptr->next; } // If we are at the goal, make delta 0 so we never enter here again if ( end_coord.x == cur_coord.x ) { cur_coord = start_node_ptr->item.coordinate; dx = 0.0f; } // Reduce tx by one, since we have just added an offset path node (tx > 0) ? (--tx):(++tx); } // ====================== Repeat above for z and y deltas as well ====================== // ty += dy; if ( (ty >= 1.0f) || (ty <= -1.0f) ) { (ty > 0) ? (++cur_coord.y) : (--cur_coord.y); if ( end_coord == cur_coord ) { return true; } else { if ( (boundary_cube->get_spot(cur_coord)) || (SS_cube->datac(cur_coord) == 0) ) { return false; } tmp_node_ptr = path_ptr->getNewNode(); tmp_node_ptr->item.type= COORD_NODE; tmp_node_ptr->item.coordinate = cur_coord; path_ptr->insertAfter( start_node_ptr, tmp_node_ptr ); start_node_ptr = start_node_ptr->next; } if ( end_coord.y == cur_coord.y ) { cur_coord = start_node_ptr->item.coordinate; dy = 0.0f; } (ty > 0) ? (--ty):(++ty); } tz += dz; if ( (tz >= 1.0f) || (tz <= -1.0f) ) { (tz > 0) ? (++cur_coord.z) : (--cur_coord.z); if ( end_coord == cur_coord ) { return true; } else { if ( (boundary_cube->get_spot(cur_coord)) || (SS_cube->datac(cur_coord) == 0) ) { return false; } tmp_node_ptr = path_ptr->getNewNode(); tmp_node_ptr->item.type= COORD_NODE; tmp_node_ptr->item.coordinate = cur_coord; path_ptr->insertAfter( start_node_ptr, tmp_node_ptr ); start_node_ptr = start_node_ptr->next; } if ( end_coord.z == cur_coord.z ) { cur_coord = start_node_ptr->item.coordinate; dz = 0.0f; } (tz > 0) ? (--tz):(++tz); } } return true; } // ---------------------------------------------------------------------------------------- // linkPointToPoint // // Description - Creates a link of face-connected path nodes from 'start_node_ptr' to 'start_node_ptr->next'. // If 'path_link' is true, will stop linking as soon as another medial path is encountered. // // Returns - Final node that was linked. // ---------------------------------------------------------------------------------------- LinkedListNode<PathNode>* MA_VoxelCoding::linkPointToPoint( LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr, ma_link_type link_type/*=NORMAL*/ ) { LinkedListNode<PathNode> *end_node_ptr = start_node_ptr->next; // Increment recursion ++ current_recursion; #if MA_DO_FULL_TRACE // Increment indentation if ( current_recursion < (MAX_LOG_ENTRY_LENGTH/2 - 50) ) { trace_text_buffer[current_recursion-1] = ' '; trace_text_buffer[current_recursion] = ' '; trace_text_buffer[current_recursion+1] = 0; } #endif // ----------------- ATTEMPT DIRECT LINEAR LINK ----------------- // if ( attemptDirectLinearLink(path_ptr, start_node_ptr) ) { #if MA_DO_FULL_TRACE // Decrement recursion and indentation if ( current_recursion < (MAX_LOG_ENTRY_LENGTH/2 - 50) ) { trace_text_buffer[current_recursion-1] = 0; } #endif // Direct link was a success, we are done with this link -- current_recursion; return end_node_ptr; } // ------------ NEED TO PERFORM RECURSIVE SHORTEST PATH LINK ------------ // LinkedListNode<PathNode> *cur_node_ptr, *prev_node_ptr; LinkedListNode<Vect3usi> *cur_coord_node_ptr; LinkedListNode<LinkedList<Vect3usi>> *cur_max_list_ptr; bool reverse_link; Vect3usi start_coord, end_coord, cur_coord, best_option_coord; Vect3usi medial_coord, optimal_coord; ss_t ss_start_pt_restore_value; bs_t larger_bs_val; int min_dist_to_end, cur_dist_to_end, num_of_options; int i, sp_clusters_skipped; unsigned int closest_distance; // We want the smaller BS coord to be start point, unless link_type is not normal // Mark end spot and grab start point if ( ( (link_type == TO_PATH) || (BS_cube->datac(start_node_ptr->item.coordinate) >= BS_cube->datac(end_node_ptr->item.coordinate)) ) && (link_type != FROM_PATH) ) { // Keep original order larger_bs_val = BS_cube->datac(start_node_ptr->item.coordinate); reverse_link = false; start_coord = end_node_ptr->item.coordinate; end_coord = start_node_ptr->item.coordinate; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Start from %v to %v, type %d normal, larger bs %d", trace_text_buffer, &start_coord, &end_coord, link_type, larger_bs_val ); } #endif SS_cube->datac( end_coord, SS_Xfrm->getEndValueForSS() ); } else { larger_bs_val = BS_cube->datac(end_node_ptr->item.coordinate); reverse_link = true; start_coord = start_node_ptr->item.coordinate; end_coord = end_node_ptr->item.coordinate; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Start from %v to %v, type %d reverse, larger bs %d", trace_text_buffer, &start_coord, &end_coord, link_type, larger_bs_val ); } #endif SS_cube->datac( end_coord, SS_Xfrm->getEndValueForSS() ); } // Mark end point and perform SS xfrm ss_start_pt_restore_value = SS_cube->datac(start_coord); SS_cube->datac( end_coord, ss_medial_path-1 ); SS_Xfrm->changeEndValueForSS( ss_medial_path-1 ); if ( SS_Xfrm->runSS(SS_cube, start_coord, DT_FACE_ONLY|DT_STOP_AT_HIT) != DT_STOPPED_AT_HIT ) { // Something happened during transform and we never reached goal coordinate write_to_log( "MA_VoxelCoding::linkPointToPoint - Could not link point %v to %v of path %d (volume %d). Path length was %d.", &start_coord, &end_coord, current_medial_path_num, current_volume_num, path_ptr->getSize() ); #if DO_A_FULL_DUMP_WHEN_STUCK doFullDump( false ); #endif path_ptr->removeAllNodes(); // Erase SS field (must manually erase final spot since it was never reached by the transform) SS_Xfrm->eraseSS( SS_cube, start_coord, ss_medial_path-1, DT_FACE_ONLY ); SS_cube->datac( end_coord, SS_Xfrm->getInfinityValue() ); ++ number_of_failed_ma_links; -- current_recursion; return NULL; } // Do shortest path end coordinate to start coordinate if ( reverse_link ) { end_coord = start_node_ptr->item.coordinate; doSingleShortestPath( path_ptr, end_node_ptr, true ); prev_node_ptr = end_node_ptr; cur_node_ptr = end_node_ptr->prev; } else { end_coord = end_node_ptr->item.coordinate; doSingleShortestPath( path_ptr, start_node_ptr, false ); prev_node_ptr = start_node_ptr; cur_node_ptr = start_node_ptr->next; } start_coord = end_coord; cur_coord = cur_node_ptr->item.coordinate; // Find max points for each of the SS clusters clusters_max_points_lists->removeAllNodes(); while ( cur_node_ptr->item.coordinate != end_coord ) { cur_max_list_ptr = clusters_max_points_lists->getNewNode(); // Make sure there are no leftover nodes from previous calls cur_max_list_ptr->item.removeAllNodes(); findLocalMaximumsInCluster( cur_node_ptr->item.coordinate, &cur_max_list_ptr->item ); clusters_max_points_lists->insertAtEnd( cur_max_list_ptr ); #if TRACE_MAX_POINT_DETAILS if ( full_trace_enabled ) { LinkedListNode<Vect3usi> *new_nodes = cur_max_list_ptr->item.getHeadPtr(); write_to_log( "%sTRACE (linkPointToPoint) - Shortest path %v cluster has the following max points:", trace_text_buffer, &cur_node_ptr->item.coordinate ); while ( new_nodes ) { write_to_log( "%sTRACE (linkPointToPoint) - => %v", trace_text_buffer, &new_nodes->item ); new_nodes = new_nodes->next; } } #endif cur_node_ptr = ( reverse_link ) ? ( cur_node_ptr->prev ) : ( cur_node_ptr->next ); } // Restore starting point cur_node_ptr = ( reverse_link ) ? ( end_node_ptr->prev ) : ( start_node_ptr->next ); // Center each of the new nodes coordinates with respect to previous nodes coordinates while ( cur_coord != end_coord ) { sp_clusters_skipped = 0; // Find next ideal medial point medial_coord = prev_node_ptr->item.coordinate; closest_distance = 0xFFFFFFFF; // Add all max points to single list, to find best candidate for link cur_max_list_ptr = clusters_max_points_lists->getHeadPtr(); i = 0; while( (i < MA_LINK_LOOK_AHEAD) && (cur_max_list_ptr) ) { cur_coord_node_ptr = cur_max_list_ptr->item.getHeadPtr(); while( cur_coord_node_ptr ) { max_points_list->insertAtEnd( cur_coord_node_ptr->item ); cur_coord_node_ptr = cur_coord_node_ptr->next; } cur_max_list_ptr = cur_max_list_ptr->next; ++ i; } // Keep coordinate that is closest to previous and final out of all num_of_options = findNearestCoordinateInList( medial_coord, max_points_list, optimal_coord); if ( num_of_options > 0 ) { min_dist_to_end = 0x7FFFFFFF; cur_coord_node_ptr = max_points_list->getHeadPtr(); do { cur_dist_to_end = cur_coord_node_ptr->item.get_dist_2_to( end_coord ); if ( cur_dist_to_end < min_dist_to_end ) { min_dist_to_end = cur_dist_to_end; optimal_coord = cur_coord_node_ptr->item; } cur_coord_node_ptr = cur_coord_node_ptr->next; -- num_of_options; } while ( num_of_options > 0 ); } // Figure out which cluster final coordinate belongs to for ( i = 0; i < MA_LINK_LOOK_AHEAD; i++ ) { if ( SS_cube->datac(cur_node_ptr->item.coordinate) == SS_cube->datac(optimal_coord) ) { // Found correct SS cluster cur_coord = cur_node_ptr->item.coordinate; cur_node_ptr->item.coordinate = optimal_coord; break; } ++ sp_clusters_skipped; cur_node_ptr = (reverse_link) ? (cur_node_ptr->prev) : (cur_node_ptr->next); } if ( i == MA_LINK_LOOK_AHEAD ) { exit (0); } // No longer need the max points. max_points_list->removeAllNodes( ); #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Skipped %d SP points, final sp %v, and %v center picked.", trace_text_buffer, sp_clusters_skipped, cur_coord, &(cur_node_ptr->item.coordinate) ); } #endif cur_coord = cur_node_ptr->item.coordinate; #if ENABLE_FORCE_LINEAR_LINK // We do not want to have huge jumps to possibly noisy BS points when simply linking two // points already part of media axis. If one of the following is true, do a linear // step towards goal coordinate: // 1) Distance from previous to current is greater than previous to end. if ( (current_recursion > (MA_FORCE_DIRECT_LINK_AT_RECURSION/6)) && ((prev_node_ptr->item.coordinate.get_dist_man_to(end_coord)) <= (prev_node_ptr->item.coordinate.get_dist_man_to(cur_coord)) ) ) { int nearest_to_start = 0x7FFFFFFF, cur_nearest; cur_node_ptr = (reverse_link) ? (prev_node_ptr->prev) : (prev_node_ptr->next); cur_coord = prev_node_ptr->item.coordinate; unsigned short SS_code = SS_cube->datac(cur_coord) - 1; for (char j = 0; j < 6; j++) { if ( get_zyx_neigh_6dir(j, cur_coord, SS_cube->dimensions) ) { cur_nearest = cur_coord.get_dist_2_to(end_coord); if ((SS_cube->datac(cur_coord) == SS_code) && (cur_nearest < nearest_to_start)) { nearest_to_start = cur_nearest; cur_node_ptr->item.coordinate = cur_coord; } } } // Indicate that no clusters were skipped sp_clusters_skipped = 0; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Center resulted in too large of a jump, linear link to %v.", trace_text_buffer, &(cur_node_ptr->item.coordinate) ); } #endif } #endif // Now remove max points lists for skipped clusters for ( i = 0; i <= sp_clusters_skipped; i++ ) { clusters_max_points_lists->removeHeadNode(); } // If link_type is not normal, see if we have encountered an existing medial path if ( (link_type != NORMAL) && (hasMedialAxisNeighbors(cur_node_ptr->item.coordinate, prev_node_ptr->item.coordinate)) ) { moveOffOfMedialAxis( cur_node_ptr->item.coordinate ); // Need to remove all previous nodes if list is reversed if ( reverse_link ) { path_ptr->removeAllNodesBetween( cur_node_ptr, prev_node_ptr ); path_ptr->removeAllNodesBefore( cur_node_ptr ); start_node_ptr = cur_node_ptr; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Hit a path, removed all nodes before %v", trace_text_buffer, &(start_node_ptr->item.coordinate) ); } #endif } // If the path is ahead, simply remove rest of list else { path_ptr->removeAllNodesBetween( prev_node_ptr, cur_node_ptr ); path_ptr->removeAllNodesAfter( cur_node_ptr ); end_node_ptr = cur_node_ptr; #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (linkPointToPoint) - Hit a path, removed all nodes after %v", trace_text_buffer, &(end_node_ptr->item.coordinate) ); } #endif } end_coord = cur_node_ptr->item.coordinate; } else { // Go on to next path node if ( reverse_link ) { path_ptr->removeAllNodesBetween( cur_node_ptr, prev_node_ptr ); prev_node_ptr = cur_node_ptr; cur_node_ptr = cur_node_ptr->prev; } else { path_ptr->removeAllNodesBetween( prev_node_ptr, cur_node_ptr ); prev_node_ptr = cur_node_ptr; cur_node_ptr = cur_node_ptr->next; } } cur_coord = cur_node_ptr->item.coordinate; } // Delete current sub SS field (make sure medial axis is not deleted) SS_Xfrm->eraseSS( SS_cube, start_coord, ss_medial_path-1, DT_FACE_ONLY ); SS_cube->datac( start_coord, ss_start_pt_restore_value ); // Recursively link any additional gaps between new path nodes cur_node_ptr = start_node_ptr; while ( (cur_node_ptr->next) && (cur_node_ptr->item.coordinate != end_node_ptr->item.coordinate) ) { // Link with next spot cur_node_ptr = linkPointToPoint( path_ptr, cur_node_ptr, link_type ); if ( cur_node_ptr == NULL ) { end_node_ptr = NULL; break; } if ( (link_type == TO_PATH) && (cur_node_ptr->next == NULL) ) { end_node_ptr = cur_node_ptr; } } #if MA_DO_FULL_TRACE // Decrement recursion and indentation if ( current_recursion < (MAX_LOG_ENTRY_LENGTH/2 - 50) ) { trace_text_buffer[current_recursion-1] = 0; } #endif // Decrement recursion -- current_recursion; return end_node_ptr; } // ---------------------------------------------------------------------------------------- // linkPointToPath // // Description - This will linked 'start_node_ptr' to first existing path // that is already marked in SS_cube with 'ss_medial_path' value. // If 'link_with_previous' is true, link order will be: // start_node_ptr->prev = link to existing_path // Otherwise // start_node_ptr->next = link to existing_path // ---------------------------------------------------------------------------------------- LinkedListNode<PathNode>* MA_VoxelCoding::linkPointToPath( LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr, bool link_with_previous ) { LinkedListNode<PathNode> *new_node_ptr; // Do distance transform from starting node, returned value will be location of first medial axis encountered Vect3usi end_coord = start_node_ptr->item.coordinate; Vect3usi tmp_coord, start_coord; SS_Xfrm->changeEndValueForSS( ss_medial_path ); if ( SS_Xfrm->runSS(SS_cube, end_coord, DT_FACE_ONLY|DT_STOP_AT_HIT) != DT_STOPPED_AT_HIT ) { // Something happened during transform and we never reached goal coordinate write_to_log( "MA_VoxelCoding::linkPointToPath - Warning: could not link point %v (of path %d, volume %d) to existing path. Length was %d.", &end_coord, current_medial_path_num, current_volume_num, path_ptr->getSize() ); #if DO_A_FULL_DUMP_WHEN_STUCK doFullDump( false ); #endif SS_Xfrm->eraseSS( SS_cube, end_coord, ss_medial_path-1, DT_FACE_ONLY ); ++ number_of_failed_ma_path_links; return start_node_ptr; } #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { if ( link_with_previous ) { write_to_log( "%sTRACE (linkPointToPath) - Linking path at point %v to %v", trace_text_buffer, &(start_node_ptr->item.coordinate), &end_coord ); } else { write_to_log( "%sTRACE (linkPointToPath) - Linking %v to path at point %v", trace_text_buffer, &(start_node_ptr->item.coordinate), &end_coord ); } } #endif // Attempt to optimize positioning on the axis tmp_coord = end_coord; moveOffOfMedialAxis( end_coord ); // Delete current sub SS field (make sure medial axis is not deleted) SS_Xfrm->eraseSS( SS_cube, end_coord, ss_medial_path-1, DT_FACE_ONLY ); if ( end_coord == start_node_ptr->item.coordinate ) { // No need to connect anything, we are next to path return start_node_ptr; } else { // Create another SS field, to determine where we can connect from start_coord = end_coord; SS_cube->datac( start_node_ptr->item.coordinate, ss_medial_path-1 ); SS_Xfrm->changeEndValueForSS( ss_medial_path-1 ); if ( SS_Xfrm->runSS(SS_cube, start_coord, DT_FACE_ONLY|DT_STOP_AT_HIT) != DT_STOPPED_AT_HIT ) { write_to_log( "MA_VoxelCoding::linkPointToPath - ERROR: second SS stopped at %v.", &start_coord ); doFullDump( true ); } // Find optimal position findMaxBSInMedialPath( tmp_coord ); if ( SS_cube->datac(tmp_coord) < ss_medial_path ) { end_coord = tmp_coord; } // Erase SS SS_Xfrm->eraseSS( SS_cube, start_coord, ss_medial_path-1, DT_FACE_ONLY ); SS_cube->datac( start_node_ptr->item.coordinate, SS_Xfrm->getInfinityValue() ); } // We now know where to link to. new_node_ptr = path_ptr->getNewNode(); new_node_ptr->item.type= COORD_NODE; new_node_ptr->item.coordinate = end_coord; // See if we are inserting before or after start_node if ( link_with_previous ) { path_ptr->insertBefore( start_node_ptr, new_node_ptr ); start_node_ptr = new_node_ptr; return linkPointToPoint( path_ptr, start_node_ptr, FROM_PATH ); } else { path_ptr->insertAfter( start_node_ptr, new_node_ptr ); return linkPointToPoint( path_ptr, start_node_ptr, TO_PATH ); } } // ---------------------------------------------------------------------------------------- // findMaxBSInMedialPath // // Description - Travels down both ends, until a max point is found or a path branch. // Then picks the coordinate closer to 'other_coord' // ---------------------------------------------------------------------------------------- void MA_VoxelCoding::findMaxBSInMedialPath( Vect3usi &path_coord ) { bool is_first_try = true; char xl, xg, yl, yg, zl, zg, dz, dy, dx; char number_of_neighbors; Vect3usi cur_coord; Vect3usi nxt_coord, tmp_coord, max_coord, pass_end_coord[2]; float distance_traveled, last_max_bs_div_face2; // Insert a mark node (queue might have something from creating a wall) coordinate_queue->insertAtStart( queue_mark_coord ); // Do two passes (one in each dirction) for ( int i=0; i < 2; i++) { cur_coord = path_coord; max_coord = cur_coord; distance_traveled = 0.0f; last_max_bs_div_face2 = (float)BS_cube->datac(max_coord)/BS_Xfrm->getFaceValue(); last_max_bs_div_face2 = last_max_bs_div_face2*last_max_bs_div_face2; while ( 1 ) { // Mark current coordinate SS_cube->datac( cur_coord, SS_Xfrm->getInfinityValue() ); coordinate_queue->insertAtStart( cur_coord ); // Boundary check xl=-1; xg=1; yl=-1; yg=1; zl=-1; zg=1; if (cur_coord.x < 1) xl = 0; if (cur_coord.y < 1) yl = 0; if (cur_coord.z < 1) zl = 0; if (cur_coord.x > boundary_cube->wX-2) xg = 0; if (cur_coord.y > boundary_cube->wY-2) yg = 0; if (cur_coord.z > boundary_cube->wZ-2) zg = 0; // Find number of neighbors, and next one number_of_neighbors = 0; for (dz = zl; dz <= zg; dz++) { for (dy = yl; dy <= yg; dy++) for (dx = xl; dx <= xg; dx++) { tmp_coord.from_zyx( cur_coord.z+dz, cur_coord.y+dy, cur_coord.x+dx ); // Call work function if ( SS_cube->datac(tmp_coord) == ss_medial_path ) { ++number_of_neighbors; if ( BS_cube->datac(max_coord) < BS_cube->datac(tmp_coord) ) { distance_traveled = 0.0f; max_coord = tmp_coord; last_max_bs_div_face2 = (float)BS_cube->datac(max_coord)/BS_Xfrm->getFaceValue(); last_max_bs_div_face2 = last_max_bs_div_face2*last_max_bs_div_face2; } nxt_coord = tmp_coord; } } } // Stop going down the path if one of the following criteria are met // 1) There are no neighbors // 2) There is more than one neighbor // 3) We have traveled further than last previous max BS's value if ( (number_of_neighbors == 0) || ((!is_first_try) && ((number_of_neighbors > 1) || ((distance_traveled*distance_traveled) > last_max_bs_div_face2))) ) { // If more than one neighbor, keep that one if ( number_of_neighbors > 1 ) { pass_end_coord[i] = cur_coord; } // If there are no neighbors, we pick the max else { pass_end_coord[i] = max_coord; } break; } is_first_try = false; distance_traveled = distance_traveled + (float)cur_coord.get_dist_2_to( nxt_coord ); cur_coord = nxt_coord; } } // Keep the one with higher BS path_coord = (BS_cube->datac(pass_end_coord[0]) > BS_cube->datac(pass_end_coord[1])) ? (pass_end_coord[0]) : (pass_end_coord[1]); moveOffOfMedialAxis( path_coord ); #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (findMaxBSInMedialPath) - Two points found %v (BS %d) and %v (BS %d), picked %v", trace_text_buffer, &pass_end_coord[0], BS_cube->datac(pass_end_coord[0]), &pass_end_coord[1], BS_cube->datac(pass_end_coord[1]), &path_coord ); } #endif // Restore coordinates tmp_coord = coordinate_queue->getHeadPtr()->item; while ( tmp_coord != queue_mark_coord ) { SS_cube->datac( tmp_coord, ss_medial_path ); coordinate_queue->removeHeadNode(); tmp_coord = coordinate_queue->getHeadPtr()->item; } coordinate_queue->removeHeadNode(); } // ---------------------------------------------------------------------------------------- // doSingleShortestPath // // Description - // ---------------------------------------------------------------------------------------- void MA_VoxelCoding::doSingleShortestPath( LinkedList<PathNode> *path_ptr, LinkedListNode<PathNode> *start_node_ptr, bool do_in_reverse ) { LinkedListNode<PathNode> *new_node_ptr; Vect3usi cur_coord = start_node_ptr->item.coordinate; Vect3usi nxt_coord = cur_coord; Vect3usi end_coord; char i; ss_t ss_minimum; if ( do_in_reverse ) { end_coord = start_node_ptr->prev->item.coordinate; } else { end_coord = start_node_ptr->next->item.coordinate; } // Travel towards center of SS transform until end coordinate is reached while ( cur_coord != end_coord) { // Find minimum unmarked neighbor ss_minimum = SS_Xfrm->getInfinityValue(); for (i = 0; i < 7; i++) //7th time is to bring zyx back to original values { if ( get_zyx_neigh_6dir(i, cur_coord, SS_cube->dimensions) ) { // See if this spot is closer to center point if ( (!boundary_cube->get_spot(cur_coord)) && (SS_cube->datac(cur_coord) < ss_minimum) && (SS_cube->datac(cur_coord) > 0) ) { nxt_coord = cur_coord; ss_minimum = SS_cube->datac( nxt_coord ); } } } // If current coordinate is same as next, something went wrong if ( cur_coord == nxt_coord ) { write_to_log( "MA_VoxelCoding::doSingleShortestPath - FATAL ERROR - could not move closer to RP point %v from %v during path %d (volume %d) linking", &end_coord, &cur_coord, current_medial_path_num, current_volume_num ); write_to_log( "%s", ((do_in_reverse) ? ("in reverse") : ("normal")) ); markPathInSSCube( path_ptr, ss_medial_path - SS_Xfrm->getInfinityValue()/4 ); doFullDump( MA_CRASH_ON_FATAL_ERROR ); } #if MA_DO_FULL_TRACE if ( full_trace_enabled ) { write_to_log( "%sTRACE (doSingleShortestPath) - current SP point %v, next %v", trace_text_buffer, &cur_coord, &nxt_coord ); } #endif // Insert this coordinate into path list new_node_ptr = path_ptr->getNewNode(); new_node_ptr->item.type = COORD_NODE; new_node_ptr->item.coordinate = nxt_coord; cur_coord = nxt_coord; if ( do_in_reverse ) { path_ptr->insertBefore( start_node_ptr, new_node_ptr ); start_node_ptr = start_node_ptr->prev; } else { path_ptr->insertAfter( start_node_ptr, new_node_ptr ); start_node_ptr = start_node_ptr->next; } } // Remove last node (duplicate) path_ptr->removeNode( start_node_ptr ); }