/*
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;
}
