//gets the derivative of the jacobian dpi/dqj wrt qk, i.e. ddpi/dqjdqk
bool DynamicChain2D::GetJacobianDeriv(const Vector2& pi, int i, int j, int k, Real&ddtheta,Vector2& ddp) const
{
if(!IsAncestor(i,j)) return false;
if(!IsAncestor(i,k)) return false;
/*if(j==k) {
Vector3 p;
links[i].T_World.mulPoint(pi,p);
//JPij = Rj0[wj]Rjloc*pj + vj
//dJPij/dj = Rj0[wj][wj]Rjloc*pj
//in 2D we might as well just cross product this thing with wj
Real JOij; Vector2 JPij;
links[j].GetJacobian(q[j],p,JOij,JPij);
ddp = cross(JPij,links[j].w);
ddtheta=Zero;
}*/
if(IsAncestor(k,j)) Swap(k,j); //order it so k<j<=i
//Get j's Jacobian then transform to k, and modify
Frame2D JPij,JPkk;
Matrix2 R0_k;
links[j].GetJacobian(q[j],links[i].T_World,JPij);
links[k].GetJacobian(q[k],links[k].T_World,JPkk);
R0_k.setTranspose(links[k].T_World.R);
Matrix2 RA1B = JPkk.R*R0_k;
Frame2D J;
J.R = RA1B*JPij.R;
J.t = RA1B*JPij.t;
ddp = J*pi;
ddtheta=Zero;
return true;
}
//---------------------------------------------------------------------------
void __fastcall TRightsFrame::CMCancelMode(TCMCancelMode & Message)
{
if (FPopup && Visible && !FPopingContextMenu &&
((Message.Sender == NULL) ||
(!IsAncestor(Message.Sender, this) &&
!IsAncestor(Message.Sender, FPopupParent) &&
(Message.Sender != this))))
{
CloseUp();
}
TFrame::Dispatch(&Message);
}
void UpdateVisualBodyTransforms(RobotState* state)
{
if (state->dirty_visuals_joint != NULL) {
std::vector<const Joint*> q;
if (state->dirty_links_joint != NULL) {
// supertree or subtree => update
if (IsAncestor(state->model, state->dirty_visuals_joint, state->dirty_links_joint) |
IsAncestor(state->model, state->dirty_links_joint, state->dirty_visuals_joint))
{
UpdateLinkTransforms(state, &q);
}
// ...otherwise, sibling tree
}
UpdateOnlyVisualBodyTransforms(state, &q);
}
}
//----------------------------------------------------------------------------------------------
bool ObjectSerializer::IsAncestor(const string& candidateAncestor, const string& candidateChild)
{
vector<TypeNode*>& parents = m_typeTable[candidateChild].Parents;
bool foundParent = false;
for (int pIdx = 0, pSize = parents.size(); pIdx < pSize; ++pIdx)
{
if (parents[pIdx]->FullName() == candidateAncestor)
{
foundParent = true;
break;
}
}
// Base Case: the candidate ancestor is a direct parent
if (foundParent)
{
return true;
}
// Inductive Case: the candidate ancestor is ancestor to one of my parents
else
{
for (int i = 0, size = parents.size(); i < size; ++i)
{
if (IsAncestor(candidateAncestor, parents[i]->FullName()))
return true;
}
}
return false;
}
//---------------------------------------------------------------------------
void __fastcall TRightsFrame::DoExit()
{
// this is bit hack to make frame hide when ComboEdit button is pressed while
// from is poped-up already.
if (FPopup && !IsAncestor(Screen->ActiveControl, FPopupParent))
{
DoCloseUp();
}
TFrame::DoExit();
}
//----------------------------------------------------------------------------------------------
void ObjectSerializer::PerformLateBinding( UserObject* p_object, TypeNode*& p_type )
{
string candidateChild = g_ObjectFactory.FromCName(p_object->CName());
string candidateAncestor = p_type->FullName();
if (candidateChild != candidateAncestor &&
IsAncestor(candidateAncestor, candidateChild))
{
p_type = m_typeTable[candidateChild].TypeGraph;
}
}
//----------------------------------------------------------------------------------------------
void ObjectSerializer::PerformLateBinding(ISerializable* pObj, TypeNode*& pType)
{
auto objLayout = pObj->GetObjectLayout();
string candidateChild = g_ObjectFactory.FromCName(objLayout.CName());
string candidateAncestor = pType->FullName();
if (candidateChild != candidateAncestor &&
IsAncestor(candidateAncestor, candidateChild))
{
pType = m_typeTable[candidateChild].TypeGraph;
}
}
static void MoveExpression(BLOCK *b, QUAD *head, BLOCK *pbl, BLOCK *pbr)
{
if (IsAncestor(pbl, b))
{
if (pbr)
{
if (IsAncestor(pbr, b))
{
if (pbr->preWalk > pbl->preWalk)
{
MoveTo(pbr, b, head);
}
else
{
MoveTo(pbl, b, head);
}
}
}
else if (pbl->preWalk < b->preWalk)
{
MoveTo(pbl, b, head);
}
}
}
void CEditTreeCtrl::DragMoveItem(HTREEITEM hDrag, HTREEITEM hDrop, EDropHint eHint, bool bCopy) {
if(eHint == DROP_NODROP)
return;
if(IsAncestor(hDrag, hDrop) || !CanDropItem(hDrag, hDrop, eHint))
return;
ASSERT(hDrag != 0);
ASSERT(hDrop != 0 || eHint == DROP_BELOW);
if(!hDrop && eHint == DROP_BELOW)
for(hDrop = GetRootItem(); GetNextSiblingItem(hDrop) != 0; hDrop = GetNextSiblingItem(hDrop));
// Setup insertion parameters
HTREEITEM hInsertAfter = 0;
HTREEITEM hParent = 0;
switch(eHint) {
case DROP_BELOW:
hInsertAfter = hDrop;
hParent = GetParentItem(hDrop);
break;
case DROP_ABOVE:
hInsertAfter = GetPrevSiblingItem(hDrop);
if(!hInsertAfter)
hInsertAfter = TVI_FIRST;
hParent = GetParentItem(hDrop);
break;
case DROP_CHILD:
hInsertAfter = TVI_LAST;
hParent = hDrop;
break;
default:
ASSERT(false);
break;
}
HTREEITEM hNew = CopyItem(hDrag, hParent, hInsertAfter);
SelectItem(hNew);
// If the control-key ist down, we copy the data, otherwise we move
// it, thus we have to delete the dragged item.
if(!bCopy)
DeleteItem(hDrag);
}
HTREEITEM CEditTreeCtrl::GetDropTarget(EDropHint & hint) {
ASSERT(m_pDragData != 0);
CPoint pt;
GetCursorPos(&pt);
ScreenToClient(&pt);
UINT flags;
HTREEITEM hDrop = HitTest(pt, &flags);
hint = GetDropHint(flags);
m_pDragData->SetDropTarget(hDrop);
if(hDrop) {
m_pDragData->DragLeave(); // allow updates
SelectDropTarget(hDrop);
if(m_pDragData->CheckExpand(hDrop))
Expand(hDrop, TVE_EXPAND);
// Make sure the surrounding items are visible, too
// This will scroll the tree if necessary.
HTREEITEM hPrev = GetPrevVisibleItem(hDrop);
if(hPrev)
EnsureVisible(hPrev);
HTREEITEM hNext = GetNextVisibleItem(hDrop);
if(hNext)
EnsureVisible(hNext);
// if the drop target is a descendent of the dragged item, then
// disallow dropping the item here...
if(IsAncestor(m_pDragData->GetDragItem(), hDrop) || !CanDropItem(m_pDragData->GetDragItem(), hDrop, hint))
hint = DROP_NODROP;
m_pDragData->DragEnter(pt);
} else if(hint != DROP_NODROP && !CanDropItem(m_pDragData->GetDragItem(), hDrop, hint))
hint = DROP_NODROP;
return hDrop;
}
bool
SingleWindow::FilterMouseEvent(RasterPoint pt,
Window *allowed) const
{
const ContainerWindow *container = this;
while (true) {
const Window *child =
const_cast<ContainerWindow *>(container)->EventChildAt(pt.x, pt.y);
if (child == nullptr)
/* no receiver for the event */
return false;
if (child == allowed)
/* the event reaches an allowed window: success */
return true;
const ContainerWindow *next = IsAncestor(allowed, child);
if (next == nullptr)
return false;
container = next;
}
}
bool
SingleWindow::FilterMouseEvent(PixelScalar x, PixelScalar y,
Window *allowed) const
{
const ContainerWindow *container = this;
while (true) {
const Window *child =
const_cast<ContainerWindow *>(container)->event_child_at(x, y);
if (child == NULL)
/* no receiver for the event */
return false;
if (child == allowed)
/* the event reaches an allowed window: success */
return true;
const ContainerWindow *next = IsAncestor(allowed, child);
if (next == NULL)
return false;
container = next;
}
}
bool IsLinkTransformDirty(const RobotState* state, const Link* link)
{
return state->dirty_links_joint != NULL &&
IsAncestor(state->model, state->dirty_links_joint, link->parent);
}
//
// FindLoops
//
// Find loops and build loop forest using Havlak's algorithm, which
// is derived from Tarjan. Variable names and step numbering has
// been chosen to be identical to the nomenclature in Havlak's
// paper (which is similar to the one used by Tarjan).
//
void FindLoops() {
if (!CFG_->GetStartBasicBlock()) return;
int size = CFG_->GetNumNodes();
IntSetVector non_back_preds(size);
IntListVector back_preds(size);
IntVector header(size);
CharVector type(size);
IntVector last(size);
NodeVector nodes(size);
BasicBlockMap number;
// Step a:
// - initialize all nodes as unvisited.
// - depth-first traversal and numbering.
// - unreached BB's are marked as dead.
//
for (MaoCFG::NodeMap::iterator bb_iter =
CFG_->GetBasicBlocks()->begin();
bb_iter != CFG_->GetBasicBlocks()->end(); ++bb_iter) {
number[(*bb_iter).second] = kUnvisited;
}
DFS(CFG_->GetStartBasicBlock(), &nodes, &number, &last, 0);
// Step b:
// - iterate over all nodes.
//
// A backedge comes from a descendant in the DFS tree, and non-backedges
// from non-descendants (following Tarjan).
//
// - check incoming edges 'v' and add them to either
// - the list of backedges (back_preds) or
// - the list of non-backedges (non_back_preds)
//
for (int w = 0; w < size; w++) {
header[w] = 0;
type[w] = BB_NONHEADER;
BasicBlock *node_w = nodes[w].bb();
if (!node_w) {
type[w] = BB_DEAD;
continue; // dead BB
}
if (node_w->GetNumPred()) {
for (BasicBlockIter inedges = node_w->in_edges()->begin();
inedges != node_w->in_edges()->end(); ++inedges) {
BasicBlock *node_v = *inedges;
int v = number[ node_v ];
if (v == kUnvisited) continue; // dead node
if (IsAncestor(w, v, &last))
back_preds[w].push_back(v);
else
non_back_preds[w].insert(v);
}
}
}
// Start node is root of all other loops.
header[0] = 0;
// Step c:
//
// The outer loop, unchanged from Tarjan. It does nothing except
// for those nodes which are the destinations of backedges.
// For a header node w, we chase backward from the sources of the
// backedges adding nodes to the set P, representing the body of
// the loop headed by w.
//
// By running through the nodes in reverse of the DFST preorder,
// we ensure that inner loop headers will be processed before the
// headers for surrounding loops.
//
for (int w = size-1; w >= 0; w--) {
NodeList node_pool; // this is 'P' in Havlak's paper
BasicBlock *node_w = nodes[w].bb();
if (!node_w) continue; // dead BB
// Step d:
IntList::iterator back_pred_iter = back_preds[w].begin();
IntList::iterator back_pred_end = back_preds[w].end();
for (; back_pred_iter != back_pred_end; back_pred_iter++) {
int v = *back_pred_iter;
if (v != w)
node_pool.push_back(nodes[v].FindSet());
else
type[w] = BB_SELF;
}
// Copy node_pool to worklist.
//
NodeList worklist;
NodeList::iterator niter = node_pool.begin();
NodeList::iterator nend = node_pool.end();
for (; niter != nend; ++niter)
worklist.push_back(*niter);
if (!node_pool.empty())
type[w] = BB_REDUCIBLE;
// work the list...
//
while (!worklist.empty()) {
UnionFindNode x = *worklist.front();
worklist.pop_front();
// Step e:
//
// Step e represents the main difference from Tarjan's method.
// Chasing upwards from the sources of a node w's backedges. If
// there is a node y' that is not a descendant of w, w is marked
// the header of an irreducible loop, there is another entry
// into this loop that avoids w.
//
// The algorithm has degenerated. Break and
// return in this case.
//
size_t non_back_size = non_back_preds[x.dfs_number()].size();
if (non_back_size > kMaxNonBackPreds) {
lsg_->KillAll();
return;
}
IntSet::iterator non_back_pred_iter =
non_back_preds[x.dfs_number()].begin();
IntSet::iterator non_back_pred_end =
non_back_preds[x.dfs_number()].end();
for (; non_back_pred_iter != non_back_pred_end; non_back_pred_iter++) {
UnionFindNode y = nodes[*non_back_pred_iter];
UnionFindNode *ydash = y.FindSet();
if (!IsAncestor(w, ydash->dfs_number(), &last)) {
type[w] = BB_IRREDUCIBLE;
non_back_preds[w].insert(ydash->dfs_number());
} else {
if (ydash->dfs_number() != w) {
NodeList::iterator nfind = find(node_pool.begin(),
node_pool.end(), ydash);
if (nfind == node_pool.end()) {
worklist.push_back(ydash);
node_pool.push_back(ydash);
}
}
}
}
}
// Collapse/Unionize nodes in a SCC to a single node
// For every SCC found, create a loop descriptor and link it in.
//
if (!node_pool.empty() || (type[w] == BB_SELF)) {
SimpleLoop* loop = lsg_->CreateNewLoop();
// At this point, one can set attributes to the loop, such as:
//
// the bottom node:
// IntList::iterator iter = back_preds[w].begin();
// loop bottom is: nodes[*backp_iter].node);
//
// the number of backedges:
// back_preds[w].size()
//
// whether this loop is reducible:
// type[w] != BB_IRREDUCIBLE
//
// TODO(rhundt): Define those interfaces in the Loop Forest.
//
nodes[w].set_loop(loop);
for (niter = node_pool.begin(); niter != node_pool.end(); niter++) {
UnionFindNode *node = (*niter);
// Add nodes to loop descriptor.
header[node->dfs_number()] = w;
node->Union(&nodes[w]);
// Nested loops are not added, but linked together.
if (node->loop())
node->loop()->set_parent(loop);
else
loop->AddNode(node->bb());
}
lsg_->AddLoop(loop);
} // node_pool.size
} // Step c
} // FindLoops
BOOL CALLBACK Main_DialogProc (HWND hDlg, UINT msg, WPARAM wp, LPARAM lp)
{
if (msg == WM_INITDIALOG)
{
g.hMain = hDlg;
AfsAppLib_SetMainWindow (g.hMain);
}
switch (msg)
{
case WM_INITDIALOG:
Main_OnInitDialog (hDlg);
break;
case WM_SHOW_YOURSELF:
if (lp || g.idCell)
{
ShowWindow (g.hMain, SW_SHOW);
SetForegroundWindow (g.hMain);
Actions_WindowToTop (TRUE);
}
break;
case WM_SHOW_ACTIONS:
if (gr.fShowActions)
Actions_OpenWindow();
break;
case WM_ACTIVATEAPP:
Actions_WindowToTop ((wp) ? TRUE : FALSE);
if (wp)
StartTask (taskEXPIRED_CREDS);
break;
case WM_EXPIRED_CREDENTIALS:
case WM_REFRESHED_CREDENTIALS:
g.hCreds = (UINT_PTR)lp;
StartTask (taskUPD_CREDS);
break;
case WM_SIZE:
// if (lp==0), we're minimizing--don't call ResizeWindow().
//
if (lp != 0)
ResizeWindow (hDlg, awdMain, rwaFixupGuts);
break;
case WM_ENDTASK:
LPTASKPACKET ptp;
if ((ptp = (LPTASKPACKET)lp) != NULL)
{
if (ptp->idTask == taskUPD_USERS)
Display_OnEndTask_UpdUsers (ptp);
else if (ptp->idTask == taskUPD_GROUPS)
Display_OnEndTask_UpdGroups (ptp);
else if (ptp->idTask == taskUPD_MACHINES)
Display_OnEndTask_UpdMachines (ptp);
FreeTaskPacket (ptp);
}
break;
case WM_ASC_NOTIFY_ACTION:
Actions_OnNotify (wp, lp);
break;
case WM_COMMAND:
switch (LOWORD(wp))
{
case IDCANCEL:
Quit (0);
break;
default:
OnContextCommand (LOWORD(wp));
break;
}
break;
case WM_NOTIFY:
switch (((LPNMHDR)lp)->code)
{
case TCN_SELCHANGE:
Main_PrepareTabChild();
break;
}
break;
case WM_HELP:
if ((lp == 0) || (IsAncestor (g.hMain, (HWND)(((LPHELPINFO)lp)->hItemHandle))))
{
WinHelp (hDlg, cszHELPFILENAME, HELP_FINDER, 0);
}
break;
}
return FALSE;
}
bool IsVisualBodyTransformDirty(const RobotState* state, const LinkVisual* visual)
{
return state->dirty_visuals_joint != NULL &&
IsAncestor(state->model, state->dirty_visuals_joint, visual->link->parent);
}
bool IsCollisionBodyTransformDirty(const RobotState* state, const LinkCollision* collision)
{
// TODO: review
return state->dirty_collisions_joint != NULL &&
IsAncestor(state->model, state->dirty_collisions_joint, collision->link->parent);
}
