double Solver::solve(double temp,
double stopTemp,
double alpha,
double beta,
double gamma,
int itersPerTemp,
std::list<double> &costs) {
Random rand;
double bestCost = calcCost();
std::vector<int> bestSolution = solution;
//dodane
#if !defined(CLI)
GUIDataObject.clear(GUIData::temperatureEnum);
#endif
while(temp > stopTemp) {
for(int i = 0; i < itersPerTemp; ++i) {
permuteSolution(temp * gamma + 1);
double cost = calcCost();
if(cost < bestCost) {
costs.push_back(cost);
bestCost = cost;
bestSolution = solution;
//dodane
#if !defined(CLI)
GUIDataObject.temperature_pushBack(temp);
#endif
} else {
if(rand.randf() < std::exp(((bestCost - cost) * beta) / temp)) {
costs.push_back(cost);
//dodane
#if !defined(CLI)
GUIDataObject.temperature_pushBack(temp);
#endif
bestCost = cost;
bestSolution = solution;
} else {
solution = bestSolution;
}
}
}
temp *= alpha;
}
return bestCost;
}
v3s16 Pathfinder::getDirHeuristic(std::vector<v3s16> &directions, PathGridnode &g_pos)
{
int minscore = -1;
v3s16 retdir = v3s16(0, 0, 0);
v3s16 srcpos = g_pos.pos;
DEBUG_OUT("Pathfinder: remaining dirs at beginning:"
<< directions.size() << std::endl);
for (std::vector<v3s16>::iterator iter = directions.begin();
iter != directions.end();
++iter) {
v3s16 pos1 = v3s16(srcpos.X + iter->X, 0, srcpos.Z+iter->Z);
int cur_manhattan = getXZManhattanDist(pos1);
PathCost cost = g_pos.getCost(*iter);
if (!cost.updated) {
cost = calcCost(g_pos.pos, *iter);
g_pos.setCost(*iter, cost);
}
if (cost.valid) {
int score = cost.value + cur_manhattan;
if ((minscore < 0)|| (score < minscore)) {
minscore = score;
retdir = *iter;
}
}
}
if (retdir != v3s16(0, 0, 0)) {
for (std::vector<v3s16>::iterator iter = directions.begin();
iter != directions.end();
++iter) {
if(*iter == retdir) {
DEBUG_OUT("Pathfinder: removing return direction" << std::endl);
directions.erase(iter);
break;
}
}
}
else {
DEBUG_OUT("Pathfinder: didn't find any valid direction clearing"
<< std::endl);
directions.clear();
}
DEBUG_OUT("Pathfinder: remaining dirs at end:" << directions.size()
<< std::endl);
return retdir;
}
double
Distance::DTW(const TimeSeries &ts1, const TimeSeries &ts2) {
int len1 = ts1.length();
int len2 = ts2.length();
double *table_d = new double[len1 * len2];
double *table_g = new double[len1 * len2];
calcCost(ts1, ts2, table_d, table_g, len1, len2);
calcGamma(table_d, table_g, len1, len2);
double dist = calcSum(table_d, table_g, len1, len2);
delete[] table_d;
delete[] table_g;
return dist;
}
void Pathfinder::pqAddto(t_tmp& actualNode, priority_queue<t_tmp>& pq, const int& targetNet, list<int>& targetNodes, rt_dir direction){
t_tmp tmp;
int lCost, nextNode;
list<int>::iterator target_it;
nextNode=getDir(actualNode.node, direction);
if(!(graph[nextNode].isVisited(trace_id) || (isSource(nextNode) && graph[nextNode].net!=-targetNet))){
tmp.node=nextNode;
tmp.father=actualNode.node;
tmp.costAccumulated=actualNode.costAccumulated+calcCost(nextNode, direction);
lCost=calcDistance(nextNode, targetNodes.front());
for(target_it=++targetNodes.begin(); target_it!=targetNodes.end(); ++target_it)
lCost=min(lCost,calcDistance(nextNode, *target_it));
tmp.aStarCost=tmp.costAccumulated+lCost;
// cout << "(" << getPosX(actualNode.node) << " " <<getPosY(actualNode.node)<< " " <<getPosZ(actualNode.node)<< ") --(" << tmp.aStarCost <<")--> (" << getPosX(tempNode.node) << " " <<getPosY(tempNode.node)<< " " <<getPosZ(tempNode.node)<< ") " << endl;
pq.push(tmp);
}
}
bool Routing::classifyPoint(Point adjacentPt, Point current, Point parentOfCurrent, Point goal)
{
unsigned int adjacentPtCost = calcCost(parentOfCurrent, current, adjacentPt) + nodeList.calcCost(current);
//Make it OPEN if it is NONE; if it is OPEN check if we can get there faster via the current node, if so we change the parent
switch (nodeList.getListType(adjacentPt))
{
case NodeList::OPEN:
nodeList.update(adjacentPt, current, adjacentPtCost);
break;
case NodeList::NONE:
if( ! maze->isPassable(adjacentPt))
nodeList.close(adjacentPt);
else
nodeList.update(adjacentPt, current, adjacentPtCost, calcHeuristic(adjacentPt, goal));
break;
case NodeList::GOAL:
nodeList.update(adjacentPt, current, adjacentPtCost);
return true;
}
return false;
}
int BaseCostFunction::forwardUpdate(int timestep){
Activation::forwardUpdate(timestep);
//timestep + 2 to keep stats until writePeriod
if((timestep + 2) % writePeriod == 0){
//reset counts
reset();
}
//Move device est and gt activity to host
updateHostData();
//Calculate costs and accuracy
currCost = calcCost();
currCorrect = calcCorrect();
sumCost += currCost;
numCorrect += currCorrect;
numTests += bSize;
if(estFilename != ""){
writeEst();
}
//timestep + 1 to skip timestep 0
if((timestep+1) % writePeriod == 0){
if(costFilename != ""){
costFile << timestep << "," << getAverageCost() << std::endl;
}
if(accuracyFilename != ""){
accuracyFile << timestep << "," << getAccuracy() << std::endl;
std::cout << "Timestep: " << timestep << " accuracy " << getAccuracy() << "\n";
}
}
return SUCCESS;
}
/// Run the Delayed Sparse Information Filter (Eustice et al.)
/// <newnode> is the index of the first new node added since the last iteration
void SysSPA2d::doDSIF(int newnode)
{
// number of nodes
int nnodes = nodes.size();
// set number of constraints
int ncons = p2cons.size();
// check for fixed frames
if (nFixed <= 0)
{
cout << "[doDSIF] No fixed frames" << endl;
return;
}
// check for newnode being ok
if (newnode >= nnodes)
{
cout << "[doDSIF] no new nodes to add" << endl;
return;
}
else // set up saved mean value of pose
{
for (int i=newnode; i<nnodes; i++)
{
nodes[i].oldtrans = nodes[i].trans;
nodes[i].oldarot = nodes[i].arot;
}
}
for (int i=0; i<nnodes; i++)
{
Node2d &nd = nodes[i];
if (i >= nFixed)
nd.isFixed = false;
else
nd.isFixed = true;
nd.setTransform(); // set up world-to-node transform for cost calculation
nd.setDr(); // always use local angles
}
// initialize vars
double cost = calcCost();
if (verbose)
cout << " Initial squared cost: " << cost << " which is "
<< sqrt(cost/ncons) << " rms error" << endl;
// set up and solve linear system
long long t0, t1, t2, t3;
t0 = utime();
setupSparseDSIF(newnode); // set up sparse linear system
#if 0
cout << "[doDSIF] B = " << csp.B.transpose() << endl;
csp.uncompress(A);
cout << "[doDSIF] A = " << endl << A << endl;
#endif
// cout << "[SPA] Solving...";
t1 = utime();
bool ok = csp.doChol();
if (!ok)
cout << "[doDSIF] Sparse Cholesky failed!" << endl;
t2 = utime();
// cout << "solved" << endl;
// get correct result vector
VectorXd &BB = csp.B;
// cout << "[doDSIF] RES = " << BB.transpose() << endl;
// update the frames
int ci = 0;
for(int i=0; i < nnodes; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.trans.head(2) = nd.oldtrans.head(2)+BB.segment<2>(ci);
nd.arot = nd.oldarot+BB(ci+2);
nd.normArot();
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr(); // set rotational derivatives
ci += 3; // advance B index
}
// new cost
double newcost = calcCost();
if (verbose)
cout << " Updated squared cost: " << newcost << " which is "
<< sqrt(newcost/ncons) << " rms error" << endl;
t3 = utime();
}
int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
int maxCGiters)
{
// number of nodes
int ncams = nodes.size();
// set number of constraints
int ncons = p2cons.size();
// check for fixed frames
if (nFixed <= 0)
{
cout << "[doSPA2d] No fixed frames" << endl;
return 0;
}
for (int i=0; i<ncams; i++)
{
Node2d &nd = nodes[i];
if (i >= nFixed)
nd.isFixed = false;
else
nd.isFixed = true;
nd.setTransform(); // set up world-to-node transform for cost calculation
nd.setDr(); // always use local angles
}
// initialize vars
if (sLambda > 0.0) // do we initialize lambda?
lambda = sLambda;
double laminc = 2.0; // how much to increment lambda if we fail
double lamdec = 0.5; // how much to decrement lambda if we succeed
int iter = 0; // iterations
sqMinDelta = 1e-8 * 1e-8;
double cost = calcCost();
if (verbose)
cout << iter << " Initial squared cost: " << cost << " which is "
<< sqrt(cost/ncons) << " rms error" << endl;
int good_iter = 0;
double cumTime = 0;
for (; iter<niter; iter++) // loop at most <niter> times
{
// set up and solve linear system
// NOTE: shouldn't need to redo all calcs in setupSys if we
// got here from a bad update
long long t0, t1, t2, t3;
t0 = utime();
if (useCSparse)
setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
else
setupSys(lambda); // set up linear system
// cout << "[SPA] Solving...";
t1 = utime();
// use appropriate linear solver
if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
{
if (csp.B.rows() != 0)
{
int iters = csp.doBPCG(maxCGiters,initTol,iter);
if (verbose)
cout << "[Block PCG] " << iters << " iterations" << endl;
}
}
#ifdef SBA_DSIF
// PCG with incomplete Cholesky
else if (useCSparse == 3)
{
int res = csp.doPCG(maxCGiters);
if (res > 1)
cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
}
#endif
else if (useCSparse > 0)
{
if (csp.B.rows() != 0)
{
bool ok = csp.doChol();
if (!ok)
cout << "[DoSBA] Sparse Cholesky failed!" << endl;
}
}
// Dense direct Cholesky
else
A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
t2 = utime();
// cout << "solved" << endl;
// get correct result vector
VectorXd &BB = useCSparse ? csp.B : B;
// check for convergence
// this is a pretty crummy convergence measure...
double sqDiff = BB.squaredNorm();
if (sqDiff < sqMinDelta) // converged, done...
{
if (verbose)
cout << "Converged with delta: " << sqrt(sqDiff) << endl;
break;
}
// update the frames
int ci = 0;
for(int i=0; i < ncams; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.oldtrans = nd.trans; // save in case we don't improve the cost
nd.oldarot = nd.arot;
nd.trans.head<2>() += BB.segment<2>(ci);
nd.arot += BB(ci+2);
nd.normArot();
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr(); // set rotational derivatives
ci += 3; // advance B index
}
// new cost
double newcost = calcCost();
if (verbose)
cout << iter << " Updated squared cost: " << newcost << " which is "
<< sqrt(newcost/ncons) << " rms error" << endl;
// check if we did good
if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
{
cost = newcost;
lambda *= lamdec; // decrease lambda
// laminc = 2.0; // reset bad lambda factor; not sure if this is a good idea...
good_iter++;
}
else
{
lambda *= laminc; // increase lambda
laminc *= 2.0; // increase the increment
// reset nodes
for(int i=0; i<ncams; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.trans = nd.oldtrans;
nd.arot = nd.oldarot;
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr();
}
cost = calcCost(); // need to reset errors
if (verbose)
cout << iter << " Downdated cost: " << cost << endl;
// NOTE: shouldn't need to redo all calcs in setupSys
}
t3 = utime();
if (iter == 0 && verbose)
{
printf("[SPA] Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms\n",
0.001*(double)(t1-t0),
0.001*(double)(t2-t1),
0.001*(double)(t3-t2));
}
double dt=1e-6*(double)(t3-t0);
cumTime+=dt;
if (print_iros_stats){
cerr << "iteration= " << iter
<< "\t chi2= " << cost
<< "\t time= " << dt
<< "\t cumTime= " << cumTime
<< "\t kurtChi2= " << cost
<< endl;
}
}
// return number of iterations performed
return good_iter;
}
//Find the shortest path using a maze router
int Pathfinder2::bfsRoute2(list<unsigned int>& sourceNodes, const unsigned int targetNet, const unsigned int label, const bool checkConflict, const unsigned int maxCost, unsigned int actualAttempt, bool definitive ){
vector<t_trace> trace(graph.size());
for (int n=0; n<trace.size(); n++) trace[n].clear();
list<t_arc*>::iterator arc_it;
t_tmp tmp, actualNode;
//Initialize priority queue PQ with the sourceNodes
priority_queue<t_tmp> pq;
tmp.costAccumulated=0;
tmp.father=-1;
for(list<unsigned int>::iterator nodes_it = sourceNodes.begin(); nodes_it != sourceNodes.end(); nodes_it++){
tmp.node=*nodes_it;
pq.push(tmp);
trace[*nodes_it].visited=true;
}
actualNode=pq.top();
//Lool until new sink is found
while(!pq.empty() && (graph[actualNode.node].net!=targetNet || actualNode.father==-1)){
//Remove lowest cost node from PQ
actualNode=pq.top();
pq.pop();
if(actualNode.father!=-1 && trace[actualNode.node].visited) continue;
//Save the path
trace[actualNode.node].setFather(actualNode.father,actualNode.link);
//Put the adjacent nodes in the Priority Queue ordened by the costs from the source
for(arc_it = graph[actualNode.node].arcs.begin(); arc_it != graph[actualNode.node].arcs.end(); arc_it++){
// if is not visited yet, isn't a source from other net, dont have blockage if checkConflict is enabled neither is walking from the same net, add it to the queue
if(!trace[(*arc_it)->node1].visited && !(graph[(*arc_it)->node1].source && graph[(*arc_it)->node1].net!=targetNet) && !(checkConflict && ((graph[(*arc_it)->node1].net!=-1 && graph[(*arc_it)->node1].net!=targetNet) || (*arc_it)->label==label)) && ((*arc_it)->lockNet==targetNet || (*arc_it)->lockNet==-1)){
tmp.node=(*arc_it)->node1;
tmp.father=actualNode.node;
tmp.link=*arc_it;
tmp.costAccumulated=calcCost(*arc_it, (*arc_it)->node1, targetNet, 1)+actualNode.costAccumulated;
if((maxCost==0 || tmp.costAccumulated<=maxCost) && ((graph[tmp.node].net==-1) || (graph[tmp.node].net==targetNet))) {
pq.push(tmp);
}
}
else if(!trace[(*arc_it)->node2].visited && !(graph[(*arc_it)->node2].source && graph[(*arc_it)->node2].net!=targetNet) && !(checkConflict && ((graph[(*arc_it)->node2].net!=-1 && graph[(*arc_it)->node2].net!=targetNet) || (*arc_it)->label==label)) && ((*arc_it)->lockNet==targetNet || (*arc_it)->lockNet==-1)){
tmp.node=(*arc_it)->node2;
tmp.father=actualNode.node;
tmp.link=*arc_it;
tmp.costAccumulated=calcCost(*arc_it, (*arc_it)->node2, targetNet, 1)+actualNode.costAccumulated;
if((maxCost==0 || tmp.costAccumulated<=maxCost) && ((graph[tmp.node].net==-1) || (graph[tmp.node].net==targetNet))) {
pq.push(tmp);
}
}
}
}
//Trace the path back to the source node
if(graph[actualNode.node].net==targetNet && actualNode.father!=-1){
int node = actualNode.node;
while (trace[node].father!=-1) {
sourceNodes.push_back(node); //Put these new nodes in the Routing Tree
if (definitive){
graph[node].net=targetNet;
trace[node].link->label=label;
}
node = trace[node].father;
}
return actualNode.costAccumulated;
}
return -1;
}
void Solution::removeShift(Shift* shift){
//Hour interval
int iniHour_ = (shift->getInitialInstant());
int finalHour_ = (shift->getFinalInstant());
//driverInst_
int driverIndex_ = shift->getDriver()->getIndex();
for(int i=iniHour_;i<=finalHour_;i++){
for(int j=0;j<driverInst_[driverIndex_][i].size();j++){//For every hour on the driver's list
if(driverInst_[driverIndex_][i].at(j)==shift){//Checking to see if the shift is present on that position
driverInst_[driverIndex_][i].erase(driverInst_[driverIndex_][i].begin()+j);//If it is, remove it.
}
}
}
//trailerInst_
int trailerIndex_ = shift->getTrailer()->getIndex();
for(int i=iniHour_;i<=finalHour_;i++){
for(int j=0;j<trailerInst_[trailerIndex_][i].size();j++){//For every hour on the trailer's list
if(trailerInst_[trailerIndex_][i].at(j)==shift){//Checking to see if the shift is present on that position
trailerInst_[trailerIndex_][i].erase(trailerInst_[trailerIndex_][i].begin()+j);//If it is, remove it.
}
}
}
//locationInstStop_
for(int j=0;j<shift->getStop()->size();j++){//j = stop
int locationIndex_ = shift->getStop()->at(j)->getLocation()->getIndex();//Getting each location index
double iniHourStop_ = shift->getStop()->at(j)->getArriveTime();//initial hour of arrival at location
double finalHourStop_ = shift->getStop()->at(j)->getArriveTime();//setting up the final hour of arrival/ hour of departure from location
if( instanceof<Customer>(shift->getStop()->at(j)->getLocation())){
finalHourStop_ += ((Customer*)shift->getStop()->at(j)->getLocation())->getSetupTime();
//Updating the Stock of the customer
for(int time=iniHourStop_;time<stockLevelInst_[locationIndex_].size();time++){
if(stockLevelInst_[locationIndex_][time]==0)
break;
stockLevelInst_[locationIndex_][time] -= shift->getStop()->at(j)->getQuantity();
if(stockLevelInst_[locationIndex_][time]<0){
stockLevelInst_[locationIndex_][time]=0;
}
}
}
else if( instanceof<Source>(shift->getStop()->at(j)->getLocation()) ){
finalHourStop_ += ((Source*)shift->getStop()->at(j)->getLocation())->getSetupTime();
}
for(int i=iniHourStop_;i<=finalHourStop_;i++){
for(int k=0;k<locationInstStop_[locationIndex_][i].size();k++){//For every hour on the locations's list
if(locationInstStop_[locationIndex_][i].at(k) == shift->getStop()->at(j)){
//Removing Stop
locationInstStop_[locationIndex_][i].erase(locationInstStop_[locationIndex_][i].begin()+k);//If it is, remove it.
}
}
}
}
for(int j=0;j<trailersShifts_.at(trailerIndex_).size();j++){
if(trailersShifts_.at(trailerIndex_).at(j)==shift){//Checking to see if the shift is present on that position
trailersShifts_.at(trailerIndex_).erase(trailersShifts_.at(trailerIndex_).begin()+j);//If it is, remove it.
}
}
shift->setSolution(NULL);
calcCost();
/**
TODO -> ( )VERIFICAR OS SHIFTS VIZINHOS DO TRAILER
(x)MODIFICAR OS ESTOQUES DE TODOS OS STOPS
(x)RETORNAR OS NOVOS CUSTO
**/
}
