vector <double> solve(int verbose, vector <vector <double> > M) {
vector <double> solution;
try {
GRBEnv env = GRBEnv();
int numPoints = M.size();
int numCircles = M.back().size();
GRBVar p[numPoints];
double coeff[numPoints];
GRBModel model = GRBModel(env);
GRBLinExpr expr;
if(!verbose)
model.getEnv().set(GRB_IntParam_OutputFlag, 0);
for (int i=0;i<numPoints;i++){
p[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY);
coeff[i] = 1.0;
}
expr.addTerms(coeff, p,numPoints);
model.update();
model.setObjective(expr, GRB_MINIMIZE);
for(int i=0;i<numCircles;i++){
GRBLinExpr cexpr;
double ccoeff[numPoints];
for(int j=0;j<numPoints;j++){
ccoeff[j] = M[j].back();
M[j].pop_back();
}
cexpr.addTerms(ccoeff,p,numPoints);
model.addConstr(cexpr, GRB_GREATER_EQUAL,1.0);
}
// Optimize model
model.optimize();
for (int i=0;i<numPoints;i++){
solution.push_back((double)p[i].get(GRB_DoubleAttr_X));
}
} catch(GRBException e) {
cout << "Error code = " << e.getErrorCode() << endl;
cout << e.getMessage() << endl;
} catch(...) {
cout << "Exception during optimization" << endl;
}
return solution;
}
void PushObject::updateModel(const Eigen::MatrixXd& traj, GRBQuadExpr& objective) {
VectorXd& times = m_problem->m_times;
MatrixXd perts_tk = getSinBasis(times/times.maxCoeff(), fmin(6, times.size()/2));
MatrixXd pinvperts_tk = perts_tk * (perts_tk.transpose() * perts_tk).inverse();
m_exactObjective = simulateTraj2(traj, true); // current value
LOG_INFO_FMT("current val: %.3f", m_exactObjective);
MatrixXd dy_jk(traj.cols(), perts_tk.cols());
double eps = 3e-4; // scale for joint angle change
Matrix3d A;
A << sq(eps/2), eps/2, 1,
0, 0, 1,
sq(eps/2), -eps/2, 1;
Matrix3d Ainv = A.inverse();
MatrixXd grad_tj(traj.rows(), traj.cols());
m_obj = GRBQuadExpr(0);
for (int j = 0; j < traj.cols(); ++j) {
VarVector v;
VectorXd vactual(traj.rows()-1);
for (int t=1; t < traj.rows(); ++t) {
v.push_back(m_problem->m_trajVars.at(t,j));
vactual(t-1) = traj(t,j);
}
for (int k = 0; k < perts_tk.cols(); ++k) {
MatrixXd newTraj = traj;
newTraj.col(j) = traj.col(j) + (eps/2)*perts_tk.col(k);
double plusVal = simulateTraj2(newTraj, false);
newTraj.col(j) = traj.col(j) - (eps/2)*perts_tk.col(k);
double minusVal = simulateTraj2(newTraj, false);
LOG_DEBUG_FMT("joint %i, basis %i, pert vals: %.4e %.4e ", j, k, plusVal-m_exactObjective,minusVal-m_exactObjective);
dy_jk(j,k) = (plusVal - minusVal)/eps;
Vector3d y;
y << plusVal-m_exactObjective, 0, minusVal - m_exactObjective;
Vector3d abc = Ainv*y;
GRBLinExpr q;
int T = traj.rows()-1;
VectorXd pertVec=perts_tk.block(1,k,T, 1);
q.addTerms(pertVec.data(),v.data(), T);
double qactual = pertVec.dot(vactual);
m_obj += fmax(abc(0),0)*(q-qactual)*(q-qactual) + abc(1)*(q-qactual);
}
// grad_tj.col(j) = pinvperts_tk * dy_jk.row(j).transpose();
}
m_obj += m_exactObjective;
// cout << "dy_jk:" << endl;
// cout << dy_jk << endl;
// cout << "grad_tj:" << endl;
// cout << grad_tj << endl;
// m_obj.addTerms(grad_tj.data()+7, m_problem->m_trajVars.m_data.data()+7, (traj.rows()-1)*traj.cols());
// m_obj += m_exactObjective - (grad_tj.middleRows(1, traj.rows()-1).array() * traj.middleRows(1, traj.rows()-1).array()).sum();
objective += m_obj;
}
int main(){
float peso1, peso2, peso3,peso4;
int origem, destino; // vértices para cada aresta;
int id = 0; // id das arestas que leremos do arquivo para criar o grafo
cin>>n; // quantidade de vértices do grafo;
arestas = new short*[n];
coeficienteObjetv = new double*[n];
matrix_peso1 = new double*[n];
matrix_peso2 = new double*[n];
matrix_peso3 = new double*[n];
matrix_peso4 = new double*[n];
for (int i=0; i<n; i++){
arestas[i] = new short[n];
coeficienteObjetv[i] = new double[n];
matrix_peso1[i] = new double[n];
matrix_peso2[i] = new double[n];
matrix_peso3[i] = new double[n];
matrix_peso4[i] = new double[n];
}
GRBEnv env = GRBEnv();;
env.set("OutputFlag","0");
GRBModel model = GRBModel(env);;
GRBVar **y, **x;
float epslon = 0.0001;
//cin>>epslon;
y = new GRBVar*[n];
x = new GRBVar*[n];
for (int i=0; i<n;i++){
y[i] = new GRBVar[n];
x[i] = new GRBVar[n];
}
int constrCont=0;
// Create variables
for (int i=0; i<n; i++){
for (int j=0; j<n; j++){
arestas[i][j] = 0;
}
}
while (cin>>origem){
cin>>destino;
cin>>peso1;
cin>>peso2;
cin>>peso3;
cin>>peso4;
coeficienteObjetv[origem][destino] = (peso1*epslon + peso2*epslon + peso3*epslon + peso4)*(-1); // o problema é de maximizacao
x[origem][destino] = model.addVar(0.0, 100000, 0.0, GRB_CONTINUOUS, "x"+to_string(origem)+to_string(destino));
x[destino][origem] = model.addVar(0.0, 100000, 0.0, GRB_CONTINUOUS, "x"+to_string(destino)+to_string(origem));
y[origem][destino] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY, "y"+to_string(origem)+to_string(destino));
arestas[origem][destino] = 1;
arestas[destino][origem] = 1;
matrix_peso1[origem][destino] = peso1*(-1);
matrix_peso2[origem][destino] = peso2*(-1);
matrix_peso3[origem][destino] = peso3*(-1);
matrix_peso4[origem][destino] = peso4*(-1);
id++;
}
int nA = id; // quantidade de arestas do grafo
int m = 1;// por default, o m falado por Lokman and Koksalan sera igual a 1
model.update();
// Set objective:
GRBLinExpr exprObjet;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1)
exprObjet.addTerms(&coeficienteObjetv[i][j], &y[i][j],1);
}
}
model.setObjective(exprObjet,GRB_MAXIMIZE);
// constraint 3.9 (FERNANDES, 2016)
GRBLinExpr constr5 ;
double coefff = 1;
for (int j=0+1; j<n; j++){
if (arestas[0][j] == 1)
constr5.addTerms(&coefff,&x[0][j],1);
}
model.addConstr(constr5, GRB_EQUAL, n-1,to_string(constrCont++));
// // Add constraint 3.10 (FERNANDES, 2016)
double com = -1;
for (int j=1; j<n; j++){
GRBLinExpr constr2 = 0;
for (int i=0; i<n; i++){
if (arestas[i][j] == 1){
constr2.addTerms(&coefff,&x[i][j],1);
constr2.addTerms(&com,&x[j][i],1);
}
}
model.addConstr(constr2, GRB_EQUAL, 1,to_string(constrCont++));
}
double coef = (double) n - 1;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1){
GRBLinExpr constr8;
GRBLinExpr constr9;
constr8.addTerms(&coef,&y[i][j],1);
constr9.addTerms(&coefff ,&x[i][j],1);
constr9.addTerms(&coefff ,&x[j][i],1);
model.addConstr(constr8, GRB_GREATER_EQUAL, constr9,to_string(constrCont++));
}
}
}
//cout<<"Modelo carregado"<<endl;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1){
GRBLinExpr constr22;
GRBLinExpr constr33;
constr22.addTerms(&coefff ,&y[i][j],1);
constr33.addTerms(&coefff ,&x[i][j],1);
constr33.addTerms(&coefff ,&x[j][i],1);
// cout<<constr22<<GRB_LESS_EQUAL<<constr33<<endl;
model.addConstr(constr22, GRB_LESS_EQUAL, constr33,to_string(constrCont++));
}
}
}
int nn = 0; // o 'n' do algoritmo de Lokman and Koksalan
//int kk_estrela = 0; // o 'k*' do algoritmo 2 de Lokman and Koksalan
int MM = 100000000; // o 'M' do algoritmo 2 de Lokman and Koksalan
int z4_k_estrela; // pra guardar o Z_p^(P^(b^(k*,n))) do algoritmo 2 de Lokman and Koksalan
/*
* Algoritmo 2 de Lokman and Koksalan
*/
try {
times(&tempsInit);
// para medir o tempo em caso limite
pthread_t thread_time;
pthread_attr_t attr;
int nnnnnnnn=0;
if(pthread_create(&thread_time, NULL, &tempo, (void*)nnnnnnnn)){ // on criee efectivement la thread de rechaufage
cout<<"Error to create the thread"<<endl;
exit(EXIT_FAILURE);
}
//
bool auxbol = false; // vira true (e o será pra sempre) quando resolvemos um modelo diferente do SIMPLES
int optimstatus;
short **result = new short*[n];
for (int ii=0; ii<n; ii++){
result[ii] = new short[n];
}
model.optimize(); // P0,4 --> n=0 (modelo SIMPLES)
optimstatus = model.get(GRB_IntAttr_Status);
int z1=0,z2=0,z3=0,z4=0;
if (optimstatus != GRB_INFEASIBLE){
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1){
result[i][j] = y[i][j].get(GRB_DoubleAttr_X); // GUARDA O RESULTADO
z1+=result[i][j]*matrix_peso1[i][j]; // calcula os pesos
z2+=result[i][j]*matrix_peso2[i][j];
z3+=result[i][j]*matrix_peso3[i][j];
z4+=result[i][j]*matrix_peso4[i][j];
}
}
}
S.push_back(result);
Pesos ppp = (Pesos){z1,z2,z3,z4};
Z.push_back(ppp);
nn++;
}
do{ // esse loop para quando z4_k_estrela==-MM
z4_k_estrela =(-1)*MM; // guarda o maximo
short **z_n_plus_1 = new short*[n];
for (int ii=0; ii<n; ii++){
z_n_plus_1[ii] = new short[n];
}
int z1_estrela, z2_estrela, z3_estrela;
for (int ki=-1; ki<nn; ki++){ // no algoritmo original, ki deve variar de 0 à n (existindo solucoes de 1 à n).
//aqui, portanto, fazemos k de -1 à n-1, porque as solucoes vao de 0 à n-1
for (int kj=-1; kj<nn; kj++){ // como i de ver menor que j, a unica possibilidade é i=1 e j=2, pois p-2=2
//cout<<ki<<" "<<kj<<endl;
//cout<< Z[ki].peso1<<" "<< Z[kj].peso2<<endl;
//if (kj!=-1 && ki!=-1 && (Z[ki].peso1 + 1 <= Z[kj].peso1)){
//Primeiramente, prepara o b1 e b2 e b3
int b1,b2,b3;
//b1
if (ki==-1) b1=(-1)*MM; // -M
else {
b1 = Z[ki].peso1 + 1;
}
//b2
if (kj==-1) b2=(-1)*MM; // -M
else {
if (Z[kj].peso1 >= b1){
b2 = Z[kj].peso2 + 1;
} else b2=(-1)*MM; // -M
}
//b3
b3 = (-1)*MM; // Snk = vazio
for (int ii=0; ii<S.size(); ii++){
if (Z[ii].peso1>=b1 && Z[ii].peso2>=b2) {
if (Z[ii].peso3 > b3){
b3 = Z[ii].peso3;
}
}
}
if (b3!=(-1)*MM) b3=b3+1; // max + 1
//cout <<"b1= "<<b1<<" b2= "<<" "<<b2<<" b3= "<<b3<<endl;
if (auxbol == true){ // remove as restricoes de z2>b2 e adiciona novas
GRBConstr cb1 = model.getConstrByName("cb1");
GRBConstr cb2 = model.getConstrByName("cb2");
GRBConstr cb3 = model.getConstrByName("cb3");
model.remove(cb1);
model.remove(cb2);
model.remove(cb3);
}
GRBLinExpr cb1;
GRBLinExpr cb2;
GRBLinExpr cb3;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1){
cb1.addTerms(&matrix_peso1[i][j], &y[i][j],1);
cb2.addTerms(&matrix_peso2[i][j], &y[i][j],1);
cb3.addTerms(&matrix_peso3[i][j], &y[i][j],1);
}
}
}
model.addConstr(cb1, GRB_GREATER_EQUAL, b1,"cb1");
model.addConstr(cb2, GRB_GREATER_EQUAL, b2,"cb2");
model.addConstr(cb3, GRB_GREATER_EQUAL, b3,"cb3");
// AGORA RESOLVE-SE O MODELO
auxbol=true;
model.optimize();
optimstatus = model.get(GRB_IntAttr_Status);
if (optimstatus != GRB_INFEASIBLE){
short **result = new short*[n];
for (int ii=0; ii<n; ii++){
result[ii] = new short[n];
}
z1=0,z2=0,z3=0,z4=0;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
if (arestas[i][j] == 1){
result[i][j] = y[i][j].get(GRB_DoubleAttr_X); // GUARDA O RESULTADO
z1+=result[i][j]*matrix_peso1[i][j]; // calcula os pesos
z2+=result[i][j]*matrix_peso2[i][j];
z3+=result[i][j]*matrix_peso3[i][j];
z4+=result[i][j]*matrix_peso4[i][j];
}
}
}
if (z4>z4_k_estrela){
z1_estrela = z1;
z2_estrela = z2;
z3_estrela = z3;
z4_k_estrela = z4;
for (int i=0; i<n; i++){
for (int j=i+1; j<n; j++){
z_n_plus_1[i][j] = result[i][j];
}
}
}
}
//}
}
}
if (z4_k_estrela!=(-1)*MM){
S.push_back(z_n_plus_1);
Pesos pppp = (Pesos){z1_estrela,z2_estrela,z3_estrela,z4_k_estrela};
Z.push_back(pppp);
nn++;
//cout<<"nn = "<<nn<<endl;
}
} while (z4_k_estrela!=(-1)*MM);
times(&tempsFinal1); /* current time */ // clock final
clock_t user_time1 = (tempsFinal1.tms_utime - tempsInit.tms_utime);
cout<<user_time1<<endl;
cout<<(float) user_time1 / (float) sysconf(_SC_CLK_TCK)<<endl;//"Tempo do usuario por segundo : "
cout<<"RESULTADO FINAL..."<<endl;
printResultado();
} catch(GRBException e) {
cout << "Error code = " << e.getErrorCode() << endl;
cout << e.getMessage() << endl;
} catch(...) {
cout << "Exception during optimization" << endl;
}
return 0;
}
vector <double> solve2(vector <vector <double> > M) {
vector <double> solution;
try {
GRBEnv env = GRBEnv();
vector <vector <double> > N = M;
int numPoints = M.size();
int numCircles = M.back().size();
//cout<<"solving for "<<numCircles<<" circles and "<<numPoints<<" points"<<endl;
GRBVar p[numPoints];
double coeff[numPoints];
GRBModel model = GRBModel(env);
GRBLinExpr expr;
// Create variables
for (int i=0;i<numPoints;i++){
p[i] = model.addVar(0.0, 1.0, 0.0, GRB_BINARY);
coeff[i] = 1.0;
}
expr.addTerms(coeff, p,numPoints);
// Integrate new variables
model.update();
// Set objective: maximize x + y + 2 z
model.setObjective(expr, GRB_MINIMIZE);
for(int i=0;i<numCircles;i++){
GRBLinExpr cexpr;
double ccoeff[numPoints];
for(int j=0;j<numPoints;j++){
ccoeff[j] = M[j].back();
M[j].pop_back();
}
cexpr.addTerms(ccoeff,p,numPoints);
model.addConstr(cexpr, GRB_GREATER_EQUAL,1.0);
}
// Optimize model
model.optimize();
int c=0;
for (int i=0;i<numPoints;i++){
solution.push_back((double)p[i].get(GRB_DoubleAttr_X));
c += solution.back();
}
model.addConstr(expr, GRB_EQUAL, c);
int temp;
int temp2;
for (int i=0;i<numPoints;i++){
temp=0;
while(N.back().size()){
temp = temp + (N.back()).back();
N.back().pop_back();
}
coeff[i] = temp;
N.pop_back();
}
expr.addTerms(coeff, p,numPoints);
if (rand()%2)
model.setObjective(expr, GRB_MINIMIZE);
else
model.setObjective(expr, GRB_MAXIMIZE);
model.optimize();
solution.clear();
c=0;
for (int i=0;i<numPoints;i++){
solution.push_back((double)p[i].get(GRB_DoubleAttr_X));
c += solution.back();
}
} catch(GRBException e) {
cout << "Error code = " << e.getErrorCode() << endl;
// cout << e.getMessage() << endl;
} catch(...) {
cout << "Exception during optimization" << endl;
}
return solution;
}
