int shortPathMain()
{
int i,j;
int cost[][7]= { //将路径图以矩阵形式表示
{0,20,50,30,200,200,200},
{200,0,25,200,200,70,200},
{200,200,0,40,25,50,200},
{200,200,200,0,55,200,200},
{200,200,200,200,0,10,70},
{200,200,200,200,200,0,50},
{200,200,200,200,200,200,0}
};
cout<<"带权的有向图如下所示:"<<endl;
cout<<" ";
for(i=0; i<7; i++) cout<<"v"<<i+1<<" ";
cout<<endl;
for(i=0; i<7; i++)
{
cout<<"v"<<i+1<<" ";
for(j=0; j<7; j++)
cout<<left<<setw(5)<<cost[i][j];
cout<<endl;
}
int dist[7];
cout<<"单源点最短路径算法运行结果:"<<endl;
ShortestPath(0,cost,dist,6);//单源点最短路径
cout<<endl<<"每对结点间的最短路径算法运行结果:"<<endl;
AllPath(cost,6);//每对结点间的最短路径
system("pause");
return 1;
}
bool TAAlgorithm(GraphTopo *p_graph) {
int e = p_graph->edgeNum;
vector<bool> A = vector<bool>(p_graph->edgeNum, true);
Request *p_request = new Request(p_graph->source, p_graph->destination,
p_graph->edgeNum);
while (0 != e) {
p_request->clear(p_graph->edgeNum);
if (!ShortestPath(p_graph, p_request, A)) {
return false;
}
vector<int> T;
if (!FindCandidateBackup(p_graph, p_request, T)) {
return false;
}
if (T.empty()) {
//record answer
RecordAnswer(p_request);
return true;
} else {
int R = MostRiskyActiveLink(p_graph, p_request, T);
// cout<<"R: "<<R<<" ("<<p_graph->getithEdge(R).from<<")->("<<p_graph->getithEdge(R).to<<")"<<endl;
A.at(R) = false;
}
T.clear();
e--;
}
return true;
}
void SwerveDrive::Stop()
{
double y = m_stick.GetY() * -1, x = m_stick.GetX();
double speed = __hypot(x, y);
double angle = (atan2(y, x) * (180/M_PI) - 90.0 );
double lf_angle, lr_angle, rf_angle, rr_angle;
if (fabs(speed) < 0.00001)
{
// pick a default if the driver isn't pointing
lf_angle = 45;
lr_angle = 315;
rf_angle = 135;
rr_angle = 225;
}
else
{
// translate the direction the driver is pointing
angle = PositionInformation::GetInstance()->TranslateFieldToRobotAngle(angle) - 90;
lf_angle = lr_angle = rf_angle = rr_angle = angle;
}
// find the shortest path to that spot, and do it
ShortestPath(speed, lf_angle, m_chassis->servo_lf.GetCurrentAngle(), m_pick_alt_lf, m_pickEvent_lf);
ShortestPath(speed, lr_angle, m_chassis->servo_lr.GetCurrentAngle(), m_pick_alt_lr, m_pickEvent_lr);
ShortestPath(speed, rf_angle, m_chassis->servo_rf.GetCurrentAngle(), m_pick_alt_rf, m_pickEvent_rf);
ShortestPath(speed, rr_angle, m_chassis->servo_rr.GetCurrentAngle(), m_pick_alt_rr, m_pickEvent_rr);
m_chassis->servo_lf.SetAngle(lf_angle);
m_chassis->servo_lr.SetAngle(lr_angle);
m_chassis->servo_rf.SetAngle(rf_angle);
m_chassis->servo_rr.SetAngle(rr_angle);
m_chassis->motor_lf.SetSpeed(0);
m_chassis->motor_lr.SetSpeed(0);
m_chassis->motor_rf.SetSpeed(0);
m_chassis->motor_rr.SetSpeed(0);
}
/// ANT COLONY ALGORITHMS ///
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///// ///// ///// /////
void ant_colony()
{
srand(time(NULL));
const int loop_number = 1000;
int** ants = CreateAnts();
int *best_path = ShortestPath(ants);
double bestLength = CycleLength(best_path);
double **pheromones = InitializePheromones();
int counter = 0;
while (counter < loop_number)
{
UpdateAnts(ants, pheromones);
UpdatePheromones(pheromones, ants);
int *currBestTrail = ShortestPath(ants);
double currBestLength = CycleLength(currBestTrail);
if (currBestLength < bestLength)
{
delete best_path;
best_path = NULL;
bestLength = currBestLength;
best_path = currBestTrail;
}
counter++;
}
cout<<bestLength;
getchar();
getchar();
}
void vtkPolyDataSingleSourceShortestPath::Execute()
{
vtkPolyData *input = this->GetInput();
vtkPolyData *output = this->GetOutput();
vtkDebugMacro(<< "vtkPolyDataSingleSourceShortestPath finding shortest path");
init();
ShortestPath(this->StartVertex, this->EndVertex);
TraceShortestPath(input, output, this->StartVertex, this->EndVertex);
}
iCelHPath* celHNavStruct::ShortestPath (const csVector3& from, iSector* fromSector, const csVector3& goal,
iSector* goalSector)
{
csRef<iMapNode> fromNode;
fromNode.AttachNew(new csMapNode("n"));
fromNode->SetPosition(from);
fromNode->SetSector(fromSector);
csRef<iMapNode> goalNode;
goalNode.AttachNew(new csMapNode("n"));
goalNode->SetPosition(goal);
goalNode->SetSector(goalSector);
return ShortestPath(fromNode, goalNode);
}
int main(int argc, char *argv[]) {
Port Connection;
Field field;
RobotVision vision;
vision.init();
vision.set();
return 0;
// while(true) {
// vision.update(field, true);
// }
//vision.preprocess(field);
bool end = false;
while (!end) {
bool pause = false;
while (!pause && !end) {
int degreeAtTarget;
vector<geometry::Vector> region1, region2, region3, destPoints;
region1.push_back(geometry::Vector(50,200));
region1.push_back(geometry::Vector(50,150));
region1.push_back(geometry::Vector(0,150));
region1.push_back(geometry::Vector(0,200));
vision.showPoints(region1);
field.regions.push_back(Region(region1));
destPoints.clear();
destPoints.push_back(geometry::Vector(25,175));
vision.showPoints(destPoints);
field.regions[0].ID = 0;
field.regions[0].addDestPoints(destPoints);
region2.push_back(geometry::Vector(190,70));
region2.push_back(geometry::Vector(300,70));
region2.push_back(geometry::Vector(190,0));
region2.push_back(geometry::Vector(300,0));
vision.showPoints(region2);
field.regions.push_back(Region(region2));
destPoints.clear();
destPoints.push_back(geometry::Vector(230,35));
destPoints.push_back(geometry::Vector(270,35));
vision.showPoints(destPoints);
field.regions[1].ID = 1;
field.regions[1].addDestPoints(destPoints);
region3.push_back(geometry::Vector(300,350));
region3.push_back(geometry::Vector(360,350));
region3.push_back(geometry::Vector(300,400));
region3.push_back(geometry::Vector(360,400));
vision.showPoints(region3);
field.regions.push_back(Region(region3));
destPoints.clear();
destPoints.push_back(geometry::Vector(330,375));
vision.showPoints(destPoints);
field.regions[2].ID = 2;
field.regions[2].addDestPoints(destPoints);
field.agent.updated = false;
while (!field.agent.updated) vision.update(field,true);
cout << "obstacles size: " << field.obstacles.size() << endl;
field.agent.coords.clear();
field.agent.coords.push_back(field.agent.COM+geometry::Vector(25,25));
field.agent.coords.push_back(field.agent.COM+geometry::Vector(25,-25));
field.agent.coords.push_back(field.agent.COM+geometry::Vector(-25,-25));
field.agent.coords.push_back(field.agent.COM+geometry::Vector(-25,25));
for(int i = 0; i < field.obstacles.size(); i++){
cout << "obj#" << i << " (" << getColorName(field.obstacles[i].color) << "): " << field.obstacles[i].COM << endl;
if (field.obstacles[i].color == yellow) field.obstacles[i].regionID = 1;
else if (field.obstacles[i].color == green) field.obstacles[i].regionID = 1;
else if (field.obstacles[i].color == red) field.obstacles[i].regionID = 0;
else field.obstacles[i].regionID = 2;
}
// cin.ignore();
pair<geometry::Vector,geometry::Vector> goalDest; // first:target, second:dest
params::Color targetColor;
std::vector<geometry::Vector> path;
bool done = false;
if (field.obstacles.size() == 0) {
cout << "No obstacle found --> PAUSE mode" << endl;
pause = true;
continue;
} else {
cerr << "----after visionUpdate" << endl;
goalDest = field.bestTarget(targetColor, done);
if (done) {
cout << "end" << endl;
goalDest.first = field.regions[2].destPoints[0].first;
}
else cout << "target and dest: " << goalDest.first << " -> " << goalDest.second << " :color " << params::getColorName(targetColor) << endl;
// cin.ignore();
// if there is no more objects, break, end = true
// set degreeAtTarget
// pathfind to target
try {
field.agent.print();
path = ShortestPath(field.agent.COM, goalDest.first, field, argc, argv);
// path.push_back(field.agent.COM);
// path.push_back(field.agent.COM + geometry::Vector(-200,100));
// path.push_back(field.agent.COM + geometry::Vector(-200,0));
// path.push_back(field.agent.COM);
// path.push_back(field.agent.COM + geometry::Vector(-100,100));
// path.push_back(field.agent.COM + geometry::Vector(-200,100));
// path.push_back(field.agent.COM + geometry::Vector(-300,0));
// path.push_back(field.agent.COM + geometry::Vector(-200,-100));
// path.push_back(field.agent.COM + geometry::Vector(-100,-100));
// path.push_back(field.agent.COM);
// path.push_back(field.agent.COM + geometry::Vector(-300,0));
// path.push_back(field.agent.COM + geometry::Vector(-300,-200));
// path.push_back(field.agent.COM + geometry::Vector(-300,0));
// path.push_back(field.agent.COM + geometry::Vector(-200,0));
// path.push_back(field.agent.COM + geometry::Vector(-200,-200));
// path.push_back(field.agent.COM + geometry::Vector(-200,0));
// path.push_back(field.agent.COM + geometry::Vector(-100,0));
// path.push_back(field.agent.COM + geometry::Vector(-100,-200));
// path.push_back(field.agent.COM + geometry::Vector(-100,0));
// path.push_back(field.agent.COM + geometry::Vector(0,-0));
// path.push_back(field.agent.COM + geometry::Vector(0,-200));
// path.push_back(field.agent.COM);
// path.push_back(geometry::Vector(202,291));
vision.showPoints(path);
cout << "PATH: ";
for (geometry::Vector pt : path) {
cout << " " << pt;
}
cout << endl;
} catch (exceptions::NoPath) {
// if there is no path then what???
cout << "No Path found --> PAUSE mode" << endl;
pause = true;
continue;
}
cerr << "----end of bestTarget" << endl;
}
int x = 100;
int index = path.size()-1;
vector<int> pathSpeeds;
bool rotating = false;
preprocessPath(path, pathSpeeds);
while (!pause && Connection.move(path, pathSpeeds ,field.agent, index, rotating)) { // && not reached target
field.agent.updated = false;
while (!field.agent.updated) vision.update(field,false);
// if shasing then break
// wall check
}
cout << "----REACHED----" << endl;
Connection.fullStop();
if (done) {
cout << "-----END------" << endl;
end = true;
break;
}
// cin.ignore();
// degreeAtTarget = (goalDest.second - goalDest.first).angle();
rotating = false;
bool lost = false;
bool pushing = false;
preprocessPath(path, pathSpeeds);
while (!pause && Connection.safeMove(goalDest.second, field.agent, rotating) && !lost) { // && not reached target
field.agent.updated = false;
while (!field.agent.updated) vision.update(field,true);
for(int i = 0; i < field.obstacles.size(); i++) {
if (field.obstacles[i].color == targetColor) {
if (pushing && (field.obstacles[i].COM - field.agent.COM).size() >= params::LOST_LIMIT){
cout << "--------OBSTACLE LOST :(-----------" << params::getColorName(targetColor) << endl;
cout << (field.obstacles[i].COM - field.agent.COM).size() << endl;
lost = true;
break;
}
else if ((field.obstacles[i].COM - field.agent.COM).size() < params::LOST_LIMIT - 20) {
// Connection.fullStop();
cout << "----PUSHING-----" << endl;
// cin.ignore();
pushing = true;
}
}
}
// if shasing then break
// wall check
}
Connection.fullStop();
if (!lost) {
cout << "-----DONE------" << endl;
// cin.ignore();
}
}
while (pause && !end) {
int input = 0;
while (input != 1) {
cout << "ENTER 1 to continue: ";
cin >> input;
}
pause = false;
// full stop
// input for continue
// while pause
}
}
// float var = 25.64;
// for(int i=0 ; i<30 ; i++){
// Connection.talkToSetare(0,0,0);
// usleep(12*var*1000);
// Connection.fullStop();
// usleep(400*1000);
// }
// while(field.agent.direction < 70 ){
// Connection.talkToSetare(0,0,0);
// cout << "---- degree is :" << field.agent.direction << endl;
// vision.update(field);
// }
// Connection.fullStop();
// Connection.talkToSetare(70,0,5);
// usleep(var*10*1000);
cin.ignore();
// close port
// delete
return 0;
}
int main()
{
printf("Welcome to Supply route problem..\n");
int testcase;
int T;
scanf("%d", &T);
for(testcase=1; testcase <=T; testcase++) {
int num;
int r;
int c;
int ans;
char ch;
int k;
/*
time_t start, stop;
clock_t ticks; long count;
time(&start);*/
clock_t start, stop;
start = clock();
scanf("%d", &num);
/* Initialize Adjacency List */
for(r=0; r< num*num; r++) {
for(c=0; c<4; c++) {
adjlist[r][c] = -1;
}
}
for(r=0; r< num; r++) {
for(c=0; c< num; c++) {
scanf(" %c", &ch);
inputmat[r][c] = ch - '0';
}
}
#if 0
for(r=0; r< num; r++) {
for(c=0; c< num; c++) {
printf("[%d]", inputmat[r][c]);
}
printf("\n");
}
#endif
CreateGraph(num);
/* Diktra preparation */
for(k=0; k < num*num; k++) {
dist[k] = INF;
visited[k] = NO;
}
dist[0] = 0;
ShortestPath(num);
ans = dist[num*num -1];
printf("#%d %d\n", testcase, ans);
stop = clock();
//time(&stop);
printf("testcase [%d] started %6.3f seconds Finished %6.3f \n", testcase, start, stop /*difftime(stop, start)*/);
}
}
vector<PathData> M2MFstAligner::write_alignment(const VectorFst<LogArc> &ifst,
int nbest)
{
//Generic alignment generator
VectorFst<StdArc> fst;
Map(ifst, &fst, LogToStdMapper());
for (StateIterator<VectorFst<StdArc> > siter(fst); !siter.Done();
siter.Next()) {
StdArc::StateId q = siter.Value();
for (MutableArcIterator<VectorFst<StdArc> > aiter(&fst, q);
!aiter.Done(); aiter.Next()) {
//Prior to decoding we make several 'heuristic' modifications to the weights:
// 1. A multiplier is applied to any multi-token substrings
// 2. Any LogWeight::Zero() arc weights are reset to '99'.
// We are basically resetting 'Infinity' values to a 'smallest non-Infinity'
// so that the ShortestPath algorithm actually produces something no matter what.
// 3. Any arcs that consist of subseq1:subseq2 being the same length and subseq1>1
// are set to '99' this forces shortestpath to choose arcs where one of the
// following conditions holds true
// * len(subseq1)>1 && len(subseq2)!=len(subseq1)
// * len(subseq2)>1 && len(subseq1)!=len(subseq2)
// * len(subseq1)==len(subseq2)==1
//I suspect these heuristics can be eliminated with a better choice of the initialization
// function and maximization function, but this is the way that m2m-aligner works, so
// it makes sense for our first cut implementation.
//In any case, this guarantees that M2MFstAligner produces results identical to those
// produced by m2m-aligner - but with a bit more reliability.
//UPDATE: this now produces a better alignment than m2m-aligner.
// The maxl heuristic is still in place. The aligner will produce *better* 1-best alignments
// *without* the maxl heuristic below, BUT this comes at the cost of producing a less
// flexible corpus. That is, for a small training corpus like nettalk, if we use the
// best alignment we wind up with more 'chunks' and thus get a worse coverage for unseen
// data. Using the aignment lattices to train the joint ngram model solves this problem.
// Oh baby. Can't wait to for everyone to see the paper!
//NOTE: this is going to fail if we encounter any alignments in a new test item that never
// occurred in the original model.
StdArc
arc = aiter.Value();
int
maxl = get_max_length(isyms->Find(arc.ilabel));
if (maxl == -1) {
arc.weight = 999;
}
else {
//Optionally penalize m-to-1 / 1-to-m links. This produces
// WORSE 1-best alignments, but results in better joint n-gram
// models for small training corpora when using only the 1-best
// alignment. By further favoring 1-to-1 alignments the 1-best
// alignment corpus results in a more flexible joint n-gram model
// with regard to previously unseen data.
//if( penalize==true ){
arc.weight = alignment_model[arc.ilabel].Value() * maxl;
//}else{
//For larger corpora this is probably unnecessary.
//arc.weight = alignment_model[arc.ilabel].Value();
//}
}
if (arc.weight == LogWeight::Zero())
arc.weight = 999;
if (arc.weight != arc.weight)
arc.weight = 999;
aiter.SetValue(arc);
}
}
VectorFst<StdArc> shortest;
ShortestPath(fst, &shortest, nbest);
RmEpsilon(&shortest);
//Skip empty results. This should only happen
// in the following situations:
// 1. seq1_del=false && len(seq1)<len(seq2)
// 2. seq2_del=false && len(seq1)>len(seq2)
//In both 1.and 2. the issue is that we need to
// insert a 'skip' in order to guarantee at least
// one valid alignment path through seq1*seq2, but
// user params didn't allow us to.
//Probably better to insert these where necessary
// during initialization, regardless of user prefs.
if (shortest.NumStates() == 0) {
vector<PathData> dummy;
return dummy;
}
FstPathFinder
pathfinder(skipSeqs);
pathfinder.isyms = isyms;
pathfinder.findAllStrings(shortest);
return pathfinder.paths;
}
Path MagicPathBetween(int x1,int y1,int x2,int y2)
{
int v1=ConvertCoordinateToIndex(x1,y1);
int v2=ConvertCoordinateToIndex(x2,y2);
return ShortestPath(mapForAttack,v1,v2);
}
void SwerveDrive::Move(
double &speed,
double &angle,
double &rotation,
bool &stop
)
{
m_speed = speed;
m_angle = angle;
m_rotation = rotation;
if (stop)
{
Stop();
return;
}
/*
Thoughts and notes
We should calculate an ideal speed and angle for each wheel (ie,
our goal), and tell the wheel servos to go there. However, we must
set the velocities of the wheels based on the current angle of the
wheels so that we don't go in weird directions
Question: is this actually true? Should we just sit until the wheels
turn (this seems like a bad idea)
Currently, neither of those is implemented.
*/
// special case: doing nothing
if (fabs(speed) < 0.0001 && fabs(rotation) < 0.0001)
{
// this forces motors to stop and keeps the wheels pointed in the
// same direction, much less annoying
m_chassis->motor_lf.SetSpeed(0);
m_chassis->motor_lr.SetSpeed(0);
m_chassis->motor_rf.SetSpeed(0);
m_chassis->motor_rr.SetSpeed(0);
return;
}
// set limitations on speed
if (speed < -1.0)
speed = -1.0;
else if (speed > 1.0)
speed = 1.0;
// set limitations on rotation
if (rotation < -1.0)
rotation = -1.0;
else if (rotation > 1.0)
rotation = 1.0;
// convert speed/angle to Vx/Vy (offset angle by 90 degrees)
double Vtx = speed * cos( ((angle+90.0) * M_PI)/180.0 );
double Vty = speed * sin( ((angle+90.0) * M_PI)/180.0 );
// LF, LR, RF, RR
double speeds[] = { 0.0, 0.0, 0.0, 0.0 };
double lf_angle, lr_angle, rf_angle, rr_angle;
// find the speed and angle of each wheel (first two params passed back)
CalculateWheel(speeds[0], lf_angle, Vtx, Vty, rotation, LF_DISPLACEMENT);
CalculateWheel(speeds[1], lr_angle, Vtx, Vty, rotation, LR_DISPLACEMENT);
CalculateWheel(speeds[2], rf_angle, Vtx, Vty, rotation, RF_DISPLACEMENT);
CalculateWheel(speeds[3], rr_angle, Vtx, Vty, rotation, RR_DISPLACEMENT);
// then limit all motors based on the highest motor speed
double highest_speed = 0;
for (int i = 0; i < 4; i++)
if (fabs(speeds[i]) > highest_speed)
highest_speed = fabs(speeds[i]);
// only need to scale if speed > 1
if (highest_speed > 1.0 )
{
// scale each speed by the highest speed
for (int i = 0; i < 4; i++)
speeds[i] /= highest_speed;
}
m_servo_lf = lf_angle;
m_servo_lr = lr_angle;
m_servo_rf = rf_angle;
m_servo_rr = rr_angle;
// calculate the shortest path to the setpoint
ShortestPath(speeds[0], lf_angle, m_chassis->servo_lf.GetCurrentAngle(), m_pick_alt_lf, m_pickEvent_lf);
ShortestPath(speeds[1], lr_angle, m_chassis->servo_lr.GetCurrentAngle(), m_pick_alt_lr, m_pickEvent_lr);
ShortestPath(speeds[2], rf_angle, m_chassis->servo_rf.GetCurrentAngle(), m_pick_alt_rf, m_pickEvent_rf);
ShortestPath(speeds[3], rr_angle, m_chassis->servo_rr.GetCurrentAngle(), m_pick_alt_rr, m_pickEvent_rr);
// set the motors
m_chassis->servo_lf.SetAngle(lf_angle);
m_chassis->servo_lr.SetAngle(lr_angle);
m_chassis->servo_rf.SetAngle(rf_angle);
m_chassis->servo_rr.SetAngle(rr_angle);
m_chassis->motor_lf.SetSpeed((float)speeds[0]);
m_chassis->motor_lr.SetSpeed((float)speeds[1]);
m_chassis->motor_rf.SetSpeed((float)speeds[2]);
m_chassis->motor_rr.SetSpeed((float)speeds[3]);
}
