bool ompl::geometric::BiTRRT::connectTrees(Motion* nmotion, TreeData& tree, Motion* xmotion)
{
// Get the nearest state to nmotion in tree (nmotion is NOT in tree)
Motion *nearest = tree->nearest(nmotion);
double dist = si_->distance(nearest->state, nmotion->state);
// Do not attempt a connection if the trees are far apart
if (dist > connectionRange_)
return false;
// Copy the resulting state into our scratch space
si_->copyState(xmotion->state, nmotion->state);
// Do not try to connect states directly. Must chop up the
// extension into segments, just in case one piece fails
// the transition test
GrowResult result;
Motion* next = NULL;
do
{
// Extend tree from nearest toward xmotion
// Store the result into next
// This function MAY trash xmotion
result = extendTree(nearest, tree, xmotion, next);
if (result == ADVANCED)
{
nearest = next;
// xmotion may get trashed during extension, so we reload it here
si_->copyState(xmotion->state, nmotion->state); // xmotion may get trashed during extension, so we reload it here
}
} while (result == ADVANCED);
// Successful connection
if (result == SUCCESS)
{
bool treeIsStart = tree == tStart_;
Motion* startMotion = treeIsStart ? next : nmotion;
Motion* goalMotion = treeIsStart ? nmotion : next;
// Make sure start-goal pair is valid
if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// Since we have connected, nmotion->state and next->state have the same value
// We need to check one of their parents to avoid a duplicate state in the solution path
// One of these must be true, since we do not ever attempt to connect start and goal directly.
if (startMotion->parent)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair(startMotion, goalMotion);
return true;
}
}
return false;
}
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion)
{
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion);
/* assume we can reach the state we go towards */
bool reach = true;
/* find state to add */
base::State *dstate = rmotion->state;
double d = si_->distance(nmotion->state, rmotion->state);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
dstate = tgi.xstate;
reach = false;
}
// if we are in the start tree, we just check the motion like we normally do;
// if we are in the goal tree, we need to check the motion in reverse, but checkMotion() assumes the first state it
// receives as argument is valid,
// so we check that one first
bool validMotion = tgi.start ?
si_->checkMotion(nmotion->state, dstate) :
si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
if (validMotion)
{
/* create a motion */
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
tree->add(motion);
if (reach)
return REACHED;
else
return ADVANCED;
}
else
return TRAPPED;
}
void ompl::geometric::TRRTConnect::freeTreeMemory(TreeData &tree) {
//Delete all motions, states and the nearest neighbors data structure
if (tree) {
std::vector<Motion*> motions;
tree->list(motions);
for (std::size_t i(0); i < motions.size(); ++i) {
if (motions[i]->state) si_->freeState(motions[i]->state);
delete motions[i];
}
}
}
void ompl::geometric::TRRTConnect::clearTree(TreeData &tree) {
if (tree) tree->clear();
//Clear TRRTConnect specific variables
tree.numStatesFailed_ = 0;
tree.temp_ = initTemperature_;
tree.nonFrontierCount_ = 0;
tree.frontierCount_ = 0;
tree.costs_.clear();
tree.sortedCostIndices_.clear();
//If opt_ is of type FOSOptimizationObjective, there is a global zero-order box and
//then this parameter will be used and initially set to false
//If opt is of another type, this parameter is set to true to avoid its use
tree.stateInBoxZos_ = !dynamic_cast<ompl::base::FOSOptimizationObjective*>(opt_.get());
}
ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion* toMotion, TreeData& tree, Motion*& result)
{
// Nearest neighbor
Motion *nearest = tree->nearest(toMotion);
return extendTree(nearest, tree, toMotion, result);
}
double pfilter(Model & sim_model, Parameter & model_params, MCMCoptions & options, Particle &particles, Trajectory & output_traj, TimeSeriesData &epi_data, TreeData &tree_data, MultiTreeData &multitree_data) {
int thread_max = omp_get_max_threads();
gsl_rng** rngs = new gsl_rng*[thread_max];
for (int thread = 0; thread < thread_max; thread++) {
rngs[thread] = gsl_rng_alloc(gsl_rng_mt19937);
gsl_rng_set(rngs[thread], omp_get_thread_num() + thread);
}
double loglik = 0.0;
int num_groups = options.num_groups;
int num_particles = options.particles;
int init_seed = options.seed;
int total_dt = options.total_dt;
double sim_dt = options.sim_dt;
int total_steps = ceil((double)total_dt/(double)options.pfilter_every);
int add_dt = 0;
double ESS_threshold = options.pfilter_threshold*(double)num_particles;
Likelihood likelihood_calc;
// std::vector <Parameter> values;// (options.num_threads, model_params);
// for (int i=0; i!=options.num_threads; ++i) values.push_back(model_params);
// for (int i=0; i!=model_params.get_total_params(); ++i) values.push_back(model_params.get(i));
std::vector <std::vector<double> > values(options.num_threads, std::vector<double>(model_params.get_total_params(), 0.0));
for (int i=0; i!=options.num_threads; ++i) {
for (int j=0; j!=model_params.get_total_params(); ++j) {
values[i][j] = model_params.get(j);
}
}
// printf("Size of values = %d\n",values.size());
double reporting_rate = 1.0;
if (model_params.param_exists("reporting")) {
reporting_rate = model_params.get("reporting");
}
std::vector <std::string> param_names = model_params.get_names_vector();
std::vector <std::vector<std::string> > param_names_threads (options.num_threads);
if (model_params.param_exists("time_before_data")) {
add_dt = model_params.get("time_before_data");
}
if (options.save_traj) {
if (add_dt > 0) {
particles.start_particle_tracing(add_dt+total_dt, num_groups);
}
else if (add_dt < 0) {
particles.start_particle_tracing(add_dt+total_dt, num_groups);
total_steps = ceil((double)(total_dt+add_dt)/(double)options.pfilter_every);
}
else {
particles.start_particle_tracing(total_dt, num_groups);
}
}
std::vector <Model> models;
for (int i=0; i<options.num_threads; ++i) {
models.push_back(sim_model);
}
std::vector <int> add_dt_threads (options.num_threads, add_dt);
std::vector <int> start_dt_threads (options.num_threads, 0);
std::vector <int> end_dt_threads (options.num_threads, add_dt);
std::vector <double> dt_threads (options.num_threads, sim_dt);
std::vector <int> total_dt_threads(options.num_threads, total_dt);
std::vector <double> reporting_rate_threads(options.num_threads, reporting_rate);
std::vector <int> num_groups_threads(options.num_threads, num_groups);
// Simulate model and calculate likelihood assuming no observed data
if (model_params.param_exists("time_before_data")) {
if (add_dt > 0) {
omp_set_num_threads(options.num_threads);
// std::vector <Trajectory *> curr_trajs;
// for (int i=0; i!=num_particles; ++i) {
// curr_trajs.push_back(particles.get_traj(i));
// }
#pragma omp parallel for shared(particles, values) schedule(static,1)
for (int tn = 0; tn < thread_max; tn++) {
for (int i = tn; i < num_particles; i += thread_max) {
// Adjust length of trajectory
particles.get_traj(i)->resize(add_dt, num_groups);
models[tn].simulate(values[tn], param_names_threads[tn], particles.get_traj(i), 0, add_dt_threads[tn], dt_threads[tn], total_dt_threads[tn], rngs[tn]);
if (options.which_likelihood < 2) {
double w = likelihood_calc.binomial_lik(reporting_rate_threads[tn], particles.get_traj(i)->get_total_traj(), add_dt_threads[tn] + total_dt_threads[tn], 0, add_dt_threads[tn], num_groups_threads[tn], false);
particles.set_weight(w, i, false);
}
if (options.save_traj) {
particles.save_traj_to_matrix(i, 0, add_dt);
particles.save_ancestry(i, 0, add_dt);
}
}
}
}
}
init_seed += num_particles;
int t=0;
int start_dt;
int end_dt;
for (t = 0; t != total_steps; ++t) {
// std::vector<double> we(options.particles, 0.0), wg(options.particles, 0.0);
start_dt = t*options.pfilter_every;
end_dt = std::min(total_dt, (t + 1)*options.pfilter_every);
std::fill(start_dt_threads.begin(), start_dt_threads.end(), start_dt);
std::fill(end_dt_threads.begin(), end_dt_threads.end(), end_dt);
omp_set_num_threads(options.num_threads);
#pragma omp parallel for shared (particles, values) schedule(static,1)
for (int tn = 0; tn < thread_max; tn++) {
for (int i = tn; i < num_particles; i+=thread_max) {
// Adjust length of trajectory
// if (tn==0) std::cout << i << ' ' << std::endl;
particles.get_traj(i)->resize(end_dt - start_dt, options.num_groups);
models[tn].simulate(values[tn], param_names_threads[tn], particles.get_traj(i), start_dt_threads[tn], end_dt_threads[tn], dt_threads[tn], total_dt_threads[tn], rngs[tn]);
double w = 1.0;
double temp = 0.0;
if (options.which_likelihood < 2) {
double A = particles.get_traj(i)->get_total_traj();
temp = likelihood_calc.binomial_lik(reporting_rate_threads[tn], A, epi_data.get_data_ptr(0), add_dt_threads[tn] + total_dt_threads[tn], start_dt_threads[tn], end_dt_threads[tn], add_dt_threads[tn], num_groups_threads[tn], false);
w *= temp;
// we[i] = log(temp);
}
if (options.which_likelihood != 1) {
temp = likelihood_calc.coalescent_lik(particles.get_traj(i)->get_traj_ptr(0, 0), particles.get_traj(i)->get_traj_ptr(1, 0),
tree_data.get_binomial_ptr(0), tree_data.get_interval_ptr(0), tree_data.get_ends_ptr(0),
start_dt_threads[tn], end_dt_threads[tn], add_dt_threads[tn], false);
w *= temp;
// wg[i] = log(temp);
}
particles.set_weight(w, i, true);
if (options.save_traj) {
particles.save_traj_to_matrix(i, start_dt_threads[tn] + add_dt_threads[tn], end_dt_threads[tn] + add_dt_threads[tn]);
particles.save_ancestry(i, start_dt_threads[tn] + add_dt_threads[tn], end_dt_threads[tn] + add_dt_threads[tn]);
}
}
}
// std::cout << "Epi Weight: " << std::accumulate(we.begin(), we.end(), 0.0) << " Gen Weight: " << std::accumulate(wg.begin(), wg.end(), 0.0) << " Total: " << particles.get_total_weight() << std::endl;
double curr_ESS = particles.get_ESS();
if (curr_ESS < ESS_threshold) {
double total_weight = particles.get_total_weight();
if (total_weight == 0.0) {
loglik += -0.1*std::numeric_limits<double>::max();
// std::cout << std::accumulate(epi_data.get_data_ptr(0)+start_dt, epi_data.get_data_ptr(0)+end_dt, 0.0) << " : " << particles.get_traj(0)->get_traj(0) << std::endl;
std::cout << "stop time: " << end_dt << std::endl;
break;
} else {
loglik += log(total_weight) - log(num_particles);
}
particles.resample(options.rng[0]);
}
else {
particles.reset_parents();
}
}
if (options.save_traj) {
output_traj.resize((total_dt+add_dt), num_groups);
//if (loglik > -0.1*std::numeric_limits<double>::max()) {
particles.retrace_traj(output_traj, options.rng[0]);
//}
}
for (int i=0; i!=num_particles; ++i) {
particles.get_traj(i)->reset();
}
std::vector < std::vector<double> >().swap(values);
for (int thread = 0; thread < thread_max; thread++) {
gsl_rng_free(rngs[thread]);
}
delete[] rngs;
return (loglik);
}
LRESULT MusicBrowserUI::TreeViewWndProc(HWND hwnd,
UINT msg,
WPARAM wParam,
LPARAM lParam)
{
WNDPROC lpOldProc = (WNDPROC)GetProp(hwnd, "oldproc" );
static bool dragging = false;
static RECT dragRect;
static HTREEITEM dragItem = NULL;
static bool selectedOnMouseDown = false;
//return CallWindowProc(lpOldProc, hwnd, msg, wParam, lParam );
switch(msg)
{
case WM_DESTROY:
{
// Put back old window proc and
SetWindowLong( hwnd, GWL_WNDPROC, (DWORD)lpOldProc );
// remove window property
RemoveProp( hwnd, "oldproc" );
RemoveProp( hwnd, "this" );
break;
}
case WM_MOUSEMOVE:
{
if(dragging)
{
if(wParam & MK_LBUTTON)
{
POINT dragPt;
dragPt.x = LOWORD(lParam);
dragPt.y = HIWORD(lParam);
if( !PtInRect(&dragRect, dragPt) )
{
SetCapture(NULL);
dragging = false;
NM_TREEVIEW nm_treeview;
memset(&nm_treeview, 0x00, sizeof(NM_TREEVIEW));
nm_treeview.hdr.code = TVN_BEGINDRAG;
nm_treeview.hdr.hwndFrom = hwnd;
nm_treeview.hdr.idFrom = GetWindowLong(hwnd, GWL_ID);
nm_treeview.itemNew.hItem = dragItem;
nm_treeview.itemNew.mask = TVIF_STATE|TVIF_PARAM;
nm_treeview.itemNew.stateMask = TVIS_BOLD|TVIS_CUT|TVIS_DROPHILITED|TVIS_EXPANDED|TVIS_SELECTED|TVIS_EXPANDEDONCE;
TreeView_GetItem(hwnd, &nm_treeview.itemNew);
nm_treeview.ptDrag.x = LOWORD(lParam);
nm_treeview.ptDrag.y = HIWORD(lParam);
SendMessage(GetParent(hwnd),
WM_NOTIFY,
(WPARAM)nm_treeview.hdr.idFrom,
(LPARAM)&nm_treeview);
return TRUE;
}
}
}
break;
}
case WM_RBUTTONDOWN:
{
SetFocus(hwnd);
HTREEITEM item;
TV_HITTESTINFO hti;
hti.pt.x = LOWORD(lParam);
hti.pt.y = HIWORD(lParam);
item = TreeView_HitTest(hwnd, &hti);
if(item && (hti.flags & TVHT_ONITEM))
{
HTREEITEM focusItem = TreeView_GetSelection(hwnd);
TV_ITEM tv_item;
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocusSelected = (tv_item.state & TVIS_SELECTED) != 0;
tv_item.hItem = item;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocus = item == focusItem;
bool wasSelected = (tv_item.state & TVIS_SELECTED) != 0;
if(!wasSelected)
{
// need to iterate all the items and
// make sure they aren't selected
HTREEITEM rootItem = TreeView_GetRoot(hwnd);
if(rootItem)
{
do
{
tv_item.hItem = rootItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = 0;
TreeView_SetBranch(hwnd, &tv_item);
}while(rootItem = TreeView_GetNextSibling(hwnd, rootItem));
}
// need to set this back cause windows won't
// set it if it is already the focus item and
// we just deselected it
if(wasSelected && wasFocus)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
}
TreeView_Select(hwnd, item, TVGN_CARET);
if(!wasFocus && wasFocusSelected && wasSelected)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
}
//return TRUE;
break;
}
case WM_RBUTTONUP:
{
break;
}
case WM_LBUTTONDBLCLK:
{
TV_ITEM tv_item;
TV_HITTESTINFO tv_htinfo;
KillTimer(hwnd, 1);
//GetCursorPos(&tv_htinfo.pt);
//ScreenToClient(m_hMusicView, &tv_htinfo.pt);
tv_htinfo.pt.x = LOWORD(lParam);
tv_htinfo.pt.y = HIWORD(lParam);
if(TreeView_HitTest(m_hMusicView, &tv_htinfo) &&
(tv_htinfo.flags & TVHT_ONITEM))
{
tv_item.hItem = TreeView_GetSelection(m_hMusicView);
tv_item.mask = TVIF_PARAM | TVIF_HANDLE;
tv_item.lParam = 0;
TreeView_GetItem(m_hMusicView, &tv_item);
TreeData* treedata = (TreeData*)tv_item.lParam;
bool playNow;
m_context->prefs->GetPrefBoolean(kPlayImmediatelyPref, &playNow);
if(treedata && treedata->IsTrack())
{
if(playNow && !m_pParent)
{
ClearPlaylistEvent();
}
PlaylistItem *item;
item = new PlaylistItem(*treedata->m_pTrack);
m_plm->AddItem(item, false);
}
else if(treedata && treedata->IsPlaylist())
{
if(playNow && !m_pParent)
{
ClearPlaylistEvent();
}
m_plm->ReadPlaylist(treedata->m_oPlaylistPath.c_str());
}
else if(treedata && treedata->IsPortable())
{
EditPortablePlaylist(treedata->m_pPortable);
}
else if(treedata && treedata->IsStream())
{
if(playNow && !m_pParent)
{
ClearPlaylistEvent();
}
PlaylistItem *item;
item = new PlaylistItem(*treedata->m_pStream);
m_plm->AddItem(item, false);
}
else if(tv_htinfo.hItem == m_hNewPlaylistItem)
{
NewPlaylist();
}
else if(tv_htinfo.hItem == m_hNewPortableItem)
{
m_context->target->AcceptEvent(new ShowPreferencesEvent(3));
}
else if(tv_htinfo.hItem == m_hNewFavoritesItem)
{
NewFavoriteEvent();
}
}
break;
}
case WM_LBUTTONDOWN:
{
bool shiftKeyPressed = IsShiftDown();
bool ctrlKeyPressed = IsCtrlDown();
selectedOnMouseDown = false;
SetFocus(hwnd);
HTREEITEM item;
TV_HITTESTINFO hti;
hti.pt.x = LOWORD(lParam);
hti.pt.y = HIWORD(lParam);
int dx = GetSystemMetrics(SM_CXDRAG);
int dy = GetSystemMetrics(SM_CYDRAG);
dragRect.top = hti.pt.y - dy;
dragRect.bottom = hti.pt.y + dy;
dragRect.left = hti.pt.x - dx;
dragRect.right = hti.pt.x + dx;
item = TreeView_HitTest(hwnd, &hti);
if(item && (hti.flags & TVHT_ONITEM))
{
dragItem = item;
HTREEITEM focusItem = TreeView_GetSelection(hwnd);
TV_ITEM tv_item;
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocusSelected = (tv_item.state & TVIS_SELECTED) != 0;
tv_item.hItem = item;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocus = item == focusItem;
bool wasSelected = (tv_item.state & TVIS_SELECTED) != 0;
HTREEITEM rootItem = TreeView_GetRoot(hwnd);
selectedOnMouseDown = wasSelected && (CountSelectedItems(rootItem) == 1);
if(ctrlKeyPressed)
{
/*TreeView_Select(hwnd, item, TVGN_CARET);
if(focusItem)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}*/
return TRUE;
}
else if(shiftKeyPressed)
{
// need to iterate all the items and
// make sure they aren't selected
HTREEITEM rootItem = TreeView_GetRoot(hwnd);
if(rootItem)
{
do
{
tv_item.hItem = rootItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = 0;
TreeView_SetBranch(hwnd, &tv_item);
}while(rootItem = TreeView_GetNextSibling(hwnd, rootItem));
}
HTREEITEM topItem = NULL;
HTREEITEM bottomItem = NULL;
HTREEITEM dummyItem = NULL;
dummyItem = focusItem;
// which item is above the other? search downward first
while(dummyItem = TreeView_GetNextSibling(hwnd, dummyItem))
{
if(dummyItem == item)
{
topItem = focusItem;
bottomItem = item;
break;
}
}
// did we find out? no? search upward next
if(!topItem)
{
dummyItem = focusItem;
// which item is above the other? search downward first
while(dummyItem = TreeView_GetPrevSibling(hwnd, dummyItem))
{
if(dummyItem == item)
{
topItem = item;
bottomItem = focusItem;
break;
}
}
}
// if they are not siblings then we do not support shift
// selection so just pass it on
if(topItem)
{
// need to iterate all the items and
// select them
rootItem = topItem;
do
{
tv_item.hItem = rootItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetBranch(hwnd, &tv_item);
}while(rootItem != bottomItem && (rootItem = TreeView_GetNextSibling(hwnd, rootItem)));
return TRUE;
}
break;
}
else
{
if(!wasSelected)
{
// need to iterate all the items and
// make sure they aren't selected
HTREEITEM rootItem = TreeView_GetRoot(hwnd);
if(rootItem)
{
do
{
tv_item.hItem = rootItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = 0;
TreeView_SetBranch(hwnd, &tv_item);
}while(rootItem = TreeView_GetNextSibling(hwnd, rootItem));
}
// need to set this back cause windows won't
// set it if it is already the focus item and
// we just deselected it
if(wasSelected && wasFocus)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
}
TreeView_Select(hwnd, item, TVGN_CARET);
if(!wasFocus && wasFocusSelected && wasSelected)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
}
SetCapture(hwnd);
dragging = true;
return TRUE;
}
break;
}
case WM_TIMER:
{
KillTimer(hwnd, 1);
EditItemLabel(hwnd, g_editItem);
break;
}
case WM_LBUTTONUP:
{
bool shiftKeyPressed = IsShiftDown();
bool ctrlKeyPressed = IsCtrlDown();
SetCapture(NULL);
dragging = false;
HTREEITEM item;
TV_HITTESTINFO hti;
hti.pt.x = LOWORD(lParam);
hti.pt.y = HIWORD(lParam);
item = TreeView_HitTest(hwnd, &hti);
if(item && (hti.flags & TVHT_ONITEM))
{
HTREEITEM focusItem = TreeView_GetSelection(hwnd);
TV_ITEM tv_item;
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocusSelected = (tv_item.state & TVIS_SELECTED) != 0;
tv_item.hItem = item;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
TreeView_GetItem(hwnd, &tv_item);
bool wasFocus = item == focusItem;
bool wasSelected = (tv_item.state & TVIS_SELECTED) != 0;
if(ctrlKeyPressed)
{
TreeView_Select(hwnd, item, TVGN_CARET);
if(wasSelected)
{
tv_item.hItem = item;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = 0;
TreeView_SetItem(hwnd, &tv_item);
}
if(!wasFocus && wasFocusSelected || !wasSelected)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
return TRUE;
}
else if(!shiftKeyPressed)
{
// need to iterate all the items and
// make sure they aren't selected
HTREEITEM rootItem = TreeView_GetRoot(hwnd);
if(rootItem)
{
do
{
tv_item.hItem = rootItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = 0;
TreeView_SetBranch(hwnd, &tv_item);
}while(rootItem = TreeView_GetNextSibling(hwnd, rootItem));
}
// need to set this back cause windows won't
// set it if it is already the focus item and
// we just deselected it
if(wasSelected && wasFocus)
{
tv_item.hItem = focusItem;
tv_item.mask = TVIF_STATE;
tv_item.stateMask = TVIS_SELECTED;
tv_item.state = TVIS_SELECTED;
TreeView_SetItem(hwnd, &tv_item);
}
if(selectedOnMouseDown)
{
if(g_editItem != item)
{
// i should do this in the notify but it is ignoring me
if(item != m_hMyMusicItem &&
item != m_hPlaylistItem &&
item != m_hAllItem &&
item != m_hUncatItem &&
item != m_hNewPlaylistItem &&
item != m_hStreamsItem &&
item != m_hFavoritesItem &&
item != m_hNewFavoritesItem &&
item != m_hPortableItem &&
TreeView_GetParent(m_hMusicView, item) != m_hPortableItem &&
item != m_hCDItem &&
TreeView_GetParent(m_hMusicView, item) != m_hCDItem)
{
// pause a half sec so this does not
// look so jarring
//Sleep(500);
SetTimer(hwnd, 1, GetDoubleClickTime(), NULL);
g_editItem = item;
//EditItemLabel(hwnd, item);
}
}
else
{
g_editItem = NULL;
}
}
}
}
break;
}
case WM_SETFOCUS:
case WM_KILLFOCUS:
{
UpdateButtonStates();
InvalidateRect(hwnd, NULL, TRUE);
break;
}
}
// Pass all non-custom messages to old window proc
return CallWindowProc(lpOldProc, hwnd, msg, wParam, lParam );
}
void MusicBrowserUI::EditItemLabel(HWND hwnd, HTREEITEM item)
{
if (item == NULL)
return;
g_editItem = item;
bool isPlaylist = false;
TV_ITEM tv_item;
tv_item.hItem = item;
tv_item.mask = TVIF_PARAM;
TreeView_GetItem(hwnd, &tv_item);
TreeData* treedata = (TreeData*)tv_item.lParam;
if(treedata)
{
isPlaylist = treedata->IsPlaylist();
if(treedata->IsTrack())
{
TV_ITEM tv_item;
tv_item.mask = TVIF_HANDLE | TVIF_TEXT;
tv_item.hItem = item;
tv_item.pszText = (char*)treedata->m_pTrack->GetMetaData().Title().c_str();
tv_item.cchTextMax = strlen(tv_item.pszText);
TreeView_SetItem(hwnd, &tv_item);
}
}
SetFocus(hwnd);
HWND hwndEdit = TreeView_EditLabel(hwnd, item);
if(hwndEdit)
{
/*RECT rect;
GetWindowRect(hwndEdit, &rect);
MapWindowPoints(NULL, hwnd, (LPPOINT)&rect, 2);
MoveWindow(hwndEdit, rect.left, rect.top, (rect.right - rect.left)/2,
rect.bottom - rect.top, TRUE);*/
SetProp(hwndEdit,
"oldproc",
(HANDLE)GetWindowLong(hwndEdit, GWL_WNDPROC));
SetProp(hwndEdit,
"playlist",
(HANDLE)isPlaylist);
// Subclass the window so we can filter characters
SetWindowLong(hwndEdit,
GWL_WNDPROC,
(DWORD)::EditLabelWndProc);
}
}
bool ompl::geometric::TRRTConnect::connect(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion) {
++connectCount;
unsigned int steps = 0;
Motion *nmotion;
if (tree.stateInBoxZos_) {
//Among the nearest K nodes in the tree, find state with lowest cost to go
//The returned nmotion is collision-free
//If no collision-free motion is found, NULL is returned
nmotion = minCostNeighbor(tree,tgi,rmotion);
if (!nmotion) return false;
} else {
//Find nearest node in the tree
//The returned motion has not checked for collisions
//It never returns NULL
nmotion = tree->nearest(rmotion);
}
//State to add
base::State *dstate;
//Distance from near state to random state
double d(si_->distance(nmotion->state,rmotion->state));
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
if (!tree.stateInBoxZos_) {
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return false;
}
//Compute motion cost
base::Cost cost;
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
//Only add this motion to the tree if the transition test accepts it
//Temperature must not be updated
while (transitionTest(cost,d,tree,false)) {
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
//Add motion to the tree
tree->add(motion);
steps++;
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
//tree.stateInBoxZos_ = cost.v < DBL_EPSILON*std::min(d,maxDistance_);
tree.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
boxZosDistance(motion->state) < DBL_EPSILON;
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
++tree.frontierCount_;//Participates in the tree expansion
//If reached
if (dstate == rmotion->state) {
// std::cout << steps << " steps" << std::endl;
return true;
}
//Current near motion is the motion just added
nmotion = motion;
//Distance from near state to random state
d = si_->distance(nmotion->state,rmotion->state);
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_ ? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return false;
//Compute motion cost
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
}
// std::cout << steps << " steps" << std::endl;
return false;
}
ompl::geometric::TRRTConnect::ExtendResult
ompl::geometric::TRRTConnect::extend(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion) {
++extendCount;
Motion *nmotion;
/*if (tree.stateInBoxZos_) {
//Among the nearest K nodes in the tree, find state with lowest cost to go
//The returned nmotion is collision-free
//If no collision-free motion is found, NULL is returned
nmotion = minCostNeighbor(tree,tgi,rmotion);
if (!nmotion) return TRAPPED;
} else {*/
//Find nearest node in the tree
//The returned motion has not been checked for collisions
//It never returns NULL
nmotion = tree->nearest(rmotion);
//}
//State to add
base::State *dstate;
//Distance from near state to random state
double d(si_->distance(nmotion->state,rmotion->state));
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
//if (!tree.stateInBoxZos_) {
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_ ? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return TRAPPED;
//}
//Minimum Expansion Control
//A possible side effect may appear when the tree expansion towards
//unexplored regions remains slow, and the new nodes contribute
//only to refine already explored regions.
if (!minExpansionControl(d,tree)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Compute motion cost
base::Cost cost;
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
//Only add this motion to the tree if the transition test accepts it
//Temperature must be updated
if (!transitionTest(cost,d,tree,true)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
//Add motion to the tree
tree->add(motion);
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
tree.stateInBoxZos_ = cost.value() < DBL_EPSILON*std::min(d,maxDistance_);
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
if (d > frontierThreshold_) {//Participates in the tree expansion
++tree.frontierCount_;
} else {//Participates in the tree refinement
++tree.nonFrontierCount_;
}
return (d > maxDistance_) ? ADVANCED : REACHED;
}
