//! Inserts a Node into the appropriate location
SearchSpaceNode * SearchSpace::insertNode(Point loc)
{
SearchSpaceNode *temp;
if(nodeExists(loc))
return NULL;
if (searchSpace == NULL ) // Constructing the ROOT NODE
{
temp = new SearchSpaceNode;
temp->location.setX(loc.x());
temp->location.setY(loc.y());
temp->obstacle_cost = 0;
temp->parent = NULL;
temp->next = NULL;
searchSpace = temp;
}
else
{
temp = new SearchSpaceNode;
temp->location.setX(loc.x());
temp->location.setY(loc.y());
temp->obstacle_cost = 0;
temp->parent = NULL;
temp->next = searchSpace;
searchSpace = temp;
}
return temp;
}
std::vector <GumboNode *> CQueryUtil::unionNodes(std::vector <GumboNode *> aNodes1,
std::vector <GumboNode *> aNodes2)
{
for (std::vector <GumboNode *>::iterator it = aNodes2.begin(); it != aNodes2.end(); it++) {
GumboNode *pNode = *it;
if (nodeExists(aNodes1, pNode)) {
continue;
}
aNodes1.push_back(pNode);
}
return aNodes1;
}
CDGNode *buildFeasiblePath(CDGNode * node, CDGNode * list) {
while (node && 0 == nodeExists(list, getID(node))) {
node = getNextNode(node);
}
if (NULL == node)
return NULL;
CDGNode *out = NULL;
out = copyToPathNode(newBlankNode(), node);
setTrueNodeSet(out, buildFeasiblePath(getTrueNodeSet(node), list));
setFalseNodeSet(out, buildFeasiblePath(getFalseNodeSet(node), list));
setNextNode(out, buildFeasiblePath(getNextNode(node), list));
return out;
}
int DeDup::deDuplicateSubBlocks(char* buffer, int curPointer, int inc, long unsigned int* segLength) {
int nextBlockPointer = curPointer + (inc * BLOCK_X);
int tempSeg = *segLength;
curPointer += inc;
tempSeg -= inc;
int blockCounter = 1;
while(curPointer != nextBlockPointer && BLOCK_S < tempSeg) {
Index subNode(blockCounter, (buffer + curPointer), BLOCK_S);
if(nodeExists(std::get<0>(subNode.getNode()))) {
*segLength = tempSeg;
return curPointer;
} else {
curPointer += inc;
tempSeg -= inc;
}
}
return 0;//TODO: return Error cases
}
int ZKWatcher::handleEvent(int event){
std::string e = toZkEventType(event);
if(event & m_event_mask){
addWatch(event);
}
switch(event){
case DELETE_EVENT:
if(!(event & m_event_mask)){
checkExists();
}
break;
case CREATE_EVENT:
if(!(event & m_event_mask)){
nodeExists();
}
break;
}
return 0;
}
SearchSpaceNode * SearchSpace::insertNode(geometry_msgs::Pose nodePose, geometry_msgs::PoseArray correspondingSensorsPoses, int id)
{
SearchSpaceNode *temp;
if(!nodeExists(nodePose))
{
// Constructing the ROOT NODE
if (searchspace == NULL )
{
temp = new SearchSpaceNode;
temp->location.position = nodePose.position;
temp->location.orientation = nodePose.orientation;
for(int i=0; i<correspondingSensorsPoses.poses.size(); i++)
{
temp->sensorLocation.poses.push_back(correspondingSensorsPoses.poses[i]);
}
temp->next = NULL;
temp->type = RegGridNode;
temp->id = id;
searchspace = temp;
}
else
{
temp = new SearchSpaceNode;
temp->location.position = nodePose.position;
temp->location.orientation = nodePose.orientation;
for(int i=0; i<correspondingSensorsPoses.poses.size(); i++)
{
temp->sensorLocation.poses.push_back(correspondingSensorsPoses.poses[i]);
}
temp->next = searchspace;
temp->type = RegGridNode;
temp->id = id;
searchspace = temp;
}
}
return temp;
}
unsigned long int DeDup::deDuplicate(char *buffer, char **outBuffer, unsigned long int seg_s ) {
unsigned int blockCounter = 1;
bool exists = false;
const int inc = BLOCK_S / BLOCK_X;
int bufferSize = 0;
char *tempBuff = NULL;// = new char[SEG_S];
unsigned long int segLength = seg_s;
unsigned long int curPointer = 0;
unsigned long int indexOffset = 0;
unsigned long int fileSize = 0;
if ((__tempBuffer__ = (char *)mmap ((caddr_t)0, SEG_S + 20, PROT_READ | PROT_WRITE,
MAP_SHARED|MAP_ANONYMOUS , -1, 0)) == (caddr_t) -1)
fprintf (stderr, "mmap error for deDupBuffer\n");
tempBuff = __tempBuffer__;
if (tempBuff) {
while(segLength > 0) {
if(BLOCK_S < segLength) {
if(!exists) {
bufferSize = deDuplicateSubBlocks(buffer, curPointer, inc, &segLength);
if(bufferSize) {
// Generate Index
Index::generateIndex(node.getParentIndex(),curPointer, 0, bufferSize - curPointer);
memcpy((tempBuff + fileSize), (buffer+curPointer), bufferSize - curPointer);
fileSize += (bufferSize - curPointer);
curPointer = bufferSize;
bufferSize = 0;
}
node = Index(curPointer, (buffer + curPointer), BLOCK_S);
} else {
node.rehashNode((buffer + curPointer), BLOCK_S);
}
std::unordered_map<std::string,IndexNode>::const_iterator tempNode;
if(nodeExists(std::get<0>(node.getNode()))) {
tempNode = strgIndex.find (std::get<0>(node.getNode()));
node.setParent(tempNode->second.offsetPointer, tempNode->second.node.size);
curPointer += BLOCK_S;
segLength -= BLOCK_S;
exists = true;
if(BLOCK_S > segLength) {
Index::generateIndex(node.getParentIndex(),node.getIndexNode().offsetPointer, 1, 0);
}
} else {
strgIndex.insert(node.getNode());
if(!exists) {
memcpy((tempBuff + fileSize), (buffer+curPointer), BLOCK_S);
fileSize += BLOCK_S;
curPointer += BLOCK_S;
segLength -= BLOCK_S;
if(BLOCK_S <= segLength) {
node.rehashNode((buffer + curPointer), BLOCK_S);
assert ( !nodeExists(std::get<0>(node.getNode())) );
strgIndex.insert(node.getNode());
} else {
//last block
memcpy((tempBuff + fileSize), (buffer+curPointer), segLength);
fileSize += segLength;
}
} else {
//Generate Index
Index::generateIndex(node.getParentIndex(),node.getIndexNode().offsetPointer, 1, 0);
exists = false;
}
}
} else {
//last block
memcpy((tempBuff + fileSize), (buffer+curPointer), segLength);
fileSize += segLength;
Index::generateIndex(node.getParentIndex(),curPointer, 0, segLength);
segLength -= segLength;
}
blockCounter++;
}
indexOffset = 0;
char c = 0;
c = (fileSize >= seg_s ? 0 : 1);
// Setting size for outbuffer.
unsigned long int retSize = sizeof(c) + (c ? ((Index::getHeaderIndexCount() * sizeof(IndexHeader)) +
sizeof(unsigned int)) : 0) + sizeof(unsigned long int) + (c ? fileSize : seg_s);
*outBuffer = __outBuffer__;//(char *) malloc(retSize * sizeof(char));
memcpy(((*outBuffer) + indexOffset), (char*)&(c), sizeof(c));
indexOffset += sizeof(c);
//Copying index into the buffer
if(c) {
indexOffset += Index::writeIndex((*outBuffer));
} else {
Index::clearIndex();
}
//Copying index into the buffer
//Copying size into the buffer
memcpy(((*outBuffer) + indexOffset), (char*)&(seg_s), sizeof(unsigned long int));
indexOffset += sizeof(unsigned long int);
//Copy compressed data into buffer
memcpy(((*outBuffer) + indexOffset), (c ? tempBuff : buffer), (c ? fileSize : seg_s));
//delete[] tempBuff;
tempBuff = NULL;
if(__tempBuffer__) munmap(__tempBuffer__, SEG_S + 20);
return retSize == ((c ? fileSize : seg_s) + indexOffset) ? retSize : 0;
}
return 0;
}
