int main(int argc, char * argv[])
{
int i;
int x;
HANDLE hComm;
DWORD CMS;
int oscal[READ_DATA_LENGTH];
int configWord[READ_DATA_LENGTH];
int command[]={1,0,0,1,1,1,1,1,1,1,1,1,1,1}; //prikaz, ktery bude zapsan do pameti
int config[]={1,1,1,1,1,1,1,1,1,1,1,1,1,1}; //konfiguracni slovo pri loadConfigurationData ?? nema vyznam
int delka;
delka=5;
int pole[5]={0,0,1,0,0};
pole[0]=7;
printf("Vitejte v programu pro cteni OSCAL z PIC16F630 s pouzitim naseho genialniho PROGRAMATORU Vasek1.\n");
hComm = CreateFile("COM4",GENERIC_WRITE,0,0,OPEN_EXISTING, FILE_FLAG_OVERLAPPED,0);
setZeros(hComm);
programVerifyMode(hComm);
printf("Now entered Program/Verify mode...\n");
//read OSCAL
//incrementAddress(hComm, 1023);
//readDataFromProgramMemory(hComm, oscal, READ_DATA_LENGTH);
//printf("Prectena hodnota OSCAL: ");
//printData(oscal);
//read CONFIG WORD
loadConfigurationData(hComm, config);
incrementAddress(hComm, 7);
readDataFromProgramMemory(hComm, configWord, READ_DATA_LENGTH);
printf("Prectena hodnota CONFIGWORD: ");
printData(configWord);
// loadDataToProgramMemory(hComm, command);
// beginProgramingInternal(hComm);
setZeros(hComm);
CloseHandle(hComm);
printf("\n");
printf("Pro ukonceni programu napiste cislo a stiskene ENTER...\n");
scanf("%i", &x);
return 0;
}
void Optimize::setJacGVal_cross(const int ref, const int i, int& idx, double *values, const double *x)
{
int first = crossE[i][ref];
int second = crossF[i][ref];
Point p;
if (first >= 0)
{
if (ref == crossRef[i].first)
{
p = JacG_cross(i, 0, x);
copy(p.data(), values + idx);
}
else if (ref == crossRef[i].first + 1)
{
p = JacG_cross(i, 1, x);
copy(p.data(), values + idx);
}
else
{
setZeros(values + idx);
}
idx += 3;
}
if (second >= 0)
{
if (ref == crossRef[i].second)
{
p = JacG_cross(i, 2, x);
copy(p.data(), values + idx);
}
else if (ref == crossRef[i].second + 1)
{
p = JacG_cross(i, 3, x);
copy(p.data(), values + idx);
}
else
{
setZeros(values + idx);
}
idx += 3;
}
}
int Matrix::creation(int numFilasIn,int numColumnasIn)
{
if(numFilasIn>0 && numColumnasIn>0)
{
data=new float[numFilasIn*numColumnasIn];
numFilas=numFilasIn;
numColumnas=numColumnasIn;
setZeros();
}
else
{
data=0;
numColumnas=0;
numFilas=0;
}
return 1;
}
TEST(SetZeroTest, MatrixSize3) {
vector<vector<int>> arr =
{
{ 1, 1, 1 },
{ 1, 0, 1 },
{ 1, 1, 1 },
};
vector<vector<int>> result =
{
{ 1, 0, 1 },
{ 0, 0, 0 },
{ 1, 0, 1 },
};
setZeros(3, 3, arr);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ASSERT_EQ(result[i][j], arr[i][j]);
}
}
}
bool Optimize::eval_h(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Number obj_factor,
Ipopt::Index m, const Ipopt::Number *lambda, bool new_lambda,
Ipopt::Index nele_hess, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
{
UNUSED(n);
UNUSED(m);
UNUSED(new_lambda);
UNUSED(new_x);
int idx_pv, idx_nv, idx_cv;
Ipopt::Number e[3];
int idx = 0;
int i, size;
int idx_pc;
if (values == NULL)
{
i = 0, size = edges.size();
for (; i < size; i++)
{
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
setHessianPos(idx_pv, idx_pv, iRow, jCol, idx);
if (idx_nv > idx_pv)
{
setHessianPos(idx_nv, idx_pv, iRow, jCol, idx);
}
}
if (idx_nv >= 0)
{
setHessianPos(idx_nv, idx_nv, iRow, jCol, idx);
if (idx_pv > idx_nv)
{
setHessianPos(idx_pv, idx_nv, iRow, jCol, idx);
}
}
}
i = 0;
size = PositionConstraints.size();
for (; i < size; i++)
{
idx_pc = PositionConstraints[i];
setHessianPos(idx_pc, idx_pc, iRow, jCol, idx);
}
//Plane Constraints
i = 0;
size = PlaneConstraints.size();
for (; i < size; ++i)
{
idx_pc = PlaneConstraints[i];
setHessianPos_bending(idx_pc, idx_pc, iRow, jCol, idx);
}
//Bending energy
i = 0;
size = EnergyVertices.size();
for (; i < size; ++i)
{
idx_pv = idxPrevVertex[i];
idx_nv = idxNextVertex[i];
idx_cv = idxTheVertex[i];
if (idx_pv >= 0)
{
setHessianPos_bending(idx_pv, idx_pv, iRow, jCol, idx);
if (idx_cv > idx_pv)
{
setHessianPos_bending(idx_cv, idx_pv, iRow, jCol, idx);
}
if (idx_nv > idx_pv)
{
setHessianPos_bending(idx_nv, idx_pv, iRow, jCol, idx);
}
}
if (idx_nv >= 0)
{
setHessianPos_bending(idx_nv, idx_nv, iRow, jCol, idx);
if (idx_pv > idx_nv)
{
setHessianPos_bending(idx_pv, idx_nv, iRow, jCol, idx);
}
if (idx_cv > idx_nv)
{
setHessianPos_bending(idx_cv, idx_nv, iRow, jCol, idx);
}
}
if (idx_cv >= 0)
{
setHessianPos_bending(idx_cv, idx_cv, iRow, jCol, idx);
if (idx_pv > idx_cv)
{
setHessianPos_bending(idx_pv, idx_cv, iRow, jCol, idx);
}
if (idx_nv > idx_cv)
{
setHessianPos_bending(idx_nv, idx_cv, iRow, jCol, idx);
}
}
}
int ref_E, ref_F, p, q;
i = 0;
size = crosses.size();
for (; i < size; ++i)
{
for (ref_E = 1; ref_E < refSize - 1; ++ref_E)
{
if (crossE[i][ref_E] >= 0 && crossE[i][ref_E + 1] >= 0)
{
setHessianPos_SkewSym(
crossE[i][ref_E],
crossE[i][ref_E + 1],
iRow, jCol, idx);
}
}
for (ref_F = 1; ref_F < refSize - 1; ++ref_F)
{
if (crossF[i][ref_F] >= 0 && crossF[i][ref_F + 1] >= 0)
{
setHessianPos_SkewSym(
crossF[i][ref_F],
crossF[i][ref_F + 1],
iRow, jCol, idx);
}
}
for (ref_E = 1; ref_E < refSize; ++ref_E)
{
p = crossE[i][ref_E];
if (p < 0) continue;
for (ref_F = 1; ref_F < refSize; ++ref_F)
{
q = crossF[i][ref_F];
if (q >= 0)
{
setHessianPos_SkewSym(p, q, iRow, jCol, idx);
}
}
}
}
for (; idx < nele_hess; idx++)
{
iRow[idx] = 0;
jCol[idx] = 0;
}
}
else
{
if (updateCross(x) == false) return false;
double tmp = 0;
setZeros(values, nele_hess);
i = 0, size = edges.size();
for (; i < size; i++)
{
computeEdge(i, x, e);
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
setHessianValues(idx, values, 2 * lambda[i]);
if (idx_nv > idx_pv)
{
setHessianValues(idx, values, -2 * lambda[i]);
}
}
if (idx_nv >= 0)
{
setHessianValues(idx, values, 2 * lambda[i]);
if (idx_pv > idx_nv)
{
setHessianValues(idx, values, -2 * lambda[i]);
}
}
}
i = 0;
size = PositionConstraints.size();
tmp = 2 * pOp->PositionConstraintsWeight * obj_factor;
for (; i < size; i++)
{
setHessianValues(idx, values, tmp);
}
//Plane constraints
Point P;
i = 0;
size = PlaneConstraints.size();
for (; i < size; ++i)
{
P = PlaneConstraintsInfo[i].first;
setHessianVal_bending_helper(P * 2 * obj_factor * pOp->PlaneConstraintsCoef, P, values + idx, 0);
idx += 6;
}
//Bending energy
i = 0;
size = EnergyVertices.size();
for (; i < size; i++)
{
tmp = 2 * obj_factor * pOp->BendingEnergyCoef / VerticesWeight[i];
idx_pv = idxPrevVertex[i];
idx_nv = idxNextVertex[i];
idx_cv = idxTheVertex[i];
if (idx_pv >= 0)
{
setHessianValues_bending(i, idx_pv, idx_pv, idx, values, x, tmp);
if (idx_cv > idx_pv)
{
setHessianValues_bending(i, idx_cv, idx_pv, idx, values, x, tmp);
}
if (idx_nv > idx_pv)
{
setHessianValues_bending(i, idx_nv, idx_pv, idx, values, x, tmp);
}
}
if (idx_nv >= 0)
{
setHessianValues_bending(i, idx_nv, idx_nv, idx, values, x, tmp);
if (idx_pv > idx_nv)
{
setHessianValues_bending(i, idx_pv, idx_nv, idx, values, x, tmp);
}
if (idx_cv > idx_nv)
{
setHessianValues_bending(i, idx_cv, idx_nv, idx, values, x, tmp);
}
}
if (idx_cv >= 0)
{
setHessianValues_bending(i, idx_cv, idx_cv, idx, values, x, tmp);
if (idx_pv > idx_cv)
{
setHessianValues_bending(i, idx_pv, idx_cv, idx, values, x, tmp);
}
if (idx_nv > idx_cv)
{
setHessianValues_bending(i, idx_nv, idx_cv, idx, values, x, tmp);
}
}
}
//crossing constraints
int ref_E, ref_F, p, q;
int base = edges.size();
i = 0;
size = crosses.size();
for (; i < size; ++i)
{
for (ref_E = 1; ref_E < refSize - 1; ++ref_E)
{
if (crossE[i][ref_E] >= 0 && crossE[i][ref_E + 1] >= 0)
{
setHessianValues_SkewSym(i, crossE[i][ref_E], crossE[i][ref_E + 1], values, idx, x, lambda[base + i]);
}
}
for (ref_F = 1; ref_F < refSize - 1; ++ref_F)
{
if (crossF[i][ref_F] >= 0 && crossF[i][ref_F + 1] >= 0)
{
setHessianValues_SkewSym(i, crossF[i][ref_F], crossF[i][ref_F + 1], values, idx, x, lambda[base + i]);
}
}
for (ref_E = 1; ref_E < refSize; ++ref_E)
{
p = crossE[i][ref_E];
if (p < 0) continue;
for (ref_F = 1; ref_F < refSize; ++ref_F)
{
q = crossF[i][ref_F];
if (q >= 0)
{
setHessianValues_SkewSym(i, p, q, values, idx, x, lambda[base + i]);
}
}
}
}
}
assert(idx <= nele_hess);
return true;
}
bool Optimize::eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Index m, Ipopt::Index nele_jac,
Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
{
UNUSED(n);
UNUSED(m);
UNUSED(new_x);
Ipopt::Number e[3];
int idx_nv, idx_pv;
int idx = 0;
if (values == NULL)
{
int i = 0, size = edges.size();
for (; i < size; i++)
{
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
iRow[idx] = i;
iRow[idx + 1] = i;
iRow[idx + 2] = i;
jCol[idx] = 3 * idx_pv;
jCol[idx + 1] = 3 * idx_pv + 1;
jCol[idx + 2] = 3 * idx_pv + 2;
idx += 3;
}
if (idx_nv >= 0)
{
iRow[idx] = i;
iRow[idx + 1] = i;
iRow[idx + 2] = i;
jCol[idx] = 3 * idx_nv;
jCol[idx + 1] = 3 * idx_nv + 1;
jCol[idx + 2] = 3 * idx_nv + 2;
idx += 3;
}
}
int ref;
int end = crosses.size();
i = 0;
for (; i < end; ++i)
{
for (ref = 1; ref < refSize; ++ref)
{
setJacGPos_cross(ref, size, i, idx, iRow, jCol);
}
}
for (; idx < nele_jac; idx++)
{
iRow[idx] = 0;
jCol[idx] = 0;
}
}
else
{
if (updateCross(x) == false) return false;
setZeros(values, nele_jac);
int i = 0, size = edges.size();
for (; i < size; i++)
{
computeEdge(i, x, e);
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
multiplyByScale(e, -2, values + idx);
idx += 3;
}
if (idx_nv >= 0)
{
multiplyByScale(e, 2, values + idx);
idx += 3;
}
}
int ref;
int end = crosses.size();
i = 0;
for (; i < end; ++i)
{
for (ref = 1; ref < refSize; ++ref)
{
setJacGVal_cross(ref, i, idx, values, x);
}
}
}
assert(idx <= nele_jac);
return true;
}
bool Optimize::eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
{
assert(n == n_variables);
UNUSED(new_x);
if (updateCross(x) == false) return false;
Ipopt::Number t1[3], t2[3], t[3], e[3], f[3], tmp;
Ipopt::Number* pV;
PointArrayEdit pf = grad_f;
PointArray px = x;
setZeros(grad_f, n);
int idx;
int i = 0, size = EnergyVertices.size();
for (; i < size; i++)
{
computeEF(i, x, e, f);
tmp = EdgesLengthProduct[i] + dot(e,f);
tmp *= tmp;
tmp = 4 * pOp->BendingEnergyCoef * EdgesLengthProduct[i]/ VerticesWeight[i] / tmp;
idx = idxPrevVertex[i];
multiplyByScale(f, tmp, t1);
if (idx >= 0)
addTo(t1, pf(idx));
multiplyByScale(e, -tmp, t2);
idx = idxNextVertex[i];
if (idx >= 0)
addTo(t2, pf(idx));
idx = idxTheVertex[i];
if (idx >= 0)
{
add(t1, t2, t);
multiplyByScaleTo(t, -1, pf(idx));
}
}
i = 0;
size = PositionConstraints.size();
for (; i < size; i++)
{
idx = PositionConstraints[i];
pV = positions[i].data();
sub(px(idx), pV, t);
multiplyByScaleTo(t, 2 * pOp->PositionConstraintsWeight, pf(idx));
}
i = 0;
size = TangentConstraints.size();
for (; i < size; ++i)
{
pV = tangents[i].data();
idx = TangentConstraints[i].first;
if (idx >= 0)
multiplyByScaleTo(pV, pOp->TangentConstraintsCoef, pf(idx));
idx = TangentConstraints[i].second;
if (idx >= 0)
multiplyByScaleTo(pV, -pOp->TangentConstraintsCoef, pf(idx));
}
Point P;
i = 0;
size = PlaneConstraints.size();
for (; i < size; ++i)
{
idx = PlaneConstraints[i];
P = PlaneConstraintsInfo[i].first;
tmp = 2 * ((P | getPoint(idx, x)) + PlaneConstraintsInfo[i].second);
multiplyByScaleTo(P.data(), tmp * pOp->PlaneConstraintsCoef, pf(idx));
}
return true;
}
