along/secondo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
dfd1e745480fabd88139d2804acb90d5b6dc055e
secondo/Algebras/TransportationMode/RoadNetwork.cpp
along dfd1e74548 firs commit
2026-01-23 17:03:45 +08:00

2397 lines
76 KiB
C++
Raw Blame History

/*
----
This file is part of SECONDO.
Copyright (C) 2004, University in Hagen, Department of Computer Science,
Database Systems for New Applications.
SECONDO is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
SECONDO is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with SECONDO; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
----
//paragraph [1] Title: [{\Large \bf \begin {center}] [\end {center}}]
//[TOC] [\tableofcontents]
[1] Source File of the Transportation Mode Algebra
July, 2011 Jianqiu Xu
[TOC]
1 Overview
This source file essentially contains the necessary implementations for
computing shortest path in road network
*/
#include "BusNetwork.h"
#include "RoadNetwork.h"
#include "PaveGraph.h"
#include "ListUtils.h"
using namespace std;
using namespace network;
/////////////////////////////////////////////////////////////////////////////
//////////////// road network graph ///////////////////////////////////////
////////////////////////////////////////////////////////////////////////////
string RoadGraph::RGNodeTypeInfo = "(rel (tuple ((Jun_id int) (Jun_gp gpoint)\
(Jun_p point) (Rid int))))";
string RoadGraph::RGBTreeNodeTypeInfo = "(btree (tuple ((jun_id int)\
(jun_gp gpoint)(jun_p point) (rid int))) int)";
string RoadGraph::RGEdgeTypeInfo1 = "(rel (tuple ((Jun_id1 int)\
(Jun_id2 int))))";
string RoadGraph::RGEdgeTypeInfo2 = "(rel (tuple ((Jun_id1 int) (Jun_id2 int)\
(Path1 gline) (Path2 sline))))";
ListExpr RoadGraph::RoadGraphProp()
{
ListExpr examplelist = nl->TextAtom();
nl->AppendText(examplelist,
"createroadgraph(<id>,<node-relation>,<edge-relation>)");
return nl->TwoElemList(
nl->TwoElemList(nl->StringAtom("Creation"),
nl->StringAtom("Example Creation")),
nl->TwoElemList(examplelist,
nl->StringAtom("let rg=createroadgraph(id,n-rel,e-rel)")));
}
bool RoadGraph::CheckRoadGraph(ListExpr type, ListExpr& errorInfo)
{
// cout<<"CheckRoadGraph()"<<endl;
return nl->IsEqual(type, "roadgraph");
}
int RoadGraph::SizeOfRoadGraph()
{
// cout<<"SizeOfRoadGraph()"<<endl;
return 0;
}
void* RoadGraph::CastRoadGraph(void* addr)
{
// cout<<"CastRoadGraph()"<<endl;
return 0;
}
Word RoadGraph::CloneRoadGraph(const ListExpr typeInfo, const Word& w)
{
// cout<<"CloneMetroGraph()"<<endl;
return SetWord(Address(0));
}
void RoadGraph::CloseRoadGraph(const ListExpr typeInfo, Word& w)
{
// cout<<"CloseRoadGraph()"<<endl;
delete static_cast<RoadGraph*> (w.addr);
w.addr = NULL;
}
Word RoadGraph::CreateRoadGraph(const ListExpr typeInfo)
{
// cout<<"CreateRoadGraph()"<<endl;
return SetWord(new RoadGraph());
}
void RoadGraph::DeleteRoadGraph(const ListExpr typeInfo, Word& w)
{
// cout<<"DeleteRoadGraph()"<<endl;
RoadGraph* rg = (RoadGraph*)w.addr;
delete rg;
w.addr = NULL;
}
bool RoadGraph::SaveRoadGraph(SmiRecord& valueRecord, size_t& offset,
const ListExpr typeInfo, Word& value)
{
// cout<<"SaveRoadGraph()"<<endl;
RoadGraph* rg = (RoadGraph*)value.addr;
bool result = rg->Save(valueRecord, offset, typeInfo);
return result;
}
bool RoadGraph::OpenRoadGraph(SmiRecord& valueRecord, size_t& offset,
const ListExpr typeInfo, Word& value)
{
// cout<<"OpenRoadGraph()"<<endl;
value.addr = RoadGraph::Open(valueRecord, offset, typeInfo);
bool result = (value.addr != NULL);
return result;
}
/*
input road network graph
*/
Word RoadGraph::InRoadGraph(ListExpr in_xTypeInfo,
ListExpr in_xValue,
int in_iErrorPos, ListExpr& inout_xErrorInfo,
bool& inout_bCorrect)
{
// cout<<"InRoadGraph()"<<endl;
RoadGraph* rg = new RoadGraph(in_xValue, in_iErrorPos, inout_xErrorInfo,
inout_bCorrect);
if(inout_bCorrect) return SetWord(rg);
else{
delete rg;
return SetWord(Address(0));
}
}
ListExpr RoadGraph::OutRoadGraph(ListExpr typeInfo, Word value)
{
// cout<<"OutRoadGraph()"<<endl;
RoadGraph* rg = (RoadGraph*)value.addr;
return rg->Out(typeInfo);
}
ListExpr RoadGraph::Out(ListExpr typeInfo)
{
// cout<<"Out()"<<endl;
ListExpr xNode = nl->TheEmptyList();
ListExpr xLast = nl->TheEmptyList();
ListExpr xNext = nl->TheEmptyList();
bool bFirst = true;
for(int i = 1;i <= node_rel->GetNoTuples();i++){
Tuple* node_tuple = node_rel->GetTuple(i, false);
int id = ((CcInt*)node_tuple->GetAttribute(RG_JUN_ID))->GetIntval();
GPoint* gp = (GPoint*)node_tuple->GetAttribute(RG_JUN_GP);
Point* loc = (Point*)node_tuple->GetAttribute(RG_JUN_P);
xNext = nl->ThreeElemList(
nl->IntAtom(id),
GPoint::OutGPoint( nl->TheEmptyList(), SetWord(gp)),
OutPoint( nl->TheEmptyList(), SetWord(loc)));
if(bFirst){
xNode = nl->OneElemList(xNext);
xLast = xNode;
bFirst = false;
}else
xLast = nl->Append(xLast,xNext);
node_tuple->DeleteIfAllowed();
}
return nl->TwoElemList(nl->IntAtom(rg_id),xNode);
// return nl->OneElemList(nl->IntAtom(rg_id));
}
RoadGraph* RoadGraph::Open(SmiRecord& valueRecord,size_t& offset,
const ListExpr typeInfo)
{
return new RoadGraph(valueRecord,offset,typeInfo);
}
RoadGraph::RoadGraph():rg_id(0), node_rel(NULL), btree_node(NULL),
edge_rel1(NULL), adj_list1(0), entry_adj_list1(0),
edge_rel2(NULL), adj_list2(0), entry_adj_list2(0)
{
// cout<<"RoadGraph::RoadGraph()"<<endl;
}
RoadGraph::RoadGraph(ListExpr in_xValue,int in_iErrorPos,
ListExpr& inout_xErrorInfo,
bool& inout_bCorrect):rg_id(0), node_rel(NULL),
btree_node(NULL),
edge_rel1(NULL), adj_list1(0), entry_adj_list1(0),
edge_rel2(NULL), adj_list2(0), entry_adj_list2(0)
{
// cout<<"RoadGraph::RoadGraph(ListExpr)"<<endl;
}
RoadGraph::RoadGraph(SmiRecord& in_xValueRecord, size_t& inout_iOffset,
const ListExpr in_xTypeInfo):rg_id(0), node_rel(NULL), btree_node(NULL),
edge_rel1(NULL), adj_list1(0), entry_adj_list1(0),
edge_rel2(NULL), adj_list2(0), entry_adj_list2(0)
{
in_xValueRecord.Read(&rg_id,sizeof(int),inout_iOffset);
inout_iOffset += sizeof(int);
ListExpr xType;
ListExpr xNumericType;
///////////////////Open relation for node////////////////////////////
nl->ReadFromString(RGNodeTypeInfo,xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
node_rel = Relation::Open(in_xValueRecord, inout_iOffset, xNumericType);
if(!node_rel) {
return;
}
///////////////////open btree built on nodes//////////////////////////
nl->ReadFromString(RGBTreeNodeTypeInfo, xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
btree_node = BTree::Open(in_xValueRecord, inout_iOffset, xNumericType);
if(!btree_node) {
node_rel->Delete();
return;
}
///////////////////////Open relation for edge1///////////////////////////
nl->ReadFromString(RGEdgeTypeInfo1, xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
edge_rel1 = Relation::Open(in_xValueRecord, inout_iOffset, xNumericType);
if(!edge_rel1) {
node_rel->Delete();
delete btree_node;
return;
}
/////////////////open adjacency list1////////////////////////////////
size_t bufsize = DbArray<int>::headerSize();
SmiSize offset = 0;
char* buf = (char*) malloc(bufsize);
in_xValueRecord.Read(buf, bufsize, inout_iOffset);
inout_iOffset += bufsize;
assert(buf != NULL);
adj_list1.restoreHeader(buf,offset);
free(buf);
offset = 0;
buf = (char*) malloc(bufsize);
in_xValueRecord.Read(buf, bufsize, inout_iOffset);
assert(buf != NULL);
entry_adj_list1.restoreHeader(buf,offset);
inout_iOffset += bufsize;
free(buf);
///////////////////////Open relation for edge2//////////////////////////
nl->ReadFromString(RGEdgeTypeInfo2, xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
edge_rel2 = Relation::Open(in_xValueRecord, inout_iOffset, xNumericType);
if(!edge_rel2) {
node_rel->Delete();
delete btree_node;
edge_rel1->Delete();
adj_list1.clean();
entry_adj_list1.clean();
return;
}
/////////////////open adjacency list2////////////////////////////////
bufsize = DbArray<int>::headerSize();
offset = 0;
buf = (char*) malloc(bufsize);
in_xValueRecord.Read(buf, bufsize, inout_iOffset);
inout_iOffset += bufsize;
assert(buf != NULL);
adj_list2.restoreHeader(buf,offset);
free(buf);
offset = 0;
buf = (char*) malloc(bufsize);
in_xValueRecord.Read(buf, bufsize, inout_iOffset);
assert(buf != NULL);
entry_adj_list2.restoreHeader(buf,offset);
inout_iOffset += bufsize;
free(buf);
}
RoadGraph::~RoadGraph()
{
if(node_rel != NULL) node_rel->Close();
if(edge_rel1 != NULL) edge_rel1->Close();
if(edge_rel2 != NULL) edge_rel2->Close();
if(btree_node != NULL) delete btree_node;
}
/*
save functions for the graph
*/
bool RoadGraph::Save(SmiRecord& in_xValueRecord,size_t& inout_iOffset,
const ListExpr in_xTypeInfo)
{
// cout<<"save "<<endl;
in_xValueRecord.Write(&rg_id,sizeof(int),inout_iOffset);
inout_iOffset += sizeof(int);
ListExpr xType;
ListExpr xNumericType;
///////////////////////////save node/////////////////////////////
nl->ReadFromString(RGNodeTypeInfo,xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
if(!node_rel->Save(in_xValueRecord, inout_iOffset, xNumericType))
return false;
////////////////save btree on nodes///////////////////////////
nl->ReadFromString(RGBTreeNodeTypeInfo, xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
if(!btree_node->Save(in_xValueRecord, inout_iOffset, xNumericType))
return false;
/////////////////////save edge1/////////////////////////////
nl->ReadFromString(RGEdgeTypeInfo1, xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
if(!edge_rel1->Save(in_xValueRecord,inout_iOffset,xNumericType))
return false;
/////////////////adjacency list 1//////////////////////////////
SecondoCatalog *ctlg = SecondoSystem::GetCatalog();
SmiRecordFile *rf = ctlg->GetFlobFile();
adj_list1.saveToFile(rf, adj_list1);
SmiSize offset = 0;
size_t bufsize = adj_list1.headerSize();
char* buf = (char*) malloc(bufsize);
adj_list1.serializeHeader(buf,offset);
assert(offset==bufsize);
in_xValueRecord.Write(buf, bufsize, inout_iOffset);
inout_iOffset += bufsize;
free(buf);
entry_adj_list1.saveToFile(rf, entry_adj_list1);
offset = 0;
buf = (char*) malloc(bufsize);
entry_adj_list1.serializeHeader(buf,offset);
assert(offset==bufsize);
in_xValueRecord.Write(buf,bufsize, inout_iOffset);
free(buf);
inout_iOffset += bufsize;
///////////////////////save edge2/////////////////////////////////////
nl->ReadFromString(RGEdgeTypeInfo2,xType);
xNumericType = SecondoSystem::GetCatalog()->NumericType(xType);
if(!edge_rel2->Save(in_xValueRecord,inout_iOffset,xNumericType))
return false;
/////////////////adjacency list 2//////////////////////////////////
adj_list2.saveToFile(rf, adj_list2);
offset = 0;
bufsize = adj_list2.headerSize();
buf = (char*) malloc(bufsize);
adj_list2.serializeHeader(buf,offset);
assert(offset==bufsize);
in_xValueRecord.Write(buf, bufsize, inout_iOffset);
inout_iOffset += bufsize;
free(buf);
entry_adj_list2.saveToFile(rf, entry_adj_list2);
offset = 0;
buf = (char*) malloc(bufsize);
entry_adj_list2.serializeHeader(buf,offset);
assert(offset==bufsize);
in_xValueRecord.Write(buf,bufsize, inout_iOffset);
free(buf);
inout_iOffset += bufsize;
return true;
}
/*
load road graph from input relations
*/
void RoadGraph::Load(int id, Relation* r1, Relation* edge_rel1,
Relation* edge_rel2)
{
if(id < 0){
cout<<"invalid id "<<id<<endl;
return;
}
rg_id = id;
//////////////////node relation////////////////////
ListExpr ptrList1 = listutils::getPtrList(r1);
string strQuery = "(consume(feed(" + RGNodeTypeInfo +
"(ptr " + nl->ToString(ptrList1) + "))))";
Word xResult;
int QueryExecuted = QueryProcessor::ExecuteQuery(strQuery, xResult);
assert(QueryExecuted);
node_rel = (Relation*)xResult.addr;
///////////////////rtree on junction points//////////////////////
ListExpr ptrList2 = listutils::getPtrList(r1);
strQuery = "(createbtree (" + RGNodeTypeInfo +
"(ptr " + nl->ToString(ptrList2) + "))" + "Rid)";//capital
QueryExecuted = QueryProcessor::ExecuteQuery(strQuery, xResult);
assert(QueryExecuted);
btree_node = (BTree*)xResult.addr;
// cout<<node_rel->GetNoTuples()<<endl;
LoadEdge1(edge_rel1);
LoadEdge2(edge_rel2);
}
/*
load road graph edges
*/
void RoadGraph::LoadEdge1(Relation* edge1)
{
ListExpr ptrList1 = listutils::getPtrList(edge1);
string strQuery = "(consume(feed(" + RGEdgeTypeInfo1 +
"(ptr " + nl->ToString(ptrList1) + "))))";
Word xResult;
int QueryExecuted = QueryProcessor::ExecuteQuery(strQuery, xResult);
assert(QueryExecuted);
edge_rel1 = (Relation*)xResult.addr;
//////////////////create adjacency list////////////////////////////////////
ListExpr ptrList2 = listutils::getPtrList(edge1);
strQuery = "(createbtree (" + RGEdgeTypeInfo1 +
"(ptr " + nl->ToString(ptrList2) + "))" + "Jun_id1)";
QueryExecuted = QueryProcessor::ExecuteQuery(strQuery,xResult);
assert(QueryExecuted);
BTree* btree = (BTree*)xResult.addr;
/////////////////////////////////////////////////////////////////////////
/////////the adjacent list here is different from dual graph and
///////// visibility graph. it is the same as indoor graph ////////////
//////////in dual graph and visibility graph, we store the node id/////
/////////now we store the edge id because the weight, path is stored
////////in the edge relation ////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
for(int i = 1;i <= node_rel->GetNoTuples();i++){
Tuple* ms_tuple = node_rel->GetTuple(i, false);
int jun_id = ((CcInt*)ms_tuple->GetAttribute(RG_JUN_ID))->GetIntval();
CcInt* nodeid = new CcInt(true, jun_id);
BTreeIterator* btree_iter = btree->ExactMatch(nodeid);
int start = adj_list1.Size();
while(btree_iter->Next()){
Tuple* edge_tuple = edge_rel1->GetTuple(btree_iter->GetId(), false);
adj_list1.Append(edge_tuple->GetTupleId());//get the edge tuple id
edge_tuple->DeleteIfAllowed();
}
delete btree_iter;
int end = adj_list1.Size();
entry_adj_list1.Append(ListEntry(start, end));
// cout<<jun_id<<" start "<<start<<" end "<<end<<endl;
delete nodeid;
ms_tuple->DeleteIfAllowed();
}
delete btree;
}
void RoadGraph::LoadEdge2(Relation* edge2)
{
ListExpr ptrList1 = listutils::getPtrList(edge2);
string strQuery = "(consume(feed(" + RGEdgeTypeInfo2 +
"(ptr " + nl->ToString(ptrList1) + "))))";
Word xResult;
int QueryExecuted = QueryProcessor::ExecuteQuery(strQuery, xResult);
assert(QueryExecuted);
edge_rel2 = (Relation*)xResult.addr;
//////////////////create adjacency list////////////////////////////////////
ListExpr ptrList2 = listutils::getPtrList(edge2);
strQuery = "(createbtree (" + RGEdgeTypeInfo2 +
"(ptr " + nl->ToString(ptrList2) + "))" + "Jun_id1)";
QueryExecuted = QueryProcessor::ExecuteQuery(strQuery,xResult);
assert(QueryExecuted);
BTree* btree = (BTree*)xResult.addr;
/////////////////////////////////////////////////////////////////////////
/////////the adjacent list here is different from dual graph and
///////// visibility graph. it is the same as indoor graph ////////////
//////////in dual graph and visibility graph, we store the node id/////
/////////now we store the edge id because the weight, path is stored
////////in the edge relation ////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
for(int i = 1;i <= node_rel->GetNoTuples();i++){
Tuple* ms_tuple = node_rel->GetTuple(i, false);
int jun_id = ((CcInt*)ms_tuple->GetAttribute(RG_JUN_ID))->GetIntval();
CcInt* nodeid = new CcInt(true, jun_id);
BTreeIterator* btree_iter = btree->ExactMatch(nodeid);
int start = adj_list2.Size();
while(btree_iter->Next()){
Tuple* edge_tuple = edge_rel2->GetTuple(btree_iter->GetId(), false);
adj_list2.Append(edge_tuple->GetTupleId());//get the edge tuple id
edge_tuple->DeleteIfAllowed();
}
delete btree_iter;
int end = adj_list2.Size();
entry_adj_list2.Append(ListEntry(start, end));
// cout<<jun_id<<" start "<<start<<" end "<<end<<endl;
delete nodeid;
ms_tuple->DeleteIfAllowed();
}
delete btree;
}
/*
get all junctions nodes of a route
*/
void RoadGraph::GetJunctionsNode(int rid, vector<GP_Point>& res_list)
{
CcInt* search_id = new CcInt(true, rid);
BTreeIterator* btree_iter = btree_node->ExactMatch(search_id);
while(btree_iter->Next()){
Tuple* tuple = node_rel->GetTuple(btree_iter->GetId(), false);
int node_id = ((CcInt*)tuple->GetAttribute(RG_JUN_ID))->GetIntval();
GPoint* gp = (GPoint*)tuple->GetAttribute(RG_JUN_GP);
Point* loc = (Point*)tuple->GetAttribute(RG_JUN_P);
GP_Point gp_p(gp->GetRouteId(), gp->GetPosition(), 0.0, *loc, *loc);
gp_p.oid = node_id;
res_list.push_back(gp_p);
tuple->DeleteIfAllowed();
}
delete btree_iter;
delete search_id;
}
/*
metro stops have the same spatial location in space
*/
void RoadGraph::FindAdj1(int node_id, vector<int>& list)
{
ListEntry list_entry;
entry_adj_list1.Get(node_id - 1, list_entry);
int low = list_entry.low;
int high = list_entry.high;
int j = low;
while(j < high){
int oid;
adj_list1.Get(j, oid);
j++;
list.push_back(oid);
}
}
/*
metro stops are connected by moving metros
*/
void RoadGraph::FindAdj2(int node_id, vector<int>& list)
{
ListEntry list_entry;
entry_adj_list2.Get(node_id - 1, list_entry);
int low = list_entry.low;
int high = list_entry.high;
int j = low;
while(j < high){
int oid;
adj_list2.Get(j, oid);
j++;
list.push_back(oid);
}
}
/////////////////////////////////////////////////////////////////////////
/////////////// query processing on the road graph//////////////////////
///////////////////////////////////////////////////////////////////////
void RoadNav::GenerateRoadLoc(Network* rn, int no,
vector<GPoint>& gp_list,
vector<Point>& gp_loc_list)
{
Relation* routes_rel = rn->GetRoutes();
for (int i = 1; i <= no;i++){
int m = GetRandom() % routes_rel->GetNoTuples() + 1;
Tuple* road_tuple = routes_rel->GetTuple(m, false);
int rid = ((CcInt*)road_tuple->GetAttribute(ROUTE_ID))->GetIntval();
SimpleLine* sl = (SimpleLine*)road_tuple->GetAttribute(ROUTE_CURVE);
double len = sl->Length();
int pos = GetRandom() % (int)len;
GPoint gp(true, rn->GetId(), rid, pos, None);
Point* gp_loc = new Point();
assert(sl->GetStartSmaller());
assert(sl->AtPosition(pos, true, *gp_loc));//look at the value (dual) for r
gp_list.push_back(gp);
gp_loc_list.push_back(*gp_loc);
delete gp_loc;
road_tuple->DeleteIfAllowed();
}
}
/*
use road graph to calculate shortest path
1000 pair of shortest path:
old method: 99.64 seconds
road graph: 22.40 seconds
old method has some bugs:
NetworkId: 1 RouteId: 1011 Position: 613 Side: 2
NetworkId: 1 RouteId: 2655 Position: 622 Side: 2
road graph:11848.8 old:11975.2
*/
void RoadNav::ShortestPath(GPoint* gp1, GPoint* gp2,
RoadGraph* rg, Network* rn, GLine* res)
{
///////////////////////////////////////////////////////////////////////////
/////// testing the performance of different shortest path computation/////
//////////////////////////////////////////////////////////////////////////
// int no = 1000;
/* int no = 100;
vector<GPoint> gp_loc_list1;
vector<Point> p_loc_list1;
GenerateRoadLoc(rn, no, gp_loc_list1, p_loc_list1);
vector<GPoint> gp_loc_list2;
vector<Point> p_loc_list2;
GenerateRoadLoc(rn, no, gp_loc_list2, p_loc_list2);
const double delta_d = 0.001;
for(unsigned int i = 0;i < gp_loc_list1.size();i++){
GPoint* loc1 = &gp_loc_list1[i];
GPoint* loc2 = &gp_loc_list2[i];
GLine* gl1 = new GLine(0);
GLine* gl2 = new GLine(0);
ShortestPathSub(loc1, loc2, rg, rn, gl1);
// loc1->ShortestPath(loc2, gl2, rn);
// if(fabs(gl1->GetLength() - gl2->GetLength()) > delta_d){
// cout<<"error:different shortest path "<<endl;
// cout<<*loc1<<" "<<*loc2<<" "
// <<gl1->GetLength()<<" "<<gl2->GetLength()<<endl;
// }
cout<<i<<" "<<gl1->GetLength()<<endl;
delete gl1;
delete gl2;
} */
//////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
ShortestPathSub(gp1, gp2, rg, rn, res);
}
/*
calculate shortest path
*/
void RoadNav::ShortestPathSub(GPoint* gp1, GPoint* gp2, RoadGraph* rg,
Network* rn, GLine* res_path)
{
if(gp1->IsDefined() == false || gp2->IsDefined() == false){
return;
}
if(gp1->GetRouteId() == gp2->GetRouteId()){//the same route
res_path->SetNetworkId(gp1->GetNetworkId());
res_path->AddRouteInterval(gp1->GetRouteId(),
gp1->GetPosition(), gp2->GetPosition());
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
return;
}
//////////////////////////////////////////////////////////////////////
/////////find the junction node for gp1 and gp2//////////////////////
/////////////////////////////////////////////////////////////////////
const double delta_d = 0.001;
////////// collect all junction points of gp1.rid, gp2.rid//////////
//////// find the node id of these junction points in road graph////
///////////////////////////////////////////////////////////////////
vector<GP_Point> gp_p_list1;
rg->GetJunctionsNode(gp1->GetRouteId(), gp_p_list1);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list1);
vector<GP_Point> gp_p_list2;
rg->GetJunctionsNode(gp2->GetRouteId(), gp_p_list2);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list2);
Tuple* road_tuple1 = rn->GetRoute(gp1->GetRouteId());
SimpleLine* sl1 = (SimpleLine*)road_tuple1->GetAttribute(ROUTE_CURVE);
Point loc_start;
assert(sl1->GetStartSmaller());
assert(sl1->AtPosition(gp1->GetPosition(), true, loc_start));
road_tuple1->DeleteIfAllowed();
Tuple* road_tuple2 = rn->GetRoute(gp2->GetRouteId());
SimpleLine* sl2 = (SimpleLine*)road_tuple2->GetAttribute(ROUTE_CURVE);
Point loc_end;
assert(sl2->GetStartSmaller());
assert(sl2->AtPosition(gp2->GetPosition(), true, loc_end));
road_tuple2->DeleteIfAllowed();
if(loc_start.Distance(loc_end) < delta_d){
res_path->SetDefined(false);
return;
}
////////////check whether the start location equals to junction node///////
bool exist_jun1 = false;
int start_index = -1;
for(unsigned int i = 0;i < gp_p_list1.size();i++){
if(gp1->GetRouteId() == gp_p_list1[i].rid &&
fabs(gp1->GetPosition() - gp_p_list1[i].pos1) < delta_d &&
fabs(loc_start.Distance(gp_p_list1[i].loc1) < delta_d)){
start_index = i;
exist_jun1 = true;
break;
}
}
////////////check whether the end location is equal to junction node///////
bool exist_jun2 = false;
//int end_index = -1;
for(unsigned int i = 0;i < gp_p_list2.size();i++){
if(gp2->GetRouteId() == gp_p_list2[i].rid &&
fabs(gp2->GetPosition() - gp_p_list2[i].pos1) < delta_d &&
fabs(loc_end.Distance(gp_p_list2[i].loc1) < delta_d)){
//end_index = i;
exist_jun2 = true;
break;
}
}
// cout<<"exist_jun1 "<<exist_jun1<<" exist_jun2 "<<exist_jun2<<endl;
// cout<<gp_p_list1.size()<<" "<<gp_p_list2.size()<<endl;
priority_queue<RNPath_elem> path_queue;
vector<RNPath_elem> expand_queue;
//////////////////////////////////////////////////////////////////////////
//////////////////initialize the start node /////////////////////////////
/////////////////////////////////////////////////////////////////////////
if(exist_jun1 == false){///start location is not equal to junction point
if(gp_p_list1.size() == 1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[0].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[0].pos1);
double hw = gp_p_list1[0].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[0].oid, w + hw, w,
ri, true, gp_p_list1[0].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
// ri.Print(cout);
}else if(gp_p_list1.size() == 2){/////////two junction points
for(unsigned int i = 0;i < gp_p_list1.size();i++){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
}
}else{
for(unsigned int i = 0;i < gp_p_list1.size();i++){
// cout<<"i "<<i<<" "<<gp_p_list1[i].pos1<<endl;
// if(i == 0 && gp1->GetPosition() < gp_p_list1[i].pos1){//first jun
if(i == 0){
if(gp1->GetPosition() < gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
// }else if(i == gp_p_list1.size() - 1 &&
// gp1->GetPosition() > gp_p_list1[i].pos1){
}else if(i == gp_p_list1.size() - 1){
if(gp1->GetPosition() > gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
}else if(gp1->GetPosition() > gp_p_list1[i - 1].pos1&&
gp1->GetPosition() < gp_p_list1[i].pos1){ //two junctions
///////////////first element//////////////////////
RouteInterval ri1(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i - 1].pos1);
double w1 = fabs(gp1->GetPosition() - gp_p_list1[i - 1].pos1);
double hw1 = gp_p_list1[i - 1].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem1(-1, cur_size, gp_p_list1[i - 1].oid, w1 + hw1,
w1, ri1, true, gp_p_list1[i - 1].loc1);
path_queue.push(rn_elem1);
expand_queue.push_back(rn_elem1);
///////////////second element//////////////////////////////
RouteInterval ri2(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w2 = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw2 = gp_p_list1[i].loc1.Distance(loc_end);
cur_size = expand_queue.size();
RNPath_elem rn_elem2(-1, cur_size, gp_p_list1[i].oid, w2 + hw2, w2,
ri2, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem2);
expand_queue.push_back(rn_elem2);
break;
}
}
}
}else{////////start location is equal to jun point
assert(0 <= start_index && start_index < (int)gp_p_list1.size());
RouteInterval ri(0, 0, 0);
double w = 0;
double hw = loc_start.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[start_index].oid, w + hw, w,
ri, false, loc_start);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
/////////////////////////////////////////////////////////////////
/////put the neighbor node (same location) into queue////////////
/////////////////////////////////////////////////////////////////
vector<int> adj_list;
rg->FindAdj1(gp_p_list1[start_index].oid, adj_list);
for(unsigned int i = 0;i < adj_list.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
edge_tuple->DeleteIfAllowed();
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
assert(jun_p->Distance(loc_start) < delta_d);
double w = 0.0;
double hw = jun_p->Distance(loc_end);
RouteInterval ri(0, 0, 0);
int cur_size = expand_queue.size();
RNPath_elem elem(0, cur_size, neighbor_id, w + hw, w,
ri, false, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}////////end for
}
vector<GP_Point> end_jun_list;
if(exist_jun2 == false){//find the junction point to end location
if(gp_p_list2.size() == 1){
end_jun_list.push_back(gp_p_list2[0]);
}else if(gp_p_list2.size() == 2){
end_jun_list.push_back(gp_p_list2[0]);
end_jun_list.push_back(gp_p_list2[1]);
}else{
for(unsigned int i = 0;i < gp_p_list2.size();i++){
// if(i == 0 && gp2->GetPosition() < gp_p_list2[i].pos1){//first jun
if(i == 0){//first jun
if(gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
/* }else if(i == gp_p_list2.size() - 1 &&
gp2->GetPosition() > gp_p_list2[i].pos1){*/
}else if(i == gp_p_list2.size() - 1){
if(gp2->GetPosition() > gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}else if(gp2->GetPosition() > gp_p_list2[i - 1].pos1 &&
gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i - 1]);
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}
}
}
//////////////////////////////start searching//////////////////////////
vector<bool> visit_flag; //junction node visit
for(int i = 1;i <= rg->GetNode_Rel()->GetNoTuples();i++){
visit_flag.push_back(false);
}
bool found = false;
RNPath_elem dest;
while(path_queue.empty() == false){
RNPath_elem top = path_queue.top();
path_queue.pop();
// top.Print();
if(top.tri_index == 0 || top.to_loc.Distance(loc_end) < delta_d){
dest = top;
found = true;
break;
}
if(visit_flag[top.tri_index - 1]) continue;
//////////////////////////////////////////////////////////////////////
if(exist_jun2 == false){//end location is not equal to junction point
for(unsigned int i = 0;i < end_jun_list.size();i++){
if(end_jun_list[i].oid == top.tri_index){
int pos_expand_path = top.cur_index;;
int cur_size = expand_queue.size();
assert(end_jun_list[i].rid == gp2->GetRouteId());
double w = top.real_w +
fabs(end_jun_list[i].pos1 - gp2->GetPosition());
double hw = 0.0;
RouteInterval ri(gp2->GetRouteId(), end_jun_list[i].pos1,
gp2->GetPosition());
RNPath_elem elem(pos_expand_path, cur_size, 0, w + hw, w,
ri, true, loc_end);//end point not oid
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
}
int pos_expand_path;
int cur_size;
if(top.len){
///////////////////////neighbor list 1////////////////////////////////
vector<int> adj_list1;
rg->FindAdj1(top.tri_index, adj_list1);
for(unsigned int i = 0;i < adj_list1.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list1[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
edge_tuple->DeleteIfAllowed();
double w = top.real_w;
double hw = top.to_loc.Distance(loc_end);
RouteInterval ri(0, 0, 0);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, false, top.to_loc);
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
///////////////////////neighbor list 2////////////////////////////////
vector<int> adj_list2;
rg->FindAdj2(top.tri_index, adj_list2);
// cout<<"adj2 size "<<adj_list2.size()<<endl;
for(unsigned int i = 0;i < adj_list2.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel2()->GetTuple(adj_list2[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN_2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
GLine* gl = (GLine*)edge_tuple->GetAttribute(RoadGraph::RG_PATHA1);
assert(gl->NoOfComponents() == 1);
RouteInterval ri;
gl->Get(0, ri);
edge_tuple->DeleteIfAllowed();
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
double w = top.real_w + fabs(ri.GetStartPos() - ri.GetEndPos());
double hw = jun_p->Distance(loc_end);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, true, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}
visit_flag[top.tri_index - 1] = true;
}
if(found){
res_path->SetNetworkId(gp1->GetNetworkId());
vector<RouteInterval> ri_list;
while(dest.prev_index != -1){
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
dest = expand_queue[dest.prev_index];
}
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
// for(int i = ri_list.size() - 1;i >= 0; i--){
// res_path->AddRouteInterval(ri_list[i]);
// }
////////////////merge intervals on the same route///////////////////
vector<RouteInterval> new_ri_list;
for(int i = ri_list.size() - 1; i >= 0;i--){
RouteInterval ri = ri_list[i];
int j = i - 1;
while(j >= 0 && ri_list[j].GetRouteId() == ri.GetRouteId()){
if(fabs(ri.GetEndPos() - ri_list[j].GetStartPos()) < delta_d){
ri.SetEndPos(ri_list[j].GetEndPos());
j--;
}else
assert(false);
}
new_ri_list.push_back(ri);
i = j + 1;
}
for(unsigned int i = 0;i < new_ri_list.size(); i++){
res_path->AddRouteInterval(new_ri_list[i]);
}
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
}else{
res_path->SetDefined(false);
}
}
/*
find all routes that intersect the route
*/
void RoadNav::DFTraverse(Network* rn, R_Tree<2,TupleId>* rtree,
SmiRecordId adr,
SimpleLine* line, vector<GPoint_Dist>& res_list)
{
Relation* routes = rn->GetRoutes();
R_TreeNode<2,TupleId>* node = rtree->GetMyNode(adr,false,
rtree->MinEntries(0), rtree->MaxEntries(0));
for(int j = 0;j < node->EntryCount();j++){
if(node->IsLeaf()){
R_TreeLeafEntry<2,TupleId> e =
(R_TreeLeafEntry<2,TupleId>&)(*node)[j];
Tuple* route_tuple = routes->GetTuple(e.info, false);
SimpleLine* road =
(SimpleLine*)route_tuple->GetAttribute(ROUTE_CURVE);
int rid =
((CcInt*)route_tuple->GetAttribute(ROUTE_ID))->GetIntval();
Points* ps = new Points(0);
line->Crossings(*road, *ps);
if(ps->Size() > 0){
// cout<<"ps size "<<ps->Size()<<endl;
for(int i = 0;i < ps->Size();i++){
Point p;
ps->Get(i, p);
double pos;
if(road->AtPoint(p, true, pos)){
GPoint gp(true, rn->GetId(), rid, pos, None);
GPoint_Dist gpd(gp, p, 0.0);
res_list.push_back(gpd);
}
}
}
ps->DeleteIfAllowed();
route_tuple->DeleteIfAllowed();
}else{
R_TreeInternalEntry<2> e =
(R_TreeInternalEntry<2>&)(*node)[j];
if(e.box.Intersects(line->BoundingBox())){
DFTraverse(rn, rtree, e.pointer, line, res_list);
}
}
}
delete node;
}
/*
find a path from a gpoint to another. avoid the road network center area
*/
void RoadNav::ShortestPath2(GPoint* gp1, GPoint* gp2, RoadGraph* rg,
Network* rn, GLine* res_path)
{
// ShortestPathSub2(gp1, gp2, rg, rn, res_path);
ShortestPathSub3(gp1, gp2, rg, rn, res_path);
}
void RoadNav::ShortestPathSub2(GPoint* gp1, GPoint* gp2, RoadGraph* rg,
Network* rn, GLine* res_path)
{
if(gp1->IsDefined() == false || gp2->IsDefined() == false){
return;
}
if(gp1->GetRouteId() == gp2->GetRouteId()){//the same route
res_path->SetNetworkId(gp1->GetNetworkId());
res_path->AddRouteInterval(gp1->GetRouteId(),
gp1->GetPosition(), gp2->GetPosition());
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
return;
}
Rectangle<2> bbox = rn->GetRTree()->BoundingBox();
double mid_x = (bbox.MinD(0) + bbox.MaxD(0))/2;
double mid_y = (bbox.MinD(1) + bbox.MaxD(1))/2;
// cout<<"mindx "<<mid_x<<" mindy "<<mid_y<<endl;
const double dist1 = 5000.0;
const double dist2 = 4000.0;
double min[2], max[2];
min[0] = mid_x - dist1;
min[1] = mid_y - dist2;
max[0] = mid_x + dist1;
max[1] = mid_y + dist2;
Rectangle<2> center_area(true, min, max);//shortest path avoid center area
//////////////////////////////////////////////////////////////////////
/////////find the junction node for gp1 and gp2//////////////////////
/////////////////////////////////////////////////////////////////////
const double delta_d = 0.001;
////////// collect all junction points of gp1.rid, gp2.rid//////////
//////// find the node id of these junction points in road graph////
///////////////////////////////////////////////////////////////////
vector<GP_Point> gp_p_list1;
rg->GetJunctionsNode(gp1->GetRouteId(), gp_p_list1);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list1);
vector<GP_Point> gp_p_list2;
rg->GetJunctionsNode(gp2->GetRouteId(), gp_p_list2);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list2);
Tuple* road_tuple1 = rn->GetRoute(gp1->GetRouteId());
SimpleLine* sl1 = (SimpleLine*)road_tuple1->GetAttribute(ROUTE_CURVE);
Point loc_start;
assert(sl1->GetStartSmaller());
assert(sl1->AtPosition(gp1->GetPosition(), true, loc_start));
road_tuple1->DeleteIfAllowed();
Tuple* road_tuple2 = rn->GetRoute(gp2->GetRouteId());
SimpleLine* sl2 = (SimpleLine*)road_tuple2->GetAttribute(ROUTE_CURVE);
Point loc_end;
assert(sl2->GetStartSmaller());
assert(sl2->AtPosition(gp2->GetPosition(), true, loc_end));
road_tuple2->DeleteIfAllowed();
if(loc_start.Distance(loc_end) < delta_d){
res_path->SetDefined(false);
return;
}
////////////check whether the start location equals to junction node///////
bool exist_jun1 = false;
int start_index = -1;
for(unsigned int i = 0;i < gp_p_list1.size();i++){
if(gp1->GetRouteId() == gp_p_list1[i].rid &&
fabs(gp1->GetPosition() - gp_p_list1[i].pos1) < delta_d &&
fabs(loc_start.Distance(gp_p_list1[i].loc1) < delta_d)){
start_index = i;
exist_jun1 = true;
break;
}
}
////////////check whether the end location is equal to junction node///////
bool exist_jun2 = false;
// int end_index = -1;
for(unsigned int i = 0;i < gp_p_list2.size();i++){
if(gp2->GetRouteId() == gp_p_list2[i].rid &&
fabs(gp2->GetPosition() - gp_p_list2[i].pos1) < delta_d &&
fabs(loc_end.Distance(gp_p_list2[i].loc1) < delta_d)){
//end_index = i;
exist_jun2 = true;
break;
}
}
priority_queue<RNPath_elem> path_queue;
vector<RNPath_elem> expand_queue;
//////////////////////////////////////////////////////////////////////////
//////////////////initialize the start node /////////////////////////////
/////////////////////////////////////////////////////////////////////////
if(exist_jun1 == false){///start location is not equal to junction point
if(gp_p_list1.size() == 1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[0].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[0].pos1);
double hw = gp_p_list1[0].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[0].oid, w + hw, w,
ri, true, gp_p_list1[0].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
// ri.Print(cout);
}else if(gp_p_list1.size() == 2){/////////two junction points
for(unsigned int i = 0;i < gp_p_list1.size();i++){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
}
}else{
for(unsigned int i = 0;i < gp_p_list1.size();i++){
// if(i == 0 && gp1->GetPosition() < gp_p_list1[i].pos1){//first jun
if(i == 0){//first jun)
if(gp1->GetPosition() < gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
// }else if(i == gp_p_list1.size() - 1 &&
// gp1->GetPosition() > gp_p_list1[i].pos1){
}else if(i == gp_p_list1.size() - 1){
if(gp1->GetPosition() > gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
}else if(gp1->GetPosition() > gp_p_list1[i - 1].pos1&&
gp1->GetPosition() < gp_p_list1[i].pos1){///two juns
///////////////first element//////////////////////
RouteInterval ri1(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i - 1].pos1);
double w1 = fabs(gp1->GetPosition() - gp_p_list1[i - 1].pos1);
double hw1 = gp_p_list1[i - 1].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem1(-1, cur_size, gp_p_list1[i - 1].oid, w1 + hw1,
w1, ri1, true, gp_p_list1[i - 1].loc1);
path_queue.push(rn_elem1);
expand_queue.push_back(rn_elem1);
///////////////second element//////////////////////////////
RouteInterval ri2(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w2 = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw2 = gp_p_list1[i].loc1.Distance(loc_end);
cur_size = expand_queue.size();
RNPath_elem rn_elem2(-1, cur_size, gp_p_list1[i].oid, w2 + hw2, w2,
ri2, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem2);
expand_queue.push_back(rn_elem2);
break;
}
}
}
}else{////////start location is equal to jun point
assert(0 <= start_index && start_index < (int)gp_p_list1.size());
RouteInterval ri(0, 0, 0);
double w = 0;
double hw = loc_start.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[start_index].oid, w + hw, w,
ri, false, loc_start);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
/////////////////////////////////////////////////////////////////
/////put the neighbor node (same location) into queue////////////
/////////////////////////////////////////////////////////////////
vector<int> adj_list;
rg->FindAdj1(gp_p_list1[start_index].oid, adj_list);
for(unsigned int i = 0;i < adj_list.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
edge_tuple->DeleteIfAllowed();
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
assert(jun_p->Distance(loc_start) < delta_d);
double w = 0.0;
double hw = jun_p->Distance(loc_end);
RouteInterval ri(0, 0, 0);
int cur_size = expand_queue.size();
RNPath_elem elem(0, cur_size, neighbor_id, w + hw, w,
ri, false, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}////////end for
}
vector<GP_Point> end_jun_list;
if(exist_jun2 == false){//find the junction point to end location
if(gp_p_list2.size() == 1){
end_jun_list.push_back(gp_p_list2[0]);
}else if(gp_p_list2.size() == 2){
end_jun_list.push_back(gp_p_list2[0]);
end_jun_list.push_back(gp_p_list2[1]);
}else{
for(unsigned int i = 0;i < gp_p_list2.size();i++){
// if(i == 0 && gp2->GetPosition() < gp_p_list2[i].pos1){//first jun
if(i == 0){//first jun
if(gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
/* }else if(i == gp_p_list2.size() - 1 &&
gp2->GetPosition() > gp_p_list2[i].pos1){*/
}else if(i == gp_p_list2.size() - 1){
if(gp2->GetPosition() > gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}else if(gp2->GetPosition() > gp_p_list2[i - 1].pos1 &&
gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i - 1]);
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}
}
}
//////////////////////////////start searching//////////////////////////
vector<bool> visit_flag; //junction node visit
for(int i = 1;i <= rg->GetNode_Rel()->GetNoTuples();i++){
visit_flag.push_back(false);
}
bool found = false;
RNPath_elem dest;
while(path_queue.empty() == false){
RNPath_elem top = path_queue.top();
path_queue.pop();
// top.Print();
if(top.tri_index == 0 || top.to_loc.Distance(loc_end) < delta_d){
dest = top;
found = true;
break;
}
if(visit_flag[top.tri_index - 1]) continue;
//////////////////////////////////////////////////////////////////////
if(exist_jun2 == false){//end location is not equal to junction point
for(unsigned int i = 0;i < end_jun_list.size();i++){
if(end_jun_list[i].oid == top.tri_index){
int pos_expand_path = top.cur_index;;
int cur_size = expand_queue.size();
assert(end_jun_list[i].rid == gp2->GetRouteId());
double w = top.real_w +
fabs(end_jun_list[i].pos1 - gp2->GetPosition());
double hw = 0.0;
RouteInterval ri(gp2->GetRouteId(), end_jun_list[i].pos1,
gp2->GetPosition());
RNPath_elem elem(pos_expand_path, cur_size, 0, w + hw, w,
ri, true, loc_end);//end point not oid
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
}
int pos_expand_path;
int cur_size;
if(top.len){
///////////////////////neighbor list 1////////////////////////////////
vector<int> adj_list1;
rg->FindAdj1(top.tri_index, adj_list1);
for(unsigned int i = 0;i < adj_list1.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list1[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
edge_tuple->DeleteIfAllowed();
double w = top.real_w;
double hw = top.to_loc.Distance(loc_end);
RouteInterval ri(0, 0, 0);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, false, top.to_loc);
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
///////////////////////neighbor list 2////////////////////////////////
vector<int> adj_list2;
rg->FindAdj2(top.tri_index, adj_list2);
// cout<<"adj2 size "<<adj_list2.size()<<endl;
for(unsigned int i = 0;i < adj_list2.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel2()->GetTuple(adj_list2[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN_2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
GLine* gl = (GLine*)edge_tuple->GetAttribute(RoadGraph::RG_PATHA1);
assert(gl->NoOfComponents() == 1);
RouteInterval ri;
gl->Get(0, ri);
edge_tuple->DeleteIfAllowed();
// cout<<ri.GetRouteId()<<" "<<ri.GetStartPos()<<" "<<ri.GetEndPos()<<endl;
///////////////////////////////////////////////////////////////////
Tuple* road_tuple = rn->GetRoute(ri.GetRouteId());
SimpleLine* sl = (SimpleLine*)road_tuple->GetAttribute(ROUTE_CURVE);
assert(sl->StartsSmaller());
SimpleLine* sub_sl = new SimpleLine(0);
if(ri.GetStartPos() < ri.GetEndPos())
sl->SubLine(ri.GetStartPos(), ri.GetEndPos(), true, *sub_sl);
else
sl->SubLine(ri.GetEndPos(), ri.GetStartPos(), true, *sub_sl);
// cout<<sub_sl->Length()<<endl;
if(center_area.Contains(sub_sl->BoundingBox())){
sub_sl->DeleteIfAllowed();
road_tuple->DeleteIfAllowed();
continue;
}
sub_sl->DeleteIfAllowed();
road_tuple->DeleteIfAllowed();
//////////////////////////////////////////////////////////////////
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
double w = top.real_w + fabs(ri.GetStartPos() - ri.GetEndPos());
double hw = jun_p->Distance(loc_end);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, true, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}
visit_flag[top.tri_index - 1] = true;
}
if(found){
res_path->SetNetworkId(gp1->GetNetworkId());
vector<RouteInterval> ri_list;
while(dest.prev_index != -1){
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
dest = expand_queue[dest.prev_index];
}
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
// for(int i = ri_list.size() - 1;i >= 0; i--){
// res_path->AddRouteInterval(ri_list[i]);
// }
////////////////merge intervals on the same route///////////////////
vector<RouteInterval> new_ri_list;
for(int i = ri_list.size() - 1; i >= 0;i--){
RouteInterval ri = ri_list[i];
int j = i - 1;
while(j >= 0 && ri_list[j].GetRouteId() == ri.GetRouteId()){
if(fabs(ri.GetEndPos() - ri_list[j].GetStartPos()) < delta_d){
ri.SetEndPos(ri_list[j].GetEndPos());
j--;
}else
assert(false);
}
new_ri_list.push_back(ri);
i = j + 1;
}
for(unsigned int i = 0;i < new_ri_list.size(); i++){
res_path->AddRouteInterval(new_ri_list[i]);
}
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
}else{
res_path->SetDefined(false);
}
}
/*
not use the distance as weight in the priority queue
*/
void RoadNav::ShortestPathSub3(GPoint* gp1, GPoint* gp2, RoadGraph* rg,
Network* rn, GLine* res_path)
{
if(gp1->IsDefined() == false || gp2->IsDefined() == false){
return;
}
if(gp1->GetRouteId() == gp2->GetRouteId()){//the same route
res_path->SetNetworkId(gp1->GetNetworkId());
res_path->AddRouteInterval(gp1->GetRouteId(),
gp1->GetPosition(), gp2->GetPosition());
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
return;
}
//////////////////////////////////////////////////////////////////////
/////////find the junction node for gp1 and gp2//////////////////////
/////////////////////////////////////////////////////////////////////
const double delta_d = 0.001;
////////// collect all junction points of gp1.rid, gp2.rid//////////
//////// find the node id of these junction points in road graph////
///////////////////////////////////////////////////////////////////
vector<GP_Point> gp_p_list1;
rg->GetJunctionsNode(gp1->GetRouteId(), gp_p_list1);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list1);
vector<GP_Point> gp_p_list2;
rg->GetJunctionsNode(gp2->GetRouteId(), gp_p_list2);
assert(gp_p_list1.size() > 0);
// LOOP_PRINT2(gp_p_list2);
Tuple* road_tuple1 = rn->GetRoute(gp1->GetRouteId());
SimpleLine* sl1 = (SimpleLine*)road_tuple1->GetAttribute(ROUTE_CURVE);
Point loc_start;
assert(sl1->GetStartSmaller());
assert(sl1->AtPosition(gp1->GetPosition(), true, loc_start));
road_tuple1->DeleteIfAllowed();
Tuple* road_tuple2 = rn->GetRoute(gp2->GetRouteId());
SimpleLine* sl2 = (SimpleLine*)road_tuple2->GetAttribute(ROUTE_CURVE);
Point loc_end;
assert(sl2->GetStartSmaller());
assert(sl2->AtPosition(gp2->GetPosition(), true, loc_end));
road_tuple2->DeleteIfAllowed();
if(loc_start.Distance(loc_end) < delta_d){
res_path->SetDefined(false);
return;
}
////////////check whether the start location equals to junction node///////
bool exist_jun1 = false;
int start_index = -1;
for(unsigned int i = 0;i < gp_p_list1.size();i++){
if(gp1->GetRouteId() == gp_p_list1[i].rid &&
fabs(gp1->GetPosition() - gp_p_list1[i].pos1) < delta_d &&
fabs(loc_start.Distance(gp_p_list1[i].loc1) < delta_d)){
start_index = i;
exist_jun1 = true;
break;
}
}
////////////check whether the end location is equal to junction node///////
bool exist_jun2 = false;
//int end_index = -1;
for(unsigned int i = 0;i < gp_p_list2.size();i++){
if(gp2->GetRouteId() == gp_p_list2[i].rid &&
fabs(gp2->GetPosition() - gp_p_list2[i].pos1) < delta_d &&
fabs(loc_end.Distance(gp_p_list2[i].loc1) < delta_d)){
//end_index = i;
exist_jun2 = true;
break;
}
}
priority_queue<RNPath_elem> path_queue;
vector<RNPath_elem> expand_queue;
//////////////////////////////////////////////////////////////////////////
//////////////////initialize the start node /////////////////////////////
/////////////////////////////////////////////////////////////////////////
if(exist_jun1 == false){///start location is not equal to junction point
if(gp_p_list1.size() == 1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[0].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[0].pos1);
double hw = gp_p_list1[0].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[0].oid, w + hw, w,
ri, true, gp_p_list1[0].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
// ri.Print(cout);
}else if(gp_p_list1.size() == 2){/////two junction points
for(unsigned int i = 0;i < gp_p_list1.size();i++){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
}
}else{
for(unsigned int i = 0;i < gp_p_list1.size();i++){
// if(i == 0 && gp1->GetPosition() < gp_p_list1[i].pos1){//first jun
if(i == 0){//first jun
if(gp1->GetPosition() < gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
// }else if(i == gp_p_list1.size() - 1 &&
// gp1->GetPosition() > gp_p_list1[i].pos1){
}else if(i == gp_p_list1.size() - 1){
if(gp1->GetPosition() > gp_p_list1[i].pos1){
RouteInterval ri(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw = gp_p_list1[i].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[i].oid, w + hw, w,
ri, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
break;
}
}else if(gp1->GetPosition() > gp_p_list1[i - 1].pos1&&
gp1->GetPosition() < gp_p_list1[i].pos1){///two juns
///////////////first element//////////////////////
RouteInterval ri1(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i - 1].pos1);
double w1 = fabs(gp1->GetPosition() - gp_p_list1[i - 1].pos1);
double hw1 = gp_p_list1[i - 1].loc1.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem1(-1, cur_size, gp_p_list1[i - 1].oid, w1 + hw1,
w1, ri1, true, gp_p_list1[i - 1].loc1);
path_queue.push(rn_elem1);
expand_queue.push_back(rn_elem1);
///////////////second element//////////////////////////////
RouteInterval ri2(gp1->GetRouteId(), gp1->GetPosition(),
gp_p_list1[i].pos1);
double w2 = fabs(gp1->GetPosition() - gp_p_list1[i].pos1);
double hw2 = gp_p_list1[i].loc1.Distance(loc_end);
cur_size = expand_queue.size();
RNPath_elem rn_elem2(-1, cur_size, gp_p_list1[i].oid, w2 + hw2, w2,
ri2, true, gp_p_list1[i].loc1);
path_queue.push(rn_elem2);
expand_queue.push_back(rn_elem2);
break;
}
}
}
}else{////////start location is equal to jun point
assert(0 <= start_index && start_index < (int)gp_p_list1.size());
RouteInterval ri(0, 0, 0);
double w = 0;
double hw = loc_start.Distance(loc_end);
int cur_size = expand_queue.size();
RNPath_elem rn_elem(-1, cur_size, gp_p_list1[start_index].oid, w + hw, w,
ri, false, loc_start);
path_queue.push(rn_elem);
expand_queue.push_back(rn_elem);
/////////////////////////////////////////////////////////////////
/////put the neighbor node (same location) into queue////////////
/////////////////////////////////////////////////////////////////
vector<int> adj_list;
rg->FindAdj1(gp_p_list1[start_index].oid, adj_list);
for(unsigned int i = 0;i < adj_list.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
edge_tuple->DeleteIfAllowed();
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
assert(jun_p->Distance(loc_start) < delta_d);
double w = 0.0;
double hw = jun_p->Distance(loc_end);
RouteInterval ri(0, 0, 0);
int cur_size = expand_queue.size();
RNPath_elem elem(0, cur_size, neighbor_id, w + hw, w,
ri, false, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}////////end for
}
vector<GP_Point> end_jun_list;
if(exist_jun2 == false){//find the junction point to end location
if(gp_p_list2.size() == 1){
end_jun_list.push_back(gp_p_list2[0]);
}else if(gp_p_list2.size() == 2){
end_jun_list.push_back(gp_p_list2[0]);
end_jun_list.push_back(gp_p_list2[1]);
}else{
for(unsigned int i = 0;i < gp_p_list2.size();i++){
// if(i == 0 && gp2->GetPosition() < gp_p_list2[i].pos1){//first jun
if(i == 0){//first jun
if(gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
/* }else if(i == gp_p_list2.size() - 1 &&
gp2->GetPosition() > gp_p_list2[i].pos1){*/
}else if(i == gp_p_list2.size() - 1){
if(gp2->GetPosition() > gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}else if(gp2->GetPosition() > gp_p_list2[i - 1].pos1 &&
gp2->GetPosition() < gp_p_list2[i].pos1){
end_jun_list.push_back(gp_p_list2[i - 1]);
end_jun_list.push_back(gp_p_list2[i]);
break;
}
}
}
}
//////////////////////////////start searching//////////////////////////
vector<bool> visit_flag; //junction node visit
for(int i = 1;i <= rg->GetNode_Rel()->GetNoTuples();i++){
visit_flag.push_back(false);
}
bool found = false;
RNPath_elem dest;
while(path_queue.empty() == false){
RNPath_elem top = path_queue.top();
path_queue.pop();
// top.Print();
if(top.tri_index == 0 || top.to_loc.Distance(loc_end) < delta_d){
dest = top;
found = true;
break;
}
if(visit_flag[top.tri_index - 1]) continue;
//////////////////////////////////////////////////////////////////////
if(exist_jun2 == false){//end location is not equal to junction point
for(unsigned int i = 0;i < end_jun_list.size();i++){
if(end_jun_list[i].oid == top.tri_index){
int pos_expand_path = top.cur_index;;
int cur_size = expand_queue.size();
assert(end_jun_list[i].rid == gp2->GetRouteId());
/* double w = top.real_w +
fabs(end_jun_list[i].pos1 - gp2->GetPosition());*/
double l = fabs(end_jun_list[i].pos1 - gp2->GetPosition());
double w = top.real_w + l;
double hw = 0.0;
RouteInterval ri(gp2->GetRouteId(), end_jun_list[i].pos1,
gp2->GetPosition());
RNPath_elem elem(pos_expand_path, cur_size, 0, w + hw, w,
ri, true, loc_end);//end point not oid
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
}
int pos_expand_path;
int cur_size;
if(top.len){
///////////////////////neighbor list 1////////////////////////////////
vector<int> adj_list1;
rg->FindAdj1(top.tri_index, adj_list1);
for(unsigned int i = 0;i < adj_list1.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(adj_list1[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
edge_tuple->DeleteIfAllowed();
double w = top.real_w;
double hw = top.to_loc.Distance(loc_end);
RouteInterval ri(0, 0, 0);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, false, top.to_loc);
path_queue.push(elem);
expand_queue.push_back(elem);
}
}
///////////////////////neighbor list 2////////////////////////////////
vector<int> adj_list2;
rg->FindAdj2(top.tri_index, adj_list2);
// cout<<"adj2 size "<<adj_list2.size()<<endl;
for(unsigned int i = 0;i < adj_list2.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel2()->GetTuple(adj_list2[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN_2))->GetIntval();
if(visit_flag[neighbor_id - 1]){
edge_tuple->DeleteIfAllowed();
continue;
}
GLine* gl = (GLine*)edge_tuple->GetAttribute(RoadGraph::RG_PATHA1);
assert(gl->NoOfComponents() == 1);
RouteInterval ri;
gl->Get(0, ri);
edge_tuple->DeleteIfAllowed();
// cout<<ri.GetRouteId()<<" "<<ri.GetStartPos()<<" "<<ri.GetEndPos()<<endl;
///////////////////////////////////////////////////////////////////
Tuple* road_tuple = rn->GetRoute(ri.GetRouteId());
SimpleLine* sl = (SimpleLine*)road_tuple->GetAttribute(ROUTE_CURVE);
assert(sl->StartsSmaller());
SimpleLine* sub_sl = new SimpleLine(0);
if(ri.GetStartPos() < ri.GetEndPos())
sl->SubLine(ri.GetStartPos(), ri.GetEndPos(), true, *sub_sl);
else
sl->SubLine(ri.GetEndPos(), ri.GetStartPos(), true, *sub_sl);
// cout<<sub_sl->Length()<<endl;
sub_sl->DeleteIfAllowed();
road_tuple->DeleteIfAllowed();
//////////////////////////////////////////////////////////////////
Tuple* node_tuple = rg->GetNode_Rel()->GetTuple(neighbor_id, false);
int oid =
((CcInt*)node_tuple->GetAttribute(RoadGraph::RG_JUN_ID))->GetIntval();
assert(neighbor_id == oid);
Point* jun_p =
(Point*)node_tuple->GetAttribute(RoadGraph::RG_JUN_P);
// double w = top.real_w + fabs(ri.GetStartPos() - ri.GetEndPos());
double l = fabs(ri.GetStartPos() - ri.GetEndPos());
double w;
if(l > 50.0) ////////10 ?
w = top.real_w + GetRandom() % (int)l;
else
w = top.real_w + l;
double hw = jun_p->Distance(loc_end);
pos_expand_path = top.cur_index;
cur_size = expand_queue.size();
RNPath_elem elem(pos_expand_path, cur_size, neighbor_id, w + hw, w,
ri, true, *jun_p);
path_queue.push(elem);
expand_queue.push_back(elem);
node_tuple->DeleteIfAllowed();
}
visit_flag[top.tri_index - 1] = true;
}
if(found){
res_path->SetNetworkId(gp1->GetNetworkId());
vector<RouteInterval> ri_list;
while(dest.prev_index != -1){
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
dest = expand_queue[dest.prev_index];
}
if(dest.len){
// res_path->AddRouteInterval(dest.ri);
ri_list.push_back(dest.ri);
}
// for(int i = ri_list.size() - 1;i >= 0; i--){
// res_path->AddRouteInterval(ri_list[i]);
// }
////////////////merge intervals on the same route///////////////////
vector<RouteInterval> new_ri_list;
for(int i = ri_list.size() - 1; i >= 0;i--){
RouteInterval ri = ri_list[i];
int j = i - 1;
while(j >= 0 && ri_list[j].GetRouteId() == ri.GetRouteId()){
if(fabs(ri.GetEndPos() - ri_list[j].GetStartPos()) < delta_d){
ri.SetEndPos(ri_list[j].GetEndPos());
j--;
}else
assert(false);
}
new_ri_list.push_back(ri);
i = j + 1;
}
for(unsigned int i = 0;i < new_ri_list.size(); i++){
res_path->AddRouteInterval(new_ri_list[i]);
}
res_path->SetDefined(true);
res_path->SetSorted(false);
res_path->TrimToSize();
}else{
res_path->SetDefined(false);
}
}
/*
get all neighbor junction nodes for a given junction
*/
void RoadNav::GetAdjNodeRG(RoadGraph* rg, int nodeid)
{
if(rg->GetNode_Rel() == NULL){
cout<<"no road graph node rel"<<endl;
return;
}
if(nodeid < 1 || nodeid > rg->GetNode_Rel()->GetNoTuples()){
cout<<"invalid node id "<<endl;
return;
}
cout<<"total "<<rg->GetNode_Rel()->GetNoTuples()<<" nodes "<<endl;
cout<<"total "<<rg->GetEdge_Rel1()->GetNoTuples() +
rg->GetEdge_Rel2()->GetNoTuples()<<" edges "<<endl;
Relation* node_rel = rg->GetNode_Rel();
Tuple* jun_tuple = node_rel->GetTuple(nodeid, false);
GPoint* jun1 = (GPoint*)jun_tuple->GetAttribute(RoadGraph::RG_JUN_GP);
// cout<<*jun1<<endl;
///////////////////////////////////////////////////////////////////////////
//////the first kind of connection (no path; the same spatial location)////
////////////////////////////////////////////////////////////////////////////
vector<int> tid_list1;
rg->FindAdj1(nodeid, tid_list1);
for(unsigned int i = 0;i < tid_list1.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel1()->GetTuple(tid_list1[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN2))->GetIntval();
edge_tuple->DeleteIfAllowed();
Tuple* jun_neighbor1 = node_rel->GetTuple(neighbor_id, false);
GPoint* gp2 = (GPoint*)jun_neighbor1->GetAttribute(RoadGraph::RG_JUN_GP);
GLine* path = new GLine(0);
jun_list1.push_back(*jun1);
jun_list2.push_back(*gp2);
gline_list.push_back(*path);
path->DeleteIfAllowed();
jun_neighbor1->DeleteIfAllowed();
type_list.push_back(1);
}
////////////////////////////////////////////////////////////////////
//////the second kind of connection (connected by moving metros)////
////////////////////////////////////////////////////////////////////
vector<int> tid_list2;
rg->FindAdj2(nodeid, tid_list2);
for(unsigned int i = 0;i < tid_list2.size();i++){
Tuple* edge_tuple = rg->GetEdge_Rel2()->GetTuple(tid_list2[i], false);
int neighbor_id =
((CcInt*)edge_tuple->GetAttribute(RoadGraph::RG_JUN_2))->GetIntval();
Tuple* jun_neighbor2 = node_rel->GetTuple(neighbor_id, false);
GPoint* gp3 =
(GPoint*)jun_neighbor2->GetAttribute(RoadGraph::RG_JUN_GP);
GLine* path =
(GLine*)edge_tuple->GetAttribute(RoadGraph::RG_PATHA1);
jun_list1.push_back(*jun1);
jun_list2.push_back(*gp3);
gline_list.push_back(*path);
edge_tuple->DeleteIfAllowed();
jun_neighbor2->DeleteIfAllowed();
type_list.push_back(2);
}
jun_tuple->DeleteIfAllowed();
}
Reference in New Issue View Git Blame Copy Permalink