/* double getContourAreaMax(vector<vector<cv::Point>> rv_contours, vector<Vec4i> rv_hierarchy) { double max_contour_area[NUMBER_OF_HANDS] = { 0.0 }; double contour_area = 0; double contour_area_thresh = 8000; int maxidx = 0; int idx = 0; // ÉgÉbÉvÉåÉxÉãDždžÇÈÇ∑Ç◊ǃÇÃó÷äsÇâ°ífǵÅAäeòAåãê¨ï™ÇÉâÉìÉ_ÉÄÇ»êFÇ≈ï`âÊ for (int i = 0; i < rv_contours.size(); i++){ Scalar color(rand() & 255, rand() & 255, rand() & 255); contour_area = cv::contourArea(rv_contours[i]); if (contour_area > contour_area_thresh){ if (max_contour_area[0] < contour_area){ max_contour_area[0] = contour_area; drawContours(dst_img, rv_contours, idx, color, 1, 8, rv_hierarchy);//ÉfÉoÉbÉOèàóù } } idx = rv_hierarchy[idx][0]; } return max_contour_area[0]; } */ void TrickEstimater::calcContourFeature() { double theta = 0.0; double norm_a = 0.0; double norm_b = 0.0; double inner_product_ab = 0.0; double outer_product_ab = 0.0; int interval = 6; unsigned short contour_id[5] = {0}; int k = 0; for (int i = 0; i < contours.size(); i++){ std::cerr << contours[i].size() << std::endl; if (contours[i].size() > 2 * interval){ for (int j = 0; j < contours[i].size() - 2 * interval; j++){ //ó÷äsÇÃâsäpÇåvéZ norm_a = calcNorm(contours[i][j].x, contours[i][j].y, contours[i][j + interval].x, contours[i][j + interval].y); norm_b = calcNorm(contours[i][j + 2 * interval].x, contours[i][j + 2 * interval].y, contours[i][j + interval].x, contours[i][j + interval].y); inner_product_ab = calcInnerProduct(contours[i][j].x, contours[i][j].y, contours[i][j + interval].x, contours[i][j + interval].y, contours[i][j + 2 * interval].x, contours[i][j + 2 * interval].y); theta = acos(inner_product_ab / (norm_a * norm_b)); outer_product_ab = calcOuterProduct(contours[i][j].x, contours[i][j].y, contours[i][j + interval].x, contours[i][j + interval].y, contours[i][j + 2 * interval].x, contours[i][j + 2 * interval].y); is_sharp_angle[i][j + interval] = isSharpAngle(theta); //*the_output_file << contours[i][j].x << "\t" << contours[i][j].y << "\t" << norm_a << "\t" << norm_b << "\t" << inner_product_ab << "\t" << theta << "\t" << outer_product_ab << std::endl; } //ó÷äsÇÃâsäpóÃàÊÇÃÉâÉxÉãêîÇì¡í•ó Ç∆ǵǃéZèo contour_id[k] = i; labelContour(contour_id[k], contours[i].size()); k++; //*the_output_file << label_index_max << std::endl; } } }
idxint scs_solve(Work * w, Data * d, Cone * k, Sol * sol, Info * info) { idxint i; timer solveTimer; struct residuals r; if (!d || !k || !sol || !info || !w || !d->b || !d->c) { scs_printf("ERROR: NULL input\n"); return FAILURE; } tic(&solveTimer); info->statusVal = 0; /* not yet converged */ updateWork(d, w, sol); if (d->VERBOSE) printHeader(d, w, k); /* scs: */ for (i = 0; i < d->MAX_ITERS; ++i) { memcpy(w->u_prev, w->u, (d->n + d->m + 1) * sizeof(pfloat)); if (projectLinSys(d, w, i) < 0) return failureDefaultReturn(d, w, sol, info, "error in projectLinSys"); if (projectCones(d, w, k, i) < 0) return failureDefaultReturn(d, w, sol, info, "error in projectCones"); updateDualVars(d, w); if ((info->statusVal = converged(d, w, &r, i)) != 0) break; if (i % PRINT_INTERVAL == 0) { if (d->VERBOSE) { printSummary(i, &r, &solveTimer); #ifdef EXTRAVERBOSE scs_printf("Norm u = %4f, ", calcNorm(w->u, d->n + d->m + 1)); scs_printf("Norm u_t = %4f, ", calcNorm(w->u_t, d->n + d->m + 1)); scs_printf("Norm v = %4f, ", calcNorm(w->v, d->n + d->m + 1)); scs_printf("tau = %4f, ", w->u[d->n + d->m]); scs_printf("kappa = %4f, ", w->v[d->n + d->m]); scs_printf("|u - u_prev| = %4f, ", calcNormDiff(w->u, w->u_prev, d->n + d->m + 1)); scs_printf("|u - u_t| = %4f\n", calcNormDiff(w->u, w->u_t, d->n + d->m + 1)); #endif } } } if (d->VERBOSE) { printSummary(i, &r, &solveTimer); } setSolution(d, w, sol, info); /* populate info */ info->iter = i; getInfo(d, w, sol, info); info->solveTime = tocq(&solveTimer); if (d->VERBOSE) printFooter(d, w, info); /* un-normalize sol, b, c but not A */ if (d->NORMALIZE) unNormalizeSolBC(d, w, sol); return info->statusVal; }
static void cgCustom(Data *d, Work *w, const double * s, int max_its, double tol) { /* solves (I+A'A)x = b */ /* warm start cg with s */ int i = 0, n = d->n; double *x = w->p->x; double *p = w->p->p; // cg direction double *Ap = w->p->Ap; // updated CG direction double *r = w->p->r; // cg residual double *G = w->p->G; // Gram matrix double alpha, beta, rsnew=0; if (s==NULL) { memset(x,0,n*sizeof(double)); } else { memcpy(x,s,n*sizeof(double)); cblas_dsymv(CblasColMajor, CblasUpper,n, -1, G, n, x,1, 1, r, 1); //b_dsymv('U', n, -1, G, n, x,1, 1, r, 1); } memcpy(p, r, n*sizeof(double)); //double rsold=cblas_dnrm2(n,r,1); double rsold=calcNorm(r,n); for (i=0; i< max_its; i++) { cblas_dsymv(CblasColMajor, CblasUpper,n, 1, G, n, p, 1, 0, Ap, 1); //b_dsymv('U', n, 1, G, n, p, 1, 0, Ap, 1); //beta = cblas_ddot(n, p, 1, Ap, 1); beta = innerProd(p,Ap,n); alpha=(rsold*rsold)/beta; addScaledArray(x,p,n,alpha); //cblas_daxpy(n,alpha,p,1,x,1); addScaledArray(r,Ap,n,-alpha); //cblas_daxpy(n,-alpha,Ap,1,r,1); //rsnew=cblas_dnrm2(n,r,1); rsnew=calcNorm(r,n); if (rsnew<tol) { break; } scaleArray(p,(rsnew*rsnew)/(rsold*rsold),n); //cblas_dscal(n,(rsnew*rsnew)/(rsold*rsold),p,1); addScaledArray(p,r,n,1); //cblas_daxpy(n,1,r,1,p,1); rsold=rsnew; } //printf("terminating cg residual = %4f, took %i itns\n",rsnew,i); }
static Work * initWork(Data *d, Cone * k) { Work * w = scs_calloc(1, sizeof(Work)); idxint l = d->n + d->m + 1; if (d->VERBOSE) { printInitHeader(d, w, k); } if (!w) { scs_printf("ERROR: allocating work failure\n"); return NULL; } /* allocate workspace: */ w->u = scs_malloc(l * sizeof(pfloat)); w->v = scs_malloc(l * sizeof(pfloat)); w->u_t = scs_malloc(l * sizeof(pfloat)); w->u_prev = scs_malloc(l * sizeof(pfloat)); w->h = scs_malloc((l - 1) * sizeof(pfloat)); w->g = scs_malloc((l - 1) * sizeof(pfloat)); w->pr = scs_malloc(d->m * sizeof(pfloat)); w->dr = scs_malloc(d->n * sizeof(pfloat)); if (!w->u || !w->v || !w->u_t || !w->u_prev || !w->h || !w->g || !w->pr || !w->dr) { scs_printf("ERROR: work memory allocation failure\n"); scs_finish(d, w); return NULL; } if (d->NORMALIZE) { normalizeA(d, w, k); #ifdef EXTRAVERBOSE printArray(w->D, d->m, "D"); scs_printf("norm D = %4f\n", calcNorm(w->D, d->m)); printArray(w->E, d->n, "E"); scs_printf("norm E = %4f\n", calcNorm(w->E, d->n)); #endif } else { w->D = NULL; w->E = NULL; } if (initCone(k) < 0) { scs_printf("ERROR: initCone failure\n"); scs_finish(d, w); return NULL; } w->p = initPriv(d); if (!w->p) { scs_printf("ERROR: initPriv failure\n"); scs_finish(d, w); return NULL; } return w; }
static scs_int pcg(const AMatrix * A, const Settings * stgs, Priv * pr, const scs_float * s, scs_float * b, scs_int max_its, scs_float tol) { scs_int i, n = A->n; scs_float ipzr, ipzrOld, alpha; scs_float *p = pr->p; /* cg direction */ scs_float *Gp = pr->Gp; /* updated CG direction */ scs_float *r = pr->r; /* cg residual */ scs_float *z = pr->z; /* for preconditioning */ scs_float *M = pr->M; /* inverse diagonal preconditioner */ if (s == NULL) { memcpy(r, b, n * sizeof(scs_float)); memset(b, 0, n * sizeof(scs_float)); } else { matVec(A, stgs, pr, s, r); addScaledArray(r, b, n, -1); scaleArray(r, -1, n); memcpy(b, s, n * sizeof(scs_float)); } /* check to see if we need to run CG at all */ if (calcNorm(r, n) < MIN(tol, 1e-18)) { return 0; } applyPreConditioner(M, z, r, n, &ipzr); memcpy(p, z, n * sizeof(scs_float)); for (i = 0; i < max_its; ++i) { matVec(A, stgs, pr, p, Gp); alpha = ipzr / innerProd(p, Gp, n); addScaledArray(b, p, n, alpha); addScaledArray(r, Gp, n, -alpha); if (calcNorm(r, n) < tol) { #if EXTRAVERBOSE > 0 scs_printf("tol: %.4e, resid: %.4e, iters: %li\n", tol, calcNorm(r, n), (long) i+1); #endif return i + 1; } ipzrOld = ipzr; applyPreConditioner(M, z, r, n, &ipzr); scaleArray(p, ipzr / ipzrOld, n); addScaledArray(p, z, n, 1); } return i; }
static void setSolution(Data * d, Work * w, Sol * sol, Info * info) { idxint l = d->n + d->m + 1; setx(d, w, sol); sety(d, w, sol); sets(d, w, sol); if (info->statusVal == 0 || info->statusVal == SOLVED) { pfloat tau = w->u[l - 1]; pfloat kap = ABS(w->v[l - 1]); if (tau > INDETERMINATE_TOL && tau > kap) { info->statusVal = solved(d, sol, info, tau); } else { if (calcNorm(w->u, l) < INDETERMINATE_TOL * SQRTF((pfloat) l)) { info->statusVal = indeterminate(d, sol, info); } else { pfloat bTy = innerProd(d->b, sol->y, d->m); pfloat cTx = innerProd(d->c, sol->x, d->n); if (bTy < cTx) { info->statusVal = infeasible(d, sol, info); } else { info->statusVal = unbounded(d, sol, info); } } } } else if (info->statusVal == INFEASIBLE) { info->statusVal = infeasible(d, sol, info); } else { info->statusVal = unbounded(d, sol, info); } }
void Pyramid::init(ID3D10Device* pDevice) { md3dDevice = pDevice; mPosition = D3DXVECTOR3(10.0f, -8.0f, -8.0f); D3DXVECTOR3 mOrigin = D3DXVECTOR3(0, 0, 0); mScale = 4.0f; buildFX(); buildVertexLayouts(); D3DXVECTOR3 baseVerts[4]; int iCount = 0; for (int x = -1; x < 2; x += 2) { for (int z = -1; z < 2; z += 2) { D3DXVECTOR3 base = mOrigin - D3DXVECTOR3(0, mScale, 0); baseVerts[iCount++] = base + D3DXVECTOR3( x * mScale * 0.5f, 0, z * mScale * 0.5f); } } mTriangles[0] = Triangle(mOrigin, baseVerts[0], baseVerts[1]); mTriangles[1] = Triangle(mOrigin, baseVerts[1], baseVerts[3]); mTriangles[2] = Triangle(mOrigin, baseVerts[3], baseVerts[2]); mTriangles[3] = Triangle(mOrigin, baseVerts[2], baseVerts[0]); mTriangles[4] = Triangle(baseVerts[1], baseVerts[0], baseVerts[2]); mTriangles[5] = Triangle(baseVerts[1], baseVerts[2], baseVerts[3]); calcNorm(&mNormals[0], mTriangles[0]); calcNorm(&mNormals[1], mTriangles[1]); calcNorm(&mNormals[2], mTriangles[2]); calcNorm(&mNormals[3], mTriangles[3]); D3D10_BUFFER_DESC vbd; vbd.Usage = D3D10_USAGE_IMMUTABLE; vbd.ByteWidth = 6 * sizeof(Triangle); vbd.BindFlags = D3D10_BIND_VERTEX_BUFFER; vbd.CPUAccessFlags = 0; vbd.MiscFlags = 0; D3D10_SUBRESOURCE_DATA vinitData; vinitData.pSysMem = mTriangles; HR(md3dDevice->CreateBuffer(&vbd, &vinitData, &mVB)); mNumVertices = 18; }
static void updateWork(Data * d, Work * w, Sol * sol) { /* before normalization */ idxint n = d->n; idxint m = d->m; w->nm_b = calcNorm(d->b, m); w->nm_c = calcNorm(d->c, n); #ifdef EXTRAVERBOSE printArray(d->b, d->m, "b"); scs_printf("pre-normalized norm b = %4f\n", calcNorm(d->b, d->m)); printArray(d->c, d->n, "c"); scs_printf("pre-normalized norm c = %4f\n", calcNorm(d->c, d->n)); #endif if (d->NORMALIZE) { normalizeBC(d, w); #ifdef EXTRAVERBOSE printArray(d->b, d->m, "bn"); scs_printf("sc_b = %4f\n", w->sc_b); scs_printf("post-normalized norm b = %4f\n", calcNorm(d->b, d->m)); printArray(d->c, d->n, "cn"); scs_printf("sc_c = %4f\n", w->sc_c); scs_printf("post-normalized norm c = %4f\n", calcNorm(d->c, d->n)); #endif } if (d->WARM_START) { warmStartVars(d, w, sol); } else { coldStartVars(d, w); } memcpy(w->h, d->c, n * sizeof(pfloat)); memcpy(&(w->h[d->n]), d->b, m * sizeof(pfloat)); memcpy(w->g, w->h, (n + m) * sizeof(pfloat)); solveLinSys(d, w->p, w->g, NULL, -1); scaleArray(&(w->g[d->n]), -1, m); w->gTh = innerProd(w->h, w->g, n + m); }
void ntFace3::init(){ //initialize vertex vert0 = new ntVertex (v0,col); vert1 = new ntVertex (v1,col); vert2 = new ntVertex (v2,col); verts.push_back(vert0); verts.push_back(vert1); verts.push_back(vert2); //initialize edges edges.push_back(ntEdge(v0,v1,col)); edges.push_back(ntEdge(v1,v2,col)); edges.push_back(ntEdge(v2,v0,col)); //initialize centroid and normal calcCentroid(); calcNorm(); }
NS_CORE InternalGearRatios DefKNuton::solveNuton( const Jacobi& jacobian, System& system ) { const double eps = 0.001; const int maxIterCount = 100; double norm; int iterCount = 0; bool notFinded = false; do { // создаем матрицу с уравнениями auto matrix = createMatrix( jacobian, system ); // создаем вектор правых частей уравнений auto rightParts = createRightParts( system ); // решаем систему уравнений и получаем дельту auto next = MatrixOperations::solveGaus( matrix, rightParts ); if ( next.size() == 0 ) { next = MatrixOperations::solveKramer( matrix, rightParts ); } // если решений нет - прерываемся, иначе вычисляем начальные условия для следующей итерации if ( next.size() == 0 || ++iterCount > maxIterCount ) { notFinded = true; } else { // вычисляем норму ( максимальную дельту ) norm = calcNorm( next ); // вычисляем начальные значения для следующей итерации for ( size_t i = 0; i < next.size(); i++ ) { system.getUnknownVariables()[i].setValue( system.getUnknownVariables()[i].getValue() + next[i] ); } } } while ( !notFinded && norm >= eps ); NS_CORE InternalGearRatios ans; if ( !notFinded ) { ans = geInternalGearRatioValuesFromSystem( system ); } return ans; }
ntFace3::ntFace3(ntVec3* v0,ntVec3* v1,ntVec3* v2,ntVertex* vert0,ntVertex* vert1,ntVertex* vert2): v0(v0),v1(v1),v2(v2),vert0(vert0),vert1(vert1),vert2(vert2){ this->vecs[0] = v0; this->vecs[1] = v1; this->vecs[2] = v2; verts.push_back(this->vert0); verts.push_back(this->vert1); verts.push_back(this->vert2); edges.push_back(ntEdge(v0,v1,vert0,vert1)); edges.push_back(ntEdge(v1,v2,vert1,vert2)); edges.push_back(ntEdge(v2,v0,vert2,vert0)); //initialize centroid and normal calcCentroid(); calcNorm(); }
void ntTriCell::init(){ // INITIALIZE VECS // 0, 1, 2 vecs_SD.push_back(v0); vecs_SD.push_back(v1); vecs_SD.push_back(v2); // INITIALIZE VERTEX vert0 = new ntVertex (v0,col); vert1 = new ntVertex (v1,col); vert2 = new ntVertex (v2,col); verts.push_back(vert0); verts.push_back(vert1); verts.push_back(vert2); // INITIALIZE EDGES edges.push_back(ntEdge(v0,v1,col)); edges.push_back(ntEdge(v1,v2,col)); edges.push_back(ntEdge(v2,v0,col)); // DUPLICATE VECS // 3, 4, 5 ntVec3* v00 = new ntVec3(v0->x, v0->y, v0->z); ntVec3* v01 = new ntVec3(v1->x, v1->y, v1->z); ntVec3* v02 = new ntVec3(v2->x, v2->y, v2->z); vecs_SD.push_back(v00); vecs_SD.push_back(v01); vecs_SD.push_back(v02); edges.push_back(ntEdge(v00, v01, col)); edges.push_back(ntEdge(v01, v02, col)); edges.push_back(ntEdge(v02, v00, col)); // INITIALIZE SUB DIVISION POINTS calcCentroid(); calcNorm(); calc_EdgeMid(); calc_innerTri(); calc_starPts(); calc_Faces(); setPolylines(); }
scs_int solveLinSys(const AMatrix * A, const Settings * stgs, Priv * p, scs_float * b, const scs_float * s, scs_int iter) { scs_int cgIts; scs_float cgTol = calcNorm(b, A->n) * (iter < 0 ? CG_BEST_TOL : CG_MIN_TOL / POWF((scs_float) iter + 1, stgs->cg_rate)); tic(&linsysTimer); /* solves Mx = b, for x but stores result in b */ /* s contains warm-start (if available) */ accumByAtrans(A, p, &(b[A->n]), b); /* solves (I+A'A)x = b, s warm start, solution stored in b */ cgIts = pcg(A, stgs, p, s, b, A->n, MAX(cgTol, CG_BEST_TOL)); scaleArray(&(b[A->n]), -1, A->m); accumByA(A, p, b, &(b[A->n])); if (iter >= 0) { totCgIts += cgIts; } totalSolveTime += tocq(&linsysTimer); #if EXTRAVERBOSE > 0 scs_printf("linsys solve time: %1.2es\n", tocq(&linsysTimer) / 1e3); #endif return 0; }