/*protected*/
std::auto_ptr<BoundableList>
AbstractSTRtree::createParentBoundables(BoundableList* childBoundables,
int newLevel)
{
assert(!childBoundables->empty());
std::auto_ptr< BoundableList > parentBoundables ( new BoundableList() );
parentBoundables->push_back(createNode(newLevel));
std::auto_ptr< BoundableList > sortedChildBoundables ( sortBoundables(childBoundables) );
for (BoundableList::iterator i=sortedChildBoundables->begin(),
e=sortedChildBoundables->end();
i!=e; i++)
//for(std::size_t i=0, scbsize=sortedChildBoundables->size(); i<scbsize; ++i)
{
Boundable *childBoundable=*i; // (*sortedChildBoundables)[i];
AbstractNode *last = lastNode(parentBoundables.get());
if (last->getChildBoundables()->size() == nodeCapacity)
{
last=createNode(newLevel);
parentBoundables->push_back(last);
}
last->addChildBoundable(childBoundable);
}
return parentBoundables;
}
void Simulator::simulate(void){
AbstractNode * node;
cout << graph.getTitle() << "\n";
node = graph.getStartNode().first;
cout << "Starting the simulation...\nThis is the starting node:\n";
cout << "Actor: " << static_cast<StartStopNode *>(node)->getActor()
<< endl;
cout << "Message: " << static_cast<StartStopNode *>(node)->getMessage()
<< endl;
cout << "Moving on to first task...\n";
node = graph.getNextNode().first;
if(node == NULL){
cout << "There was an error somewhere... Exiting...\n";
exit(1);
}
while(node->getTraverseType() != STOP){
askIfCompleted(static_cast<Task *>(node));
node = graph.getNextNode().first;
if(node == NULL){
cout << "There was an error somewhere... Exiting...\n";
exit(1);
}
}
cout << "Actor: " << static_cast<Task *>(node)->getActor() << "\n";
cout << "This is the end of the simulation... Exiting..." << "\n";
}
/*protected*/
std::auto_ptr<BoundableList>
SIRtree::createParentBoundables(BoundableList *childBoundables,int newLevel)
{
assert(!childBoundables->empty());
std::auto_ptr<BoundableList> parentBoundables ( new BoundableList() );
parentBoundables->push_back(createNode(newLevel));
std::auto_ptr<BoundableList> sortedChildBoundables ( sortBoundables(childBoundables) );
//for(unsigned int i=0;i<sortedChildBoundables->size();i++)
for (BoundableList::iterator i=sortedChildBoundables->begin(),
e=sortedChildBoundables->end();
i!=e; ++i)
{
//Boundable *childBoundable=(AbstractNode*)(*sortedChildBoundables)[i];
Boundable *childBoundable=*i;
AbstractNode* lNode = lastNode(parentBoundables.get());
if (lNode->getChildBoundables()->size() == nodeCapacity)
{
parentBoundables->push_back(createNode(newLevel));
}
lNode->addChildBoundable(childBoundable);
}
return parentBoundables;
}
AbstractNode *NodeFactory::createOpenBracket()
{
AbstractNode *an = new AbstractNode();
Data *tmp = new Operator();
tmp->setOperator('(');
tmp->setPriority(3);
an->setData(tmp);
return an;
}
AbstractNode *NodeFactory::createAdd()
{
AbstractNode *an = new AbstractNode();
Data *tmp = new Operator();
tmp->setOperator('+');
tmp->setPriority(1);
an->setData(tmp);
return an;
}
bool AbstractNode::isDisjunctionOfComplexConjunctions()
{
if(AbstractNode::TYPE_OPERATION_OR != getType())
return false; //not a disjunction
int numOperations = 0;
ChainIterator<AbstractNode*> *iterator = children->getIterator();
while(true == iterator->hasNext()) {
AbstractNode *child = iterator->next(); //Get an element which might be conjunction
//Disjunction contains elements which are not parts of conjunction
if(!((AbstractNode::TYPE_VARIABLE == child->getType()) ||
(AbstractNode::TYPE_OPERATION_AND == child->getType()) ||
(AbstractNode::TYPE_OPERATION_NOT == child->getType()))) {
return false;
}
if(AbstractNode::TYPE_OPERATION_AND == child->getType()) {
if(child->getChildren()->getSize() > 1)
numOperations++;
}
if(AbstractNode::TYPE_OPERATION_NOT == child->getType()) {
AbstractNode *grandChild = child->getChildren()->getFirstElement();
if(AbstractNode::TYPE_VARIABLE != grandChild->getType()) {
return false; //Contains complex NOT operation
}
}
}
return (numOperations > 0);
}
AbstractNode *NodeFactory::createOperand(std::string &token)
{
AbstractNode *an = new AbstractNode();
Data *tmp = new Operand();
tmp->setOperandValue(atof(token.c_str()));
tmp->setOperand(token);
tmp->setPriority(4);
an->setData(tmp);
return an;
}
unsigned long AttrMap::length() const
{
unsigned long result = 0;
AbstractNode* pAttr = _pElement->_pFirstAttr;
while (pAttr)
{
pAttr = static_cast<AbstractNode*>(pAttr->nextSibling());
++result;
}
return result;
}
AbstractContainerNode::~AbstractContainerNode()
{
AbstractNode* pChild = static_cast<AbstractNode*>(_pFirstChild);
while (pChild)
{
AbstractNode* pDelNode = pChild;
pChild = pChild->_pNext;
pDelNode->_pNext = 0;
pDelNode->_pParent = 0;
pDelNode->release();
}
}
void AbstractNode::cloneChildren(AbstractNode *dest)
{
ChainIterator<AbstractNode*> *iterator = children->getIterator();
int i = 1;
while(true == iterator->hasNext()) {
AbstractNode *child = iterator->next();
AbstractNode *clonedChild = child->clone();
dest->addChild(clonedChild);
i++;
}
delete iterator;
}
void BalanceGraph::calculateDownForces()
{
for(Segment* segment : segments) {
AbstractNode* head = segment->nodes.first();
if (head->getPredecessors().isEmpty()) {
segment->dForce = 0;
} else {
qreal sum = 0;
for(AbstractNode* pred : head->getPredecessors()) {
sum += pred->getOutport().x() - head->getInport().x();
}
segment->dForce = (double) (sum / head->getPredecessors().size());
}
}
}
/*protected*/
void
AbstractSTRtree::query(const void* searchBounds, const AbstractNode& node,
ItemVisitor& visitor)
{
const BoundableList& boundables = *(node.getChildBoundables());
for (BoundableList::const_iterator i=boundables.begin(), e=boundables.end();
i!=e; i++)
{
const Boundable* childBoundable = *i;
if (!getIntersectsOp()->intersects(childBoundable->getBounds(), searchBounds)) {
continue;
}
if(const AbstractNode *an=dynamic_cast<const AbstractNode*>(childBoundable))
{
query(searchBounds, *an, visitor);
}
else if (const ItemBoundable *ib=dynamic_cast<const ItemBoundable *>(childBoundable))
{
visitor.visitItem(ib->getItem());
}
else
{
assert(0); // unsupported childBoundable type
}
}
}
/* private */
bool
AbstractSTRtree::remove(const void* searchBounds, AbstractNode& node, void* item)
{
// first try removing item from this node
if ( removeItem(node, item) ) return true;
BoundableList& boundables = *(node.getChildBoundables());
// next try removing item from lower nodes
for (BoundableList::iterator i=boundables.begin(), e=boundables.end();
i!=e; i++)
{
Boundable* childBoundable = *i;
if (!getIntersectsOp()->intersects(childBoundable->getBounds(), searchBounds))
continue;
if (AbstractNode *an=dynamic_cast<AbstractNode*>(childBoundable))
{
// if found, record child for pruning and exit
if ( remove(searchBounds, *an, item) )
{
if (an->getChildBoundables()->empty()) {
boundables.erase(i);
}
return true;
}
}
}
return false;
}
bool AbstractNode::isSingleVariable(){
if(!(AbstractNode::TYPE_VARIABLE == getType() ||
AbstractNode::TYPE_OPERATION_NOT == getType()))
return false;
AbstractNode *var = NULL;
if(AbstractNode::TYPE_VARIABLE == getType()) {
var = this;
} else if(AbstractNode::TYPE_OPERATION_NOT == getType()) {
var = children->getFirstElement();
}
if(AbstractNode::TYPE_VARIABLE == var->getType())
return true;
return false;
}
/**
* \brief Determines the neighbor that is closest to the random node, sets its predecessor
* to the current node, and adds it to the list of explored nodes.
* \param[in] currentNode The current node.
* \param[in] neighbors The list of neighbors of the current node.
* \param[in] randomNode The randomly drawn node.
* \param[in,out] list The list of already expanded nodes.
*/
void RRT::addNearestNeighbor(GridNode* const currentNode, const std::vector<AbstractNode*>& neighbors,
GridNode* const randomNode, std::vector<AbstractNode*>& list) const {
/* TODO: Determine the neighbor that is closest to the random node, set its predecessor
* to the current node, and add it to the list of explored nodes.
*/
/* Available methods and fields:
* - node->setPredecessor(AbstractNode* node): store the predecessor node for a node (required
* later for extracting the path)
* - getClosestNodeInList(node, list): Defined above
*/
int min_cost = INT_MAX;
AbstractNode* minNode = NULL;
std::vector<AbstractNode *>::const_iterator it;
it = neighbors.begin();
for (;it != neighbors.end(); it++ )
{
double cost = distance(*it,randomNode);
if (cost < min_cost)
{
minNode = *it;
min_cost = cost;
}
}
minNode->setPredecessor(currentNode) ;
list.push_back(minNode);
// int min_cost = INT_MAX;
// AbstractNode* minNode = NULL;
// std::vector<AbstractNode*>::iterator it;
// for(it=neighbors.begin() ; it < neighbors.end(); it++)
// {
// double cost = distance(*it,randomNode);
// if (cost < min_cost)
// {
// minNode = *it;
// min_cost = cost;
// }
// }
// minNode->predecessor = currentNode;
// list.push_back(minNode);
}
void BalanceGraph::calculateUpForces()
{
for(Segment* segment : segments) {
AbstractNode* tail = segment->nodes.last();
if (tail->getSuccessors().isEmpty()) {
segment->dForce = 0;
} else {
qreal sum = 0;
for(AbstractNode* succ : tail->getSuccessors()) {
sum += succ->getInport().x() - tail->getOutport().x();
}
segment->dForce = (double) (sum / tail->getSuccessors().size());
}
}
}
void NodeVisitor::visitChildren_via_iter(AbstractNode& node)
{
IIterator* i = node.createIterator();
for (i->First(); !i->IsDone(); i->Next()) {
i->CurrentItem()->accept(*this);
}
delete i;
}
void AssignLayers::run(Graph &graph)
{
emit setStatusMsg("Assigning layers...");
// copy the nodes to a linked list
QLinkedList<AbstractNode*> vertices;
for(AbstractNode* v : graph.getNodes()) {
vertices.append(v);
}
QSet<AbstractNode*> U;
QSet<AbstractNode*> Z;
QList<QList<AbstractNode*>> layers;
//add the first layer
int currentLayer = 0;
layers.append(QList<AbstractNode*>());
while(!vertices.isEmpty()) {
AbstractNode* selected = nullptr;
for(AbstractNode* v : vertices) {
if(Z.contains(v->getPredecessors().toSet())) {
selected = v;
break;
}
}
if(selected != nullptr) {
selected->setLayer(currentLayer);
layers.last().append(selected);
U.insert(selected);
vertices.removeOne(selected);
} else {
currentLayer++;
layers.append(QList<AbstractNode*>());
Z.unite(U);
}
}
graph.setLayers(layers);
graph.repaintLayers();
emit setStatusMsg("Assigning layers... Done!");
}
void NodeAppender::appendChild(Node* newChild)
{
poco_check_ptr (newChild);
poco_assert (_pLast == 0 || _pLast->_pNext == 0);
if (static_cast<AbstractNode*>(newChild)->_pOwner != _pParent->_pOwner)
throw DOMException(DOMException::WRONG_DOCUMENT_ERR);
if (newChild->nodeType() == Node::DOCUMENT_FRAGMENT_NODE)
{
AbstractContainerNode* pFrag = static_cast<AbstractContainerNode*>(newChild);
AbstractNode* pChild = pFrag->_pFirstChild;
if (pChild)
{
if (_pLast)
_pLast->_pNext = pChild;
else
_pParent->_pFirstChild = pChild;
while (pChild)
{
_pLast = pChild;
pChild->_pParent = _pParent;
pChild = pChild->_pNext;
}
pFrag->_pFirstChild = 0;
}
}
else
{
AbstractNode* pAN = static_cast<AbstractNode*>(newChild);
pAN->duplicate();
if (pAN->_pParent)
pAN->_pParent->removeChild(pAN);
pAN->_pParent = _pParent;
if (_pLast)
_pLast->_pNext = pAN;
else
_pParent->_pFirstChild = pAN;
_pLast = pAN;
}
}
bool XmlSettingsEntry::remove( QStringList &xpath )
{
QStringList::ConstIterator itXPath;
AbstractNode *node = NULL;
QString lastElem(xpath.last()); /* last element from xpath */
xpath.removeLast();
XmlSettingsEntry entry( find(xpath, itXPath, node) );
if( itXPath != xpath.end() ) {
return false;
}
JQ_ASSERT(node);
if( node->type() == AbstractNode::Map ) {
MapNode *mapNode = dynamic_cast<MapNode*>(node);
MapNode::iterator it = mapNode->find( lastElem );
if( it == mapNode->end() )
return false;
mapNode->erase( it );
m_master->m_modified = true;
return true;
}
if( node->type() == AbstractNode::Vector ) {
JQ_ASSERT( lastElem.startsWith("[") );
JQ_ASSERT( lastElem.endsWith("]") && lastElem.size() > 2 );
bool ok;
int index = lastElem.mid(1, lastElem.size()-2).toUInt(&ok);
JQ_ASSERT(ok == true);
VectorNode *vectorNode = dynamic_cast<VectorNode*>(node);
vectorNode->erase( vectorNode->begin() + index );
m_master->m_modified = true;
return true;
}
return false;
}
void BalanceGraph::createLinearSegment(AbstractNode *node)
{
// Create a segment
Segment* segment = new Segment();
segments.push_back(segment);
// Add the head of the chain
segment->nodes.push_back(node);
nodeToSegment.insert(node,segment);
AbstractNode* currentNode = node->getSuccessors().first();
// We are still within in the chain
while(currentNode->getPredecessors().size() == 1 && currentNode->getSuccessors().size() == 1) {
segment->nodes.push_back(currentNode);
nodeToSegment.insert(currentNode,segment);
currentNode = currentNode->getSuccessors().first();
}
// We are at the end of the chain now
// If we have many predecessors, let it be
if(currentNode->getPredecessors().size() > 1) {
return;
}
// if we have no successors, end of chain, add and return
if(currentNode->getSuccessors().isEmpty()) {
segment->nodes.push_back(currentNode);
nodeToSegment.insert(currentNode,segment);
}
}
void goToChild(AbstractNode* currentNode, AbstractNode* endNode)
{
AbstractNode* root = goBackToRoot(currentNode);
std::stack<AbstractNode*> path;
AbstractNode* node = endNode;
while(node != NULL)
{
path.push(node);
node = node->getParent();
}
while(!path.empty())
{
node = path.top();
path.pop();
node->onEnter();
}
}
RenderTree* RenderTreeLoader::CreateRenderTree(std::string& pathToFile)
{
RenderTree* renderTree = new RenderTree(0);
tinyxml2::XMLDocument document;
document.LoadFile(pathToFile.c_str());
tinyxml2::XMLNode* mainNode = document.FirstChild();
tinyxml2::XMLNode* child = mainNode->FirstChildElement();
while (child)
{
std::string nodeType = child->Value();
AbstractNode* newNode = NodeFactory::CreateNode(nodeType, renderTree);
float x, y, z;
tinyxml2::XMLElement* element = child->FirstChildElement("Position");
x = element->FloatAttribute("x");
y = element->FloatAttribute("y");
z = element->FloatAttribute("z");
newNode->Position(glm::vec3(x, y, z));
element = child->FirstChildElement("Rotation");
x = element->FloatAttribute("x");
y = element->FloatAttribute("y");
z = element->FloatAttribute("z");
newNode->Rotation(glm::vec3(x, y, z));
element = child->FirstChildElement("Scale");
x = element->FloatAttribute("x");
y = element->FloatAttribute("y");
z = element->FloatAttribute("z");
newNode->Scale(glm::vec3(x, y, z));
std::string model;
element = child->FirstChildElement("Model");
model = element->Attribute("Name");
newNode->Model(model);
renderTree->AddChild(newNode);
child = child->NextSibling();
}
return renderTree;
}
/**
* \brief Plans a path on a grid map using RRT.
* \param[in] startNode The start node of the path.
* \param[in] goalNode The goal node where the path should end up.
* \param[in] map The occupancy grid map of the environment.
* \param[in] maxIterations The maximum number of iterations.
* \return The planned path, or an empty path in case the algorithm exceeds the maximum number of iterations.
*/
std::deque<AbstractNode *> RRT::planPath(AbstractNode * const startNode, AbstractNode * const goalNode, const GridMap& map, const size_t& maxIterations) {
std::deque<AbstractNode *> result;
std::vector<AbstractNode *> startList;
std::vector<AbstractNode *> goalList;
// Add the start and goal nodes to the corresponding lists:
startList.push_back(startNode);
goalList.push_back(goalNode);
/* TODO: Expand trees from both the start node and the goal node at the same time
* until they meet. When extendClosestNode() returns a connection node, then call
* constructPath() to find the complete path and return it. */
int iter = 0;
while(iter < maxIterations)
{
AbstractNode * Qrand;
Qrand = getRandomNode(map,startList,goalList.back() );
AbstractNode * Qclosest= getClosestNodeInList(Qrand,startList);
AbstractNode * Qconnection = extendClosestNode(Qrand,Qclosest,startList,map,goalList);
if (Qconnection->getConnection() !=goalNode) {
startList.push_back(Qconnection);
startList.swap(goalList);
iter++;
}
else {
result = constructPath(Qconnection,startNode,goalNode);
return result;
}
}
return result;
}
/**
* \brief Tries to connect the two trees and returns the connection node.
* \param[in] currentNode The current node.
* \param[in] neighbors The list of neighbors of the current node.
* \param[in] otherList The list of already expanded nodes in the other tree.
* \return The neighbor node that connects both trees, or NULL if the trees cannot be connected.
*/
AbstractNode * RRT::tryToConnect(GridNode* const currentNode, const std::vector<AbstractNode*>& neighbors,
const std::vector<AbstractNode*>& otherList) const {
AbstractNode* connectionNode = NULL;
/* TODO: Check if one of the neighbors is already contained in the "otherList"
* (list of already expanded nodes in the other tree). If so, return that neighbor
* as the connection node and establish the connection with neighbor->setConnection(closestNode).
/* Available methods and fields:
* - node->setConnection(AbstractNode * connection): sets the other predecessor node of the
* current node (must be from the other list) (i.e. set connection between the two lists).
*/
std::vector<AbstractNode*>::const_iterator it;
for(it = neighbors.begin(); it!=neighbors.end();it++)
{
if (std::find(otherList.begin(), otherList.end(), *it) != otherList.end())
{
connectionNode = *it;
connectionNode->setConnection(currentNode);
}
}
return connectionNode;
}
bool AbstractNode::consistsFromSimpleElements()
{
ChainIterator<AbstractNode*> *iterator = children->getIterator();
while(true == iterator->hasNext()) {
AbstractNode *child = iterator->next();
if(AbstractNode::TYPE_VARIABLE == child->getType())
continue;
if(AbstractNode::TYPE_OPERATION_NOT == child->getType()) {
AbstractNode *notChild = child->getChildren()->getFirstElement();
if(AbstractNode::TYPE_VARIABLE == notChild->getType())
continue;
}
return false;
}
return true;
}
/*private*/
bool
AbstractSTRtree::removeItem(AbstractNode& node, void* item)
{
BoundableList& boundables = *(node.getChildBoundables());
BoundableList::iterator childToRemove = boundables.end();
for (BoundableList::iterator i=boundables.begin(),
e=boundables.end();
i!=e; i++)
{
Boundable* childBoundable = *i;
if (ItemBoundable *ib=dynamic_cast<ItemBoundable*>(childBoundable))
{
if ( ib->getItem() == item) childToRemove = i;
}
}
if (childToRemove != boundables.end()) {
boundables.erase(childToRemove);
return true;
}
return false;
}
void BalanceGraph::newProcessRegion(Region *region, Graph &graph)
{
// if we do not need to move the region
if(region->getDforce() == 0) {
return;
}
double minMovement = region->getDforce();
if(minMovement < 0) {
minMovement = (-1)*minMovement;
}
for(Segment* s : region->segments) {
for(AbstractNode* node : s->nodes) {
int layer = node->getLayer();
int positionInLayer = node->getPositionInLayer();
// attempting to move to the left
if(region->getDforce() < 0) {
// If we are the leftmost node
if(positionInLayer == 0) {
continue;
}
AbstractNode* leftNode = graph.getLayers().at(layer).at(positionInLayer-1);
// if leftNode is in the same region
if(nodeToSegment.value(leftNode)->region == s->region) {
continue;
}
double availableMovement = node->x() - (leftNode->x() + leftNode->boundingRect().width() + 10);
if(availableMovement < minMovement) {
minMovement = availableMovement;
}
}
// Attempting to move to the right
if(region->getDforce() > 0) {
// if we are the rightmost in layer
if(positionInLayer == graph.getLayers().at(layer).size()-1) {
continue;
}
AbstractNode* rightNode = graph.getLayers().at(layer).at(positionInLayer+1);
// if rightNode is in the same region
if(nodeToSegment.value(rightNode)->region == s->region) {
continue;
}
double availableMovement = rightNode->x() - (node->x() + node->boundingRect().width() + 10);
if(availableMovement < minMovement) {
minMovement = availableMovement;
}
}
}
} // Calculate the maximal movement
// move the whole region
for(Segment* s : region->segments) {
for(AbstractNode* n : s->nodes) {
// movement to the left
if(region->getDforce() < 0) {
n->moveBy(-minMovement,0);
} else {
n->moveBy(minMovement,0);
}
}
}
}
void NodeVisitor::visitChildren_via_getChild(AbstractNode& node)
{
for (int i = 0; i != node.childrenNumber(); ++i) {
node.getChild(i)->accept(*this);
}
}
Node* AttrMap::item(unsigned long index) const
{
AbstractNode* pAttr = _pElement->_pFirstAttr;
while (index-- > 0 && pAttr) pAttr = static_cast<AbstractNode*>(pAttr->nextSibling());
return pAttr;
}
