void LKH::LKHAlg::AllocateSegments()
{
Segment *S = 0, *SPrev;
SSegment *SS = 0, *SSPrev;
int i;
FreeSegments();
#ifdef THREE_LEVEL_TREE
GroupSize = pow((double) Dimension, 1.0 / 3.0);
#elif defined TWO_LEVEL_TREE
GroupSize = sqrt((double) Dimension);
#else
GroupSize = Dimension;
#endif
Groups = 0;
for (i = Dimension, SPrev = 0; i > 0; i -= GroupSize, SPrev = S) {
assert(S = (Segment *) malloc(sizeof(Segment)));
S->Rank = ++Groups;
if (!SPrev)
FirstSegment = S;
else
SLink(SPrev, S);
}
SLink(S, FirstSegment);
#ifdef THREE_LEVEL_TREE
SGroupSize = sqrt((double) Groups);
#else
SGroupSize = Dimension;
#endif
SGroups = 0;
for (i = Groups, SSPrev = 0; i > 0; i -= SGroupSize, SSPrev = SS) {
SS = (SSegment *) malloc(sizeof(SSegment));
SS->Rank = ++SGroups;
if (!SSPrev)
FirstSSegment = SS;
else
SLink(SSPrev, SS);
}
SLink(SS, FirstSSegment);
}
void SimpleSkeletonGrower::backward_grow()
{
if(_surface.is_loaded())
{
std::vector <osg::Vec3> backward_grow = _surface._relative_pts;
//1. Initialization: setup a grow_map for book-keeping the backward growing
//Key: rank of the <x,z> coords; Value: <low depth(y), high depth(y), isDone(1 or -1)>
//this is used in addition to the relative bound for growing a dense foilage
std::map <int, osg::Vec3> grow_map;
for(unsigned int i=0; i<_surface._surface_pts.size(); i++)
{
osg::Vec3 pt = _surface._surface_pts[i];
//only consider points that lay inside the boundary
if(!_pruner.is_inside_ortho(pt))
continue;
int rank = (pt.x()+1000)*2500 + (pt.z()+1000);
//check if exists, update its value
if(grow_map.find(rank) != grow_map.end())
{
osg::Vec3 paras = grow_map[rank];
osg::Vec3 new_paras = paras;
if(pt.y() < paras.x())
new_paras.x() = pt.y();
if(pt.y() > paras.y())
new_paras.y() = pt.y();
grow_map[rank] = new_paras;
}
//new cloud point
else
{
osg::Vec3 paras(pt.y(), pt.y(), -1);//new point has the same-depth and is un-done
grow_map[rank] = paras;
}
}
float inter_cons = 2.0f;
float neighbor_dist = pow(inter_cons*1.05, 0.5) * _close_threshold;
float neighbor_diag = neighbor_dist * 0.707f;
//2. Loop each node pt in grow_map, un-rank it, and pretend to be a point in the relative bound
for(std::map <int, osg::Vec3>::iterator it=grow_map.begin(); it!=grow_map.end(); it++)
{
//a. recover point, the y-coord is randomly picked between low and high
int rank = it->first;
//osg::Vec3 paras = it->second;
//int y = rand()%(int(paras.y()-paras.x())) + paras.x();
//osg::Vec3 pt(rank/2500-1000, y, rank%2500-1000);
osg::Vec3 pt(rank/2500-1000, rank%2500-1000, 0.0f);
backward_grow.push_back(pt);
//add more neighbor points
osg::Vec3 probe;
if(false)
{
probe = osg::Vec3(pt.x()+neighbor_dist, pt.y(), 0.0f);//right
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x()-neighbor_dist, pt.y(), 0.0f);//left
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x(), pt.y()+neighbor_dist, 0.0f);//top
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x(), pt.y()-neighbor_dist, 0.0f);//bottom
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
}
probe = osg::Vec3(pt.x()+neighbor_diag, pt.y()+neighbor_diag, 0.0f);//top-right
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x()-neighbor_diag, pt.y()+neighbor_diag, 0.0f);//top-left
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x()+neighbor_diag, pt.y()-neighbor_diag, 0.0f);//bottom-right
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
probe = osg::Vec3(pt.x()-neighbor_diag, pt.y()-neighbor_diag, 0.0f);//bottom-left
if(!_pruner.is_inside_ortho(probe))
backward_grow.push_back(probe);
}
//given the non-even retaive contour points, backward grow it to the nearest tree nodes
//each each node cannot grow more than twice, also the newly added retaive points are
//required to be mutually separated from previously added points, in addition to the old tree nodes
std::vector <LibraryElement> elements = _library._library_element;
std::vector <osg::Vec3> newly_added;
std::map <BDLSkeletonNode *, int> grown_cnt;
//put inside the loop becasue trees is updated, put outside for non-continuous effect
std::vector <BDLSkeletonNode *> trees = bfs();
BDLSkeletonNode *copied_root = BDLSkeletonNode::copy_tree(_root);
for(unsigned int j=0; j<backward_grow.size(); j++)
{
osg::Vec3 temp = backward_grow[j];
osg::Vec3 pt(temp.x(), 0.0f, temp.y());//add depth later
//b. find min projected distance from tree
float min_proj_dist = -1.0f;
for(unsigned int i=0; i<trees.size(); i++)
{
osg::Vec3 T = Transformer::toVec3(trees[i]);
float proj_dist = osg::Vec2(pt.x()-T.x(), pt.z()-T.z()).length2();
if(min_proj_dist == -1.0f || proj_dist < min_proj_dist)
min_proj_dist = proj_dist;
}
//c. find min projected distance from previously added points
float min_proj_dist2 = -1.0f;
for(unsigned int i=0; i<newly_added.size(); i++)
{
osg::Vec3 na = newly_added[i];
float proj_dist = osg::Vec2(pt.x()-na.x(), pt.z()-na.z()).length2();
if(min_proj_dist2 == -1.0f || proj_dist < min_proj_dist2)
min_proj_dist2 = proj_dist;
}
//c. if minimum projected distance from trees T is larger than _close_threshold, then backward grow it
float close_threshold2 = _close_threshold * _close_threshold;
if(min_proj_dist > close_threshold2 && (min_proj_dist2 > inter_cons*close_threshold2 || newly_added.empty()))
{
//find the closest node N in trees T to grow
BDLSkeletonNode *N = NULL;
float min_tree_dist = -1.0f;
for(unsigned int i=0; i<trees.size(); i++)
{
BDLSkeletonNode *tnode = trees[i];
//ensure N has _prev
if(!tnode->_prev)
continue;
//ensure N has no child
//if(tnode->_children.size() > 1)
// continue;
//check if N has been used more than twice first
if(grown_cnt.find(tnode) != grown_cnt.end() && grown_cnt[tnode] > 0)
continue;
float cur_dist = (pt - Transformer::toVec3(tnode)).length2();
if(min_tree_dist == -1.0f || cur_dist < min_tree_dist)
{
min_tree_dist = cur_dist;
N = tnode;
}
}
//fuel for backward grow is used up
if(!N)
return;
//add depth of pt = 'depth of N' +/- 'half length of N and N->_prev'
float par_len = BDLSkeletonNode::dist(N, N->_prev);
if(par_len > 1.0f)
pt.y() = N->_sy + rand()%int(par_len) - par_len/2.0f;
else
pt.y() = N->_sy + (rand()%3-1)*par_len/2.0f;
//update book-keeping
newly_added.push_back(pt);
if(grown_cnt.find(N) != grown_cnt.end())
grown_cnt[N]++;
else
grown_cnt[N] = 1;
//shorten the N-pt segment by 10% to prevent unwanted pruning
osg::Vec3 wrap = Transformer::toVec3(N);
osg::Vec3 dir = pt - wrap;
float desired_len = dir.length() * 0.95f;
dir.normalize();
pt = wrap + dir * desired_len;
//new tip node
BDLSkeletonNode *new_node = new BDLSkeletonNode(pt.x(), pt.y(), pt.z());
//use advanced approach to grow it as in laser data
if(true)
{
//before attaching, check if the divergent angle is too large
float div_ang = Transformer::divergent_angle(N->_prev, N, new_node);
if(div_ang > 30.0f)
{
//osg::Vec3 translateN = Transformer::toVec3(N->_prev) - wrap;
//float len = translateN.length();
//translateN.normalize();
//translateN = wrap + translateN * len * 0.5f;
//N->_sx = wrap.x();
//N->_sy = wrap.y();
//N->_sz = wrap.z();
//if(N->_prev->_prev)
//{
// N = N->_prev;
// div_ang = Transformer::divergent_angle(N->_prev, N, new_node);
//}
//search for the most similar divergent angle in the original tree
BDLSkeletonNode *a, *b;
if(Transformer::similar_angle_in_tree(copied_root, div_ang, a, b))
{
//printf("angle(%f) div(%f)\n", angle, Transformer::divergent_angle(a->_prev, a, b));
//construct an elemnt out of a and b
BDLSkeletonNode *support_tip = NULL;
BDLSkeletonNode *sub_tree = Transformer::extract_subtree(a->_prev, b, support_tip);
if(sub_tree && support_tip)
{
//should not add
//new_node->_prev = N;
//new_node->_prev_support = N;
//N->_children.push_back(new_node);
N->_prev_support = N->_prev;
LibraryElement element(sub_tree, SLink(sub_tree, sub_tree->_children[0]));
//rotation can't be random!
int min_tran_ang = -1;
float min_tran_ang_dist = -1.0f;
for(int ta=0; ta<360; ta+=12)
{
LibraryElement element_test = element;
Transformer::transform(N, ta, element_test);
std::vector <BDLSkeletonNode *> compare = Transformer::terminal_nodes(element_test._root);
for(unsigned int ter=0; ter<compare.size(); ter++)
{
float cur_dist = BDLSkeletonNode::dist(compare[ter], new_node);
if(min_tran_ang_dist == -1.0f || cur_dist < min_tran_ang_dist)
{
min_tran_ang_dist = cur_dist;
min_tran_ang = ta;
}
}
}
//printf("min_tran_ang_dist(%f)\n", min_tran_ang_dist);
std::vector <BDLSkeletonNode *> subtree = replace_branch(N, element, min_tran_ang);
prune(subtree);
//printf("%f %f %f\n", pt.x(), pt.y(), pt.z());
//pt = Transformer::toVec3(N);
//printf("%f %f %f\n", pt.x(), pt.y(), pt.z());
if(false || j==617)
{
Transformer::straight_up(element);
char buffer[100];
sprintf(buffer, "/tmp/log_sk/log_skeleton_%d", j);
BDLSkeletonNode::save_skeleton(element._root, buffer);
}
}
else
printf("SimplSkeletonGrower::backward_grow():extract_subtree: a(%p) b(%p) error\n", a, b);
delete new_node;
}
else
{
//if no suitable sub-tree can be found, just don't grow the node
//printf("%f %f %f\n", pt.x(), pt.y(), pt.z());
delete new_node;
}
}
else
{
//directly attach it to produce a straight branch
new_node->_prev = N;
N->_children.push_back(new_node);
//too far away, replace it
//if(desired_len > _close_threshold * 2.0f)
//{
// new_node->_prev_support = N;
// std::vector <BDLSkeletonNode *> subtree = replace_branch(new_node, elements[rand()%elements.size()], rand()%360);
// //prune(subtree); //prune leniently
//}
}
}
//visualize pt
//printf("%f %f %f\n", pt.x(), pt.y(), pt.z());
}
//d. continue for the next point
} //end for loop
BDLSkeletonNode::delete_this(copied_root);
}
}
