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; } }