real ln_sqrtx2y2(const real& x, const real& y) throw(STD_FKT_OUT_OF_DEF) // ln( sqrt(x^2+y^2) ) == 0.5*ln(x^2+y^2); Blomquist, 21.11.03; // Relative error bound: 5.160563E-016; // Absolute error bound: 2.225075E-308; if x=1 and 0<=y<=b0; { int j,N; real a,b,r,r1; dotprecision dot; a = sign(x)<0 ? -x : x; // a = |x| >= 0; b = sign(y)<0 ? -y : y; // b = |y| >= 0; int exa=expo(a), exb=expo(b), ex; if (b > a) { r = a; a = b; b = r; ex = exa; exa = exb; exb = ex; } // It holds now: 0 <= b <= a if (sign(a)==0) cxscthrow(STD_FKT_OUT_OF_DEF ("real ln_sqrtx2y2(const real&, const real&)")); if (exa>20) // to avoid overflow by calculating a^2 + b^2 { // a>=2^(20): j = Interval_Nr(B_lnx2y2_1,21,exa); // j: No. of subinterval N = B_lnx2y2_N1[j]; // N: Optimal int value if (exb-exa > -25) { // For (exb-exa>-25) we use the complete term: // N*ln(2) + [ln(2^(-N)*a)+0.5*ln(1+(b/a)^2)] b = b/a; // a > 0 b = lnp1(b*b); times2pown(b,-1); // exact division by 2 times2pown(a,-N); r = b + ln(a); // [ ... ] calculated! r += B_lnx2y2_c1[j]; } else { // For (exb-exa<=-25) only two summands!: times2pown(a,-N); r = ln(a) + B_lnx2y2_c1[j]; } } else // exa<=20 or a<2^(20): { // Now calculation of a^2+b^2 without overflow: if (exa<=-20) // to avoid underflow by calculating a^2+b^2 if (exa<=-1022) // a in the denormalized range { r = b/a; r = lnp1(r*r); times2pown(r,-1); // r: 0.5*ln(1+..) times2pown(a,1067); r += ln(a); // [ .... ] ready r -= ln2_1067; // rel. error = 2.459639e-16; } else // MinReal=2^(-1022) <= a < 2^(-20) { // Calculating the number j of the subinterval: j = 20 - Interval_Nr(B_lnx2y2_2,21,exa); r = a; times2pown(r,B_lnx2y2_N1[j]); r = ln(r); // r: ln(2^N*a); if (exb-exa > -25) { // calculating the complete term b = b/a; a = lnp1(b*b); times2pown(a,-1); r += a; // [ ... ] ready now } // We now have: exb-exa<=-25, ==> b/a <= 2^(-24); r -= B_lnx2y2_c1[j]; // 0.5*ln(1+(b/a)^2) neglected! // relative error = 4.524090e-16 in both cases; } else // calculation of a^2+b^2 without overflow or underflow: { // exa>-20 respective a>=2^(-20): dot = 0; accumulate(dot,a,a); accumulate(dot,b,b); // dot = a^2+b^2, exact! real s = rnd(dot); // s = a^2 + b^2, rounded! if (s>=0.25 && s<=1.75) if (s>=0.828125 && s<=1.171875) { // Series: if (a==1 && exb<=-28) { r = b; times2pown(r,-1); r *= b; } else { dot -= 1; r = rnd(dot); // r = a^2+b^2-1 rounded! r = lnp1(r); times2pown(r,-1); } } else { // Reading dot = a^2+b^2 twice: r = rnd(dot); dot -= r; r1 = rnd(dot); // a^2+b^2 = r+r1, rounded! r1 = lnp1(r1/r); r = ln(r) + r1; times2pown(r,-1); // exact division by 2 } else { // calculating straight from: 0.5*ln(x^2+y^2) r = ln(s); times2pown(r,-1); } } } return r; } // ln_sqrtx2y2
/* returns 0 on success, 1 on error with errno set */ static int calc_lon(privpath_t *pp) { point_t *pt = pp->pt; int n = pp->len; int i, j, k, k1; int ct[4], dir; point_t constraint[2]; point_t cur; point_t off; int *pivk = NULL; /* pivk[n] */ int *nc = NULL; /* nc[n]: next corner */ point_t dk; /* direction of k-k1 */ int a, b, c, d; SAFE_MALLOC(pivk, n, int); SAFE_MALLOC(nc, n, int); /* initialize the nc data structure. Point from each point to the furthest future point to which it is connected by a vertical or horizontal segment. We take advantage of the fact that there is always a direction change at 0 (due to the path decomposition algorithm). But even if this were not so, there is no harm, as in practice, correctness does not depend on the word "furthest" above. */ k = 0; for (i=n-1; i>=0; i--) { if (pt[i].x != pt[k].x && pt[i].y != pt[k].y) { k = i+1; /* necessarily i<n-1 in this case */ } nc[i] = k; } SAFE_MALLOC(pp->lon, n, int); /* determine pivot points: for each i, let pivk[i] be the furthest k such that all j with i<j<k lie on a line connecting i,k. */ for (i=n-1; i>=0; i--) { ct[0] = ct[1] = ct[2] = ct[3] = 0; /* keep track of "directions" that have occurred */ dir = (3+3*(pt[mod(i+1,n)].x-pt[i].x)+(pt[mod(i+1,n)].y-pt[i].y))/2; ct[dir]++; constraint[0].x = 0; constraint[0].y = 0; constraint[1].x = 0; constraint[1].y = 0; /* find the next k such that no straight line from i to k */ k = nc[i]; k1 = i; while (1) { dir = (3+3*sign(pt[k].x-pt[k1].x)+sign(pt[k].y-pt[k1].y))/2; ct[dir]++; /* if all four "directions" have occurred, cut this path */ if (ct[0] && ct[1] && ct[2] && ct[3]) { pivk[i] = k1; goto foundk; } cur.x = pt[k].x - pt[i].x; cur.y = pt[k].y - pt[i].y; /* see if current constraint is violated */ if (xprod(constraint[0], cur) < 0 || xprod(constraint[1], cur) > 0) { goto constraint_viol; } /* else, update constraint */ if (abs(cur.x) <= 1 && abs(cur.y) <= 1) { /* no constraint */ } else { off.x = cur.x + ((cur.y>=0 && (cur.y>0 || cur.x<0)) ? 1 : -1); off.y = cur.y + ((cur.x<=0 && (cur.x<0 || cur.y<0)) ? 1 : -1); if (xprod(constraint[0], off) >= 0) { constraint[0] = off; } off.x = cur.x + ((cur.y<=0 && (cur.y<0 || cur.x<0)) ? 1 : -1); off.y = cur.y + ((cur.x>=0 && (cur.x>0 || cur.y<0)) ? 1 : -1); if (xprod(constraint[1], off) <= 0) { constraint[1] = off; } } k1 = k; k = nc[k1]; if (!cyclic(k,i,k1)) { break; } } constraint_viol: /* k1 was the last "corner" satisfying the current constraint, and k is the first one violating it. We now need to find the last point along k1..k which satisfied the constraint. */ dk.x = sign(pt[k].x-pt[k1].x); dk.y = sign(pt[k].y-pt[k1].y); cur.x = pt[k1].x - pt[i].x; cur.y = pt[k1].y - pt[i].y; /* find largest integer j such that xprod(constraint[0], cur+j*dk) >= 0 and xprod(constraint[1], cur+j*dk) <= 0. Use bilinearity of xprod. */ a = xprod(constraint[0], cur); b = xprod(constraint[0], dk); c = xprod(constraint[1], cur); d = xprod(constraint[1], dk); /* find largest integer j such that a+j*b>=0 and c+j*d<=0. This can be solved with integer arithmetic. */ j = INFTY; if (b<0) { j = floordiv(a,-b); } if (d>0) { j = min(j, floordiv(-c,d)); } pivk[i] = mod(k1+j,n); foundk: ; } /* for i */ /* clean up: for each i, let lon[i] be the largest k such that for all i' with i<=i'<k, i'<k<=pivk[i']. */ j=pivk[n-1]; pp->lon[n-1]=j; for (i=n-2; i>=0; i--) { if (cyclic(i+1,pivk[i],j)) { j=pivk[i]; } pp->lon[i]=j; } for (i=n-1; cyclic(mod(i+1,n),j,pp->lon[i]); i--) { pp->lon[i] = j; } free(pivk); free(nc); return 0; malloc_error: free(pivk); free(nc); return 1; }
int srTDriftSpace::PropagateRadiationSimple_AnalytTreatQuadPhaseTerm(srTSRWRadStructAccessData* pRadAccessData) {// e in eV; Length in m !!! int result = 0; SetupPropBufVars_AnalytTreatQuadPhaseTerm(pRadAccessData); if(pRadAccessData->Pres != 0) if(result = SetRadRepres(pRadAccessData, 0)) return result; PropBufVars.PassNo = 1; //Remove quadratic term from the Phase in coord. repres. if(result = TraverseRadZXE(pRadAccessData)) return result; //testOC09302011 //if(Length == 34.63) return 0; double xStartOld = pRadAccessData->xStart, zStartOld = pRadAccessData->zStart; pRadAccessData->xStart = -(pRadAccessData->nx >> 1)*pRadAccessData->xStep; pRadAccessData->zStart = -(pRadAccessData->nz >> 1)*pRadAccessData->zStep; double xShift = pRadAccessData->xStart - xStartOld, zShift = pRadAccessData->zStart - zStartOld; pRadAccessData->xWfrMin += xShift; pRadAccessData->xWfrMax += xShift; pRadAccessData->zWfrMin += zShift; pRadAccessData->zWfrMax += zShift; pRadAccessData->WfrEdgeCorrShouldBeDone = 0; if(result = SetRadRepres(pRadAccessData, 1)) return result; //To angular repres. PropBufVars.PassNo = 2; //Loop in angular repres. if(result = TraverseRadZXE(pRadAccessData)) return result; if(pRadAccessData->UseStartTrToShiftAtChangingRepresToCoord) { pRadAccessData->xStartTr += xShift; pRadAccessData->zStartTr += zShift; } if(result = SetRadRepres(pRadAccessData, 0)) return result; //Back to coord. repres. pRadAccessData->xStart = xStartOld; pRadAccessData->zStart = zStartOld; if(pRadAccessData->UseStartTrToShiftAtChangingRepresToCoord) { pRadAccessData->xStart = pRadAccessData->xStartTr - xShift; pRadAccessData->zStart = pRadAccessData->zStartTr - zShift; } //Change scale //double kx = (pRadAccessData->RobsX + Length)/pRadAccessData->RobsX; //pRadAccessData->xStart = kx*(pRadAccessData->xStart) - (Length/pRadAccessData->RobsX)*(pRadAccessData->xc); //pRadAccessData->xStep *= kx; pRadAccessData->xStart = PropBufVars.kx_AnalytTreatQuadPhaseTerm*(pRadAccessData->xStart) - PropBufVars.kxc_AnalytTreatQuadPhaseTerm*(pRadAccessData->xc); pRadAccessData->xStep *= PropBufVars.kx_AnalytTreatQuadPhaseTerm; //double kz = (pRadAccessData->RobsZ + Length)/pRadAccessData->RobsZ; //pRadAccessData->zStart = kz*(pRadAccessData->zStart) - (Length/pRadAccessData->RobsZ)*(pRadAccessData->zc); //pRadAccessData->zStep *= kz; pRadAccessData->zStart = PropBufVars.kz_AnalytTreatQuadPhaseTerm*(pRadAccessData->zStart) - PropBufVars.kzc_AnalytTreatQuadPhaseTerm*(pRadAccessData->zc); pRadAccessData->zStep *= PropBufVars.kz_AnalytTreatQuadPhaseTerm; PropBufVars.PassNo = 3; //Add new quadratic term to the Phase in coord. repres. if(result = TraverseRadZXE(pRadAccessData)) return result; //pRadAccessData->MirrorFieldData(sign(kx), sign(kz)); pRadAccessData->MirrorFieldData((int)sign(PropBufVars.kx_AnalytTreatQuadPhaseTerm), (int)sign(PropBufVars.kz_AnalytTreatQuadPhaseTerm)); //if(kx < 0) if(PropBufVars.kx_AnalytTreatQuadPhaseTerm < 0) { double xEnd = pRadAccessData->xStart + (pRadAccessData->nx - 1)*pRadAccessData->xStep; //or nx ??? pRadAccessData->xStart = xEnd; pRadAccessData->xStep *= -1; } //if(kz < 0) if(PropBufVars.kz_AnalytTreatQuadPhaseTerm < 0) { double zEnd = pRadAccessData->zStart + (pRadAccessData->nz - 1)*pRadAccessData->zStep; //or nz ??? pRadAccessData->zStart = zEnd; pRadAccessData->zStep *= -1; } pRadAccessData->SetNonZeroWavefrontLimitsToFullRange(); return result; }
EXPORT int ridders_method( double level_set_left, // left hand initial value for root finding double level_set_right, // right hand initial value for root finding double function_g_left, // left hand initial function value for root finding double function_g_right, // right hand initial function value for root findin double volume_of_fluid, // volume of fluid value for which we need the // the corresponding level-set value double &level_set, // the level-set value corresponding to given // volume of fluid value double d_level_set_d_x1, // first partial derivative in x1 // direction of level-set double d_level_set_d_x2, // first partial derivative in x2 // direction of level-set double d_level_set_d_x3, // first partial derivative in x3 // direction of level-set double vof_2_level_set_tolerance, // tolerance in the conversion from volume // of fluid value to level-set value int number_iterations_ridder, // maximum number of iterations allowed in the // nonlinear root finding algorithm int lower_bound_derivatives // lower bound for the first partial derivatives // to consider it a limiting case of vanishing // partial derivatives ) { /* function definitions */ double new_mid_point_beta; // new estimate of the root location based on // midpoint search interval double function_g_value_beta; // function value in the new estimate of the // root location based on the midpoint of the // search interval double function_g_value=10E10; // function value in the new estimate of the // root location double new_volume_of_fluid; // volume of fluid at the new estimate of the // root location double ridders_denominator; // denominator of Ridders expression for the // estimate of the root double ridders_enumerator; // enumerator of Ridders expression for the // estimate of the root int iteration_index_ridder=1; // index of the iteration in the root finding // algorithm /* first check if either of the two starting values are sufficiently */ /* close to the root */ std::cout<<"start with vof "<< volume_of_fluid << " \n"; if( fabs(function_g_left)< vof_2_level_set_tolerance) { /* left hand starting value is a root */ level_set=level_set_left; return 0; } else { if(fabs(function_g_right)< vof_2_level_set_tolerance) { /* right hand starting value is a root */ level_set=level_set_right; return 0; } else { while( fabs(function_g_value)>vof_2_level_set_tolerance && iteration_index_ridder<number_iterations_ridder) { new_mid_point_beta=0.5*(level_set_left+level_set_right); if(level_set_2_vof(new_mid_point_beta, d_level_set_d_x1, d_level_set_d_x2, d_level_set_d_x3, new_volume_of_fluid, lower_bound_derivatives)) { std::cout<< "error in computation of volume of fluid in ridders_method \n"; std::cout<< "new mid point, aborting \n"; return 1; } function_g_value_beta=new_volume_of_fluid-volume_of_fluid; ridders_enumerator=0.5*(level_set_right-level_set_left)* sign(1.0,function_g_left)*function_g_value_beta; ridders_denominator=sqrt(function_g_value_beta*function_g_value_beta- function_g_right*function_g_left); /* make a new estimate of the root */ level_set=new_mid_point_beta+ridders_enumerator/ridders_denominator; if(level_set_2_vof(level_set, d_level_set_d_x1, d_level_set_d_x2, d_level_set_d_x3, new_volume_of_fluid, lower_bound_derivatives)) { std::cout<< "error in computation of volume of fluid in ridders_method \n"; std::cout<< "new mid point, aborting \n"; return 1; } // std::cerr<<" new_volume_of_fluid "<< new_volume_of_fluid << " \n"; /* compute the new residual */ function_g_value=new_volume_of_fluid-volume_of_fluid; std::cout<<"g_value "<< function_g_value << " \n"; /* depending on the sign of the residual, update left or right point */ if(function_g_left*function_g_value<0) { level_set_right=level_set; function_g_right=function_g_value; } else { level_set_left=level_set; function_g_left=function_g_value; } /* if the latest approximation is sufficiently close to the root */ /* terminate the iteration */ if(fabs(function_g_value)<vof_2_level_set_tolerance) { return 0; } iteration_index_ridder++; } if(fabs(function_g_value)>vof_2_level_set_tolerance) { /* apparently the algorithm failed to converge in the allowed number of iterations */ return 1; } else { /* apparently the algorithm is converged in the allowed number of iterations */ /* compiler was unsure of outcome */ return 0; } } } }
PHPAPI int php_version_compare(const char *orig_ver1, const char *orig_ver2) { char *ver1; char *ver2; char *p1, *p2, *n1, *n2; long l1, l2; int compare = 0; if (!*orig_ver1 || !*orig_ver2) { if (!*orig_ver1 && !*orig_ver2) { return 0; } else { return *orig_ver1 ? 1 : -1; } } if (orig_ver1[0] == '#') { ver1 = estrdup(orig_ver1); } else { ver1 = php_canonicalize_version(orig_ver1); } if (orig_ver2[0] == '#') { ver2 = estrdup(orig_ver2); } else { ver2 = php_canonicalize_version(orig_ver2); } p1 = n1 = ver1; p2 = n2 = ver2; while (*p1 && *p2 && n1 && n2) { if ((n1 = strchr(p1, '.')) != NULL) { *n1 = '\0'; } if ((n2 = strchr(p2, '.')) != NULL) { *n2 = '\0'; } if (isdigit(*p1) && isdigit(*p2)) { /* compare element numerically */ l1 = strtol(p1, NULL, 10); l2 = strtol(p2, NULL, 10); compare = sign(l1 - l2); } else if (!isdigit(*p1) && !isdigit(*p2)) { /* compare element names */ compare = compare_special_version_forms(p1, p2); } else { /* mix of names and numbers */ if (isdigit(*p1)) { compare = compare_special_version_forms("#N#", p2); } else { compare = compare_special_version_forms(p1, "#N#"); } } if (compare != 0) { break; } if (n1 != NULL) { p1 = n1 + 1; } if (n2 != NULL) { p2 = n2 + 1; } } if (compare == 0) { if (n1 != NULL) { if (isdigit(*p1)) { compare = 1; } else { compare = php_version_compare(p1, "#N#"); } } else if (n2 != NULL) { if (isdigit(*p2)) { compare = -1; } else { compare = php_version_compare("#N#", p2); } } } efree(ver1); efree(ver2); return compare; }
Foam::SVD::SVD(const scalarRectangularMatrix& A, const scalar minCondition) : U_(A), V_(A.m(), A.m()), S_(A.m()), VSinvUt_(A.m(), A.n()), nZeros_(0) { // SVDcomp to find U_, V_ and S_ - the singular values const label Um = U_.m(); const label Un = U_.n(); scalarList rv1(Um); scalar g = 0; scalar scale = 0; scalar s = 0; scalar anorm = 0; label l = 0; for (label i = 0; i < Um; i++) { l = i+2; rv1[i] = scale*g; g = s = scale = 0; if (i < Un) { for (label k = i; k < Un; k++) { scale += mag(U_[k][i]); } if (scale != 0) { for (label k = i; k < Un; k++) { U_[k][i] /= scale; s += U_[k][i]*U_[k][i]; } scalar f = U_[i][i]; g = -sign(Foam::sqrt(s), f); scalar h = f*g - s; U_[i][i] = f - g; for (label j = l-1; j < Um; j++) { s = 0; for (label k = i; k < Un; k++) { s += U_[k][i]*U_[k][j]; } f = s/h; for (label k = i; k < A.n(); k++) { U_[k][j] += f*U_[k][i]; } } for (label k = i; k < Un; k++) { U_[k][i] *= scale; } } } S_[i] = scale*g; g = s = scale = 0; if (i+1 <= Un && i != Um) { for (label k = l-1; k < Um; k++) { scale += mag(U_[i][k]); } if (scale != 0) { for (label k=l-1; k < Um; k++) { U_[i][k] /= scale; s += U_[i][k]*U_[i][k]; } scalar f = U_[i][l-1]; g = -sign(Foam::sqrt(s),f); scalar h = f*g - s; U_[i][l-1] = f - g; for (label k = l-1; k < Um; k++) { rv1[k] = U_[i][k]/h; } for (label j = l-1; j < Un; j++) { s = 0; for (label k = l-1; k < Um; k++) { s += U_[j][k]*U_[i][k]; } for (label k = l-1; k < Um; k++) { U_[j][k] += s*rv1[k]; } } for (label k = l-1; k < Um; k++) { U_[i][k] *= scale; } } } anorm = max(anorm, mag(S_[i]) + mag(rv1[i])); } for (label i = Um-1; i >= 0; i--) { if (i < Um-1) { if (g != 0) { for (label j = l; j < Um; j++) { V_[j][i] = (U_[i][j]/U_[i][l])/g; } for (label j=l; j < Um; j++) { s = 0; for (label k = l; k < Um; k++) { s += U_[i][k]*V_[k][j]; } for (label k = l; k < Um; k++) { V_[k][j] += s*V_[k][i]; } } } for (label j = l; j < Um;j++) { V_[i][j] = V_[j][i] = 0.0; } } V_[i][i] = 1; g = rv1[i]; l = i; } for (label i = min(Um, Un) - 1; i >= 0; i--) { l = i+1; g = S_[i]; for (label j = l; j < Um; j++) { U_[i][j] = 0.0; } if (g != 0) { g = 1.0/g; for (label j = l; j < Um; j++) { s = 0; for (label k = l; k < Un; k++) { s += U_[k][i]*U_[k][j]; } scalar f = (s/U_[i][i])*g; for (label k = i; k < Un; k++) { U_[k][j] += f*U_[k][i]; } } for (label j = i; j < Un; j++) { U_[j][i] *= g; } } else { for (label j = i; j < Un; j++) { U_[j][i] = 0.0; } } ++U_[i][i]; } for (label k = Um-1; k >= 0; k--) { for (label its = 0; its < 35; its++) { bool flag = true; label nm; for (l = k; l >= 0; l--) { nm = l-1; if (mag(rv1[l]) + anorm == anorm) { flag = false; break; } if (mag(S_[nm]) + anorm == anorm) break; } if (flag) { scalar c = 0.0; s = 1.0; for (label i = l; i < k+1; i++) { scalar f = s*rv1[i]; rv1[i] = c*rv1[i]; if (mag(f) + anorm == anorm) break; g = S_[i]; scalar h = sqrtSumSqr(f, g); S_[i] = h; h = 1.0/h; c = g*h; s = -f*h; for (label j = 0; j < Un; j++) { scalar y = U_[j][nm]; scalar z = U_[j][i]; U_[j][nm] = y*c + z*s; U_[j][i] = z*c - y*s; } } } scalar z = S_[k]; if (l == k) { if (z < 0.0) { S_[k] = -z; for (label j = 0; j < Um; j++) V_[j][k] = -V_[j][k]; } break; } if (its == 34) { WarningInFunction << "no convergence in 35 SVD iterations" << endl; } scalar x = S_[l]; nm = k-1; scalar y = S_[nm]; g = rv1[nm]; scalar h = rv1[k]; scalar f = ((y - z)*(y + z) + (g - h)*(g + h))/(2.0*h*y); g = sqrtSumSqr(f, scalar(1)); f = ((x - z)*(x + z) + h*((y/(f + sign(g, f))) - h))/x; scalar c = 1.0; s = 1.0; for (label j = l; j <= nm; j++) { label i = j + 1; g = rv1[i]; y = S_[i]; h = s*g; g = c*g; scalar z = sqrtSumSqr(f, h); rv1[j] = z; c = f/z; s = h/z; f = x*c + g*s; g = g*c - x*s; h = y*s; y *= c; for (label jj = 0; jj < Um; jj++) { x = V_[jj][j]; z = V_[jj][i]; V_[jj][j] = x*c + z*s; V_[jj][i] = z*c - x*s; } z = sqrtSumSqr(f, h); S_[j] = z; if (z) { z = 1.0/z; c = f*z; s = h*z; } f = c*g + s*y; x = c*y - s*g; for (label jj=0; jj < Un; jj++) { y = U_[jj][j]; z = U_[jj][i]; U_[jj][j] = y*c + z*s; U_[jj][i] = z*c - y*s; } } rv1[l] = 0.0; rv1[k] = f; S_[k] = x; } } // zero singular values that are less than minCondition*maxS const scalar minS = minCondition*S_[findMax(S_)]; forAll(S_, i) { if (S_[i] <= minS) { //Info<< "Removing " << S_[i] << " < " << minS << endl; S_[i] = 0; nZeros_++; } } // now multiply out to find the pseudo inverse of A, VSinvUt_ multiply(VSinvUt_, V_, inv(S_), U_.T()); // test SVD /*scalarRectangularMatrix SVDA(A.n(), A.m()); multiply(SVDA, U_, S_, transpose(V_)); scalar maxDiff = 0; scalar diff = 0; for (label i = 0; i < A.n(); i++) { for (label j = 0; j < A.m(); j++) { diff = mag(A[i][j] - SVDA[i][j]); if (diff > maxDiff) maxDiff = diff; } } Info<< "Maximum discrepancy between A and svd(A) = " << maxDiff << endl; if (maxDiff > 4) { Info<< "singular values " << S_ << endl; } */ }
void Table::addCoord(Coordinate c) { switch(mType) { case PolyEdit::Polygon: { int row = rowCount()-1; setItem(row,0, new Cell(QString::number(c.rx()))); setItem(row,1, new Cell(QString::number(c.ry()))); break; } case PolyEdit::Circle: { Cell *item[3]; item[0] = getItem(0,0); item[1] = getItem(1,0); item[2] = getItem(2,0); if (!item[2]->text().isEmpty() && mMask->size() == 1) { clearContents(); mMask->clear(); mMask->addc(c); mMask->setRadius(0); setItem(0,0, new Cell(QString::number(c.rx()))); setItem(1,0, new Cell(QString::number(c.ry()))); } else if (item[0]->text().isEmpty() || item[1]->text().isEmpty()) { setItem(0,0, new Cell(QString::number(c.rx()))); setItem(1,0, new Cell(QString::number(c.ry()))); mMask->addc(c); } else { int x1 = item[0]->getInt(); int y1 = item[1]->getInt(); Coordinate c1(x1,y1); //int x2 = c.rx(); //int y2 = c.ry(); int radius = distance(c, c1);//sqrt(pow((x2-x1),2) + pow((y2-y1),2)); setItem(2,0, new Cell(QString::number(radius))); } break; } case PolyEdit::Box: { bool item[4]; item[0] = getItem(0,0)->text().isEmpty(); item[1] = getItem(1,0)->text().isEmpty(); item[2] = getItem(2,0)->text().isEmpty(); item[3] = getItem(3,0)->text().isEmpty(); if (!item[0] && !item[1] && !item[2] && !item[3]) { clearContents(); setItem(0,0, new Cell(QString::number(c.rx()))); setItem(1,0, new Cell(QString::number(c.ry()))); mMask->clear(); mMask->addc(c); } if (item[0] || item[1]) { setItem(0,0, new Cell(QString::number(c.rx()))); setItem(1,0, new Cell(QString::number(c.ry()))); mMask->addc(c); } else if (item[2] || item[3]) { int width = c.rx() - getItem(0,0)->getInt(); int height = getItem(1,0)->getInt() - c.ry(); if (sign(width) == -1) { setItem(0,0, new Cell(QString::number(c.rx()))); setItem(2,0, new Cell(QString::number(abs(width)))); } else setItem(2,0, new Cell(QString::number(width))); if (sign(height) == -1) { setItem(1,0, new Cell(QString::number(c.ry()))); setItem(3,0, new Cell(QString::number(abs(height)))); } else setItem(3,0, new Cell(QString::number(height))); } break; } case PolyEdit::Invalid: { break; } } }
//--------------------------------------------------------------------------- void __fastcall TQrySaleForm::btnQryYearClick(TObject *Sender) { CString szSQL = ""; ///////////////////////////////////////////////////////////////////////////////////////////// //业绩查询 szSQL = "select op_code_sl, isnull(busi_volume_cur,0) gross, \ dbo.gen_should_com(busi_volume_cur, dbo.gen_new_cli_prize_yearmonth(year_month,op_code_sl), busi_volume_base, ratio) should_com, \ cast(100*(1-isnull(busi_volume_cur,0)/isnull(busi_volume_base,100)) as decimal(9,2)) as pect_busi, \ case sign(busi_volume_base-busi_volume_cur) \ when 1 then (busi_volume_base-busi_volume_cur)\ else 0 end as last_busi,\ *\ from commition \ where 1=1 "; if (g_theOperator.op_class==E_OPERATOR_TYPE_SALER){ szSQL += " and op_code_sl="; szSQL += Str2DBString(g_theOperator.op_code); } if (!cbbYear->Text.IsEmpty()){ szSQL += " and year(year_month)="; szSQL += Str2DBString(cbbYear->Text.c_str()); } if (!cbbMonth->Text.IsEmpty()){ szSQL += " and month(year_month)="; szSQL += Str2DBString(cbbMonth->Text.c_str()); } szSQL += "order by year_month, op_code_sl"; Edit1->Text = AnsiString(szSQL); //run RunSQL(szSQL, true); if (dm1->Query1->Eof){ ShowMessage("记录不存在"); } lvTask->Clear(); //show double sum_should_com = 0; while(!dm1->Query1->Eof) { TListItem *pItem = lvTask->Items->Add(); double should_com = dm1->Query1->FieldByName("should_com")->AsFloat; sum_should_com += should_com; pItem->Caption = dm1->Query1->FieldByName("op_code_sl")->AsString; pItem->SubItems->Add(dm1->Query1->FieldByName("gross")->AsString); pItem->SubItems->Add(should_com); pItem->SubItems->Add(0.85*should_com); pItem->SubItems->Add(0.15*should_com); pItem->SubItems->Add(dm1->Query1->FieldByName("passed")->AsString); pItem->SubItems->Add(dm1->Query1->FieldByName("busi_volume_cur")->AsString); pItem->SubItems->Add(dm1->Query1->FieldByName("last_busi")->AsString); double last_busi = dm1->Query1->FieldByName("pect_busi")->AsFloat; last_busi = last_busi<0?0:last_busi; pItem->SubItems->Add(last_busi); dm1->Query1->Next(); } lbTotalPrize->Caption = 0.85*sum_should_com; ///////////////////////////////////////////////////////////////////////////////////////////// //回扣查询 szSQL = "select *, \ CONVERT(varchar, acceptdate, 111) as date_accept, \ case year(date) when 2099 then '-' else CONVERT(varchar, date, 111) end as date_kb \ from detail_kb, operation \ where dkoid=oid "; if (g_theOperator.op_class==E_OPERATOR_TYPE_SALER){ szSQL += " and op_code_sl="; szSQL += Str2DBString(g_theOperator.op_code); } if (!cbbClShortName->Text.IsEmpty()){ szSQL += " and cl_shortname="; szSQL += Str2DBString(cbbClShortName->Text.c_str()); } if (!cbbYear->Text.IsEmpty()){ szSQL += " and year(acceptdate)="; szSQL += Str2DBString(cbbYear->Text.c_str()); } if (!cbbMonth->Text.IsEmpty()){ szSQL += " and month(acceptdate)="; szSQL += Str2DBString(cbbMonth->Text.c_str()); } RunSQL(szSQL, true); if (dm1->Query1->Eof){ ShowMessage("回扣记录不存在"); } lvKB->Clear(); while(!dm1->Query1->Eof) { TListItem *pItem = lvKB->Items->Add(); pItem->Caption = dm1->Query1->FieldByName("oid")->AsString; pItem->SubItems->Add(dm1->Query1->FieldByName("date_accept")->AsString); pItem->SubItems->Add(dm1->Query1->FieldByName("cl_shortname")->AsString); pItem->SubItems->Add(dm1->Query1->FieldByName("kb_value")->AsFloat); pItem->SubItems->Add(dm1->Query1->FieldByName("date_kb")->AsString); dm1->Query1->Next(); } }
/* * When doing (row,column) to (lat,lon) conversions it's sometimes useful * to have some auxillary values around to simplify the calculations. This * function computes those auxillary values, if any, for a projection and * stores them in the projection's auxargs array. * Input: proj - the projection */ static void compute_aux_proj_args( struct projection *proj ) { float lat1, lat2; switch (proj->Kind) { case PROJ_LAMBERT: /* Lambert conformal */ #define Lat1 proj->Args[0] #define Lat2 proj->Args[1] #define PoleRow proj->Args[2] #define PoleCol proj->Args[3] #define CentralLon proj->Args[4] #define ColInc proj->Args[5] #define Hemisphere proj->AuxArgs[0] #define ConeFactor proj->AuxArgs[1] #define Cone proj->AuxArgs[2] proj->AuxArgs = (float *) MALLOC( 3 * sizeof(float) ); if (Lat1==Lat2) { /* polar stereographic? */ if (Lat1>0.0) { lat1 = (90.0 - Lat1) * DEG2RAD; /* Cone = 1.0; */ } else { lat1 = (90.0 + Lat1) * DEG2RAD; /* Cone = -1.0; */ } Cone = cos(lat1); /* WLH 9-27-96 */ Hemisphere = 1.0; } else { /* general Lambert conformal */ float a, b; if (sign(Lat1) != sign(Lat2)) { printf("Error: standard latitudes must have the same sign.\n"); exit(1); } if (Lat1<Lat2) { printf("Error: Lat1 must be >= Lat2\n"); exit(1); } Hemisphere = 1.0; lat1 = (90.0 - Lat1) * DEG2RAD; lat2 = (90.0 - Lat2) * DEG2RAD; a = log( sin(lat1) ) - log( sin(lat2) ); b = log( tan(lat1/2.0) ) - log( tan(lat2/2.0) ); Cone = a / b; } /* Cone is in [-1,1] */ ConeFactor = EARTH_RADIUS * sin(lat1) / (ColInc * Cone * pow(tan(lat1/2.0), Cone) ); #undef Lat1 #undef Lat2 #undef PoleRow #undef PoleCol #undef CentralLon #undef ColInc #undef Hemisphere #undef ConeFactor #undef Cone break; default: /* No auxillary args */ proj->AuxArgs = NULL; } }
static int point2DcmpX(const Point2D &p1, const Point2D &p2) { return sign(p1.x-p2.x); }
int main (int argc, char const *argv[]) { port = 0; bitrate = 10000; // Assign Target Frequencies target_frequency1 = 30000; //atoi(argv[2]); target_frequency2 = 27000; //atoi(argv[3]); target_wavelength1 = (double)SPEED_SOUND_WATER / (double)target_frequency1; target_wavelength2 = (double)SPEED_SOUND_WATER / (double)target_frequency2; // Calculate Sampling Frequency and Target Bin // For affine bin prediction, use m = 10.59, b = -1193.82 and a // window-HALFWIDTH of 5. FFTBinAffinePrediction FP_f1_bin(bitrate, FP_FFT_LENGTH, target_frequency1, BIN_PREDICTION_M, BIN_PREDICTION_B, 3); FFTBinAffinePrediction FP_f2_bin(bitrate, FP_FFT_LENGTH, target_frequency2, BIN_PREDICTION_M, BIN_PREDICTION_B, 3); printf("Sonar: First-pass Target Bin 1: %d\n", FP_f1_bin.getTargetBin()); printf("Sonar: First-pass Target Bin 2: %d\n", FP_f2_bin.getTargetBin()); // Declare Data Vectors //double *FP_adc1_samples, *FP_adc2_samples, *FP_adc3_samples; double FP_adc1_samples[FP_FFT_LENGTH], FP_adc2_samples[FP_FFT_LENGTH], FP_adc3_samples[FP_FFT_LENGTH]; fftw_complex *FP_fft1, *FP_fft2, *FP_fft3; fftw_complex *SP_fft1, *SP_fft2, *SP_fft3; fftw_plan FP_fft_plan1, FP_fft_plan2, FP_fft_plan3; fftw_plan SP_fft_plan1, SP_fft_plan2, SP_fft_plan3; // Allocate Memory for Data Vectors FP_fft1 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( FP_FFT_LENGTH / 2 ) + 1) ); FP_fft2 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( FP_FFT_LENGTH / 2 ) + 1) ); FP_fft3 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( FP_FFT_LENGTH / 2 ) + 1) ); adc1_data_history = new std::vector<double>; adc2_data_history = new std::vector<double>; adc3_data_history = new std::vector<double>; FP_fft_plan1 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc1_samples, FP_fft1, FFTW_ESTIMATE); FP_fft_plan2 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc2_samples, FP_fft2, FFTW_ESTIMATE); FP_fft_plan3 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc3_samples, FP_fft3, FFTW_ESTIMATE); int z = 0; while (z++ < 700) { // Append current data to history vectors adc1_data_history->insert(adc1_data_history->end(), data1 + z * 256, data1 + (z + 1) * 256); adc2_data_history->insert(adc2_data_history->end(), data2 + z * 256, data2 + (z + 1) * 256); adc3_data_history->insert(adc3_data_history->end(), data3 + z * 256, data3 + (z + 1) * 256); // Check data history vector length constraints if (adc1_data_history->size() > DATA_RETENTION_LENGTH) { //printf("Data history is too long, reducing vector sizes\n"); tmp_data_buffer = new std::vector<double> (adc1_data_history->begin() + adc1_data_history->size() - DATA_RETENTION_LENGTH, adc1_data_history->end()); delete adc1_data_history; adc1_data_history = tmp_data_buffer; tmp_data_buffer = NULL; } if (adc2_data_history->size() > DATA_RETENTION_LENGTH) { tmp_data_buffer = new std::vector<double> (adc2_data_history->begin() + adc2_data_history->size() - DATA_RETENTION_LENGTH, adc2_data_history->end()); delete adc2_data_history; adc2_data_history = tmp_data_buffer; tmp_data_buffer = NULL; } if (adc3_data_history->size() > DATA_RETENTION_LENGTH) { tmp_data_buffer = new std::vector<double> (adc3_data_history->begin() + adc3_data_history->size() - DATA_RETENTION_LENGTH, adc3_data_history->end()); delete adc3_data_history; adc3_data_history = tmp_data_buffer; tmp_data_buffer = NULL; } for (int vector_idx = 0; vector_idx <= adc1_data_history->size() - FP_FFT_LENGTH; vector_idx += FP_FFT_LENGTH) { //fprintf(stderr, "Copying vector memory\n"); std::copy(adc1_data_history->begin() + vector_idx, adc1_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc1_samples); std::copy(adc2_data_history->begin() + vector_idx, adc2_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc2_samples); std::copy(adc3_data_history->begin() + vector_idx, adc3_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc3_samples); //fprintf(stderr, "Done copying vector memory\n"); //FP_adc1_samples = data1 + z * FP_FFT_LENGTH; //FP_adc2_samples = data2 + z * FP_FFT_LENGTH; //FP_adc3_samples = data3 + z * FP_FFT_LENGTH; /*for (int m = 0; m < FP_FFT_LENGTH; ++m) printf("%5.0f ", FP_adc1_samples[m]); printf("\n");*/ FP_fft_plan1 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc1_samples, FP_fft1, FFTW_ESTIMATE); FP_fft_plan2 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc2_samples, FP_fft2, FFTW_ESTIMATE); FP_fft_plan3 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, FP_adc3_samples, FP_fft3, FFTW_ESTIMATE); //FP_fft_plan1 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, (double *)&adc1_data_history[vector_idx], FP_fft1, FFTW_ESTIMATE); //FP_fft_plan2 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, (double *)&adc2_data_history[vector_idx], FP_fft2, FFTW_ESTIMATE); //FP_fft_plan3 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, (double *)&adc3_data_history[vector_idx], FP_fft3, FFTW_ESTIMATE); // Perform FFT on current input data //fprintf(stderr, "Pre-FFT Notice\n"); fftw_execute(FP_fft_plan1); fftw_execute(FP_fft_plan2); fftw_execute(FP_fft_plan3); //fprintf(stderr, "FFT Complete\n"); // Calculate Gaussian weighted sums surrounding predicted bin int FP_target_bin = FP_f1_bin.getTargetBin(); // Populate current bin magnitude array bin_current_mag_sq_1 = FP_fft1[FP_target_bin][0] * FP_fft1[FP_target_bin][0] + FP_fft1[FP_target_bin][1] * FP_fft1[FP_target_bin][1]; bin_current_mag_sq_2 = FP_fft2[FP_target_bin][0] * FP_fft2[FP_target_bin][0] + FP_fft2[FP_target_bin][1] * FP_fft2[FP_target_bin][1]; bin_current_mag_sq_3 = FP_fft3[FP_target_bin][0] * FP_fft3[FP_target_bin][0] + FP_fft3[FP_target_bin][1] * FP_fft3[FP_target_bin][1]; //fprintf(stderr, "Bin Magnitudes Selected\n"); // Index Check if (FP_bin_mean_idx >= FP_BIN_HISTORY_LENGTH) FP_bin_mean_idx = 0; // Initilize Mean Array if (FP_bin_history_fill < FP_BIN_HISTORY_LENGTH) { FP_adc1_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_1; FP_adc2_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_2; FP_adc3_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_3; ++FP_bin_mean_idx; fftw_destroy_plan(FP_fft_plan1); fftw_destroy_plan(FP_fft_plan2); fftw_destroy_plan(FP_fft_plan3); ++FP_bin_history_fill; continue; } adc1_bin_mean = 0; adc2_bin_mean = 0; adc3_bin_mean = 0; for (int i = 0; i < FP_BIN_HISTORY_LENGTH; ++i) { adc1_bin_mean += FP_adc1_bin_history[i]; adc2_bin_mean += FP_adc2_bin_history[i]; adc3_bin_mean += FP_adc3_bin_history[i]; } adc1_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; adc2_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; adc3_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; if (bin_current_mag_sq_1 > MEAN_SCALE_FACTOR * adc1_bin_mean && bin_current_mag_sq_2 > MEAN_SCALE_FACTOR * adc2_bin_mean && bin_current_mag_sq_3 > MEAN_SCALE_FACTOR * adc3_bin_mean) { FP_bin_indicator.push_back(true); printf("1 "); printf("%15.0f\n", bin_current_mag_sq_3); } else { FP_bin_indicator.push_back(false); //printf("0 "); //printf("%15.0f\n", bin_current_mag_sq_3); FP_adc1_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_1; FP_adc2_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_2; FP_adc3_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_3; //printf("%10.0f, %10.0f, %10.0f\n", adc1_bin_mean, adc2_bin_mean, adc3_bin_mean); ++FP_bin_mean_idx; } if (FP_bin_indicator[0] == 0 && FP_bin_indicator[1] == 1) { printf("Just had a pulse!\n"); int SP_bin_count = 0; while (FP_bin_indicator[SP_bin_count + 1] != 0) ++SP_bin_count; printf("Signal Bin Count of %d\n", SP_bin_count); if (SP_bin_count * FP_FFT_LENGTH >= FP_MIN_SAMPLE_LENGTH) { int SP_fft_length = SP_bin_count * FP_FFT_LENGTH; // Copy the sample data into new double array double *SP_adc1_samples = new double[SP_fft_length]; double *SP_adc2_samples = new double[SP_fft_length]; double *SP_adc3_samples = new double[SP_fft_length]; //std::copy(adc1_data_history->end() - (SP_bin_count + 1) * FP_FFT_LENGTH, adc1_data_history->end() - FP_FFT_LENGTH, SP_adc1_samples); std::copy(adc1_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc1_data_history->begin() + vector_idx, SP_adc1_samples); std::copy(adc2_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc2_data_history->begin() + vector_idx, SP_adc2_samples); std::copy(adc3_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc3_data_history->begin() + vector_idx, SP_adc3_samples); // Allocate Memory for Data Arrays SP_fft1 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( (SP_fft_length) / 2 ) + 1) ); SP_fft2 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( (SP_fft_length) / 2 ) + 1) ); SP_fft3 = (double (*)[2]) fftw_malloc ( sizeof ( fftw_complex ) * (( (SP_fft_length) / 2 ) + 1) ); SP_fft_plan1 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, SP_adc1_samples, SP_fft1, FFTW_ESTIMATE); SP_fft_plan2 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, SP_adc1_samples, SP_fft2, FFTW_ESTIMATE); SP_fft_plan3 = fftw_plan_dft_r2c_1d(FP_FFT_LENGTH, SP_adc1_samples, SP_fft3, FFTW_ESTIMATE); // Perform FFT on current input data fftw_execute(SP_fft_plan1); fftw_execute(SP_fft_plan2); fftw_execute(SP_fft_plan3); FFTBinAffinePrediction SP_f1_bin(bitrate, SP_fft_length, target_frequency1, BIN_PREDICTION_M, BIN_PREDICTION_B, 3); /*FFTBinAffinePrediction SP_f2_bin(bitrate, SP_fft_length, target_frequency2, BIN_PREDICTION_M, BIN_PREDICTION_B, 3);*/ int SP_target_bin = SP_f1_bin.getTargetBin(); fprintf(stderr, "SP Target Bin: %d\n", SP_target_bin); // Calculate Phase of each signal found on each ADC double phase1 = atan2((double)SP_fft1[(int)SP_target_bin][1], (double)SP_fft1[(int)SP_target_bin][0]); double phase2 = atan2((double)SP_fft2[(int)SP_target_bin][1], (double)SP_fft2[(int)SP_target_bin][0]); double phase3 = atan2((double)SP_fft3[(int)SP_target_bin][1], (double)SP_fft3[(int)SP_target_bin][0]); // Calculate usable phase differences double delta1 = phase2 - phase1; double delta2 = phase3 - phase2; double delta3 = phase1 - phase3; fprintf(stderr, "Phase: %5.5f %5.5f %5.5f\n", phase1, phase2, phase3); fprintf(stderr, "Deltas: %5.5f %5.5f %5.5f\n", delta1, delta2, delta3); // Free Data Array Memory fftw_free(SP_fft1); fftw_free(SP_fft2); fftw_free(SP_fft3); // Determine minimum phase difference for pair selection int min_index = 3; if (fabs(delta2) < fabs(delta1) && fabs(delta2) < fabs(delta3)) min_index = 2; if (fabs(delta1) < fabs(delta2) && fabs(delta1) < fabs(delta3)) min_index = 1; double delta_tmp; switch (min_index) { case 1: delta_tmp = delta1; if (delta3 > delta2) delta_tmp = -1.0 * delta1 + sign(delta_tmp) * 2.0 * M_PI; angle_estimate1 = delta_tmp * (double)(((double)target_wavelength1 / 2.0) / SENSOR_SPACING_2_1) * 180.0 / M_PI / 2.0; break; case 2: delta_tmp = delta2; if (delta1 > delta3) delta_tmp = -1.0 * delta2 + 2.0 * M_PI; angle_estimate1 = (delta_tmp - 4.0 / 3.0 * M_PI) * ((target_wavelength1 / 2.0) / SENSOR_SPACING_3_2) * 180.0 / M_PI / 2.0; break; case 3: delta_tmp = delta3; if (delta2 > delta1) delta_tmp = -1.0 * delta3 - 2.0 * M_PI; angle_estimate1 = (delta_tmp + 4.0 / 3.0 * M_PI ) * (((double)target_wavelength1 / 2.0) / SENSOR_SPACING_1_3) * 180.0 / M_PI / 2.0; break; default: fprintf(stderr, "Sonar: Invalid min_index for phase difference."); } fprintf(stderr, "Min Index: %d\n", min_index); fprintf(stderr, "Angle Estimate (1): %3.2f\n", angle_estimate1); // DEBUG for (int blah = 0; blah < SP_bin_count * FP_FFT_LENGTH; ++blah) printf("%5.0f", SP_adc1_samples[blah]); printf("\n"); } } if (FP_bin_indicator.size() > FP_BIN_HISTORY_LENGTH) FP_bin_indicator.erase(FP_bin_indicator.begin()); } } fftw_destroy_plan(FP_fft_plan1); fftw_destroy_plan(FP_fft_plan2); fftw_destroy_plan(FP_fft_plan3); fftw_destroy_plan(SP_fft_plan1); fftw_destroy_plan(SP_fft_plan2); fftw_destroy_plan(SP_fft_plan3); return 0; }
int main(int argc, char * const argv[]) { int err = 0, r, c, long_optind = 0; int do_decipher = 0; int do_sign = 0; int action_count = 0; struct sc_pkcs15_object *key; sc_context_param_t ctx_param; while (1) { c = getopt_long(argc, argv, "sck:r:i:o:Rp:vw", options, &long_optind); if (c == -1) break; if (c == '?') util_print_usage_and_die(app_name, options, option_help, NULL); switch (c) { case 's': do_sign++; action_count++; break; case 'c': do_decipher++; action_count++; break; case 'k': opt_key_id = optarg; action_count++; break; case 'r': opt_reader = optarg; break; case 'i': opt_input = optarg; break; case 'o': opt_output = optarg; break; case 'R': opt_raw = 1; break; case OPT_SHA1: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_SHA1; break; case OPT_SHA256: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_SHA256; break; case OPT_SHA384: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_SHA384; break; case OPT_SHA512: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_SHA512; break; case OPT_SHA224: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_SHA224; break; case OPT_MD5: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_MD5; break; case OPT_HASH_NONE: opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_NONE; break; case OPT_PKCS1: opt_crypt_flags |= SC_ALGORITHM_RSA_PAD_PKCS1; break; case 'v': verbose++; break; case 'p': opt_pincode = optarg; break; case OPT_BIND_TO_AID: opt_bind_to_aid = optarg; break; case 'w': opt_wait = 1; break; } } if (action_count == 0) util_print_usage_and_die(app_name, options, option_help, NULL); if (!(opt_crypt_flags & SC_ALGORITHM_RSA_HASHES)) opt_crypt_flags |= SC_ALGORITHM_RSA_HASH_NONE; memset(&ctx_param, 0, sizeof(ctx_param)); ctx_param.ver = 0; ctx_param.app_name = app_name; r = sc_context_create(&ctx, &ctx_param); if (r) { fprintf(stderr, "Failed to establish context: %s\n", sc_strerror(r)); return 1; } if (verbose > 1) { ctx->debug = verbose; sc_ctx_log_to_file(ctx, "stderr"); } err = util_connect_card(ctx, &card, opt_reader, opt_wait, verbose); if (err) goto end; if (verbose) fprintf(stderr, "Trying to find a PKCS #15 compatible card...\n"); if (opt_bind_to_aid) { struct sc_aid aid; aid.len = sizeof(aid.value); if (sc_hex_to_bin(opt_bind_to_aid, aid.value, &aid.len)) { fprintf(stderr, "Invalid AID value: '%s'\n", opt_bind_to_aid); return 1; } r = sc_pkcs15_bind(card, &aid, &p15card); } else { r = sc_pkcs15_bind(card, NULL, &p15card); } if (r) { fprintf(stderr, "PKCS #15 binding failed: %s\n", sc_strerror(r)); err = 1; goto end; } if (verbose) fprintf(stderr, "Found %s!\n", p15card->tokeninfo->label); if (do_decipher) { if ((err = get_key(SC_PKCS15_PRKEY_USAGE_DECRYPT, &key)) || (err = decipher(key))) goto end; action_count--; } if (do_sign) { if ((err = get_key(SC_PKCS15_PRKEY_USAGE_SIGN| SC_PKCS15_PRKEY_USAGE_SIGNRECOVER| SC_PKCS15_PRKEY_USAGE_NONREPUDIATION, &key)) || (err = sign(key))) goto end; action_count--; } end: if (p15card) sc_pkcs15_unbind(p15card); if (card) { sc_unlock(card); sc_disconnect_card(card); } if (ctx) sc_release_context(ctx); return err; }
/* * Evaluate the object's overall power level. */ s32b object_power(const object_type* o_ptr, int verbose, ang_file *log_file, bool known) { s32b p = 0; object_kind *k_ptr; int immunities = 0; int misc = 0; int lowres = 0; int highres = 0; int sustains = 0; int extra_stat_bonus = 0; int i; bitflag flags[OF_SIZE]; const slay_t *s_ptr; /* Extract the flags */ if (known) { LOG_PRINT("Object is known\n"); object_flags(o_ptr, flags); } else { LOG_PRINT("Object is not fully known\n"); object_flags_known(o_ptr, flags); } if (verbose) { LOG_PRINT("Object flags ="); for (i = 0; i < (int)OF_SIZE; i++) LOG_PRINT1(" %02x", flags[i]); LOG_PRINT("\n"); } k_ptr = &k_info[o_ptr->k_idx]; /* Evaluate certain abilities based on type of object. */ switch (o_ptr->tval) { case TV_BOW: { int mult; /* * Damage multiplier for bows should be weighted less than that * for melee weapons, since players typically get fewer shots * than hits (note, however, that the multipliers are applied * afterwards in the bow calculation, not before as for melee * weapons, which tends to bring these numbers back into line). * ToDo: rework evaluation of negative pvals */ p += (o_ptr->to_d * DAMAGE_POWER / 2); LOG_PRINT1("Adding power from to_dam, total is %d\n", p); /* * Add the average damage of fully enchanted (good) ammo for this * weapon. Could make this dynamic based on k_info if desired. * ToDo: precisely that. */ if (o_ptr->sval == SV_SLING) { p += (AVG_SLING_AMMO_DAMAGE * DAMAGE_POWER / 2); } else if (o_ptr->sval == SV_SHORT_BOW || o_ptr->sval == SV_LONG_BOW) { p += (AVG_BOW_AMMO_DAMAGE * DAMAGE_POWER / 2); } else if (o_ptr->sval == SV_LIGHT_XBOW || o_ptr->sval == SV_HEAVY_XBOW) { p += (AVG_XBOW_AMMO_DAMAGE * DAMAGE_POWER / 2); } LOG_PRINT1("Adding power from ammo, total is %d\n", p); mult = bow_multiplier(o_ptr->sval); LOG_PRINT1("Base multiplier for this weapon is %d\n", mult); if (of_has(flags, OF_MIGHT) && (known || object_pval_is_visible(o_ptr))) { if (o_ptr->pval >= INHIBIT_MIGHT || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ mult = 1; /* don't overflow */ LOG_PRINT("INHIBITING - too much extra might\n"); } else { mult += o_ptr->pval; } LOG_PRINT1("Extra might multiple is %d\n", mult); } p *= mult; LOG_PRINT2("Multiplying power by %d, total is %d\n", mult, p); if (of_has(flags, OF_SHOTS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra shots: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_SHOTS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING - too many extra shots\n"); } else if (o_ptr->pval > 0) { p = (p * (1 + o_ptr->pval)); LOG_PRINT2("Multiplying power by 1 + %d, total is %d\n", o_ptr->pval, p); } } /* Apply the correct slay multiplier */ p = (p * slay_power(o_ptr, verbose, log_file, flags)) / tot_mon_power; LOG_PRINT1("Adjusted for slay power, total is %d\n", p); if (o_ptr->weight < k_ptr->weight) { p++; LOG_PRINT("Incrementing power by one for low weight\n"); } /* * Correction to match ratings to melee damage ratings. * We multiply all missile weapons by 1.5 in order to compare damage. * (CR 11/20/01 - changed this to 1.25). * (CC 25/07/10 - changed this back to 1.5). * Melee weapons assume MAX_BLOWS per turn, so we must * also divide by MAX_BLOWS to get equal ratings. */ p = sign(p) * (ABS(p) * BOW_RESCALER / (10 * MAX_BLOWS)); LOG_PRINT1("Rescaling bow power, total is %d\n", p); p += sign(o_ptr->to_h) * (ABS(o_ptr->to_h) * TO_HIT_POWER / 2); LOG_PRINT1("Adding power from to_hit, total is %d\n", p); break; } case TV_SHOT: case TV_ARROW: case TV_BOLT: case TV_DIGGING: case TV_HAFTED: case TV_POLEARM: case TV_SWORD: { p += (o_ptr->dd * (o_ptr->ds + 1) * DAMAGE_POWER / 4); LOG_PRINT1("Adding power for dam dice, total is %d\n", p); /* Apply the correct slay multiplier */ p = (p * slay_power(o_ptr, verbose, log_file, flags)) / tot_mon_power; LOG_PRINT1("Adjusted for slay power, total is %d\n", p); p += (o_ptr->to_d * DAMAGE_POWER / 2); LOG_PRINT1("Adding power for to_dam, total is %d\n", p); if (of_has(flags, OF_BLOWS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra blows: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_BLOWS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING, too many extra blows or a negative number\n"); } else if (o_ptr->pval > 0) { p = sign(p) * ((ABS(p) * (MAX_BLOWS + o_ptr->pval)) / MAX_BLOWS); /* Add an extra amount per extra blow to account for damage/branding rings */ p += ((MELEE_DAMAGE_BOOST + RING_BRAND_DMG) * o_ptr->pval * DAMAGE_POWER / (2 * MAX_BLOWS)); LOG_PRINT1("Adding power for blows, total is %d\n", p); } } /* * add extra power for multiple slays/brands, as these * add diminishing amounts to average damage */ i = 0; for (s_ptr = slay_table; s_ptr->slay_flag; s_ptr++) { if (!of_has(flags, s_ptr->slay_flag)) continue; i++; if (i > 1) p += (i * 3); } LOG_PRINT1("Adding power for multiple slays/brands, total is %d\n", p); /* add launcher bonus for ego ammo, and multiply */ if (o_ptr->tval == TV_SHOT) { if (o_ptr->name2) p += (AVG_LAUNCHER_DMG * DAMAGE_POWER / 2); p = p * AVG_SLING_MULT * BOW_RESCALER / (20 * MAX_BLOWS); } if (o_ptr->tval == TV_ARROW) { if (o_ptr->name2) p += (AVG_LAUNCHER_DMG * DAMAGE_POWER / 2); p = p * AVG_BOW_MULT * BOW_RESCALER / (20 * MAX_BLOWS); } if (o_ptr->tval == TV_BOLT) { if (o_ptr->name2) p += (AVG_LAUNCHER_DMG * DAMAGE_POWER / 2); p = p * AVG_XBOW_MULT * BOW_RESCALER / (20 * MAX_BLOWS); } LOG_PRINT1("After multiplying ammo and rescaling, power is %d\n", p); p += sign(o_ptr->to_h) * (ABS(o_ptr->to_h) * TO_HIT_POWER / 2); LOG_PRINT1("Adding power for to hit, total is %d\n", p); /* Remember, weight is in 0.1 lb. units. */ if (o_ptr->weight < k_ptr->weight) { p += (k_ptr->weight - o_ptr->weight) / 20; LOG_PRINT1("Adding power for low weight, total is %d\n", p); } break; } case TV_BOOTS: case TV_GLOVES: case TV_HELM: case TV_CROWN: case TV_SHIELD: case TV_CLOAK: case TV_SOFT_ARMOR: case TV_HARD_ARMOR: case TV_DRAG_ARMOR: { p += BASE_ARMOUR_POWER; LOG_PRINT1("Armour item, base power is %d\n", p); p += sign(o_ptr->ac) * ((ABS(o_ptr->ac) * BASE_AC_POWER) / 2); LOG_PRINT1("Adding power for base AC value, total is %d\n", p); /* Add power for AC per unit weight */ if (o_ptr->weight > 0) { i = 1000 * (o_ptr->ac + o_ptr->to_a) / o_ptr->weight; /* Stop overpricing Elven Cloaks */ if (i > 400) i = 400; /* Adjust power */ p *= i; p /= 100; } /* Weightless (ethereal) items get fixed boost */ else p *= 5; /* Add power for +hit and +dam */ p += sign(o_ptr->to_h) * (ABS(o_ptr->to_h) * TO_HIT_POWER); LOG_PRINT1("Adding power for to_hit, total is %d\n", p); p += o_ptr->to_d * DAMAGE_POWER; LOG_PRINT1("Adding power for to_dam, total is %d\n", p); /* Apply the correct brand/slay multiplier */ p += (((2 * (o_ptr->to_d + RING_BRAND_DMG) * slay_power(o_ptr, verbose, log_file, flags)) / tot_mon_power) - (2 * (o_ptr->to_d + RING_BRAND_DMG))); LOG_PRINT1("Adjusted for brand/slay power, total is %d\n", p); /* Add power for extra blows */ if (of_has(flags, OF_BLOWS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra blows: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_BLOWS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING, too many extra blows or a negative number\n"); } else if (o_ptr->pval > 0) { p += ((MELEE_DAMAGE_BOOST + RING_BRAND_DMG + o_ptr->to_d) * o_ptr->pval * DAMAGE_POWER / MAX_BLOWS); LOG_PRINT1("Adding power for extra blows, total is %d\n", p); } } /* Add power for extra shots */ if (of_has(flags, OF_SHOTS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra shots: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_SHOTS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING - too many extra shots\n"); } else if (o_ptr->pval > 0) { p += ((AVG_XBOW_AMMO_DAMAGE + AVG_LAUNCHER_DMG) * AVG_XBOW_MULT * o_ptr->pval * DAMAGE_POWER * BOW_RESCALER / (20 * MAX_BLOWS)); LOG_PRINT1("Adding power for extra shots - total is %d\n", p); } } break; } case TV_LIGHT: { p += BASE_LIGHT_POWER; LOG_PRINT("Light source, adding base power\n"); p += sign(o_ptr->to_h) * (ABS(o_ptr->to_h) * TO_HIT_POWER); LOG_PRINT1("Adding power for to_hit, total is %d\n", p); p += o_ptr->to_d * DAMAGE_POWER; LOG_PRINT1("Adding power for to_dam, total is %d\n", p); /* Apply the correct brand/slay multiplier */ p += (((2 * (o_ptr->to_d + RING_BRAND_DMG) * slay_power(o_ptr, verbose, log_file, flags)) / tot_mon_power) - (2 * (o_ptr->to_d + RING_BRAND_DMG))); LOG_PRINT1("Adjusted for brand/slay power, total is %d\n", p); /* Add power for extra blows */ if (of_has(flags, OF_BLOWS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra blows: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_BLOWS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING, too many extra blows or a negative number\n"); } else if (o_ptr->pval > 0) { p += ((MELEE_DAMAGE_BOOST + RING_BRAND_DMG + o_ptr->to_d) * o_ptr->pval * DAMAGE_POWER / MAX_BLOWS); LOG_PRINT1("Adding power for extra blows, total is %d\n", p); } } /* Add power for extra shots */ if (of_has(flags, OF_SHOTS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra shots: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_SHOTS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING - too many extra shots\n"); } else if (o_ptr->pval > 0) { p += ((AVG_XBOW_AMMO_DAMAGE + AVG_LAUNCHER_DMG) * AVG_XBOW_MULT * o_ptr->pval * DAMAGE_POWER * BOW_RESCALER / (20 * MAX_BLOWS)); LOG_PRINT1("Adding power for extra shots - total is %d\n", p); } } /* * Big boost for extra light radius * n.b. Another few points are added below */ if (of_has(flags, OF_LIGHT)) p += 30; break; } case TV_RING: case TV_AMULET: { p += BASE_JEWELRY_POWER; LOG_PRINT("Jewellery - adding base power\n"); p += sign(o_ptr->to_h) * (ABS(o_ptr->to_h) * TO_HIT_POWER); LOG_PRINT1("Adding power for to_hit, total is %d\n", p); p += o_ptr->to_d * DAMAGE_POWER; LOG_PRINT1("Adding power for to_dam, total is %d\n", p); /* Apply the correct brand/slay multiplier */ p += (((2 * (o_ptr->to_d + RING_BRAND_DMG) * slay_power(o_ptr, verbose, log_file, flags)) / tot_mon_power) - (2 * (o_ptr->to_d + RING_BRAND_DMG))); LOG_PRINT1("Adjusted for brand/slay power, total is %d\n", p); /* Add power for extra blows */ if (of_has(flags, OF_BLOWS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra blows: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_BLOWS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING, too many extra blows or a negative number\n"); } else if (o_ptr->pval > 0) { p += ((MELEE_DAMAGE_BOOST + RING_BRAND_DMG + o_ptr->to_d) * o_ptr->pval * DAMAGE_POWER / MAX_BLOWS); LOG_PRINT1("Adding power for extra blows, total is %d\n", p); } } /* Add power for extra shots */ if (of_has(flags, OF_SHOTS) && (known || object_pval_is_visible(o_ptr))) { LOG_PRINT1("Extra shots: %d\n", o_ptr->pval); if (o_ptr->pval >= INHIBIT_SHOTS || o_ptr->pval < 0) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING - too many extra shots\n"); } else if (o_ptr->pval > 0) { p += ((AVG_XBOW_AMMO_DAMAGE + AVG_LAUNCHER_DMG) * AVG_XBOW_MULT * o_ptr->pval * DAMAGE_POWER * BOW_RESCALER / (20 * MAX_BLOWS)); LOG_PRINT1("Adding power for extra shots - total is %d\n", p); } } break; } } /* Other abilities are evaluated independent of the object type. */ p += sign(o_ptr->to_a) * (ABS(o_ptr->to_a) * TO_AC_POWER / 2); LOG_PRINT2("Adding power for to_ac of %d, total is %d\n", o_ptr->to_a, p); if (o_ptr->to_a > HIGH_TO_AC) { p += ((o_ptr->to_a - (HIGH_TO_AC - 1)) * TO_AC_POWER / 2); LOG_PRINT1("Adding power for high to_ac value, total is %d\n", p); } if (o_ptr->to_a > VERYHIGH_TO_AC) { p += ((o_ptr->to_a - (VERYHIGH_TO_AC -1)) * TO_AC_POWER / 2); LOG_PRINT1("Adding power for very high to_ac value, total is %d\n", p); } if (o_ptr->to_a >= INHIBIT_AC) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING: AC bonus too high\n"); } if ((o_ptr->pval > 0) && (known || object_pval_is_visible(o_ptr))) { if (of_has(flags, OF_STR)) { p += STR_POWER * o_ptr->pval; LOG_PRINT2("Adding power for STR bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_INT)) { p += INT_POWER * o_ptr->pval; LOG_PRINT2("Adding power for INT bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_WIS)) { p += WIS_POWER * o_ptr->pval; LOG_PRINT2("Adding power for WIS bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_DEX)) { p += DEX_POWER * o_ptr->pval; LOG_PRINT2("Adding power for DEX bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_CON)) { p += CON_POWER * o_ptr->pval; LOG_PRINT2("Adding power for CON bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_STEALTH)) { p += STEALTH_POWER * o_ptr->pval; LOG_PRINT2("Adding power for Stealth bonus %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_SEARCH)) { p += SEARCH_POWER * o_ptr->pval; LOG_PRINT2("Adding power for searching bonus %d, total is %d\n", o_ptr->pval , p); } /* Add extra power term if there are a lot of ability bonuses */ if (o_ptr->pval > 0) { extra_stat_bonus += (of_has(flags, OF_STR) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_INT) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_WIS) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_DEX) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_CON) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_CHR) ? 0 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_STEALTH) ? 1 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_INFRA) ? 0 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_TUNNEL) ? 0 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_SEARCH) ? 0 * o_ptr->pval : 0); extra_stat_bonus += (of_has(flags, OF_SPEED) ? 0 * o_ptr->pval : 0); if (o_ptr->tval == TV_BOW) { extra_stat_bonus += (of_has(flags, OF_MIGHT) ? 5 * o_ptr->pval / 2 : 0); extra_stat_bonus += (of_has(flags, OF_SHOTS) ? 3 * o_ptr->pval : 0); } else if ( (o_ptr->tval == TV_DIGGING) || (o_ptr->tval == TV_HAFTED) || (o_ptr->tval == TV_POLEARM) || (o_ptr->tval == TV_SWORD) ) { extra_stat_bonus += (of_has(flags, OF_BLOWS) ? 3 * o_ptr->pval : 0); } if (extra_stat_bonus > 24) { /* Inhibit */ LOG_PRINT1("Inhibiting! (Total ability bonus of %d is too high)\n", extra_stat_bonus); p += INHIBIT_POWER; } else { p += ability_power[extra_stat_bonus]; LOG_PRINT2("Adding power for combination of %d, total is %d\n", ability_power[extra_stat_bonus], p); } } } else if ((o_ptr->pval < 0) && (known || object_pval_is_visible(o_ptr))) { if (of_has(flags, OF_STR)) p += 4 * o_ptr->pval; if (of_has(flags, OF_INT)) p += 2 * o_ptr->pval; if (of_has(flags, OF_WIS)) p += 2 * o_ptr->pval; if (of_has(flags, OF_DEX)) p += 3 * o_ptr->pval; if (of_has(flags, OF_CON)) p += 4 * o_ptr->pval; if (of_has(flags, OF_STEALTH)) p += o_ptr->pval; LOG_PRINT1("Subtracting power for negative ability values, total is %d\n", p); } if (known || object_pval_is_visible(o_ptr)) { if (of_has(flags, OF_CHR)) { p += CHR_POWER * o_ptr->pval; LOG_PRINT2("Adding power for CHR bonus/penalty %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_INFRA)) { p += INFRA_POWER * o_ptr->pval; LOG_PRINT2("Adding power for infra bonus/penalty %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_TUNNEL)) { p += TUNN_POWER * o_ptr->pval; LOG_PRINT2("Adding power for tunnelling bonus/penalty %d, total is %d\n", o_ptr->pval, p); } if (of_has(flags, OF_SPEED)) { p += sign(o_ptr->pval) * speed_power[ABS(o_ptr->pval)]; LOG_PRINT2("Adding power for speed bonus/penalty %d, total is %d\n", o_ptr->pval, p); } } #define ADD_POWER1(string, val, flag) \ if (of_has(flags, flag)) { \ p += (val); \ LOG_PRINT1("Adding power for " string ", total is %d\n", p); \ } #define ADD_POWER2(string, val, flag, extra) \ if (of_has(flags, flag)) { \ p += (val); \ extra; \ LOG_PRINT1("Adding power for " string ", total is %d\n", p); \ } ADD_POWER2("sustain STR", 9, OF_SUST_STR, sustains++); ADD_POWER2("sustain INT", 4, OF_SUST_INT, sustains++); ADD_POWER2("sustain WIS", 4, OF_SUST_WIS, sustains++); ADD_POWER2("sustain DEX", 7, OF_SUST_DEX, sustains++); ADD_POWER2("sustain CON", 8, OF_SUST_CON, sustains++); ADD_POWER1("sustain CHR", 1, OF_SUST_CHR); for (i = 2; i <= sustains; i++) { p += i; LOG_PRINT1("Adding power for multiple sustains, total is %d\n", p); if (i == 5) { p += SUST_POWER; LOG_PRINT1("Adding power for full set of sustains, total is %d\n", p); } } ADD_POWER2("acid immunity", 38, OF_IM_ACID, immunities++); ADD_POWER2("elec immunity", 35, OF_IM_ELEC, immunities++); ADD_POWER2("fire immunity", 40, OF_IM_FIRE, immunities++); ADD_POWER2("cold immunity", 37, OF_IM_COLD, immunities++); for (i = 2; i <= immunities; i++) { p += IMMUNITY_POWER; LOG_PRINT1("Adding power for multiple immunities, total is %d\n", p); if (i >= INHIBIT_IMMUNITIES) { p += INHIBIT_POWER; /* inhibit */ LOG_PRINT("INHIBITING: Too many immunities\n"); } } ADD_POWER2("free action", 14, OF_FREE_ACT, misc++); ADD_POWER2("hold life", 12, OF_HOLD_LIFE, misc++); ADD_POWER1("feather fall", 1, OF_FEATHER); ADD_POWER2("permanent light", 3, OF_LIGHT, misc++); ADD_POWER2("see invisible", 10, OF_SEE_INVIS, misc++); ADD_POWER2("telepathy", 70, OF_TELEPATHY, misc++); ADD_POWER2("slow digestion", 2, OF_SLOW_DIGEST, misc++); ADD_POWER2("resist acid", 5, OF_RES_ACID, lowres++); ADD_POWER2("resist elec", 6, OF_RES_ELEC, lowres++); ADD_POWER2("resist fire", 6, OF_RES_FIRE, lowres++); ADD_POWER2("resist cold", 6, OF_RES_COLD, lowres++); ADD_POWER2("resist poison", 28, OF_RES_POIS, highres++); ADD_POWER2("resist fear", 6, OF_RES_FEAR, highres++); ADD_POWER2("resist light", 6, OF_RES_LIGHT, highres++); ADD_POWER2("resist dark", 16, OF_RES_DARK, highres++); ADD_POWER2("resist blindness", 16, OF_RES_BLIND, highres++); ADD_POWER2("resist confusion", 24, OF_RES_CONFU, highres++); ADD_POWER2("resist sound", 14, OF_RES_SOUND, highres++); ADD_POWER2("resist shards", 8, OF_RES_SHARD, highres++); ADD_POWER2("resist nexus", 15, OF_RES_NEXUS, highres++); ADD_POWER2("resist nether", 20, OF_RES_NETHR, highres++); ADD_POWER2("resist chaos", 20, OF_RES_CHAOS, highres++); ADD_POWER2("resist disenchantment", 20, OF_RES_DISEN, highres++); ADD_POWER2("regeneration", 9, OF_REGEN, misc++); ADD_POWER1("blessed", 1, OF_BLESSED); ADD_POWER1("no fuel", 5, OF_NO_FUEL); for (i = 2; i <= misc; i++) { p += i; LOG_PRINT1("Adding power for multiple misc abilities, total is %d\n", p); } for (i = 2; i <= lowres; i++) { p += i; LOG_PRINT1("Adding power for multiple low resists, total is %d\n", p); if (i == 4) { p += RBASE_POWER; LOG_PRINT1("Adding power for full rbase set, total is %d\n", p); } } for (i = 2; i <= highres; i++) { p += (i * 2); LOG_PRINT1("Adding power for multiple high resists, total is %d\n", p); } /* Note: the following code is irrelevant until curses are reworked */ if (of_has(flags, OF_TELEPORT)) { p -= 1; LOG_PRINT1("Subtracting power for teleportation, total is %d\n", p); } if (of_has(flags, OF_DRAIN_EXP)) { p -= 1; LOG_PRINT1("Subtracting power for drain experience, total is %d\n", p); } if (of_has(flags, OF_AGGRAVATE)) { p -= 1; LOG_PRINT1("Subtracting power for aggravation, total is %d\n", p); } if (of_has(flags, OF_LIGHT_CURSE)) { p -= 1; LOG_PRINT1("Subtracting power for light curse, total is %d\n", p); } if (of_has(flags, OF_HEAVY_CURSE)) { p -= 1; LOG_PRINT1("Subtracting power for heavy curse, total is %d\n", p); } /* if (of_has(flags, OF_PERMA_CURSE)) p -= 40; */ /* add power for effect */ if (known || object_effect_is_known(o_ptr)) { if (o_ptr->name1 && a_info[o_ptr->name1].effect) { p += effect_power(a_info[o_ptr->name1].effect); LOG_PRINT1("Adding power for artifact activation, total is %d\n", p); } else { p += effect_power(k_info[o_ptr->k_idx].effect); LOG_PRINT1("Adding power for item activation, total is %d\n", p); } } /* add tiny amounts for ignore flags */ if (of_has(flags, OF_IGNORE_ACID)) p++; if (of_has(flags, OF_IGNORE_FIRE)) p++; if (of_has(flags, OF_IGNORE_COLD)) p++; if (of_has(flags, OF_IGNORE_ELEC)) p++; LOG_PRINT1("After ignore flags, FINAL POWER IS %d\n", p); return (p); }
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * // void Foam::extendedMomentInversion::invert(const univariateMomentSet& moments) { univariateMomentSet m(moments); reset(); // Terminate execution if negative number density is encountered if (m[0] < 0.0) { FatalErrorIn ( "Foam::extendedMomentInversion::invert\n" "(\n" " const univariateMomentSet& moments\n" ")" ) << "The zero-order moment is negative." << abort(FatalError); } // Exclude cases where the zero-order moment is very small to avoid // problems in the inversion due to round-off error if (m[0] < SMALL) { sigma_ = 0.0; nullSigma_ = true; return; } label nRealizableMoments = m.nRealizableMoments(); if (nRealizableMoments % 2 == 0) { // If the number of realizable moments is even, we apply the standard // QMOM directly to maximize the number of preserved moments. // Info << "Even number of realizable moments: using QMOM" << endl; // Info << "Moments: " << m << endl; // Info << "Invertible: " << m.nInvertibleMoments() << endl; // Info << "Realizable: " << m.nRealizableMoments() << endl; m.invert(); sigma_ = 0.0; nullSigma_ = true; secondaryQuadrature(m); } else { // Resizing the moment set to avoid copying again m.resize(nRealizableMoments); // Local set of starred moments univariateMomentSet mStar(nRealizableMoments, 0, m.support()); // Compute target function for sigma = 0 scalar sigmaLow = 0.0; scalar fLow = targetFunction(sigmaLow, m, mStar); sigma_ = sigmaLow; // Check if sigma = 0 is root if (mag(fLow) <= targetFunctionTol_) { m.invert(); sigma_ = 0.0; nullSigma_ = true; secondaryQuadrature(m); return; } // Compute target function for sigma = sigmaMax scalar sigMax = sigmaMax(m); scalar sigmaHigh = sigMax; scalar fHigh = targetFunction(sigmaHigh, m, mStar); if (fLow*fHigh > 0) { // Root not found. Minimize target function in [0, sigma_] sigma_ = minimizeTargetFunction(0, sigmaHigh, m, mStar); // Check if sigma is small and use QMOM if (mag(sigma_) < sigmaTol_) { m.invert(); sigma_ = 0.0; nullSigma_ = true; secondaryQuadrature(m); return; } targetFunction(sigma_, m, mStar); secondaryQuadrature(mStar); return; } // Apply Ridder's algorithm to find sigma for (label iter = 0; iter < maxSigmaIter_; iter++) { scalar fMid, sigmaMid; sigmaMid = (sigmaLow + sigmaHigh)/2.0; fMid = targetFunction(sigmaMid, m, mStar); scalar s = sqrt(sqr(fMid) - fLow*fHigh); if (s == 0.0) { FatalErrorIn ( "Foam::extendedMomentInversion::invert\n" "(\n" " const univariateMomentSet& moments\n" ")" )<< "Singular value encountered while attempting to find root." << "Moment set = " << m << endl << "sigma = " << sigma_ << endl << "fLow = " << fLow << endl << "fMid = " << fMid << endl << "fHigh = " << fHigh << abort(FatalError); } sigma_ = sigmaMid + (sigmaMid - sigmaLow)*sign(fLow - fHigh)*fMid/s; momentsToMomentsStar(sigma_, m, mStar); scalar fNew = targetFunction(sigma_, m, mStar); scalar dSigma = (sigmaHigh - sigmaLow)/2.0; // Check for convergence if (mag(fNew) <= targetFunctionTol_ || mag(dSigma) <= sigmaTol_) { if (mag(sigma_) < sigmaTol_) { m.invert(); sigma_ = 0.0; nullSigma_ = true; secondaryQuadrature(m); return; } scalar momentError = normalizedMomentError(sigma_, m, mStar); if ( momentError < momentsTol_ ) { // Found a value of sigma that preserves all the moments secondaryQuadrature(mStar); return; } else { // Root not found. Minimize target function in [0, sigma_] sigma_ = minimizeTargetFunction(0, sigma_, m, mStar); if (mag(sigma_) < sigmaTol_) { m.invert(); sigma_ = 0.0; nullSigma_ = true; secondaryQuadrature(m); return; } targetFunction(sigma_, m, mStar); secondaryQuadrature(mStar); return; } } else { if (fNew*fMid < 0 && sigma_ < sigmaMid) { sigmaLow = sigma_; fLow = fNew; sigmaHigh = sigmaMid; fHigh = fMid; } else if (fNew*fMid < 0 && sigma_ > sigmaMid) { sigmaLow = sigmaMid; fLow = fMid; sigmaHigh = sigma_; fHigh = fNew; } else if (fNew*fLow < 0) { sigmaHigh = sigma_; fHigh = fNew; } else if (fNew*fHigh < 0) { sigmaLow = sigma_; fLow = fNew; } } } FatalErrorIn ( "Foam::extendedMomentInversion::invert\n" "(\n" " const univariateMomentSet& moments\n" ")" ) << "Number of iterations exceeded." << abort(FatalError); } }
//------------------------------------------------------------------------------ // getTriDiagonal //------------------------------------------------------------------------------ bool Matrix::getTriDiagonal(Matrix* const pA) const { //------------------------------------------------------- // initial compatibility and error checks //------------------------------------------------------- //if (!isSymmetric()) return 0; const int N = getRows(); const auto pAI = new Matrix(*this); for (int k=0; k<N-2; k++) { double gama = (*pAI)(k+1,k); double alfa = (gama * gama); for (int j=k+2; j<N; j++) { double zeta = (*pAI)(j,k); alfa += (zeta * zeta); } alfa = -sign(gama) * std::sqrt(alfa); double beta = std::sqrt(0.5 * alfa * (alfa - gama)); //---------------------------------------------------- // construct column vector X //---------------------------------------------------- const auto pX = new CVector(N); for (int p=0; p<k+1; p++) { (*pX)[p] = 0.0; } (*pX)[k+1] = (gama - alfa) / beta / 2.0; for (int q=k+2; q<N; q++) { (*pX)[q] = (*pAI)(q,k) / beta / 2.0; } //---------------------------------------------------- // construct row vector Y = X' //---------------------------------------------------- RVector* pY = pX->getTranspose(); //---------------------------------------------------- // M = 2*X*Y = 2*X*X' //---------------------------------------------------- Matrix* pM = outerProduct(*pX, *pY); pM->multiply(2.0); //---------------------------------------------------- // H = I - M = I - 2*X*X' //---------------------------------------------------- const auto pH = new Matrix(N,N); pH->makeIdent(); pH->subtract(*pM); //---------------------------------------------------- // A = H*A*H //---------------------------------------------------- Matrix* pAI0 = base::multiply(*pH, *pAI); Matrix* pAI1 = base::multiply(*pAI0, *pH); *pAI = *pAI1; //---------------------------------------------------- // unref intermediate loop pointers //---------------------------------------------------- pX->unref(); pY->unref(); pM->unref(); pH->unref(); pAI0->unref(); pAI1->unref(); } //------------------------------------------------------- // A = H(N-3)*...*H1*H0* A *H0*H1*...*H(N-3) //------------------------------------------------------- *pA = *pAI; //---------------------------------------------------- // unref pointers //---------------------------------------------------- pAI->unref(); return true; }
void CApp::SolveCollisions(int numIterations, double& dt) { for (int j=0; j<numIterations; j++) { // fill collision container by checking every speculative collisions EntityCol::EntityColList.clear(); for(std::vector<Entity*>::iterator itEntity = Entity::EntityList.begin(); itEntity != Entity::EntityList.end(); ++itEntity) { if((*itEntity) == NULL) continue; (*itEntity)->PosValidOnEntities(); } if(EntityCol::EntityColList.empty()) { return; } for (std::map<std::pair<int, int>, EntityCol>::iterator itEntityCol = EntityCol::EntityColList.begin(); itEntityCol != EntityCol::EntityColList.end(); ++itEntityCol) { EntityCol& contact = itEntityCol->second; contact.updateContactSize(); if(contact._rectangle.isVoid()) { continue; } Entity& EntityA = *(contact.EntityA); Entity& EntityB = *(contact.EntityB); // SAW kills SHEEP if(EntityB.getType() == SHEEP && EntityA.getType() == SAW) { EntityB.b->kill(); continue; } if (EntityA.getType() == SHEEP && EntityB.getType() == SAW) { EntityA.b->kill(); continue; } // ACTIVE SHEEP TOUCHES INACTIVE SHEEP if(EntityB.getType() == SHEEP && EntityA.getType() == SHEEP) { if(EntityA.b->getAttribute(PLAYER_CONTROLLED).get_int() == 0) { std::cout << "activating a sheep" << std::endl; EntityA.b->setAttribute(PLAYER_CONTROLLED, 1); Sheeps.push_back(&EntityA); } } // SHEEP triggers SWITCH if(EntityB.getType() == SHEEP && EntityA.getType() == SWITCH) { EntityA.b->OnTriggeredAction(TRIGGER_ON_COLLISION); continue; } if(contact._rectangle.getWidth() >= contact._rectangle.getHeight()) { // vertical collision if(EntityA.getType() == EntityB.getType() ) { PointDouble repulsA = PointDouble(0, (sign(EntityA.getNextPosition().getY() - EntityB.getNextPosition().getY()) * contact._rectangle.getHeight())/2); PointDouble repulsB = PointDouble(0, -(sign(EntityA.getNextPosition().getY() - EntityB.getNextPosition().getY()) * contact._rectangle.getHeight())/2); EntityA.OnMove(repulsA, dt); EntityB.OnMove(repulsB, dt); }else if (EntityA.getType() > EntityB.getType() ) { // A heavier than B : A does not move PointDouble repulsB = PointDouble(0, -(sign(EntityA.getNextPosition().getY() - EntityB.getNextPosition().getY()) * contact._rectangle.getHeight())); EntityB.OnMove(repulsB, dt); }else if (EntityB.getType() > EntityA.getType() ) { // B heavier than A : B does not move PointDouble repulsA = PointDouble(0, -(sign(EntityB.getNextPosition().getY() - EntityA.getNextPosition().getY()) * contact._rectangle.getHeight())); EntityA.OnMove(repulsA, dt); } } else { // latteral collision if(EntityA.getType() == EntityB.getType() ) { PointDouble repulsA = PointDouble((sign(EntityA.getNextPosition().getX() - EntityB.getNextPosition().getX()) * contact._rectangle.getWidth())/2, 0); PointDouble repulsB = PointDouble(-(sign(EntityA.getNextPosition().getX() - EntityB.getNextPosition().getX()) * contact._rectangle.getWidth())/2, 0); EntityA.OnMove( repulsA, dt); EntityB.OnMove( repulsB, dt); }else if (EntityA.getType() > EntityB.getType() ) { // A heavier than B : A does not move PointDouble repulsB = PointDouble(-(sign(EntityA.getNextPosition().getX() - EntityB.getNextPosition().getX()) * contact._rectangle.getWidth()), 0); EntityB.OnMove( repulsB, dt); }else if (EntityB.getType() > EntityA.getType() ) { // B heavier than A : B does not move PointDouble repulsA = PointDouble((sign(EntityA.getNextPosition().getX() - EntityB.getNextPosition().getX()) * contact._rectangle.getWidth()), 0); EntityA.OnMove( repulsA, dt); } } } } }
brent(Fn fn, Real ax, Real bx, Real cx, Real tol = 3e-8) { const int ITMAX = 100; const Real CGOLD = 0.3819660; const Real ZEPS = std::numeric_limits<Real>::epsilon() * 1e-3; Real d = 0; Real e = 0; // distance moved on the step before last Real a = std::min(ax, cx); Real b = std::max(ax, cx); Real x, w, v, fx, fw, fv; x = w = v = bx; fx = fw = fv = fn(bx); for (int it = 0; it < ITMAX; ++it) { Real xm = 0.5 * (a + b); Real tol_half = tol * abs(x) + ZEPS; Real tol = 2 * tol_half; // test for done if (abs(x - xm) <= tol - 0.5 * (b - a)) { fmin = fx; xmin = x; return; } // use x, w, v to make parabolic fix // check if need to re-eval if (abs(e) > tol_half) { Real r = (x - w) * (fx - fv); Real q = (x - v) * (fx - fw); Real p = (x - v) * q - (x - w) * r; q = 2 * (q - r); if (q > 0) p = -p; q = abs(q); Real eprev = e; e = d; // the acceptability of the parabolic fit if ( // imply a movement from the best current value x that is // less than half the movement of the step before last abs(p) >= abs(0.5 * q * eprev) || // out of bracket p <= q * (a - x) || p >= q * (b - x) ) { // take the golden section step into the larget of the two // segments d = CGOLD * (e = (x >= xm ? a - x : b - x)); } else { d = p / q; Real u = x + d; // step too large, make it small if (u - a < tol || b - u < tol) { d = sign(tol_half, xm - x); } } } else { d = CGOLD * (e = (x >= xm ? a - x : b - x)); // TODO: refactor } // next pos, check if d too small Real u = (abs(d) >= tol_half ? x + d : x + sign(tol_half, d)); // this is the one function evaluation pre iteration Real fu = fn(u); if (fu <= fx) { (u >= x ? a : b) = x; shift << v << w << x << u; shift << fv << fw << fx << fu; } else { (u < x ? a : b) = u; if (fu <= fw || w == x) { shift << v << w << u; shift << fv << fw << fu; } else if (fu <= fv || v == x || v == w) { v = u; fv = fu; } } } CHECK(std::runtime_error, false && "Too many iterations in brent."); }
bool cyclicApproach(Cyclic &x, const float target, const float amount) { const float distance = cyclicDistance(x, target); x += sign(distance) * std::min(amount, std::abs(distance)); return amount < distance; }
if (Mode == MODE_INTRA) { /* Intra */ qcoeff[0] = mmax(1,mmin(254,coeff[0] >> 3)); /* was: /8 */ if(QP == 8) { for(i = 1; i < 64; i++) { level = (abs(coeff[i])) >> 4; qcoeff[i] = mmin(127, mmax(-127,sign(coeff[i]) * level)); if(qcoeff[i]) CBP = CBP_Mask; } } else { /* QP != 8 */ for(i = 1; i < 64; i++) { level = (abs(coeff[i])) / (2*QP); qcoeff[i] = mmin(127,mmax(-127,sign(coeff[i]) * level)); if(qcoeff[i]) CBP = CBP_Mask; } } /* End QP == 8 */ } else { /* non Intra */ if(QP == 8) { for(i = 0; i < 64; i++) { tempje = abs(coeff[i]) - 4; if(tempje < 0) { *(qcoeff++) = 0; } else { level = tempje >> 4; /* Note 2*QP == 2*8 = 2^4 == ">>4" */ if((*(qcoeff++) = mmin(127,mmax(-127,(sign(coeff[i]) > 0) ?
void IterateVoxel(inout vec3 voxel, Ray ray, inout vec4 colorAccum) { float maxX = 0.0; float maxY = 0.0; float maxZ = 0.0; if(ray.Direction.x != 0.0) { maxX = max((voxel.x - ray.Origin.x) / ray.Direction.x, (voxel.x + 1.0 - ray.Origin.x) / ray.Direction.x); } if(ray.Direction.y != 0.0) { maxY = max((voxel.y - ray.Origin.y) / ray.Direction.y, (voxel.y + 1.0 - ray.Origin.y) / ray.Direction.y); } if(ray.Direction.z != 0.0) { maxZ = max((voxel.z - ray.Origin.z) / ray.Direction.z, (voxel.z + 1.0 - ray.Origin.z) / ray.Direction.z); } vec2 hitPoint; float texture; if(maxX <= min(maxY, maxZ)) { voxel.x += sign(ray.Direction.x); int block = GetVoxel(voxel); if(block != 0) { texture = (ray.Direction.x > 0) ? textureFaces[block*6 + 0] : textureFaces[block*6 + 1]; hitPoint = fract(ray.Origin + ray.Direction * maxX).zy; colorAccum = texture2DArray(textures, vec3(1.0 - abs(hitPoint), texture)); colorAccum.xyz *= 0.9; } } if(maxY <= min(maxX, maxZ)) { voxel.y += sign(ray.Direction.y); int block = GetVoxel(voxel); if(block != 0) { texture = (ray.Direction.y > 0) ? textureFaces[block*6 + 3] : textureFaces[block*6 + 2]; hitPoint = fract(ray.Origin + ray.Direction * maxY).xz; colorAccum = texture2DArray(textures, vec3(1.0 - abs(hitPoint), texture)); colorAccum.xyz *= 1.0; } } if(maxZ <= min(maxX, maxY)) { voxel.z += sign(ray.Direction.z); int block = GetVoxel(voxel); if(block != 0) { texture = (ray.Direction.z > 0) ? textureFaces[block*6 + 4] : textureFaces[block*6 + 5]; hitPoint = fract(ray.Origin + ray.Direction * maxZ).xy; colorAccum = texture2DArray(textures, vec3(1.0 - abs(hitPoint), texture)); colorAccum.xyz *= 0.8; } } }
EXPORT int modify_volume_of_fluid_values( Array3<double> level_set, // level-set field Array3<double> volume_of_fluid, // volume of fluid field, uncorrected // so with possible vapour cells and // under/overfilled cells Array3<double> volume_of_fluid_correction, // correction to the volume of fluid field // to make it valid Array3<double> invalid_vof_cells, // indication field to show what is wrong // indicator field showing cells that are either // within bounds =0 // underfilled =-1 // overfilled =+1 // vapour cells = 5 double mesh_width_x1, // grid spacing in x1 direction (uniform) double mesh_width_x2, // grid spacing in x2 direction (uniform) double mesh_width_x3, // grid spacing in x3 direction (uniform) int number_primary_cells_i, // number of primary (pressure) cells in x1 direction int number_primary_cells_j, // number of primary (pressure) cells in x2 direction int number_primary_cells_k, // number of primary (pressure) cells in x3 direction double volume_of_fluid_tolerance // tolerance for volume of fluid value ) { double current_level_set_value; // current level-set field value in the control volume double current_volume_of_fluid_value; // current volume of fluid field value in // the control volume double correct_vof_value; // the value a cell with an invalid volume of // fluid is set to: 1, 0 dependent on the sign // of the level-set function int number_cells_vof_out_of_bounds=0; // number of control volumes where the volume of fluid // function is OUTSIDE the interval [0,1] int number_cells_numerical_vapor=0; // number of control volumes where the volume of fluid // function is INSIDE the interval [0,1] // while the cell has 6 neighbours with the same sign: // the cell is NOT an interface cell int number_cells_invalid_volume_of_fluid; // sum of number of vapour cells and number of cells int i_index, j_index, k_index; // local variables for loop indexing int adapt_vof_value; // =1 adapt the volume of fluid value in this // cell, because it has an invalid value bool detailed_output=0; // =1, provide detailed output // =0, provide non output /* we start with the assumption that all volume of fluid values are valid */ set_constant_matrix2(number_primary_cells_i+2, number_primary_cells_j+2, number_primary_cells_k+2, invalid_vof_cells, 0.0); set_constant_matrix2(number_primary_cells_i+2, number_primary_cells_j+2, number_primary_cells_k+2, volume_of_fluid_correction, 0.0); /* all nonvirtual cells are modified */ /* the required modification is stored in volume_of_fluid_correction */ for(i_index=1;i_index<number_primary_cells_i+1;i_index++) { for(j_index=1;j_index<number_primary_cells_j+1;j_index++) { for(k_index=1;k_index<number_primary_cells_k+1;k_index++) { current_level_set_value=level_set[i_index][j_index][k_index]; current_volume_of_fluid_value=volume_of_fluid[i_index][j_index][k_index]; /* assume the cell has a valid vof value */ adapt_vof_value=0; /* check if the cell is a vapour cell, when this update is applied */ if( /* is the cell surrounded by neighbours with the same sign ?*/ ( (level_set[i_index-1][j_index ][k_index ]*current_level_set_value>0)&& (level_set[i_index+1][j_index ][k_index ]*current_level_set_value>0)&& (level_set[i_index ][j_index-1][k_index ]*current_level_set_value>0)&& (level_set[i_index ][j_index+1][k_index ]*current_level_set_value>0)&& (level_set[i_index ][j_index ][k_index+1]*current_level_set_value>0)&& (level_set[i_index ][j_index ][k_index-1]*current_level_set_value>0) ) && /* is the volume of fluid at the same time in the OPEN interval <0,1>? */ (current_volume_of_fluid_value<(1.0-volume_of_fluid_tolerance) && current_volume_of_fluid_value>(0.0+volume_of_fluid_tolerance)) ) { /* then the cell is a vapour cell, and the volume_of_fluid_correction is not correct yet */ /* additional iterations are required */ /* update the number of vapour cells */ number_cells_numerical_vapor++; adapt_vof_value=1; invalid_vof_cells[i_index][j_index][k_index]=5.0; } if( (current_volume_of_fluid_value> 1.0+volume_of_fluid_tolerance)|| (current_volume_of_fluid_value < 0.0-volume_of_fluid_tolerance)) { /* the volume of fluid value is out of bounds */ /* additional iterations are required */ /* update the number of out of bounds cells */ number_cells_vof_out_of_bounds++; adapt_vof_value=1; if(current_volume_of_fluid_value> 1.0+volume_of_fluid_tolerance) { invalid_vof_cells[i_index][j_index][k_index]=1.0; } else { invalid_vof_cells[i_index][j_index][k_index]=-1.0; } } if(adapt_vof_value) { /* cell is either a vapour cell or an over/under filled cell */ /* the correct value shoul be either 1/0 depending on the sign */ /* of the level-set field */ correct_vof_value=0.5+sign(0.5,current_level_set_value); if(current_level_set_value==0.0) { /* this is not really very likely to happen but still */ correct_vof_value=0.5; } volume_of_fluid_correction[i_index][j_index][k_index]= volume_of_fluid[i_index][j_index][k_index]-correct_vof_value; volume_of_fluid[i_index][j_index][k_index]=correct_vof_value; } } } } number_cells_invalid_volume_of_fluid=number_cells_numerical_vapor+number_cells_vof_out_of_bounds; /* for debugging mode show detailed output */ if(detailed_output) { dump_redistribution_for_debugging(level_set, volume_of_fluid, level_set, invalid_vof_cells, volume_of_fluid_correction, number_primary_cells_i, number_primary_cells_j, number_primary_cells_k, mesh_width_x1, mesh_width_x2, mesh_width_x3, 1 ); for(i_index=1;i_index<number_primary_cells_i+1;i_index++) { for(j_index=1;j_index<number_primary_cells_j+1;j_index++) { for(k_index=1;k_index<number_primary_cells_k+1;k_index++) { if( fabs(invalid_vof_cells[i_index][j_index][k_index]-5.0)<0.0001) { std::cerr.setf(std::ios::scientific); std::cerr<<" this is vapour cell "<< i_index <<" "<< j_index <<" "<<k_index <<" "<<level_set[i_index][j_index][k_index]<<" "<<volume_of_fluid[i_index][j_index][k_index] <<"\n"; std::cerr<<"with neighbours: level-set vof \n"; std::cerr<<" [i_index+1][j_index ][k_index ] "<<level_set[i_index+1][j_index ][k_index ]<< " "<<volume_of_fluid[i_index+1][j_index ][k_index ]<<"\n"; std::cerr<<" [i_index-1][j_index ][k_index ] "<<level_set[i_index-1][j_index ][k_index ]<< " "<<volume_of_fluid[i_index-1][j_index ][k_index ]<<"\n"; std::cerr<<" [i_index ][j_index+1][k_index ] "<<level_set[i_index ][j_index+1][k_index ]<< " "<<volume_of_fluid[i_index ][j_index+1][k_index ]<<"\n"; std::cerr<<" [i_index ][j_index-1][k_index ] "<<level_set[i_index ][j_index-1][k_index ]<< " "<<volume_of_fluid[i_index ][j_index+1][k_index ]<<"\n"; std::cerr<<" [i_index ][j_index ][k_index+1] "<<level_set[i_index ][j_index ][k_index+1]<< " "<<volume_of_fluid[i_index ][j_index ][k_index+1]<<"\n"; std::cerr<<" [i_index ][j_index ][k_index-1] "<<level_set[i_index ][j_index ][k_index-1]<< " "<<volume_of_fluid[i_index ][j_index ][k_index+1]<<"\n"; } } } } } return number_cells_invalid_volume_of_fluid; }
void EBFineToCoarRedist:: define(const DisjointBoxLayout& a_dblFine, const DisjointBoxLayout& a_dblCoar, const EBISLayout& a_ebislFine, const EBISLayout& a_ebislCoar, const Box& a_domainCoar, const int& a_nref, const int& a_nvar, int a_redistRad, const EBIndexSpace* const a_ebisPtr) { CH_TIME("EBFineToCoarRedist::stardard_define"); m_isDefined = true; m_nComp = a_nvar; m_refRat = a_nref; m_domainCoar = a_domainCoar; m_gridsFine = a_dblFine; m_gridsCoar = a_dblCoar; m_ebislFine = a_ebislFine; m_ebislCoar = a_ebislCoar; m_redistRad = a_redistRad; //created the coarsened fine layout m_gridsRefCoar = DisjointBoxLayout(); refine(m_gridsRefCoar, m_gridsCoar, m_refRat); CH_assert(a_ebisPtr->isDefined()); int nghost = 3*m_redistRad; Box domainFine = refine(m_domainCoar, m_refRat); a_ebisPtr->fillEBISLayout(m_ebislRefCoar, m_gridsRefCoar, domainFine, nghost); m_ebislRefCoar.setMaxCoarseningRatio(m_refRat,a_ebisPtr); //define the intvectsets over which the objects live m_setsFine.define(m_gridsFine); m_setsRefCoar.define(m_gridsCoar); //make sets //global set consists of redistrad on the fine side of the coarse-fine //interface //the fine set is that within one fine box. //the refcoar set is that set within one refined coarse box. { CH_TIME("make_fine_sets"); for (DataIterator dit = m_gridsFine.dataIterator(); dit.ok(); ++dit) { const Box& fineBox = m_gridsFine.get(dit()); IntVectSet& fineSet = m_setsFine[dit()]; //add entire fine box fineSet = IntVectSet(fineBox); IntVectSet irregIVS = m_ebislFine[dit()].getIrregIVS(fineBox); fineSet &= irregIVS; //subtract fine box shrunk by the redistribution radius fineSet -= grow(fineBox, -m_redistRad); //subtract all other the fine boxes shifted in all //directions for (LayoutIterator lit = m_gridsFine.layoutIterator(); lit.ok(); ++lit) { const Box& otherBox = m_gridsFine.get(lit()); if (otherBox != fineBox) { for (int idir = 0; idir < SpaceDim; idir++) { for (SideIterator sit; sit.ok(); ++sit) { Box shiftBox = otherBox; shiftBox.shift(idir, sign(sit())*m_redistRad); fineSet -= shiftBox; } } } } } } { CH_TIME("make_coar_sets"); for (DataIterator dit = m_gridsCoar.dataIterator(); dit.ok(); ++dit) { Box grownBox = grow(m_gridsRefCoar.get(dit()), m_redistRad); grownBox &= domainFine; //find the complement of what we really want IntVectSet ivsComplement(grownBox); for (LayoutIterator litFine = m_gridsFine.layoutIterator(); litFine.ok(); ++litFine) { const Box& fineBox = m_gridsFine.get(litFine()); IntVectSet fineSet(fineBox); //add entire fine box fineSet = IntVectSet(fineBox); Box interiorBox = grow(fineBox, -m_redistRad); //subtract fine box shrunk by the redistribution radius fineSet -= interiorBox; //subtract all other the fine boxes shifted in all //directions for (LayoutIterator lit = m_gridsFine.layoutIterator(); lit.ok(); ++lit) { const Box& otherBox = m_gridsFine.get(lit()); if (otherBox != fineBox) { for (int idir = 0; idir < SpaceDim; idir++) { for (SideIterator sit; sit.ok(); ++sit) { Box shiftBox = otherBox; shiftBox.shift(idir, sign(sit())*m_redistRad); fineSet -= shiftBox; } } } } //subtract the fine set from the complement ivsComplement -= fineSet; } //now the set we want is the grownbox - complement IntVectSet& refCoarSet = m_setsRefCoar[dit()]; refCoarSet = IntVectSet(grownBox); refCoarSet -= ivsComplement; IntVectSet irregIVS = m_ebislRefCoar[dit()].getIrregIVS(grownBox); refCoarSet &= irregIVS; } } defineDataHolders(); setToZero(); }
void _householder(double **Q, double **R, double **B, int n, int m) { int i, j, k; Vector Beta = VectorNew(m); Vector W = VectorNew(n); Matrix V = MatrixNew(n, n); double *beta = Beta->e; // Q = I for (i = 0; i < n; i++) { for (j = 0; j < n; j++) { if (i == j) { Q[i][j] = 1.0; } else { Q[i][j] = 0.0; } } } // R = B for (i = 0; i < n; i++) { double *Ri = R[i]; double *Bi = B[i]; for (k = 0; k < m; k++) { Ri[k] = Bi[k]; } } // find the Householder vectors for each column of R for (k = 0; k < m; k++) { double xi; double norm_x = 0.0; double *w = W->e; double *v = V->e[k]; for (i = k; i < n; i++) { xi = R[i][k]; norm_x += xi * xi; v[i] = xi; } norm_x = sqrt(norm_x); double x1 = v[k]; double sign_x1 = sign(x1); double gamma = -1.0 * sign_x1 * norm_x; double v_dot_v = 2.0 * norm_x * (norm_x + sign_x1 * x1); if (v_dot_v < DBL_EPSILON) { for (i = k; i < m; i++) { R[i][i] = 0.0; } VectorDelete(Beta); VectorDelete(W); MatrixDelete(V); return; } v[k] -= gamma; beta[k] = -2.0 / v_dot_v; // w(k:m) = R(k:n, k:m)^T * v(k:n) for (i = k; i < m; i++) { double s = 0.0; for (j = k; j < n; j++) { // FIX: make this row-wise s += R[j][i] * v[j]; } w[i] = s; } // R(k:n, k:m) += beta * v(k:n) * w(k:m)^T //a[k : n, k : m] = a[k : n, k : m] + beta * (v * wT) for (i = k; i < n; i++) { for (j = k; j < m; j++) { R[i][j] += beta[k] * v[i] * w[j]; } } } for (k = m-1; k >= 0; k--) { double *v = V->e[k]; double *u = W->e; //uT = v.transpose() * Q[k : n, k : n] for (i = k; i < n; i++) { double s = 0.0; for (j = k; j < n; j++) { s += Q[j][i] * v[j]; } u[i] = s; } //Q[k : n, k : n] = Q[k : n, k : n] + Beta[k] * (v * uT) for (i = k; i < n; i++) { for (j = k; j < n; j++) { Q[i][j] += beta[k] * v[i] * u[j]; } } } VectorDelete(Beta); VectorDelete(W); MatrixDelete(V); }
static void swilk(double *x, int n, double *w, double *pw, int *ifault) { int nn2 = n / 2; double a[nn2 + 1]; /* 1-based */ /* ALGORITHM AS R94 APPL. STATIST. (1995) vol.44, no.4, 547-551. Calculates the Shapiro-Wilk W test and its significance level */ double small = 1e-19; /* polynomial coefficients */ double g[2] = { -2.273,.459 }; double c1[6] = { 0.,.221157,-.147981,-2.07119, 4.434685, -2.706056 }; double c2[6] = { 0.,.042981,-.293762,-1.752461,5.682633, -3.582633 }; double c3[4] = { .544,-.39978,.025054,-6.714e-4 }; double c4[4] = { 1.3822,-.77857,.062767,-.0020322 }; double c5[4] = { -1.5861,-.31082,-.083751,.0038915 }; double c6[3] = { -.4803,-.082676,.0030302 }; /* Local variables */ int i, j, i1; double ssassx, summ2, ssumm2, gamma, range; double a1, a2, an, m, s, sa, xi, sx, xx, y, w1; double fac, asa, an25, ssa, sax, rsn, ssx, xsx; *pw = 1.; if (n < 3) { *ifault = 1; return;} an = (double) n; if (n == 3) { a[1] = 0.70710678;/* = sqrt(1/2) */ } else { an25 = an + .25; summ2 = 0.; for (i = 1; i <= nn2; i++) { a[i] = qnorm((i - 0.375) / an25, 0., 1., 1, 0); double r__1 = a[i]; summ2 += r__1 * r__1; } summ2 *= 2.; ssumm2 = sqrt(summ2); rsn = 1. / sqrt(an); a1 = poly(c1, 6, rsn) - a[1] / ssumm2; /* Normalize a[] */ if (n > 5) { i1 = 3; a2 = -a[2] / ssumm2 + poly(c2, 6, rsn); fac = sqrt((summ2 - 2. * (a[1] * a[1]) - 2. * (a[2] * a[2])) / (1. - 2. * (a1 * a1) - 2. * (a2 * a2))); a[2] = a2; } else { i1 = 2; fac = sqrt((summ2 - 2. * (a[1] * a[1])) / ( 1. - 2. * (a1 * a1))); } a[1] = a1; for (i = i1; i <= nn2; i++) a[i] /= - fac; } /* Check for zero range */ range = x[n - 1] - x[0]; if (range < small) {*ifault = 6; return;} /* Check for correct sort order on range - scaled X */ /* *ifault = 7; <-- a no-op, since it is changed below, in ANY CASE! */ *ifault = 0; xx = x[0] / range; sx = xx; sa = -a[1]; for (i = 1, j = n - 1; i < n; j--) { xi = x[i] / range; if (xx - xi > small) { /* Fortran had: print *, "ANYTHING" * but do NOT; it *does* happen with sorted x (on Intel GNU/linux 32bit): * shapiro.test(c(-1.7, -1,-1,-.73,-.61,-.5,-.24, .45,.62,.81,1)) */ *ifault = 7; } sx += xi; i++; if (i != j) sa += sign(i - j) * a[min(i, j)]; xx = xi; } if (n > 5000) *ifault = 2; /* Calculate W statistic as squared correlation between data and coefficients */ sa /= n; sx /= n; ssa = ssx = sax = 0.; for (i = 0, j = n - 1; i < n; i++, j--) { if (i != j) asa = sign(i - j) * a[1 + min(i, j)] - sa; else asa = -sa; xsx = x[i] / range - sx; ssa += asa * asa; ssx += xsx * xsx; sax += asa * xsx; } /* W1 equals (1-W) calculated to avoid excessive rounding error for W very near 1 (a potential problem in very large samples) */ ssassx = sqrt(ssa * ssx); w1 = (ssassx - sax) * (ssassx + sax) / (ssa * ssx); *w = 1. - w1; /* Calculate significance level for W */ if (n == 3) {/* exact P value : */ double pi6 = 1.90985931710274, /* = 6/pi */ stqr = 1.04719755119660; /* = asin(sqrt(3/4)) */ *pw = pi6 * (asin(sqrt(*w)) - stqr); if(*pw < 0.) *pw = 0.; return; } y = log(w1); xx = log(an); if (n <= 11) { gamma = poly(g, 2, an); if (y >= gamma) { *pw = 1e-99;/* an "obvious" value, was 'small' which was 1e-19f */ return; } y = -log(gamma - y); m = poly(c3, 4, an); s = exp(poly(c4, 4, an)); } else {/* n >= 12 */ m = poly(c5, 4, xx); s = exp(poly(c6, 3, xx)); } /*DBG printf("c(w1=%g, w=%g, y=%g, m=%g, s=%g)\n",w1,*w,y,m,s); */ *pw = pnorm(y, m, s, 0/* upper tail */, 0); return; } /* swilk */
/* optimize the path p, replacing sequences of Bezier segments by a single segment when possible. Return 0 on success, 1 with errno set on failure. */ static int opticurve(privpath_t *pp, double opttolerance) { int m = pp->curve.n; int *pt = NULL; /* pt[m+1] */ double *pen = NULL; /* pen[m+1] */ int *len = NULL; /* len[m+1] */ opti_t *opt = NULL; /* opt[m+1] */ int om; int i,j,r; opti_t o; dpoint_t p0; int i1; double area; double alpha; double *s = NULL; double *t = NULL; int *convc = NULL; /* conv[m]: pre-computed convexities */ double *areac = NULL; /* cumarea[m+1]: cache for fast area computation */ SAFE_MALLOC(pt, m+1, int); SAFE_MALLOC(pen, m+1, double); SAFE_MALLOC(len, m+1, int); SAFE_MALLOC(opt, m+1, opti_t); SAFE_MALLOC(convc, m, int); SAFE_MALLOC(areac, m+1, double); /* pre-calculate convexity: +1 = right turn, -1 = left turn, 0 = corner */ for (i=0; i<m; i++) { if (pp->curve.tag[i] == POTRACE_CURVETO) { convc[i] = sign(dpara(pp->curve.vertex[mod(i-1,m)], pp->curve.vertex[i], pp->curve.vertex[mod(i+1,m)])); } else { convc[i] = 0; } } /* pre-calculate areas */ area = 0.0; areac[0] = 0.0; p0 = pp->curve.vertex[0]; for (i=0; i<m; i++) { i1 = mod(i+1, m); if (pp->curve.tag[i1] == POTRACE_CURVETO) { alpha = pp->curve.alpha[i1]; area += 0.3*alpha*(4-alpha)*dpara(pp->curve.c[i][2], pp->curve.vertex[i1], pp->curve.c[i1][2])/2; area += dpara(p0, pp->curve.c[i][2], pp->curve.c[i1][2])/2; } areac[i+1] = area; } pt[0] = -1; pen[0] = 0; len[0] = 0; /* Fixme: we always start from a fixed point -- should find the best curve cyclically ### */ for (j=1; j<=m; j++) { /* calculate best path from 0 to j */ pt[j] = j-1; pen[j] = pen[j-1]; len[j] = len[j-1]+1; for (i=j-2; i>=0; i--) { r = opti_penalty(pp, i, mod(j,m), &o, opttolerance, convc, areac); if (r) { break; } if (len[j] > len[i]+1 || (len[j] == len[i]+1 && pen[j] > pen[i] + o.pen)) { pt[j] = i; pen[j] = pen[i] + o.pen; len[j] = len[i] + 1; opt[j] = o; } } } om = len[m]; r = privcurve_init(&pp->ocurve, om); if (r) { goto malloc_error; } SAFE_MALLOC(s, om, double); SAFE_MALLOC(t, om, double); j = m; for (i=om-1; i>=0; i--) { if (pt[j]==j-1) { pp->ocurve.tag[i] = pp->curve.tag[mod(j,m)]; pp->ocurve.c[i][0] = pp->curve.c[mod(j,m)][0]; pp->ocurve.c[i][1] = pp->curve.c[mod(j,m)][1]; pp->ocurve.c[i][2] = pp->curve.c[mod(j,m)][2]; pp->ocurve.vertex[i] = pp->curve.vertex[mod(j,m)]; pp->ocurve.alpha[i] = pp->curve.alpha[mod(j,m)]; pp->ocurve.alpha0[i] = pp->curve.alpha0[mod(j,m)]; pp->ocurve.beta[i] = pp->curve.beta[mod(j,m)]; s[i] = t[i] = 1.0; } else { pp->ocurve.tag[i] = POTRACE_CURVETO; pp->ocurve.c[i][0] = opt[j].c[0]; pp->ocurve.c[i][1] = opt[j].c[1]; pp->ocurve.c[i][2] = pp->curve.c[mod(j,m)][2]; pp->ocurve.vertex[i] = interval(opt[j].s, pp->curve.c[mod(j,m)][2], pp->curve.vertex[mod(j,m)]); pp->ocurve.alpha[i] = opt[j].alpha; pp->ocurve.alpha0[i] = opt[j].alpha; s[i] = opt[j].s; t[i] = opt[j].t; } j = pt[j]; } /* calculate beta parameters */ for (i=0; i<om; i++) { i1 = mod(i+1,om); pp->ocurve.beta[i] = s[i] / (s[i] + t[i1]); } pp->ocurve.alphacurve = 1; free(pt); free(pen); free(len); free(opt); free(s); free(t); free(convc); free(areac); return 0; malloc_error: free(pt); free(pen); free(len); free(opt); free(s); free(t); free(convc); free(areac); return 1; }
void BkStressLimSurface2D::setTrialPlasticStrains(double lamda, const Vector &f, const Vector &g) { // double epx = isotropicRatio*lamda*g(0); // double epy = isotropicRatio*lamda*g(1); // set wrt absolute for easier calibration double epx = lamda*g(0); double epy = lamda*g(1); // opserr << "epx = " << epx << ", epy = " << epy << endln; // opserr << "gx = " << g(0) << ", gy = " << g(1) << endln; // opserr << "\a"; if(epx > 0) defPosX = true; else defPosX = false; if(epy > 0) defPosY = true; else defPosY = false; isoMatXPos->setTrialIncrValue(epx); isoMatXNeg->setTrialIncrValue(-1*epx); isoMatYPos->setTrialIncrValue(epy); isoMatYNeg->setTrialIncrValue(-1*epy); //!! when should sumIsoEp be reset? - using same as plastic hardening double x0 = translate_hist(0); double y0 = translate_hist(1); double fx = f(0); double fy = f(1); limSurface->hModel->toOriginalCoord(x0, y0); double drift = limSurface->getDrift(x0, y0); if(direction_orig > 1) direction = fabs(y0); if(fabs(y0) >= 0.80) direction = 1.0; // constP int resType = resAlgo; double dR = fabs(drift); // in-case outside pinching starts late switch (resType) { case 1: { if(drift >= 0.0) { if(sign(g(0)) != sign(translate_hist(0))) dR = 1.5 + drift; // approx value = 2 metal case else dR = 0.0; } else // no pinching { //old limSurface->hModel->toOriginalCoord(fx, fy); //old dR = limSurface->interpolate(x0, y0, fx, fy); // y0 range -1 to +1 if(sign(g(0)) != sign(translate_hist(0))) { // dR = 2.0 + drift; // drift < 0 dR = fabs(limSurface->getDrift(0.0, y0))*2 - fabs(drift); // not required // else // dR = fabs(drift); // opserr << "!!drift 0, y0 = " << limSurface->getDrift(0.0, y0) // << ", drift = " << drift << endln; } } break; } //case 1 - Metals case 2: { if(drift >= 0.0) dR = 0.0; else // pinching starts early { // limSurface->hModel->toOriginalCoord(fx, fy); // dR = limSurface->interpolate(x0, y0, fx, fy); if(sign(g(0)) != sign(translate_hist(0))) dR = fabs(limSurface->getDrift(0.0, y0))*2 - fabs(drift); } break; }//case 2 - Pinching, Kp = Kp0 -> 0 case 3: { if(drift >= 0.0) dR = 0.0; break; }//case 3 - Pinching, Kp = 0 -> Kp0 -> 0 case 4: { if(drift >= 0.0) { if(sign(g(0)) == sign(translate_hist(0))) dR = 0.0; } /* else { if(sign(g(0)) != sign(translate_hist(0))) dR = 0.0; } */ break; } default: { opserr << "WARNING - Unknown residual algo\n"; opserr << *this; if(drift >= 0.0) dR = 0.0; } } // switch - algo double sfactor = 1.0; resHardening = false; resApproach = false; if(drift >= 0.0) { if(sign(g(0)) == sign(translate_hist(0))) { resHardening = true; if(resType > 1) sfactor = resFactor; } else { resApproach = true; if(resType > 1) sfactor = appFactor; } // opserr << "----- Drift > 0 --- ( " << sfactor << ")\n"; } // absolute values - no need to have history kinMatX->setTrialValue(dR, sfactor); kinMatY->setTrialValue(dR, sfactor); }
/* calculate best fit from i+.5 to j+.5. Assume i<j (cyclically). Return 0 and set badness and parameters (alpha, beta), if possible. Return 1 if impossible. */ static int opti_penalty(privpath_t *pp, int i, int j, opti_t *res, double opttolerance, int *convc, double *areac) { int m = pp->curve.n; int k, k1, k2, conv, i1; double area, alpha, d, d1, d2; dpoint_t p0, p1, p2, p3, pt; double A, R, A1, A2, A3, A4; double s, t; /* check convexity, corner-freeness, and maximum bend < 179 degrees */ if (i==j) { /* sanity - a full loop can never be an opticurve */ return 1; } k = i; i1 = mod(i+1, m); k1 = mod(k+1, m); conv = convc[k1]; if (conv == 0) { return 1; } d = ddist(pp->curve.vertex[i], pp->curve.vertex[i1]); for (k=k1; k!=j; k=k1) { k1 = mod(k+1, m); k2 = mod(k+2, m); if (convc[k1] != conv) { return 1; } if (sign(cprod(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2])) != conv) { return 1; } if (iprod1(pp->curve.vertex[i], pp->curve.vertex[i1], pp->curve.vertex[k1], pp->curve.vertex[k2]) < d * ddist(pp->curve.vertex[k1], pp->curve.vertex[k2]) * COS179) { return 1; } } /* the curve we're working in: */ p0 = pp->curve.c[mod(i,m)][2]; p1 = pp->curve.vertex[mod(i+1,m)]; p2 = pp->curve.vertex[mod(j,m)]; p3 = pp->curve.c[mod(j,m)][2]; /* determine its area */ area = areac[j] - areac[i]; area -= dpara(pp->curve.vertex[0], pp->curve.c[i][2], pp->curve.c[j][2])/2; if (i>=j) { area += areac[m]; } /* find intersection o of p0p1 and p2p3. Let t,s such that o = interval(t,p0,p1) = interval(s,p3,p2). Let A be the area of the triangle (p0,o,p3). */ A1 = dpara(p0, p1, p2); A2 = dpara(p0, p1, p3); A3 = dpara(p0, p2, p3); /* A4 = dpara(p1, p2, p3); */ A4 = A1+A3-A2; if (A2 == A1) { /* this should never happen */ return 1; } t = A3/(A3-A4); s = A2/(A2-A1); A = A2 * t / 2.0; if (A == 0.0) { /* this should never happen */ return 1; } R = area / A; /* relative area */ alpha = 2 - sqrt(4 - R / 0.3); /* overall alpha for p0-o-p3 curve */ res->c[0] = interval(t * alpha, p0, p1); res->c[1] = interval(s * alpha, p3, p2); res->alpha = alpha; res->t = t; res->s = s; p1 = res->c[0]; p2 = res->c[1]; /* the proposed curve is now (p0,p1,p2,p3) */ res->pen = 0; /* calculate penalty */ /* check tangency with edges */ for (k=mod(i+1,m); k!=j; k=k1) { k1 = mod(k+1,m); t = tangent(p0, p1, p2, p3, pp->curve.vertex[k], pp->curve.vertex[k1]); if (t<-.5) { return 1; } pt = bezier(t, p0, p1, p2, p3); d = ddist(pp->curve.vertex[k], pp->curve.vertex[k1]); if (d == 0.0) { /* this should never happen */ return 1; } d1 = dpara(pp->curve.vertex[k], pp->curve.vertex[k1], pt) / d; if (fabs(d1) > opttolerance) { return 1; } if (iprod(pp->curve.vertex[k], pp->curve.vertex[k1], pt) < 0 || iprod(pp->curve.vertex[k1], pp->curve.vertex[k], pt) < 0) { return 1; } res->pen += sq(d1); } /* check corners */ for (k=i; k!=j; k=k1) { k1 = mod(k+1,m); t = tangent(p0, p1, p2, p3, pp->curve.c[k][2], pp->curve.c[k1][2]); if (t<-.5) { return 1; } pt = bezier(t, p0, p1, p2, p3); d = ddist(pp->curve.c[k][2], pp->curve.c[k1][2]); if (d == 0.0) { /* this should never happen */ return 1; } d1 = dpara(pp->curve.c[k][2], pp->curve.c[k1][2], pt) / d; d2 = dpara(pp->curve.c[k][2], pp->curve.c[k1][2], pp->curve.vertex[k1]) / d; d2 *= 0.75 * pp->curve.alpha[k1]; if (d2 < 0) { d1 = -d1; d2 = -d2; } if (d1 < d2 - opttolerance) { return 1; } if (d1 < d2) { res->pen += sq(d1 - d2); } } return 0; }
static void hqr(int n, int low, int igh, double *h, double *ev, int *ierr) { int i, j, k, l = 0, m = 0, en, ll, mm, na, its, mp2, enm2; double p = 0.0, q = 0.0, r = 0.0, s, t, w, x, y, zz, norm, machep = 1.e-10; int notlas; *ierr = 0; norm = 0.0; k = 1; for (i = 1; i <= n; i++) { for (j = k; j <= n; j++) norm = norm + fabs(h[i - 1 + (j - 1) * n]); k = i; if ((i >= low) && (i <= igh)) continue; ev[(i - 1) * 2] = h[i - 1 + (i - 1) * n]; ev[1 + (i - 1) * 2] = 0.0; } en = igh; t = 0.0; l60: if (en < low) return; its = 0; na = en - 1; enm2 = na - 1; l70: for (ll = low; ll <= en; ll++) { l = en + low - ll; if (l == low) break; s = fabs(h[l - 2 + (l - 2) * n]) + fabs(h[l - 1 + (l - 1) * n]); if (s == 0.0) s = norm; if (fabs(h[l - 1 + (l - 2) * n]) <= machep * s) break; } x = h[en - 1 + (en - 1) * n]; if (l == en) goto l270; y = h[na - 1 + (na - 1) * n]; w = h[en - 1 + (na - 1) * n] * h[na - 1 + (en - 1) * n]; if (l == na) goto l280; if (its == 30) goto l1000; if ((its != 10) && (its != 20)) goto l130; t = t + x; for (i = low; i <= en; i++) h[i - 1 + (i - 1) * n] = h[i - 1 + (i - 1) * n] - x; s = fabs(h[en - 1 + (na - 1) * n]) + fabs(h[na - 1 + (enm2 - 1) * n]); x = 0.75 * s; y = x; w = -0.4375 * s * s; l130: its++; /*its = its++; This may be undefined. Use its++ instead.*/ for (mm = l; mm <= enm2; mm++) { m = enm2 + l - mm; zz = h[m - 1 + (m - 1) * n]; r = x - zz; s = y - zz; p = (r * s - w) / h[m + (m - 1) * n] + h[m - 1 + m * n]; q = h[m + m * n] - zz - r - s; r = h[m + 1 + m * n]; s = fabs(p) + fabs(q) + fabs(r); p = p / s; q = q / s; r = r / s; if (m == l) break; if ((fabs(h[m - 1 + (m - 2) * n]) * (fabs(q) + fabs(r))) <= (machep * fabs(p) * (fabs(h[m - 2 + (m - 2) * n]) + fabs(zz) + fabs(h[m + m * n])))) break; } mp2 = m + 2; for (i = mp2; i <= en; i++) { h[i - 1 + (i - 3) * n] = 0.0; if (i == mp2) continue; h[i - 1 + (i - 4) * n] = 0.0; } for (k = m; k <= na; k++) /*260 */ { notlas = 0; if (k != na) notlas = 1; if (k == m) goto l170; p = h[k - 1 + (k - 2) * n]; q = h[k + (k - 2) * n]; r = 0.0; if (notlas) r = h[k + 1 + (k - 2) * n]; x = fabs(p) + fabs(q) + fabs(r); if (x == 0.0) continue; p = p / x; q = q / x; r = r / x; l170: s = sign(sqrt(p * p + q * q + r * r), p); if (k != m) h[k - 1 + (k - 2) * n] = -s * x; else if (l != m) h[k - 1 + (k - 2) * n] = -h[k - 1 + (k - 2) * n]; p = p + s; x = p / s; y = q / s; zz = r / s; q = q / p; r = r / p; for (j = k; j <= en; j++) { p = h[k - 1 + (j - 1) * n] + q * h[k + (j - 1) * n]; if (notlas) { p = p + r * h[k + 1 + (j - 1) * n]; h[k + 1 + (j - 1) * n] = h[k + 1 + (j - 1) * n] - p * zz; } h[k + (j - 1) * n] = h[k + (j - 1) * n] - p * y; h[k - 1 + (j - 1) * n] = h[k - 1 + (j - 1) * n] - p * x; } j = imin(en, k + 3); for (i = l; i <= j; i++) { p = x * h[i - 1 + (k - 1) * n] + y * h[i - 1 + k * n]; if (notlas) { p = p + zz * h[i - 1 + (k + 1) * n]; h[i - 1 + (k + 1) * n] = h[i - 1 + (k + 1) * n] - p * r; } h[i - 1 + k * n] = h[i - 1 + k * n] - p * q; h[i - 1 + (k - 1) * n] = h[i - 1 + (k - 1) * n] - p; } } goto l70; l270: ev[(en - 1) * 2] = x + t; ev[1 + (en - 1) * 2] = 0.0; en = na; goto l60; l280: p = (y - x) / 2.0; q = p * p + w; zz = sqrt(fabs(q)); x = x + t; if (q < 0.0) goto l320; zz = p + sign(zz, p); ev[(na - 1) * 2] = x + zz; ev[(en - 1) * 2] = ev[(na - 1) * 2]; if (zz != 0.0) ev[(en - 1) * 2] = x - w / zz; ev[1 + (na - 1) * 2] = 0.0; ev[1 + (en - 1) * 2] = 0.0; goto l330; l320: ev[(na - 1) * 2] = x + p; ev[(en - 1) * 2] = x + p; ev[1 + (na - 1) * 2] = zz; ev[1 + (en - 1) * 2] = -zz; l330: en = enm2; goto l60; l1000: *ierr = en; }
void srTDriftSpace::SetupPropBufVars_AnalytTreatQuadPhaseTerm(srTSRWRadStructAccessData* pRadAccessData) {// Compute any necessary buf. vars PropBufVars.xc = pRadAccessData->xc; PropBufVars.zc = pRadAccessData->zc; PropBufVars.invRx = PropBufVars.invRz = 0; PropBufVars.Pi_d_LambdaM_d_Rx = PropBufVars.Pi_d_LambdaM_d_Rz = 0; PropBufVars.Lx = PropBufVars.Lz = 0; PropBufVars.invRxL = PropBufVars.invRzL = 1./Length; const double infLarge = 1E+23; double Pi_d_LambdaM = pRadAccessData->eStart*2.53384080189E+06; PropBufVars.kx_AnalytTreatQuadPhaseTerm = infLarge; PropBufVars.kxc_AnalytTreatQuadPhaseTerm = infLarge; PropBufVars.kz_AnalytTreatQuadPhaseTerm = infLarge; PropBufVars.kzc_AnalytTreatQuadPhaseTerm = infLarge; //double Lx_eff_max, Lz_eff_max, trueRx, trueRz; //EstimateTrueWfrRadAndMaxLeff_AnalytTreatQuadPhaseTerm(pRadAccessData, trueRx, trueRz, Lx_eff_max, Lz_eff_max); double trueRx = pRadAccessData->RobsX; double trueRz = pRadAccessData->RobsZ; if(pRadAccessData->ne > 1) PropBufVars.UseExactRxRzForAnalytTreatQuadPhaseTerm = true; //OC120412 (commented-out) //OC180813 (uncommented) //testOC30092011 if(!PropBufVars.UseExactRxRzForAnalytTreatQuadPhaseTerm) { if(PropBufVars.AnalytTreatSubType == 1) EstimateWfrRadToSub_AnalytTreatQuadPhaseTerm(pRadAccessData, trueRx, trueRz); //OC15102011 -- under testing (disadvantage of the previous version is the dependence of "trueR" on statistical moments) else if(PropBufVars.AnalytTreatSubType == 2) EstimateWfrRadToSub2_AnalytTreatQuadPhaseTerm(pRadAccessData, trueRx, trueRz); //OC22042013 (uncommented) } //if(pRadAccessData->RobsX != 0) if(trueRx != 0) { //PropBufVars.invRx = 1./pRadAccessData->RobsX; PropBufVars.invRx = 1./trueRx; PropBufVars.Pi_d_LambdaM_d_Rx = Pi_d_LambdaM*PropBufVars.invRx; //PropBufVars.kx_AnalytTreatQuadPhaseTerm = (pRadAccessData->RobsX + Length)/pRadAccessData->RobsX; //PropBufVars.kxc_AnalytTreatQuadPhaseTerm = Length/pRadAccessData->RobsX; PropBufVars.kx_AnalytTreatQuadPhaseTerm = (trueRx + Length)/trueRx; PropBufVars.kxc_AnalytTreatQuadPhaseTerm = Length/trueRx; //if(-Length != pRadAccessData->RobsX) if(-Length != trueRx) { PropBufVars.Lx = Length/(1. + Length*PropBufVars.invRx); //PropBufVars.invRxL = 1./(Length + pRadAccessData->RobsX); PropBufVars.invRxL = 1./(Length + trueRx); } else { PropBufVars.Lx = infLarge; PropBufVars.invRxL = infLarge; } } //if(pRadAccessData->RobsZ != 0) if(trueRz != 0) { //PropBufVars.invRz = 1./pRadAccessData->RobsZ; PropBufVars.invRz = 1./trueRz; PropBufVars.Pi_d_LambdaM_d_Rz = Pi_d_LambdaM*PropBufVars.invRz; //PropBufVars.kz_AnalytTreatQuadPhaseTerm = (pRadAccessData->RobsZ + Length)/pRadAccessData->RobsZ; //PropBufVars.kzc_AnalytTreatQuadPhaseTerm = Length/pRadAccessData->RobsZ; PropBufVars.kz_AnalytTreatQuadPhaseTerm = (trueRz + Length)/trueRz; PropBufVars.kzc_AnalytTreatQuadPhaseTerm = Length/trueRz; //if(-Length != pRadAccessData->RobsZ) if(-Length != trueRz) { PropBufVars.Lz = Length/(1. + Length*PropBufVars.invRz); //PropBufVars.invRzL = 1./(Length + pRadAccessData->RobsZ); PropBufVars.invRzL = 1./(Length + trueRz); } else { PropBufVars.Lz = infLarge; PropBufVars.invRzL = infLarge; } } //double Lx_eff_max, Lz_eff_max; //EstimateMaxLeff_AnalytTreatQuadPhaseTerm(pRadAccessData, Lx_eff_max, Lz_eff_max); /**test OC061108 if(::fabs(PropBufVars.Lx) > Lx_eff_max) { int signLx = sign(PropBufVars.Lx); PropBufVars.Lx = signLx*Lx_eff_max; //double Rx_eff = sign(pRadAccessData->RobsX)*infLarge; double Rx_eff = sign(trueRx)*infLarge; if(PropBufVars.Lx != Length) Rx_eff = Length*PropBufVars.Lx/(Length - PropBufVars.Lx); PropBufVars.invRx = 1./Rx_eff; double Rx_eff_p_Length = Rx_eff + Length; PropBufVars.invRxL = (Rx_eff_p_Length == 0.)? infLarge : 1./Rx_eff_p_Length; PropBufVars.kx_AnalytTreatQuadPhaseTerm = Rx_eff_p_Length/Rx_eff; PropBufVars.kxc_AnalytTreatQuadPhaseTerm = Length/Rx_eff; } if(::fabs(PropBufVars.Lz) > Lz_eff_max) { int signLz = sign(PropBufVars.Lz); PropBufVars.Lz = signLz*Lz_eff_max; //double Rz_eff = sign(pRadAccessData->RobsZ)*infLarge; double Rz_eff = sign(trueRz)*infLarge; if(PropBufVars.Lz != Length) Rz_eff = Length*PropBufVars.Lz/(Length - PropBufVars.Lz); PropBufVars.invRz = 1./Rz_eff; double Rz_eff_p_Length = Rz_eff + Length; PropBufVars.invRzL = (Rz_eff_p_Length == 0.)? infLarge : 1./Rz_eff_p_Length; PropBufVars.kz_AnalytTreatQuadPhaseTerm = Rz_eff_p_Length/Rz_eff; PropBufVars.kzc_AnalytTreatQuadPhaseTerm = Length/Rz_eff; } **/ PropBufVars.sqrt_LxLz_d_L = ::sqrt(::fabs(PropBufVars.Lx*PropBufVars.Lz))/Length; PropBufVars.phase_term_signLxLz = 0.25*3.141592653589793*(2. - sign(PropBufVars.Lx) - sign(PropBufVars.Lz)); // Continue for more buf vars }
/*Mainloop*/ void loop() { /*Auswertung des anstehenden Aufgaben im Scheduler*/ int i; unsigned char new_value = 0; unsigned char new_heading = 0; for (i = 0; i < MAX_TASKS; i++) { if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;} switch (scheduler[i].task) { case NOTHING: break; case MEASURE: /*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/ srf[scheduler[i].param - SE0_ADDRESS].measure(); schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param); scheduler[i].task = NOTHING; break; case READ_MEASURE: { unsigned short index = scheduler[i].param - SE0_ADDRESS; /*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/ srf[index].read_it(get_current_millis()); schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param); if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) { nvalue[index] = 1; } /*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) { nvalue2[scheduler[i].param - SE0_ADDRESS] = 1; }*/ new_value = scheduler[i].param; /*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/ if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); /*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/ static unsigned char rm_state[SE_COUNT]; static short rm_min[SE_COUNT]; if (state != IDLE && state != INIT && state != DELAY) { if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) { rm_state[index] = 1; if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF; else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF; srf[index].set_mean(SE_MIN); } else if (rm_state[index] == 1) { if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else { srf[index].set_mean(SE_MIN); } } } if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;} if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;} if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) { if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) { slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis()); nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0; } } /*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/ #if LOG > 0 FILE *fd; if (scheduler[i].param == SE0_ADDRESS) fd = fd_112; else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113; else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114; else fd = fd_115; fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean()); #endif scheduler[i].task = NOTHING; break; } case SAVE_LOG: /*Sichert die bisher geloggten Daten*/ #if LOG > 0 fclose(fd_112); fclose(fd_113); fclose(fd_114); fclose(fd_115); fclose(fd_data); char tmp_dir[32]; strcpy(tmp_dir,log_dir); fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir); fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir); fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir); fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir); fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir); schedule_add(LOG_SPEED,SAVE_LOG,0); #endif scheduler[i].task = NOTHING; break; case CHECK_STILL: /*Prueft auf Stillstand des Copters*/ if (still && state == HOLD_STILL) { if (scheduler[i].param < CHECK_STILL_COUNT) { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1); } else { state = MEASURE_ENVIRONMENT; still = 0; schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0); } } else { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0); } scheduler[i].task = NOTHING; break; case CHANGE_STATE: /*Ändert den aktuellen Status zum als Parameter übergebenen Status*/ state = (states)scheduler[i].param; if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0; scheduler[i].task = NOTHING; break; case SHOW_ME: { /*Schreibt Messwerte und Neigungswinkel auf das Terminal*/ char st[16]; switch (state) { case INIT: sprintf(st, "INIT"); break; case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break; case IDLE: sprintf(st, "IDLE"); break; case HOLD_STILL: sprintf(st, "HOLD_STILL"); break; case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break; case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break; case DELAY: sprintf(st, "DELAY"); break; default: sprintf(st, "???"); break; } if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); schedule_add(100,SHOW_ME,0); scheduler[i].task = NOTHING; /*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/ mavlink_message_t msg; uint16_t len; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; static int i; i++; if (i == 10) { i = 0; mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); } mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data()); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); break; } default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break; } } /*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/ struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0; /*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/ FD_ZERO(&tty_fdset); FD_SET(tty_fd, &tty_fdset); if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);} /*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/ if (FD_ISSET(tty_fd, &tty_fdset)) { static mavlink_message_t msg; static mavlink_status_t status; char c[1]; if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}} #if DEBUG_LEVEL > 2 printf("%#x\n",c[0]); #endif if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) { #if DEBUG_LEVEL > 1 printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); #endif //printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: /*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/ mavlink_heartbeat_t hb; mavlink_msg_heartbeat_decode(&msg,&hb); if (!first_heartbeat) { first_heartbeat = 1; request_data_stream(MAV_DATA_STREAM_ALL,0,0); } //if (state == IDLE) { if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) { std::cout << "State changed from IDLE to INIT.\n"; state = INIT; schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1); init_state = 1; } break; case MAVLINK_MSG_ID_VFR_HUD: /*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/ mavlink_vfr_hud_t hud_data; mavlink_msg_vfr_hud_decode(&msg, &hud_data); old_heading = current_heading; current_heading = hud_data.heading; new_heading = 1; break; case MAVLINK_MSG_ID_ATTITUDE: mavlink_attitude_t adata; mavlink_msg_attitude_decode(&msg,&adata); old_heading = current_heading; current_heading = ((short)lround(DEG(adata.yaw))+360)%360; current_pitch_rad = adata.pitch; current_roll_rad = adata.roll; new_heading = 1; break; case MAVLINK_MSG_ID_RAW_IMU: /*Speichert die Beschleunigungen auf der x- und y-Achse*/ mavlink_raw_imu_t raw_imu; mavlink_msg_raw_imu_decode(&msg,&raw_imu); xacc = raw_imu.xacc; yacc = raw_imu.yacc; //slam_insert_acc(xacc,yacc,current_heading,get_current_millis()); std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n"; break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: /*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/ /*if (state != IDLE) {break;} mavlink_rc_channels_raw_t rc; mavlink_msg_rc_channels_raw_decode(&msg,&rc); if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1; if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) { std::cout << "State changed from IDLE to INIT.\n"; schedule_add(0,CHANGE_STATE,(int)INIT); schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0); request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1); request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1); } if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) { init_state = 1; }*/ break; default: break; } } } /*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/ if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;} switch (state) { case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break; case INIT: std::cout << "INIT\n"; /*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/ schedule_add(5000,CHECK_STILL,0); #if LOG > 0 schedule_add(LOG_SPEED,SAVE_LOG,0); //schedule_add(SLAM_SPEED,SAVE_SLAM,0); #endif for (int i = 0; i < SE_COUNT; i++) { if ((int)pow(2,i) & ENABLED_SENSORS) { schedule_add(0, MEASURE, SE0_ADDRESS + i); } } slam_init(current_heading,get_current_millis()); state = DELAY; break; case HOLD_STILL: if(1) { static short set_point[SE_COUNT]; if (hs_state == 0) { hs_state = 1; set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2; set_point[1] = set_point[0]; set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2; set_point[3] = set_point[2]; //for (int i = 0; i < SE_COUNT; i++) { // set_point[i] = srf[i].get_mean(); // std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; //} /*for (int i = 0; i < SE_COUNT; i++) { if (set_point[i] < 100 || set_point[i] > 0) { if (i == 0 || i == 2) { set_point[i+1] -= (100 - set_point[i]); } else { set_point[i-1] -= (100 - set_point[i]); } set_point[i] = 100; } }*/ for (int i = 0; i < SE_COUNT; i++) { std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; } } /*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/ if (!new_value) break; if (nvalue[0] == 1 && nvalue[1] == 1) { if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll); else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll); else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll); nvalue[0] = 0; nvalue[1] = 0; } if (nvalue[2] == 1 && nvalue[3] == 1) { if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch); else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch); else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch); nvalue[2] = 0; nvalue[3] = 0; } yaw = 0; send_ext_ctrl(); /*Prüfung auf Stillstand*/ if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) { //still = 1; still = 0; /*FIXME*/ } else { still = 0; } } break; case MEASURE_ENVIRONMENT: /*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/ if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;} if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;} if (first_heading == -1) { /*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/ std::cout << "State changed to MEASURE_ENVIRONMENT\n"; first_heading = current_heading; roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW; send_ext_ctrl(); } //if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360); /*Berechnung der aktuellen Drehung*/ if (new_heading) { if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360; else rotation += current_heading - old_heading; for(int i = 0; i < SE_COUNT; i++) { if(nvalue[i] == 1) { env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean(); nvalue[i] = 0; } } //if (abs(rotation) >= 360/SE_COUNT) { if (abs(rotation) >= rotation_angle) { /*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/ //state = HEAD_TO_MIDDLE; state = GET_ALIGNMENT; roll = 0; pitch = 0; yaw = 0; send_ext_ctrl(); rotation_angle_sign = sign(rotation); } } break; case GET_ALIGNMENT: { /*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/ /*short min_v[SE_COUNT]; short min_h[SE_COUNT]; for (int i = 0; i < SE_COUNT; i++) { min_v[i] = 0; min_h[i] = 0; }*/ short min_v = 0; for (int i = 0; i < rotation_angle; i++) { if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) { min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]; desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360; } /*for (int j = 0; j < SE_COUNT; j++) { if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) { min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]; min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360; } }*/ } //desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4); /*Bestimmung der künftigen Drehrichtung*/ for (int k = 0; k < rotation_angle/2; k++) { /*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/ if (desired_heading == (first_heading+rotation_angle_sign*k)%360) { rotation_angle_sign *= -1; if (init_state == 2) init_state = 3; break; } } /*Cleanup*/ first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset(); for(int i = 0; i < 360; i++) env[i] = 0; if (init_state == 1) init_state = 2; if (init_state == 3) { state = GET_ANCHOR; pid_yaw.reset(); pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV); pid_yaw.set_target(0); roll = 0; pitch = 0; yaw = 0; std::cout << "State changed to GET_ANCHOR\n"; tv_old_heading = get_current_millis(); } else { state = HOLD_STILL; hs_state = 0; } break; } case GET_ANCHOR: /*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/ if (new_heading) { short heading_diff; /*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/ if (abs(desired_heading - current_heading) > 180) { heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360; } else { heading_diff = desired_heading - current_heading; } yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading); tv_old_heading = get_current_millis(); } if (new_value) { roll = 0; pitch = 0; if (nvalue[align_se] == 1) { if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); nvalue[align_se] = 0; } if (srf[0].get_mean() < 30) roll = 1000; else if (srf[1].get_mean() < 30) roll = -1000; else if (srf[2].get_mean() < 30) pitch = 1000; else if (srf[3].get_mean() < 30) pitch = -1000; if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) { for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0; /*state = MOVE_BACKWARD*/ /*TODO Neuausrichtung des Copters?*/ } } if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();} else if (breakpoint != 2) {send_ext_ctrl();} break; case DELAY: break; case HTM_DELAY: break; default: break; } #if LOG > 0 fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad); #endif }