bool AStar::getShortestPath(Node _start, Node _end){
Node goal = _end;
Node start =_start;
came_from_map[start.id] = -1;
addNodeToSet(openSet,start);
while(!(openSet.size() == 0)){
#if DEBUG
printf("Size of open list is %d \n", openSet.size());
#endif
Node current = getMinimumNode(openSet);
#if DEBUG
printf("Expanding node:id= %lld\n", current.id);
#endif
if(current.data == goal.data){
cout<<"\nPath found.Reconstructing the path.\n";
reconstructPath(current.id,0);
return true;
}
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf ("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
Node nodeInClosedList = getNodeFromSet(closedSet,nbr);
int temp_g_score = current.g_score + distance(current,nbr);
if(temp_g_score < nodeInClosedList.g_score){
printf(" Removing a Node from closed list id %lld in previous %lld \n", nodeInClosedList.id,came_from_map[nodeInClosedList.id]);
printf(" New gscore is %d , old gscore is %d new wannabe parent %lld\n",temp_g_score, nodeInClosedList.g_score,current.id );
printf(" hscore for %lld wannabe parent %d \n" ,current.id , H(current,goal));
//printf(" hscore for %lld previous parent \n" ,current.id , H(current,goal));
// reconstructPath(nodeInClosedList.id],0);
//exit(0);
removeNodeFromSet(closedSet,nodeInClosedList);
}
else{
continue;
}
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
}
printf("Solution Not Found. Goal is not reachable from start\n");
return false;
}
void AStar::update(Node current, Node goal){
removeMinimum(openSet,current);
addNodeToSet(closedSet,current);
#if DEBUG
printf("Adding node= %lld to Closed multiset\n", current.id);
#endif
vector<Node> nodeNbrs = getNeighbours(current);
for(int i=0;i<nodeNbrs.size();i++){
//nbr is a neighbouring node
Node nbr = nodeNbrs[i];
if(findInSet(closedSet,nbr)){
//TODO: remove the node from the closed and shift in open
#if DEBUG
cout<<"Node found in closed multiset "<< nbr.id<<endl;
#endif
continue;
}
int tentative_g_score = current.g_score + distance(current,nbr);
#if DEBUG
nbr.printData();
#endif
//TODO update this with the follwing logic.
// update if it is found in open multiset with higher g score
// or else it is not in open multiset.
pair<Node,bool> check = findInOpenSet(nbr);
if(check.second && tentative_g_score < check.first.g_score){
removeNodeFromSet(openSet,nbr);
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//check.first.came_from = ¤t;
check.first.g_score = tentative_g_score;
check.first.f_score = tentative_g_score + H(check.first,goal);
addNodeToSet(openSet,check.first);
#if DEBUG
printf("Updating node in open multiset %lld with new parent = %lld\n",check.first.id,current.id);
#endif
}
else if(check.second==false){
came_from_map[check.first.id] = current.id;
#if DEBUG
printf("Setting parent of %lld to %lld\n", check.first.id, current.id);
#endif
//nbr.came_from = ¤t;
nbr.g_score = tentative_g_score;
nbr.f_score = tentative_g_score + H(nbr,goal);
addNodeToSet(openSet,nbr);
#if DEBUG
printf("Adding node= %lld to Open multiset with f_score %d \n", nbr.id,nbr.f_score);
#endif
}
}
#if DEBUG
cout<<"------------------------------------------------------------------------------------"<<endl;
#endif
return;
}
void METKAutoFading::calcNewPosition()
{
SbVec3f tempPos;
SbVec4f tempOrientVec4;
bool result = false;
if (_UseMETKValues->getBoolValue())
{
result = myObjMgr->getObjAttributeVec3f(_ViewerName->getStringValue(), LAY_VIEWER_CAMERA, INF_VIEWER_CAMERA_POSITION, tempPos);
result = result && myObjMgr->getObjAttributeVec4f(_ViewerName->getStringValue(), LAY_VIEWER_CAMERA, INF_VIEWER_CAMERA_ORIENTATION, tempOrientVec4);
}
else
{
result = true;
tempPos[0] = _camPosition->getVec3fValue()[0]; tempPos[1] = _camPosition->getVec3fValue()[1]; tempPos[2] = _camPosition->getVec3fValue()[2];
tempOrientVec4[0] = _camOrientation->getVec4fValue()[0]; tempOrientVec4[1] = _camOrientation->getVec4fValue()[1]; tempOrientVec4[2] = _camOrientation->getVec4fValue()[2]; tempOrientVec4[3] = _camOrientation->getVec4fValue()[3];
}
if (result)
{
myCamera->setPosition(tempPos);
SbRotation tempOrient;
tempOrient.setValue(SbVec3f(tempOrientVec4[0],tempOrientVec4[1],tempOrientVec4[2]),tempOrientVec4[3]);
myCamera->setOrientation(tempOrient);
SbVec3f lookDir = myCamera->getLookDir();
SbVec3f inverseLookDir = -1.0 * lookDir;
inverseLookDir.normalize();
//SphereCenter
float sphere_x, sphere_y, sphere_z, sphere_radius; int sphere_div;
m_calcVis.getSphereValues(sphere_x, sphere_y, sphere_z, sphere_radius, sphere_div);
SbVec3f spherePoint;
spherePoint[0] = sphere_x + inverseLookDir[0] * sphere_radius;
spherePoint[1] = sphere_y + inverseLookDir[1] * sphere_radius;
spherePoint[2] = sphere_z + inverseLookDir[2] * sphere_radius;
m_calcVis.clearStack(1);
m_calcVis.addPointRegionToStackField(1, spherePoint[0], spherePoint[1], spherePoint[2], 0.1);
SbVec3f similarPosition;
int posID = m_calcVis.getStackMaxPos(1, similarPosition[0], similarPosition[1], similarPosition[2]);
_similarPosition->setVec3fValue(vec3(similarPosition[0],similarPosition[1],similarPosition[2]));
//Alle Occluder an dieser Position für ObOfIn
std::vector<float> visValues;
m_calcVis.getMatrixValues(posID, _CurrentObject->getStringValue(), &visValues);
int objID = m_calcVis.findObjectId(_CurrentObject->getStringValue());
string occluName;
set<string> newFading;
for (int i=0; i<visValues.size()-3; i++)
{
if (visValues[i]!=0 && i!=objID)
{
occluName = m_calcVis.getObjectName(i);
newFading.insert(occluName);
//htNewFading.insert(occluName, occluName);
}
}
//std::cout << "newFading size = " << newFading.size() << std::endl;
//std::cout << "currentFading size = " << currentFading.size() << std::endl;
//Strukturen die neu hinzukommen finden
string objName = "";
Appearance oldValues;
set<string>::iterator iter = newFading.begin();
for (iter=newFading.begin(); iter!=newFading.end(); iter++)
{
objName = *iter;
//if (!htOldValues.find(objName))
if (!findInSet(currentFading, objName) && objName!="")
{
std::cout << "new:" << objName << std::endl;
//Get Old Values
myObjMgr->getObjAttributeFloat(objName,LAY_APPEARANCE, INF_TRANSPARENCY, oldValues.Transparency);
myObjMgr->getObjAttributeVec3f(objName,LAY_APPEARANCE, INF_COLOR, oldValues.Color);
myObjMgr->getObjAttributeVec3f(objName,LAY_APPEARANCE, INF_SILHOUETTECOLOR, oldValues.SilhouetteColor);
myObjMgr->getObjAttributeBool(objName,LAY_APPEARANCE, INF_SILHOUETTE, oldValues.SilhouetteVisible);
myObjMgr->getObjAttributeFloat(objName,LAY_APPEARANCE, INF_SILHOUETTEWIDTH, oldValues.SilhouetteWidth);
htOldValues.insert(objName, oldValues);
currentFading.insert(objName);
//Ausblenden
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_TRANSPARENCY, new double(0.9), omINFOTYPE_DOUBLE, true, false);
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTEWIDTH, new double(2), omINFOTYPE_DOUBLE, true, false);
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTECOLOR, new vec3(0,0,0), omINFOTYPE_VEC3, true, false);
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTE, new bool(true), omINFOTYPE_BOOL, true, false);
}
}
//Alte Element aus currentFading wieder einblenden mit oldValues, die nicht in newFading drin sind
for (iter=currentFading.begin(); iter!=currentFading.end(); iter++)
{
objName = *iter;
if (!findInSet(newFading,objName) && objName!="")
{
std::cout << "Wiedereinblenden: " << objName << std::endl;
if (htOldValues.find(objName))
{
oldValues = *(htOldValues.find(objName));
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_TRANSPARENCY, new double(oldValues.Transparency), omINFOTYPE_DOUBLE, true, false);
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTEWIDTH, new double(oldValues.SilhouetteWidth), omINFOTYPE_DOUBLE, true, false);
vec3* tempVec3 = new vec3(oldValues.SilhouetteColor[0],oldValues.SilhouetteColor[1],oldValues.SilhouetteColor[2]);
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTECOLOR, tempVec3, omINFOTYPE_DOUBLE, true, false);
delete tempVec3;
myObjMgr->setObjAttribute(objName,LAY_APPEARANCE, INF_SILHOUETTE, new bool(oldValues.SilhouetteVisible), omINFOTYPE_BOOL, true, false);
}
else
std::cout << "No oldValues found in Hashtable for " << objName << std::endl;
htOldValues.remove(objName);
}
}
currentFading.clear();
//Umkopieren von newFading in currentFading
for (iter=newFading.begin(); iter!=newFading.end(); iter++)
{
currentFading.insert(*iter);
}
//std::cout << "ende currentFading size = " << currentFading.size() << std::endl;
sendNotification();
}
else
{
std::cout << "Cam Position or Orientation in METK not available" << std::endl;
}
}
