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
}
