double QgsDistanceArea::computeDistanceBearing(
const QgsPoint& p1, const QgsPoint& p2,
double* course1, double* course2 )
{
if ( p1.x() == p2.x() && p1.y() == p2.y() )
return 0;
// ellipsoid
double a = mSemiMajor;
double b = mSemiMinor;
double f = 1 / mInvFlattening;
double p1_lat = DEG2RAD( p1.y() ), p1_lon = DEG2RAD( p1.x() );
double p2_lat = DEG2RAD( p2.y() ), p2_lon = DEG2RAD( p2.x() );
double L = p2_lon - p1_lon;
double U1 = atan(( 1 - f ) * tan( p1_lat ) );
double U2 = atan(( 1 - f ) * tan( p2_lat ) );
double sinU1 = sin( U1 ), cosU1 = cos( U1 );
double sinU2 = sin( U2 ), cosU2 = cos( U2 );
double lambda = L;
double lambdaP = 2 * M_PI;
double sinLambda = 0;
double cosLambda = 0;
double sinSigma = 0;
double cosSigma = 0;
double sigma = 0;
double alpha = 0;
double cosSqAlpha = 0;
double cos2SigmaM = 0;
double C = 0;
double tu1 = 0;
double tu2 = 0;
int iterLimit = 20;
while ( qAbs( lambda - lambdaP ) > 1e-12 && --iterLimit > 0 )
{
sinLambda = sin( lambda );
cosLambda = cos( lambda );
tu1 = ( cosU2 * sinLambda );
tu2 = ( cosU1 * sinU2 - sinU1 * cosU2 * cosLambda );
sinSigma = sqrt( tu1 * tu1 + tu2 * tu2 );
cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
sigma = atan2( sinSigma, cosSigma );
alpha = asin( cosU1 * cosU2 * sinLambda / sinSigma );
cosSqAlpha = cos( alpha ) * cos( alpha );
cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
C = f / 16 * cosSqAlpha * ( 4 + f * ( 4 - 3 * cosSqAlpha ) );
lambdaP = lambda;
lambda = L + ( 1 - C ) * f * sin( alpha ) *
( sigma + C * sinSigma * ( cos2SigmaM + C * cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) ) );
}
if ( iterLimit == 0 )
return -1; // formula failed to converge
double uSq = cosSqAlpha * ( a * a - b * b ) / ( b * b );
double A = 1 + uSq / 16384 * ( 4096 + uSq * ( -768 + uSq * ( 320 - 175 * uSq ) ) );
double B = uSq / 1024 * ( 256 + uSq * ( -128 + uSq * ( 74 - 47 * uSq ) ) );
double deltaSigma = B * sinSigma * ( cos2SigmaM + B / 4 * ( cosSigma * ( -1 + 2 * cos2SigmaM * cos2SigmaM ) -
B / 6 * cos2SigmaM * ( -3 + 4 * sinSigma * sinSigma ) * ( -3 + 4 * cos2SigmaM * cos2SigmaM ) ) );
double s = b * A * ( sigma - deltaSigma );
if ( course1 )
{
*course1 = atan2( tu1, tu2 );
}
if ( course2 )
{
// PI is added to return azimuth from P2 to P1
*course2 = atan2( cosU1 * sinLambda, -sinU1 * cosU2 + cosU1 * sinU2 * cosLambda ) + M_PI;
}
return s;
}
void adm_decouple_d(const adm_dwt_band_t_d *ref, const adm_dwt_band_t_d *dis, const adm_dwt_band_t_d *r, const adm_dwt_band_t_d *a, int w, int h, int ref_stride, int dis_stride, int r_stride, int a_stride)
{
#ifdef ADM_OPT_AVOID_ATAN
const double cos_1deg_sq = cos(1.0 * M_PI / 180.0) * cos(1.0 * M_PI / 180.0);
#endif
const double eps = 1e-30;
int ref_px_stride = ref_stride / sizeof(double);
int dis_px_stride = dis_stride / sizeof(double);
int r_px_stride = r_stride / sizeof(double);
int a_px_stride = a_stride / sizeof(double);
double oh, ov, od, th, tv, td;
double kh, kv, kd, tmph, tmpv, tmpd;
#ifdef ADM_OPT_AVOID_ATAN
double ot_dp, o_mag_sq, t_mag_sq;
#else
double oa, ta, diff;
#endif
int angle_flag;
int i, j;
for (i = 0; i < h; ++i) {
for (j = 0; j < w; ++j) {
oh = ref->band_h[i * ref_px_stride + j];
ov = ref->band_v[i * ref_px_stride + j];
od = ref->band_d[i * ref_px_stride + j];
th = dis->band_h[i * dis_px_stride + j];
tv = dis->band_v[i * dis_px_stride + j];
td = dis->band_d[i * dis_px_stride + j];
kh = DIVD(th, oh + eps);
kv = DIVD(tv, ov + eps);
kd = DIVD(td, od + eps);
kh = kh < 0.0 ? 0.0 : (kh > 1.0 ? 1.0 : kh);
kv = kv < 0.0 ? 0.0 : (kv > 1.0 ? 1.0 : kv);
kd = kd < 0.0 ? 0.0 : (kd > 1.0 ? 1.0 : kd);
tmph = kh * oh;
tmpv = kv * ov;
tmpd = kd * od;
#ifdef ADM_OPT_AVOID_ATAN
/* Determine if angle between (oh,ov) and (th,tv) is less than 1 degree.
* Given that u is the angle (oh,ov) and v is the angle (th,tv), this can
* be done by testing the inequvality.
*
* { (u.v.) >= 0 } AND { (u.v)^2 >= cos(1deg)^2 * ||u||^2 * ||v||^2 }
*
* Proof:
*
* cos(theta) = (u.v) / (||u|| * ||v||)
*
* IF u.v >= 0 THEN
* cos(theta)^2 = (u.v)^2 / (||u||^2 * ||v||^2)
* (u.v)^2 = cos(theta)^2 * ||u||^2 * ||v||^2
*
* IF |theta| < 1deg THEN
* (u.v)^2 >= cos(1deg)^2 * ||u||^2 * ||v||^2
* END
* ELSE
* |theta| > 90deg
* END
*/
ot_dp = oh * th + ov * tv;
o_mag_sq = oh * oh + ov * ov;
t_mag_sq = th * th + tv * tv;
angle_flag = (ot_dp >= 0.0) && (ot_dp * ot_dp >= cos_1deg_sq * o_mag_sq * t_mag_sq);
#else
oa = atan(DIVD(ov, oh + eps));
ta = atan(DIVD(tv, th + eps));
if (oh < 0.0)
oa += M_PI;
if (th < 0.0)
ta += M_PI;
diff = fabs(oa - ta) * 180.0 / M_PI;
angle_flag = diff < 1.0;
#endif
if (angle_flag) {
tmph = th;
tmpv = tv;
tmpd = td;
}
r->band_h[i * r_px_stride + j] = tmph;
r->band_v[i * r_px_stride + j] = tmpv;
r->band_d[i * r_px_stride + j] = tmpd;
a->band_h[i * a_px_stride + j] = th - tmph;
a->band_v[i * a_px_stride + j] = tv - tmpv;
a->band_d[i * a_px_stride + j] = td - tmpd;
}
}
}
UndistorterPTAM::UndistorterPTAM(const char* configFileName)
{
valid = true;
remapX = nullptr;
remapY = nullptr;
// read parameters
std::ifstream infile(configFileName);
assert(infile.good());
std::string l1,l2,l3,l4;
std::getline(infile,l1);
std::getline(infile,l2);
std::getline(infile,l3);
std::getline(infile,l4);
// l1 & l2
if(std::sscanf(l1.c_str(), "%f %f %f %f %f", &inputCalibration[0], &inputCalibration[1], &inputCalibration[2], &inputCalibration[3], &inputCalibration[4]) == 5 &&
std::sscanf(l2.c_str(), "%d %d", &in_width, &in_height) == 2)
{
printf("Input resolution: %d %d\n",in_width, in_height);
printf("In: %f %f %f %f %f\n",
inputCalibration[0], inputCalibration[1], inputCalibration[2], inputCalibration[3], inputCalibration[4]);
}
else
{
printf("Failed to read camera calibration (invalid format?)\nCalibration file: %s\n", configFileName);
valid = false;
}
// l3
if(l3 == "crop")
{
outputCalibration[0] = -1;
printf("Out: Crop\n");
}
else if(l3 == "full")
{
outputCalibration[0] = -2;
printf("Out: Full\n");
}
else if(l3 == "none")
{
printf("NO RECTIFICATION\n");
}
else if(std::sscanf(l3.c_str(), "%f %f %f %f %f", &outputCalibration[0], &outputCalibration[1], &outputCalibration[2], &outputCalibration[3], &outputCalibration[4]) == 5)
{
printf("Out: %f %f %f %f %f\n",
outputCalibration[0], outputCalibration[1], outputCalibration[2], outputCalibration[3], outputCalibration[4]);
}
else
{
printf("Out: Failed to Read Output pars... not rectifying.\n");
valid = false;
}
// l4
if(std::sscanf(l4.c_str(), "%d %d", &out_width, &out_height) == 2)
{
printf("Output resolution: %d %d\n",out_width, out_height);
}
else
{
printf("Out: Failed to Read Output resolution... not rectifying.\n");
valid = false;
}
// prep warp matrices
if(valid)
{
float dist = inputCalibration[4];
float d2t = 2.0f * tan(dist / 2.0f);
// current camera parameters
float fx = inputCalibration[0] * in_width;
float fy = inputCalibration[1] * in_height;
float cx = inputCalibration[2] * in_width - 0.5;
float cy = inputCalibration[3] * in_height - 0.5;
// scale calibration parameters to input size
double xfactor = in_width / (1.0 * in_width);
double yfactor = in_height / (1.0 * in_height);
fx = fx * xfactor;
fy = fy * yfactor;
cx = (cx + 0.5) * xfactor - 0.5;
cy = (cy + 0.5) * yfactor - 0.5;
// output camera parameters
float ofx, ofy, ocx, ocy;
// find new camera matrix for "crop" and "full"
if (inputCalibration[4] == 0)
{
ofx = inputCalibration[0] * out_width;
ofy = inputCalibration[1] * out_height;
ocx = (inputCalibration[2] * out_width) - 0.5;
ocy = (inputCalibration[3] * out_height) - 0.5;
}
else if(outputCalibration[0] == -1) // "crop"
{
// find left-most and right-most radius
float left_radius = (cx)/fx;
float right_radius = (in_width-1 - cx)/fx;
float top_radius = (cy)/fy;
float bottom_radius = (in_height-1 - cy)/fy;
float trans_left_radius = tan(left_radius * dist)/d2t;
float trans_right_radius = tan(right_radius * dist)/d2t;
float trans_top_radius = tan(top_radius * dist)/d2t;
float trans_bottom_radius = tan(bottom_radius * dist)/d2t;
//printf("left_radius: %f -> %f\n", left_radius, trans_left_radius);
//printf("right_radius: %f -> %f\n", right_radius, trans_right_radius);
//printf("top_radius: %f -> %f\n", top_radius, trans_top_radius);
//printf("bottom_radius: %f -> %f\n", bottom_radius, trans_bottom_radius);
ofy = fy * ((top_radius + bottom_radius) / (trans_top_radius + trans_bottom_radius)) * ((float)out_height / (float)in_height);
ocy = (trans_top_radius/top_radius) * ofy*cy/fy;
ofx = fx * ((left_radius + right_radius) / (trans_left_radius + trans_right_radius)) * ((float)out_width / (float)in_width);
ocx = (trans_left_radius/left_radius) * ofx*cx/fx;
printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy);
printf("old K: %f %f %f %f\n",fx,fy,cx,cy);
}
else if(outputCalibration[0] == -2) // "full"
{
float left_radius = cx/fx;
float right_radius = (in_width-1 - cx)/fx;
float top_radius = cy/fy;
float bottom_radius = (in_height-1 - cy)/fy;
// find left-most and right-most radius
float tl_radius = sqrt(left_radius*left_radius + top_radius*top_radius);
float tr_radius = sqrt(right_radius*right_radius + top_radius*top_radius);
float bl_radius = sqrt(left_radius*left_radius + bottom_radius*bottom_radius);
float br_radius = sqrt(right_radius*right_radius + bottom_radius*bottom_radius);
float trans_tl_radius = tan(tl_radius * dist)/d2t;
float trans_tr_radius = tan(tr_radius * dist)/d2t;
float trans_bl_radius = tan(bl_radius * dist)/d2t;
float trans_br_radius = tan(br_radius * dist)/d2t;
//printf("trans_tl_radius: %f -> %f\n", tl_radius, trans_tl_radius);
//printf("trans_tr_radius: %f -> %f\n", tr_radius, trans_tr_radius);
//printf("trans_bl_radius: %f -> %f\n", bl_radius, trans_bl_radius);
//printf("trans_br_radius: %f -> %f\n", br_radius, trans_br_radius);
float hor = std::max(br_radius,tr_radius) + std::max(bl_radius,tl_radius);
float vert = std::max(tr_radius,tl_radius) + std::max(bl_radius,br_radius);
float trans_hor = std::max(trans_br_radius,trans_tr_radius) + std::max(trans_bl_radius,trans_tl_radius);
float trans_vert = std::max(trans_tr_radius,trans_tl_radius) + std::max(trans_bl_radius,trans_br_radius);
ofy = fy * ((vert) / (trans_vert)) * ((float)out_height / (float)in_height);
ocy = std::max(trans_tl_radius/tl_radius,trans_tr_radius/tr_radius) * ofy*cy/fy;
ofx = fx * ((hor) / (trans_hor)) * ((float)out_width / (float)in_width);
ocx = std::max(trans_bl_radius/bl_radius,trans_tl_radius/tl_radius) * ofx*cx/fx;
printf("new K: %f %f %f %f\n",ofx,ofy,ocx,ocy);
printf("old K: %f %f %f %f\n",fx,fy,cx,cy);
}
else
{
ofx = outputCalibration[0] * out_width;
ofy = outputCalibration[1] * out_height;
ocx = outputCalibration[2] * out_width-0.5; // TODO: -0.5 here or not?
ocy = outputCalibration[3] * out_height-0.5;
}
outputCalibration[0] = ofx / out_width;
outputCalibration[1] = ofy / out_height;
outputCalibration[2] = (ocx+0.5) / out_width;
outputCalibration[3] = (ocy+0.5) / out_height;
outputCalibration[4] = 0;
remapX = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float));
remapY = (float*)Eigen::internal::aligned_malloc(out_width * out_height *sizeof(float));
for(int y=0;y<out_height;y++)
{
for(int x=0;x<out_width;x++)
{
float ix = (x - ocx) / ofx;
float iy = (y - ocy) / ofy;
float r = sqrt(ix*ix + iy*iy);
float fac = (r==0 || dist==0) ? 1 : atan(r * d2t)/(dist*r);
ix = fx*fac*ix+cx;
iy = fy*fac*iy+cy;
// make rounding resistant.
if(ix == 0) ix = 0.01;
if(iy == 0) iy = 0.01;
if(ix == in_width-1) ix = in_width-1.01;
if(iy == in_height-1) ix = in_height-1.01;
if(ix > 0 && iy > 0 && ix < in_width-1 && iy < in_height-1)
{
remapX[x+y*out_width] = ix;
remapY[x+y*out_width] = iy;
}
else
{
remapX[x+y*out_width] = -1;
remapY[x+y*out_width] = -1;
}
}
}
printf("Prepped Warp matrices\n");
}
else
{
printf("Not Rectifying\n");
outputCalibration[0] = inputCalibration[0];
outputCalibration[1] = inputCalibration[1];
outputCalibration[2] = inputCalibration[2];
outputCalibration[3] = inputCalibration[3];
outputCalibration[4] = inputCalibration[4];
out_width = in_width;
out_height = in_height;
}
originalK_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0));
originalK_.at<double>(0, 0) = inputCalibration[0];
originalK_.at<double>(1, 1) = inputCalibration[1];
originalK_.at<double>(2, 2) = 1;
originalK_.at<double>(2, 0) = inputCalibration[2];
originalK_.at<double>(2, 1) = inputCalibration[3];
K_ = cv::Mat(3, 3, CV_64F, cv::Scalar(0));
K_.at<double>(0, 0) = outputCalibration[0] * out_width;
K_.at<double>(1, 1) = outputCalibration[1] * out_height;
K_.at<double>(2, 2) = 1;
K_.at<double>(2, 0) = outputCalibration[2] * out_width - 0.5;
K_.at<double>(2, 1) = outputCalibration[3] * out_height - 0.5;
}
double
g( double x ) {
return +1.0 + ( atan( 1.0 + x ) + atan( 1.0 - x ) ) / M_PI;
}
/* calcPerform
*
* Evalutate the postfix expression
*/
epicsShareFunc long
calcPerform(double *parg, double *presult, const char *pinst)
{
double stack[CALCPERFORM_STACK+1]; /* zero'th entry not used */
double *ptop; /* stack pointer */
double top; /* value from top of stack */
epicsInt32 itop; /* integer from top of stack */
epicsUInt32 utop; /* unsigned integer from top of stack */
int op;
int nargs;
/* initialize */
ptop = stack;
/* RPN evaluation loop */
while ((op = *pinst++) != END_EXPRESSION){
switch (op){
case LITERAL_DOUBLE:
memcpy(++ptop, pinst, sizeof(double));
pinst += sizeof(double);
break;
case LITERAL_INT:
memcpy(&itop, pinst, sizeof(epicsInt32));
*++ptop = itop;
pinst += sizeof(epicsInt32);
break;
case FETCH_VAL:
*++ptop = *presult;
break;
case FETCH_A:
case FETCH_B:
case FETCH_C:
case FETCH_D:
case FETCH_E:
case FETCH_F:
case FETCH_G:
case FETCH_H:
case FETCH_I:
case FETCH_J:
case FETCH_K:
case FETCH_L:
*++ptop = parg[op - FETCH_A];
break;
case STORE_A:
case STORE_B:
case STORE_C:
case STORE_D:
case STORE_E:
case STORE_F:
case STORE_G:
case STORE_H:
case STORE_I:
case STORE_J:
case STORE_K:
case STORE_L:
parg[op - STORE_A] = *ptop--;
break;
case CONST_PI:
*++ptop = PI;
break;
case CONST_D2R:
*++ptop = PI/180.;
break;
case CONST_R2D:
*++ptop = 180./PI;
break;
case UNARY_NEG:
*ptop = - *ptop;
break;
case ADD:
top = *ptop--;
*ptop += top;
break;
case SUB:
top = *ptop--;
*ptop -= top;
break;
case MULT:
top = *ptop--;
*ptop *= top;
break;
case DIV:
top = *ptop--;
*ptop /= top;
break;
case MODULO:
itop = (epicsInt32) *ptop--;
if (itop)
*ptop = (epicsInt32) *ptop % itop;
else
*ptop = epicsNAN;
break;
case POWER:
top = *ptop--;
*ptop = pow(*ptop, top);
break;
case ABS_VAL:
*ptop = fabs(*ptop);
break;
case EXP:
*ptop = exp(*ptop);
break;
case LOG_10:
*ptop = log10(*ptop);
break;
case LOG_E:
*ptop = log(*ptop);
break;
case MAX:
nargs = *pinst++;
while (--nargs) {
top = *ptop--;
if (*ptop < top || isnan(top))
*ptop = top;
}
break;
case MIN:
nargs = *pinst++;
while (--nargs) {
top = *ptop--;
if (*ptop > top || isnan(top))
*ptop = top;
}
break;
case SQU_RT:
*ptop = sqrt(*ptop);
break;
case ACOS:
*ptop = acos(*ptop);
break;
case ASIN:
*ptop = asin(*ptop);
break;
case ATAN:
*ptop = atan(*ptop);
break;
case ATAN2:
top = *ptop--;
*ptop = atan2(top, *ptop); /* Ouch!: Args backwards! */
break;
case COS:
*ptop = cos(*ptop);
break;
case SIN:
*ptop = sin(*ptop);
break;
case TAN:
*ptop = tan(*ptop);
break;
case COSH:
*ptop = cosh(*ptop);
break;
case SINH:
*ptop = sinh(*ptop);
break;
case TANH:
*ptop = tanh(*ptop);
break;
case CEIL:
*ptop = ceil(*ptop);
break;
case FLOOR:
*ptop = floor(*ptop);
break;
case FINITE:
nargs = *pinst++;
top = finite(*ptop);
while (--nargs) {
--ptop;
top = top && finite(*ptop);
}
*ptop = top;
break;
case ISINF:
*ptop = isinf(*ptop);
break;
case ISNAN:
nargs = *pinst++;
top = isnan(*ptop);
while (--nargs) {
--ptop;
top = top || isnan(*ptop);
}
*ptop = top;
break;
case NINT:
top = *ptop;
*ptop = (epicsInt32) (top >= 0 ? top + 0.5 : top - 0.5);
break;
case RANDOM:
*++ptop = calcRandom();
break;
case REL_OR:
top = *ptop--;
*ptop = *ptop || top;
break;
case REL_AND:
top = *ptop--;
*ptop = *ptop && top;
break;
case REL_NOT:
*ptop = ! *ptop;
break;
/* For bitwise operations on values with bit 31 set, double values
* must first be cast to unsigned to correctly set that bit; the
* double value must be negative in that case. The result must be
* cast to a signed integer before converting to the double result.
*/
case BIT_OR:
utop = *ptop--;
*ptop = (epicsInt32) ((epicsUInt32) *ptop | utop);
break;
case BIT_AND:
utop = *ptop--;
*ptop = (epicsInt32) ((epicsUInt32) *ptop & utop);
break;
case BIT_EXCL_OR:
utop = *ptop--;
*ptop = (epicsInt32) ((epicsUInt32) *ptop ^ utop);
break;
case BIT_NOT:
utop = *ptop;
*ptop = (epicsInt32) ~utop;
break;
/* The shift operators use signed integers, so a right-shift will
* extend the sign bit into the left-hand end of the value. The
* double-casting through unsigned here is important, see above.
*/
case RIGHT_SHIFT:
utop = *ptop--;
*ptop = ((epicsInt32) (epicsUInt32) *ptop) >> (utop & 31);
break;
case LEFT_SHIFT:
utop = *ptop--;
*ptop = ((epicsInt32) (epicsUInt32) *ptop) << (utop & 31);
break;
case NOT_EQ:
top = *ptop--;
*ptop = *ptop != top;
break;
case LESS_THAN:
top = *ptop--;
*ptop = *ptop < top;
break;
case LESS_OR_EQ:
top = *ptop--;
*ptop = *ptop <= top;
break;
case EQUAL:
top = *ptop--;
*ptop = *ptop == top;
break;
case GR_OR_EQ:
top = *ptop--;
*ptop = *ptop >= top;
break;
case GR_THAN:
top = *ptop--;
*ptop = *ptop > top;
break;
case COND_IF:
if (*ptop-- == 0.0 &&
cond_search(&pinst, COND_ELSE)) return -1;
break;
case COND_ELSE:
if (cond_search(&pinst, COND_END)) return -1;
break;
case COND_END:
break;
default:
errlogPrintf("calcPerform: Bad Opcode %d at %p\n", op, pinst-1);
return -1;
}
}
/* The stack should now have one item on it, the expression value */
if (ptop != stack + 1)
return -1;
*presult = *ptop;
return 0;
}
con_vec drive(situation &s)
{
con_vec result = CON_VEC_EMPTY; // This is what is returned.
double alpha, vc; // components of result
static double lane = -10000; // an absurd value to show not initialized
double bias, speed, width;
if( s.starting )
{
result.fuel_amount = MAX_FUEL; // fuel when starting
}
// service routine in the host software to handle getting unstuck from
// from crashes and pileups:
if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
return result;
width = s.to_lft + s.to_rgt; // compute width of track
// This is a little trick so that the car will not try to change lanes
// during the "dragout" at the start of the race. We set "lane" to
// whatever position we have been placed by the host.
if(lane < -9000) // will be true only once
lane = s.to_lft; // better not to change lanes at the start
// Set "lane" during curves. This robot sets "lane" during curves to
// try to maintain a fixed distance to the inner rail.
// For straightaways, we leave "lane" unchanged until later.
if(s.cur_rad > 0.0) // turning left
lane = DIST_FROM_INSIDE;
else if(s.cur_rad < 0.0) // turning right
lane = width - DIST_FROM_INSIDE;
// set the bias:
// Bias is an additive term in the steering servo, so that the servo
// doesn't have to "hunt" much for the correct alpha value. It is an
// estimate of the alpha value that would be found by the servo if there
// was plenty of settling time. It is zero for straightaways.
// Also, for convenience, we call the corn_spd() function here. On
// the straightaway, we call it to find out the correct speed for the
// corner ahead, using s.nex_rad for the radius. In the curve we of
// course use the radius of the curve we are in.
if(s.cur_rad == 0.0)
{
bias = 0.0;
speed = corn_spd(s.nex_rad + DIST_FROM_INSIDE);
}
else
{
speed = corn_spd(s.cur_rad + DIST_FROM_INSIDE);
// See initial paragraphs for discussion of this formula.
bias = (s.v*s.v/(speed*speed)) * atan(BIG_SLIP / speed);
if(s.cur_rad < 0.0) // bias must be negative for right turn
bias = -bias;
}
// set alpha: (This line is the complete steering servo.)
alpha = STEER_GAIN * (s.to_lft - lane)/width - DAMP_GAIN * s.vn/s.v + bias;
// set vc: When nearing end of straight, change "lane" for the turn, also.
if(s.cur_rad == 0.0) // If we are on a straightaway,
{
if(s.to_end > ACCEL_FRACTION * s.cur_len) // if we are far from the end,
{
vc = s.v + 50.0; // pedal to the metal!
}
else // otherwise, adjust speed for the coming turn:
{
if(s.v > 1.02 * speed) // if we're 2% too fast,
vc = .95 * s.v; // brake hard.
else if(s.v < .98 * speed) // if we're 2% too slow,
vc = 1.05 * speed; // accelerate hard.
else // if we are very close to speed,
vc = .5 * (s.v + speed); // approach the speed gently.
// approach the lane you want for the turn:
if(s.nex_rad > 0.0)
lane = DIST_FROM_INSIDE;
else
lane = width - DIST_FROM_INSIDE;
}
}
else // This is when we are in a curve: (seek correct speed)
{
vc = .5 * (s.v + speed)/cos(alpha); // to maintain speed in corner
}
// During the acceleration portion of a straightaway, the lane variable
// is not changed by the code above. Hence the code below changes it a
// little at a time until there is no car dead_ahead. This code here has
// no affect at all in the turns, nor in the braking portion
// of the straight.
if(s.dead_ahead) // Change the lane a little if someone's
if(s.to_lft > s.to_rgt) // in your way.
lane -= DELTA_LANE; // lane must be a static variable
else
lane += DELTA_LANE;
result.vc = vc; result.alpha = alpha;
// Pit: if the fuel is too low
// Fuel: full
// Damage: repair all
if( s.fuel<10.0 )
{
result.request_pit = 1;
result.repair_amount = s.damage;
result.fuel_amount = MAX_FUEL;
}
return result;
}
INT4 InitPcsi (void)
/***********************************************************************
*
*_Title InitPcsi - Initialize pcsi parameters
*
*_DESC Initialize pcsi parameters
*
*_HIST Mar 9 2003 Janet Barrett, Created by modifying the initpcp
* subroutine used by the pc2d program.
* Jul 9 2003 JB - Fixed a problem with handling Level 1 labels
* in a Level 2 image.
* Jan 11 2010 JB - The both.level element is not being set by
* calls to lev1_init or lev2_init. This was fixed by
* using the spi_level value in place of it.
*
*_END
************************************************************************/
{
BOOLEAN levels,frame;
INT4 level,spi_level;
INT4 lzin,llog,lnote;
INT4 length,icount,ret;
INT4 naxes,order,core_size[3],suf_size[3];
INT4 issi,isli;
INT4 iesi,ieli;
INT4 iflag,jflag;
INT4 lpos;
INT4 icard;
INT4 ccont[3];
INT4 scont[3];
INT4 dflag;
INT4 icountx,icounty;
INT4 levgroups;
INT4 found;
INT4 i,j;
int (*p)();
FLOAT4 dzxm,dzym;
FLOAT4 z01,z02,z03,z04;
FLOAT4 xsinci,xlinci;
FLOAT4 dzdip;
FLOAT4 arg;
FLOAT4 si0;
FLOAT4 se0;
FLOAT4 bclipin;
FLOAT4 db1,db2;
FLOAT4 cardaz;
FLOAT4 delaza,delazb;
FLOAT4 xcenter,ycenter;
FLOAT4 clat,clon;
FLOAT8 azi;
FLOAT8 lat,lon;
FLOAT8 inc,emi,pha;
FLOAT8 scaz,sunaz;
FLOAT8 ssclat,ssclon;
FLOAT8 ssunlat,ssunlon;
FLOAT8 radii[3];
FLOAT8 slantrange;
CHAR axis_name[3][MAXLITLEN+1];
CHAR item_type[MAXLITLEN+1];
CHAR prmname[30];
CHAR tmpstr[256];
CHAR planet[11];
CHAR sfrom[136];
CHAR scan_sc[NUMOFSCAN][LEV_SPACECRAFT_NAME_SIZE] =
{"MARS_GLOBAL_SURVEYOR",
"MARS_GLOBAL_SURVEYOR",
"MARS_ODYSSEY"};
CHAR scan_inst[NUMOFSCAN][LEV_INSTRUMENT_NAME_SIZE] =
{"MOC_NA_A",
"MOC_WA_A",
"THEMIS_IR"};
CHAR frame_sc[NUMOFFRAME][LEV_SPACECRAFT_NAME_SIZE] =
{"GALILEO_1",
"VIKING_ORBITER_1",
"VIKING_ORBITER_1",
"VIKING_ORBITER_2",
"VIKING_ORBITER_2",
"MARS_ODYSSEY",
"VOYAGER_1",
"VOYAGER_1",
"VOYAGER_2",
"VOYAGER_2"};
/* "CASSINI",
"LUNAR_ORBITER_4"};*/
CHAR frame_inst[NUMOFFRAME][LEV_INSTRUMENT_NAME_SIZE] =
{"SSI",
"VISUAL_IMAGING_SUBSYSTEM_CAMERA_A",
"VISUAL_IMAGING_SUBSYSTEM_CAMERA_B",
"VISUAL_IMAGING_SUBSYSTEM_CAMERA_A",
"VISUAL_IMAGING_SUBSYSTEM_CAMERA_B",
"THEMIS_VIS",
"WA",
"NA",
"WA",
"NA"};
/* "VIMS",
"24_INCH_FOCAL_LENGTH_CAMERA"};*/
FLOAT8 frame_flinpix[NUMOFFRAME] =
{98455.811,
40323.83,
40328.08,
40341.85,
40298.585,
22222.222,
16989.138,
127248.26,
17209.598,
127528.17};
LevData data,tmpdata;
Lev both;
FLOAT8 sampres,lineres,azres,sres;
INT4 PcsiFirstGuess (void);
CHAR obj_name[5]="QUBE";
CHAR grp_name[1] = "";
CHAR grp2_name[21]="IMAGE_MAP_PROJECTION";
INT4 mulpht;
INT4 whcnt,hg1cnt,hg2cnt,hhcnt,b0cnt,thetacnt,bhcnt,chcnt,kcnt,lcnt;
/******************************************************************************
* Get input and output file parameters from TAE
******************************************************************************/
strcpy(prmname,"TO");
u_get_file_parm("TO",1,(CHAR *)topofile,sizeof(topofile),&length,&icount,&ret);
if (ret) goto user_parameter_error;
if (icount <= 0 || strlen(topofile) == 0) goto to_file_error;
(void) u_strtrim(topofile,topofile);
if (strlen(topofile) == 0) goto to_file_error;
if (u_file_exist(topofile)) goto to_file_exist;
if (strncmp(&(topofile[strlen(topofile)-1]),"/",1) == 0) goto to_file_error;
(void) u_ver_flspec(topofile,"cub",topofile,sizeof(topofile),&ret);
if (ret) goto to_file_invalid;
strcpy(prmname,"ZOUT");
u_get_file_parm("ZOUT",1,(CHAR *)zoutfile,sizeof(zoutfile),&length,&icount,&ret);
if (ret) goto user_parameter_error;
if (icount <= 0 || strlen(zoutfile) == 0) {
dozout = FALSE;
} else {
(void) u_strtrim(zoutfile,zoutfile);
(void) u_ver_flspec(zoutfile,"cub",zoutfile,sizeof(zoutfile),&ret);
if (ret) goto flspec_error;
if (u_file_exist(zoutfile)) {
if (remove(zoutfile)) goto zout_remove_error;
}
dozout = TRUE;
}
(void) xctae_files(zin,sizeof(zin),&lzin,&usezin,&usedatum,logfile,
sizeof(logfile),&llog,note,sizeof(note),&lnote);
/******************************************************************************
* Get subcube specifiers
******************************************************************************/
strcpy(prmname,"SFROM");
u_get_str_parm("SFROM",1,(CHAR *)sfrom,sizeof(sfrom),&length,&icount,&ret);
if (ret) goto user_parameter_error;
if (icount <= 0) (void) strcpy(sfrom," ");
/******************************************************************************
* Open the FROM file and get information
******************************************************************************/
strcpy(prmname,"FROM");
u_get_file_parm("FROM",1,(CHAR *)from,sizeof(from),&length,&icount,&ret);
if (ret) goto user_parameter_error;
q_open(&fid,0,from,READ_ONLY,0,"",0,0,1,0,&ret);
if (ret) goto cube_open_error;
q_check_std(fid,&ret);
if (ret) goto invalid_cube;
q_get_sys_keys(fid,1,&naxes,&order,core_size,suf_size,
(char *)axis_name,sizeof(axis_name[0]),&image_bytes,
item_type,sizeof(item_type),image_scale,&ret);
if (ret) goto bad_sys_keys;
/******************************************************************************
* Parse the SFROM subcube specification and apply to the FROM file and
* then read system keywords again to reflect the virtual cube
******************************************************************************/
(void) u_parse_isub(fid,sfrom,1,core_size,suf_size,ccont,scont,&ret);
if (ret) goto parse_sfrom_error;
(void) q_set_virtual(fid,&ret);
if (ret) goto sfrom_error;
q_get_sys_keys(fid,1,&naxes,&order,core_size,suf_size,
(char *)axis_name,sizeof(axis_name[0]),&image_bytes,
item_type,sizeof(item_type),image_scale,&ret);
if (ret) goto bad_sys_keys;
ins = core_size[0];
inl = core_size[1];
/******************************************************************************
* Set the variables describing the FROM subcube
******************************************************************************/
u_get_isub_desc(fid,"SAMPLE",&issi,&iesi,&xsinci,&dflag,&ret);
if (ret) goto bad_subsamp;
if (dflag != 1) goto bad_subsamp;
u_get_isub_desc(fid,"LINE",&isli,&ieli,&xlinci,&dflag,&ret);
if (ret) goto bad_subline;
if (dflag != 1) goto bad_subline;
if (xsinci != xlinci) goto inc_error; /* Need to be equal or
map scale will not be accurate */
/******************************************************************************
* Set center of virtual subcube
******************************************************************************/
xcenter = 0.5*(ins+1);
ycenter = 0.5*(inl+1);
/******************************************************************************
* Get photoclinometry parameters from TAE
******************************************************************************/
strcpy(prmname,"MAXMEM");
u_get_int_parm("MAXMEM",1,&C_MEM3->nmax,&icount,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"DNATM");
u_get_real_parm("DNATM",1,1,6,&C_DNORM->dnatm,&icount,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"DNDATUM");
u_get_real_parm("DNDATUM",1,1,6,&dndatum,&icount,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"DATUMTYP");
u_get_int_parm("DATUMTYP",1,&C_DATUM2->datumtyp,&icount,&ret);
if (ret) goto user_parameter_error;
nucenter = FALSE; /* Reset to True if X,Y inputs given */
strcpy(prmname,"X");
u_get_dbl_parm("X",1,1,6,&x,&icountx,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"Y");
u_get_dbl_parm("Y",1,1,6,&y,&icounty,&ret);
if (ret) goto user_parameter_error;
if ((icountx >= 1) && (icounty >= 1)) nucenter = TRUE;
strcpy(prmname,"DISTORTD");
u_get_str_parm("DISTORTD",1,(CHAR *)tmpstr,sizeof(tmpstr),&length,&icount,&ret);
if (ret) goto user_parameter_error;
distortd = TRUE;
if (strncmp(tmpstr,"NO",1) == 0)
distortd = FALSE;
strcpy(prmname,"METHOD");
u_get_str_parm("METHOD",1,(CHAR *)method,sizeof(method),&length,&icount,&ret);
if (ret) goto user_parameter_error;
if (strncmp(method,"SOR",3) == 0) {
sordir = 0;
} else if (strncmp(method,"DIR",3) == 0) {
sordir = 1;
} else {
sordir = 2;
}
strcpy(prmname,"PHOFUNC");
u_get_str_parm("PHOFUNC",1,(CHAR *)C_PPARS4->phofunc,sizeof(C_PPARS4->phofunc),
&length,&icount,&ret);
if (ret) goto user_parameter_error;
C_PPARS1->piopt = 0;
if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0)
C_PPARS1->piopt = 1;
if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0)
C_PPARS1->piopt = 1;
C_PPARS3->hapke = FALSE;
if (strncmp(C_PPARS4->phofunc,"H",1) == 0)
C_PPARS3->hapke = TRUE;
strcpy(prmname,"WH");
u_get_real_parm("WH",1,1,6,&C_PPARS5->pwh,&whcnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"HG1");
u_get_real_parm("HG1",1,1,6,&C_PPARS5->phg1,&hg1cnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"HG2");
u_get_real_parm("HG2",1,1,6,&C_PPARS5->phg2,&hg2cnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"HH");
u_get_real_parm("HH",1,1,6,&C_PPARS5->phh,&hhcnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"B0");
u_get_real_parm("B0",1,1,6,&C_PPARS5->pb0,&b0cnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"THETA");
u_get_real_parm("THETA",1,1,6,&C_PPARS5->ptheta,&thetacnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"BH");
u_get_real_parm("BH",1,1,6,&C_PPARS5->pbh,&bhcnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"CH");
u_get_real_parm("CH",1,1,6,&C_PPARS5->pch,&chcnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"K");
u_get_real_parm("K",1,1,6,&C_PPARS2->pex,&kcnt,&ret);
if (ret) goto user_parameter_error;
strcpy(prmname,"L");
u_get_real_parm("L",1,1,6,&C_PPARS2->pl2,&lcnt,&ret);
if (ret) goto user_parameter_error;
if (strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0) {
if (whcnt < 1 || thetacnt < 1 || b0cnt < 1 || hhcnt < 1 ||
hg1cnt < 1 || hg2cnt < 1) goto haphen_error;
} else if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0) {
if (whcnt < 1 || thetacnt < 1 || b0cnt < 1 || hhcnt < 1 ||
bhcnt < 1 || chcnt < 1) goto hapleg_error;
} else if (strncmp(C_PPARS4->phofunc,"HAPH_S",6) == 0) {
if (whcnt < 1 || hg1cnt < 1 || hg2cnt < 1) goto haph_s_error;
} else if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0) {
if (whcnt < 1 || bhcnt < 1 || chcnt < 1) goto hapl_s_error;
} else if (strncmp(C_PPARS4->phofunc,"LAMBER",6) == 0) {
C_PPARS2->pex = 1.0;
C_PPARS2->pl2 = 0.0;
} else if (strncmp(C_PPARS4->phofunc,"LOMSEL",6) == 0) {
C_PPARS2->pex = 1.0;
C_PPARS2->pl2 = 1.0;
} else if (strncmp(C_PPARS4->phofunc,"MIN",3) == 0) {
C_PPARS2->pl2 = 0.0;
if (kcnt < 1) goto min_error;
} else if (strncmp(C_PPARS4->phofunc,"LUNLAM",6) == 0) {
C_PPARS2->pex = 1.0;
if (lcnt < 1) goto lunlam_error;
}
/******************************************************************************
* Evaluate default (center) location in image if needed. This default is in
* ALL cases now the center of the virtual subcube, transferred to full cube.
******************************************************************************/
if (!nucenter) {
x = (FLOAT8) (issi) + (xcenter-1.0) * xsinci;
y = (FLOAT8) (isli) + (ycenter-1.0) * xlinci;
}
/******************************************************************************
* Check for the 3 levels Level 1 keyword groups (ISIS_INSTRUMENT,
* ISIS_GEOMETRY, ISIS_TARGET)
******************************************************************************/
levgroups = 0;
(void) p_check_key(fid,"QUBE","ISIS_INSTRUMENT","",&icount,&ret);
if (!ret) levgroups = levgroups + 1;
(void) p_check_key(fid,"QUBE","ISIS_GEOMETRY","",&icount,&ret);
if (!ret) levgroups = levgroups + 1;
(void) p_check_key(fid,"QUBE","ISIS_TARGET","",&icount,&ret);
if (!ret) levgroups = levgroups + 1;
/******************************************************************************
* All 3 levels Level 1 keyword groups are missing. This is either a
* pre-levels Level 1 file or a Level 2 file that has had the groups erased
* from the labels (or, under the old system, never got them). If the file
* contains either ISIS_MOSAIC or IMAGE_MAP_PROJECTION keyword groups, then
* it is not a Level 1 image and cannot be used. Otherwise, try using old
* SPICE routines.
******************************************************************************/
if (levgroups == 0) {
(void) p_check_key(fid,"QUBE","ISIS_MOSAIC","",&icount,&ret);
if (!ret) {
/* Level 2 mosaic without required groups */
goto mosaic_group_error;
} else {
(void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","",&icount,&ret);
if (!ret) {
(void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION",
"MAP_PROJECTION_TYPE",&icount,&ret);
if (!ret) {
/* Level 2 image without required groups */
goto level2_group_error;
}
}
}
/* Pre-levels Level 1 image */
iflag = 0;
jflag = 1;
if (!distortd) jflag = 0;
spi_phosun(fid,obj_name,sizeof(obj_name),grp_name,sizeof(grp_name),
grp2_name,sizeof(grp2_name),jflag,0,&iflag,
&y,&x,&lat,&lon,&emi,&inc,&pha,&ssclat,&ssclon,
&scaz,&ssunlat,&ssunlon,&sunaz,&aznor,&res,&ret);
if (ret) goto spi_phosun_error;
/******************************************************************************
* If we got to here without an error, this is a valid pre-levels Level 1
* file.
******************************************************************************/
levels = FALSE;
level = 1;
/******************************************************************************
* All pre-levels instruments are framing cameras.
******************************************************************************/
frame = TRUE;
/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the framing camera case: Z axis is viewing direction, so
* emission angle is used to calculate datum derivatives rather than emission
* vector. Also calculate the scaling variable, cosemi, to be used for
* line-of-sight to vertical on output and vertical to line-of-sight on
* input.
******************************************************************************/
res = res * xsinci; /* xsinci=xlinci guaranteed */
sres = res;
C_AEPAR1->aspect = 1.0;
C_PSNPAR->phase[0] = pha;
clinc = pha;
azinc = sunaz;
clemi = 0.0;
azemi = 0.0;
dip = emi;
az = scaz + 180.0;
cosemi = cos(emi*L_DEG2RAD);
/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
if (C_DATUM2->datumtyp == 2) {
spi_lbplan(fid,"QUBE","","IMAGE_MAP_PROJECTION",planet,
sizeof(planet),radii,&lpos,&ret);
if (ret) goto spi_lbplan_error;
if (radii[1] == 0.0) radii[1] = radii[0];
if (radii[2] == 0.0) radii[2] = radii[0];
radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
radii[2]*radii[2])/3.0);
C_DATUM1->r0 = radius/res; /* Planet radius in pixels */
/******************************************************************************
* Obtain planet center in full frame and convert to subframe. xs0 and yl0 are
* with respect to the pixels of the subcube; xs0f and yl0f are computed by
* geomset at each grid resolution and refer to coordinates equal to indices
* in the DEM.
******************************************************************************/
spi_findlin(fid,(FLOAT4) ssclat,(FLOAT4) ssclon,
&C_DATUM1->xs0,&C_DATUM1->yl0,&ret);
if (ret) goto spi_findlin_error;
C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
+ 1.0;
C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
+ 1.0;
/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 -
pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
pow((ycenter-C_DATUM1->yl0),2.0));
}
/******************************************************************************
* All 3 levels Level 1 keyword groups were found. This is a levels file,
* either Level 1 or Level 2 with the Level 1 groups included.
******************************************************************************/
} else if (levgroups == 3) {
levels = TRUE;
spi_level = lev_find_level(fid);
if (spi_level == 2) {
if (lev2_init_from_labels(fid,&both.map)) goto lev_init_error;
if (lev1_init(fid,&both.spi,0)) goto lev_init_error;
} else {
if (lev1_init(fid,&both.spi,0)) goto lev_init_error;
}
/* if (lev_init(fid,&both)) goto lev_init_error;*/
/******************************************************************************
* If input file is level 2 (map projected), then get the center latitude
* and center longitude.
******************************************************************************/
if (spi_level > 1 && strcmp(both.map.proj.name,"ORTHOGRAPHIC") == 0) {
(void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LATITUDE",
&icount,&ret);
if (ret) goto clat_error;
icount = 1;
p_get_real_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LATITUDE",
2,&icount,&clat,&ret);
if (ret) goto clat_error;
(void) p_check_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LONGITUDE",
&icount,&ret);
if (ret) goto clon_error;
icount = 1;
p_get_real_key(fid,"QUBE","IMAGE_MAP_PROJECTION","CENTER_LONGITUDE",
2,&icount,&clon,&ret);
if (ret) goto clon_error;
}
/******************************************************************************
* This is a levels Level 1 image.
******************************************************************************/
if (spi_level == 1) {
level = 1;
data.samp = x;
data.line = y;
if (lev1_linesamp_to_latlon(&both.spi,&data)) goto ls_to_latlon;
/******************************************************************************
* Return emission, incidence, phase as emi, inc, pha respectively. These
* angles are evaluated at location x,y relative to the full image cube.
******************************************************************************/
lev1_calc_emission_angle(&data,&emi);
lev1_calc_incidence_angle(&data,&inc);
lev1_calc_phase_angle(&data,&pha);
/******************************************************************************
* Check spacecraft/instrument to determine if it's a scanner or a
* framing camera .
******************************************************************************/
found = -1;
for (icount=0; icount<NUMOFSCAN; icount++) {
if (strcmp(both.spi.inst.spacecraft,scan_sc[icount]) == 0
&& strcmp(both.spi.inst.name,scan_inst[icount]) == 0) {
frame = FALSE;
found = icount;
}
}
if (found == -1) {
for (icount=0; icount<NUMOFFRAME; icount++) {
if (strcmp(both.spi.inst.spacecraft,frame_sc[icount]) == 0
&& strcmp(both.spi.inst.name,frame_inst[icount]) == 0) {
frame = TRUE;
found = icount;
}
}
}
/* Invalid instrument */
if (found == -1) goto unknown_inst;
/******************************************************************************
* This is a scanner. Spherical datum model does not make sense.
******************************************************************************/
if (frame == FALSE) {
/******************************************************************************
* Return sample resolution, line resolution, spacecraft azimuth, sun azimuth
* as sampres, lineres, scaz, sunaz respectively. All of these values are
* evaluated at location x,y relative to the full image cube.
******************************************************************************/
if (lev1_calc_resolution(&both.spi,&data,&sampres,&lineres))
goto res_error;
if (sampres > lineres) azres = sampres;
else azres = lineres;
lev1_calc_spacecraft_azimuth(&both.spi,&data,azres,&scaz);
lev1_calc_sun_azimuth(&both.spi,&data,azres,&sunaz);
/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the scanner case: Z axis is local vertical, so
* emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for scanners).
******************************************************************************/
res = lineres*xlinci;
sres = sampres*xsinci;
C_AEPAR1->aspect = sampres/lineres;
C_PSNPAR->phase[0] = pha;
clinc = inc;
azinc = sunaz;
clemi = emi;
azemi = scaz;
dip = 0.0;
az = 0.0;
C_DATUM2->datumtyp = 1;
cosemi = 1.0;
/******************************************************************************
* This is a framing camera.
******************************************************************************/
} else {
lev1_calc_slant_distance(&data,&slantrange);
/******************************************************************************
* Return resolution, spacecraft azimuth, sun azimuth as res[index],
* scaz, sunaz respectively. All of these values are evaluated at
* location x,y relative to the full image cube.
******************************************************************************/
res = (slantrange/frame_flinpix[found])*xlinci;
sres = res;
azres = res;
lev1_calc_spacecraft_azimuth(&both.spi,&data,azres,&scaz);
lev1_calc_sun_azimuth(&both.spi,&data,azres,&sunaz);
/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the framing camera case: Z axis is viewing direction, so
* emission angle is used to calculate datum derivatives rather than
* emission vector. Also calculate the scaling variable, cosemi, to be used
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input.
******************************************************************************/
C_AEPAR1->aspect = 1.0;
C_PSNPAR->phase[0] = pha;
clinc = pha;
azinc = sunaz;
clemi = 0.0;
azemi = 0.0;
dip = emi;
az = scaz + 180.0;
cosemi = cos(emi*L_DEG2RAD);
/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
if (C_DATUM2->datumtyp == 2) {
radii[0] = both.spi.target.radii[0];
radii[1] = both.spi.target.radii[1];
radii[2] = both.spi.target.radii[2];
if (radii[1] == 0.0) radii[1] = radii[0];
if (radii[2] == 0.0) radii[2] = radii[0];
radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
radii[2]*radii[2])/3.0);
C_DATUM1->r0 = radius/res; /* Planet radius in pixels */
lev1_calc_subspace(&both.spi,&data,&tmpdata);
ssclat = tmpdata.lat;
ssclon = tmpdata.lon;
data.lat = ssclat;
data.lon = ssclon;
if (lev1_latlon_to_linesamp(&both.spi,&data)) goto latlon_to_ls;
C_DATUM1->xs0 = data.samp;
C_DATUM1->yl0 = data.line;
/******************************************************************************
* We have planet center in terms of full image cube, so convert to
* subcube.
******************************************************************************/
C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
+ 1.0;
C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
+ 1.0;
/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 -
pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
pow((ycenter-C_DATUM1->yl0),2.0));
}
}
/******************************************************************************
* This is a levels Level 2 image. Having two Level 2 files is permitted
* in software. The two image subcubes are the same size and they have
* the same projection parameters. It doesn't matter if the scalar datum
* variables are set twice from labels for both images; the results are
* guaranteed to be the same.
******************************************************************************/
} else {
level = 2;
/******************************************************************************
* Next, check the projection. Orthographic projection allows a set of
* images to be treated like one image. The coverage could be highly
* localized (setting projection center to subspacecraft point minimizes
* parallax) or hemispheric in scale. Spherical datum type is always used
* in this projection, with center at center of projection. Coordinate
* system has Z axis out of planet at center of projection. This point is
* known by lat and lon. First find its Level 1 coordinates so we can find
* ema, inc, phase here. NOTE that azimuths are returned but they are with
* respect to Level 1 space so they don't get used.
******************************************************************************/
if (strcmp(both.map.proj.name,"ORTHOGRAPHIC") == 0) {
data.lat = (FLOAT8) clat;
data.lon = (FLOAT8) clon;
if (lev1_latlon_to_linesamp(&both.spi,&data))
goto latlon_to_ls;
lev1_calc_emission_angle(&data,&emi);
lev1_calc_incidence_angle(&data,&inc);
lev1_calc_phase_angle(&data,&pha);
/******************************************************************************
* Now find the Level 2 coordinates of the center so we can find the
* azimuths. This routine gives line and sample of a given lat,lon in
* terms of the full image. Will later calculate position relative to
* subcube.
******************************************************************************/
if (lev2_latlon_to_linesamp(&both.map,&data))
goto latlon_to_ls;
C_DATUM1->xs0 = data.samp;
C_DATUM1->yl0 = data.line;
/******************************************************************************
* Find azimuth to sun and spacecraft in Level 2.
******************************************************************************/
/* Convert mapscale to units of resolution */
res = both.map.proj.kmperpix * xlinci;
sres = res;
azres = res;
tmpdata = data;
lev2_calc_spacecraft_azimuth(&both.spi,&data,
&both.map,&tmpdata,azres,&scaz);
tmpdata = data;
lev2_calc_sun_azimuth(&both.spi,&data,
&both.map,&tmpdata,azres,&sunaz);
/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the map projected case: Z axis is the local vertical,
* so emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for map-projected images).
******************************************************************************/
C_AEPAR1->aspect = 1.0;
C_PSNPAR->phase[0] = pha;
clinc = inc;
azinc = sunaz;
clemi = emi;
azemi = scaz;
C_DATUM2->datumtyp = 2;
cosemi = 1.0;
/******************************************************************************
* Obtain parameters needed for spherical datum
******************************************************************************/
radii[0] = both.spi.target.radii[0];
radii[1] = both.spi.target.radii[1];
radii[2] = both.spi.target.radii[2];
if (radii[1] == 0.0) radii[1] = radii[0];
if (radii[2] == 0.0) radii[2] = radii[0];
radius = sqrt((radii[0]*radii[0]+radii[1]*radii[1]+
radii[2]*radii[2])/3.0);
C_DATUM1->r0 = radius/res; /* Planet radius in pixels */
/******************************************************************************
* Call above gave planet center in full image cube, so convert to subcube
******************************************************************************/
C_DATUM1->xs0=(C_DATUM1->xs0 - (FLOAT4) issi)/xsinci
+ 1.0;
C_DATUM1->yl0=(C_DATUM1->yl0 - (FLOAT4) isli)/xlinci
+ 1.0;
/******************************************************************************
* Must calculate z00s for sphere, putting middle of image at height 0
******************************************************************************/
C_DATUM1->z00s = -sqrt(C_DATUM1->r0*C_DATUM1->r0 -
pow((xcenter-C_DATUM1->xs0)*C_AEPAR1->aspect,2.0) -
pow((ycenter-C_DATUM1->yl0),2.0));
/******************************************************************************
* Finally, calculate dip and azimuth of dip for sphere at reference
* point. Do this by calling datum to get the elevation differences across
* the pixel near the reference point (converted from full image cube to
* subcube coordinates.
******************************************************************************/
(void) geomset(1.0); /* Setup needed before datum */
i = (INT4) ((x - (FLOAT4) issi)/
xsinci + .5) + 1;
j = (INT4) ((y - (FLOAT4) isli)/
xlinci + .5) + 1;
(void) datum(i,j,4,&z01,&z02,&z03,&z04);
dzxm = 0.5*(-z01+z02+z03-z04);
dzym = 0.5*(-z01-z02+z03+z04);
dip = atan(sqrt((dzxm/C_AEPAR1->aspect)*(dzxm/C_AEPAR1->aspect)+
dzym*dzym))*L_RAD2DEG;
az = atan2(dzym*C_AEPAR1->aspect*C_AEPAR1->aspect,dzxm)*L_RAD2DEG;
/******************************************************************************
* Non-orthographic projections can be used to rectify scanner images.
* Intent is to represent a small region only, so datum type is planar and
* in fact has zero dip, similar to handling of Level 1 scanner images. No
* error checking is done to see if the projection has severe skew or other
* distortions that would make the calculation nonsensical.
*
* User input or default reference point location is in Level 2 space, so
* convert to Level 1 space using lat,lon coordinates.
******************************************************************************/
} else {
data.samp = x;
data.line = y;
if (lev2_linesamp_to_latlon(&both.map,&data)) goto ls_to_latlon;
if (lev2_latlon_to_linesamp(&both.map,&data)) goto latlon_to_ls;
tmpdata = data;
if (lev1_latlon_to_linesamp(&both.spi,&data))
goto latlon_to_ls;
else if (lev1_linesamp_to_latlon(&both.spi,&data))
goto ls_to_latlon;
/******************************************************************************
* Return emission, incidence, phase as emi, inc, pha respectively. These
* angles are evaluated at a location relative to the whole image.
******************************************************************************/
lev1_calc_emission_angle(&data,&emi);
lev1_calc_incidence_angle(&data,&inc);
lev1_calc_phase_angle(&data,&pha);
/******************************************************************************
* Get ground resolutions of map at the reference point.
******************************************************************************/
if (lev2_calc_map_resolution(&both.spi,&data,
&both.map,&tmpdata,&sampres,&lineres))
goto res_error;
/******************************************************************************
* Now, find azimuth to sun and spacecraft in Level 2.
******************************************************************************/
res = lineres*xlinci;
sres = sampres*xsinci;
azres = res;
lev2_calc_spacecraft_azimuth(&both.spi,&data,
&both.map,&tmpdata,azres,&scaz);
lev2_calc_sun_azimuth(&both.spi,&data,
&both.map,&tmpdata,azres,&sunaz);
/******************************************************************************
* Now assign emi, inc, pha, scaz, sunaz to variables used below as
* appropriate for the map projected case: Z axis is the local vertical,
* so emission angle is used to calculate emission vector rather than datum
* derivatives. Also calculate the scaling variable, cosemi, to be used
* for line-of-sight to vertical on output and vertical to line-of-sight on
* input (cosemi=1.0 for map-projected images).
******************************************************************************/
C_AEPAR1->aspect = sampres/lineres;
C_PSNPAR->phase[0] = pha;
clinc = inc;
azinc = sunaz;
clemi = emi;
azemi = scaz;
dip = 0.0;
az = 0.0;
C_DATUM2->datumtyp = 1;
cosemi = 1.0;
}
}
/******************************************************************************
* Only some of the needed levels Level 1 keyword groups found. Image is
* invalid.
******************************************************************************/
} else {
goto labels_not_complete;
}
/******************************************************************************
* Now proceed to calculate stored values with label information. Still
* need to calculate derivatives of the plane datum.
******************************************************************************/
dzdip = tan(dip*L_DEG2RAD)/sqrt(pow(cos(az*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
+pow(sin(az*L_DEG2RAD),2.0));
C_DATUM1->dzx0 = -cos(az*L_DEG2RAD)*dzdip*C_AEPAR1->aspect*C_AEPAR1->aspect;
C_DATUM1->dzy0 = -sin(az*L_DEG2RAD)*dzdip;
C_DATUM1->z00 = -(C_DATUM1->dzx0*xcenter+C_DATUM1->dzy0*ycenter);
C_DATUM1->dz10 = -(C_DATUM1->dzx0+C_DATUM1->dzy0);
C_DATUM1->dz20 = -(C_DATUM1->dzx0-C_DATUM1->dzy0);
(void) geomset(1.0);
scale = 1000.0*res;
C_PGEOM->ci[0] = cos(clinc*L_DEG2RAD)*SQ2;
si0 = sin(clinc*L_DEG2RAD)/
sqrt(pow(cos(azinc*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
+pow(sin(azinc*L_DEG2RAD),2.0));
C_PGEOM->si1[0] = si0*cos((45.0-azinc)*L_DEG2RAD);
C_PGEOM->si2[0] = si0*sin((45.0-azinc)*L_DEG2RAD);
C_PGEOM->ce[0] = cos(clemi*L_DEG2RAD)*SQ2;
se0 = sin(clemi*L_DEG2RAD)/
sqrt(pow(cos(azemi*L_DEG2RAD)*C_AEPAR1->aspect,2.0)
+pow(sin(azemi*L_DEG2RAD),2.0));
C_PGEOM->se1[0] = se0*cos((45.0-azemi)*L_DEG2RAD);
C_PGEOM->se2[0] = se0*sin((45.0-azemi)*L_DEG2RAD);
/******************************************************************************
* Calculations above are purely GEOMETRIC; those below are PHOTOMETRIC,
* i.e., depending on surface properties and description thereof.
******************************************************************************/
if (C_PPARS3->hapke) {
if (strncmp(C_PPARS4->phofunc,"HAPH_S",6) == 0) {
C_PPARS2->falpha[0] = (1.0-C_PPARS5->phg2)*
(1.0-C_PPARS5->phg1*C_PPARS5->phg1)/
(pow(1.0+C_PPARS5->phg1*C_PPARS5->phg1+2.0*
C_PPARS5->phg1*cos(C_PSNPAR->phase[0]*L_DEG2RAD),1.5))+
C_PPARS5->phg2*(1.0-C_PPARS5->phg1*C_PPARS5->phg1)/
(pow(1.0+C_PPARS5->phg1*C_PPARS5->phg1-2.0*C_PPARS5->phg1*
cos(C_PSNPAR->phase[0]*L_DEG2RAD),1.5))-1.0;
} else if (strncmp(C_PPARS4->phofunc,"HAPL_S",6) == 0) {
C_PPARS2->falpha[0] = 1.0+C_PPARS5->pbh*cos(C_PSNPAR->phase[0]*
L_DEG2RAD)+C_PPARS5->pch*(1.5*cos(C_PSNPAR->phase[0]*L_DEG2RAD)+
0.5);
}
C_PPARS2->twogam = 2.0*sqrt(1.0-C_PPARS5->pwh);
if (strncmp(C_PPARS4->phofunc,"HAPLEG",6) == 0 ||
strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0) {
mulpht = 62;
if (strncmp(C_PPARS4->phofunc,"HAPHEN",6) == 0)
mulpht = mulpht + 23;
if (C_PPARS5->phh != 0.0) mulpht = mulpht + 3;
if (C_PPARS5->ptheta != 0.0) mulpht = mulpht + 127;
C_PPARS1->mulps = (16+mulpht)*2;
C_PPARS1->mulpsp = (42+mulpht*4)*2;
} else {
C_PPARS1->mulps = 15;
C_PPARS1->mulpsp = 15;
}
} else {
C_PPARS2->pex1 = C_PPARS2->pex-1.0;
C_PPARS2->pl2 = C_PPARS2->pl2+C_PPARS2->pl2;
C_PPARS2->pl1 = 1.0-.5*C_PPARS2->pl2;
if (C_PPARS2->pl2 == 0.0) {
if (C_PPARS2->pex == 1.0) {
C_PPARS1->mulps = 0;
C_PPARS1->mulpsp = 0;
} else {
C_PPARS1->mulps = 15;
C_PPARS1->mulpsp = 4;
}
} else if (C_PPARS2->pl2 == 2.0) {
C_PPARS1->mulps = 3;
C_PPARS1->mulpsp = 4;
} else {
if (C_PPARS2->pex == 1.0) {
C_PPARS1->mulps = 5;
C_PPARS1->mulpsp = 7;
} else {
C_PPARS1->mulps = 21;
C_PPARS1->mulpsp = 14;
}
}
}
/******************************************************************************
* End of material-dependent code. Find photometric function
* normalization, etc. (AEPARs)
******************************************************************************/
bclipin = 1.0e30;
C_AEPAR1->bmax = 1.0e30;
i = (INT4) (xcenter+.5);
j = (INT4) (ycenter+.5);
(void) pbder(i,j,0.0,0.0,&C_AEPAR1->bnorm,&db1,&db2,1);
(void) findbmax();
if (C_AEPAR1->bmax < C_AEPAR1->bnorm*bclipin) C_AEPAR1->bclip = C_AEPAR1->bmax;
else C_AEPAR1->bclip = C_AEPAR1->bnorm*bclipin;
C_AEPAR2->flop = FALSE;
C_AEPAR2->logimg = FALSE;
/******************************************************************************
* Find azimuth of characteristic strips. This is calculated in terms of
* true Cartesian azimuth, whereas the other azimuths are those on the
* pixel grid, which may not have a unit aspect ratio. We will output the
* azimuth of characteristics transformed to a grid azimuth. Starting point
* for the search is the (numerator) sun azimuth, transformed to Cartesian.
******************************************************************************/
azi = atan2(sin(azinc*L_DEG2RAD)/C_AEPAR1->aspect,cos(azinc*L_DEG2RAD))*
L_RAD2DEG;
if (azi >= 0) icount = (INT4) (azi/90.0+.5);
else icount = (INT4) (azi/90.0-.5);
icard = icount-4*(icount/4);
cardaz = 90.0*((FLOAT4) icard);
delaza = cardaz-azi-45.0;
delazb = cardaz-azi+45.0;
p = ddberr;
C_AEPAR1->delaz = zbrent(p,delaza,delazb,1.0e-6);
if (ins > inl) arg = ins;
else arg = inl;
if (fabs(sin(C_AEPAR1->delaz*L_DEG2RAD)*cos(C_AEPAR1->delaz*L_DEG2RAD))*
arg <= 0.5) C_AEPAR1->delaz = 0.0;
while (icard < 0) {
icard = icard+4;
cardaz = cardaz+360.;
}
C_AEPAR1->charaz = cardaz-C_AEPAR1->delaz;
if (C_AEPAR1->dbrat*(0.5-(icard%2)) < 0.0) C_AEPAR1->charaz =
C_AEPAR1->charaz+90.;
C_AEPAR1->charaz = C_AEPAR1->charaz-360.0*((INT4) (C_AEPAR1->charaz/360.0));
if (C_AEPAR1->charaz > 180.) C_AEPAR1->charaz = C_AEPAR1->charaz-360.;
if (C_AEPAR1->charaz >= 0.) arg = 180.;
else arg = -180.;
if (fabs(C_AEPAR1->charaz) > 90.) C_AEPAR1->charaz = C_AEPAR1->charaz-arg;
/******************************************************************************
* We use Cartesian charaz but print grid-relative charazgrid for the
* user. No longer need to use ioct in main program to control geometric
* transformation of images on input. Instead, set ioct1=1 and use it in
* calls to pblinein1, etc. By setting the octant of the characteristics
* ioct here and using it to control the SOR sweep direction, we get
* optimal results for any charaz. ioct is passed via common.
******************************************************************************/
charazgrid = atan2(sin(C_AEPAR1->charaz*L_DEG2RAD)*C_AEPAR1->aspect,
cos(C_AEPAR1->charaz*L_DEG2RAD))*L_RAD2DEG;
if ((-90.0 <= C_AEPAR1->charaz) && (C_AEPAR1->charaz < -45.0))
C_AEPAR3->ioct = -2;
else if ((-45.0 <= C_AEPAR1->charaz) && (C_AEPAR1->charaz < 0.0))
C_AEPAR3->ioct = -1;
else if ((45.0 < C_AEPAR1->charaz) && (C_AEPAR1->charaz <= 90.0))
C_AEPAR3->ioct = 2;
else
C_AEPAR3->ioct = 1;
sprintf(C_MSG->pcpmsg,"X= %f",x);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Y= %f",y);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"RADIUS= %f",radius);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Dip of datum= %f",dip);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Azimuth of dip= %f",az);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Azimuth of characteristics= %f",charazgrid);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Resolution (km/pix) = %f",sres);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Aspect = %f",1.0/C_AEPAR1->aspect);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Emission angle = %f",emi);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Incidence angle = %f",inc);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Phase angle = %f",pha);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Spacecraft azimuth = %f",scaz);
u_write_msg(3,C_MSG->pcpmsg);
sprintf(C_MSG->pcpmsg,"Sun azimuth = %f",sunaz);
u_write_msg(3,C_MSG->pcpmsg);
/******************************************************************************
* Finish getting TAE parameters
******************************************************************************/
(void) pcsi_ipars(&nsmooth);
(void) write2log(1,ins,inl,note,lnote,from,clinc,
azinc,clemi,azemi,sres,
dip,az,charazgrid,radius,dndatum,
1.0/C_AEPAR1->aspect);
if (PcsiFirstGuess()) goto first_guess_error;
(void) write2log(2,ins,inl,note,lnote,from,clinc,
azinc,clemi,azemi,sres,
dip,az,charazgrid,radius,dndatum,
1.0/C_AEPAR1->aspect);
return(0);
/*****************************************************
Error section
*****************************************************/
user_parameter_error:
sprintf(C_MSG->pcpmsg,"Error retrieving TAE parameter %s",prmname);
u_error("INITPCSI-TAEERR",C_MSG->pcpmsg,-1,1);
return(-1);
spi_phosun_error:
sprintf(C_MSG->pcpmsg,
"Unable to get spacecraft and sun position information for %s",from);
u_error("INITPCSI-SPIPHOSUN",C_MSG->pcpmsg,-2,1);
return(-2);
spi_findlin_error:
sprintf(C_MSG->pcpmsg,"Unable to convert lat,lon to line,samp for %s",
from);
u_error("INITPCSI-SPIFINDLIN",C_MSG->pcpmsg,-3,1);
return(-3);
spi_lbplan_error:
sprintf(C_MSG->pcpmsg,"Unable to read planet radii and lpos from %s",
from);
u_error("INITPCSI-SPILBP",C_MSG->pcpmsg,-4,1);
return(-4);
first_guess_error:
sprintf(C_MSG->pcpmsg,"Unable to generate an initial solution for topography");
u_error("INITPCSI-SOLNERR",C_MSG->pcpmsg,-5,1);
return(-5);
lev_init_error:
sprintf(C_MSG->pcpmsg,"Unable to initialize input file %s",from);
u_error("INITPCSI-LEVINIT",C_MSG->pcpmsg,-6,1);
return(-6);
ls_to_latlon:
sprintf(C_MSG->pcpmsg,"Unable to convert line,sample to lat,lon for %s",
from);
u_error("INITPCSI-LSTOLATLON",C_MSG->pcpmsg,-7,1);
return(-7);
latlon_to_ls:
sprintf(C_MSG->pcpmsg,"Unable to convert lat,lon to line,sample for %s",
from);
u_error("INITPCSI-LATLONTOLS",C_MSG->pcpmsg,-8,1);
return(-8);
res_error:
sprintf(C_MSG->pcpmsg,
"Unable to determine line,sample resolution for %s",from);
u_error("INITPCSI-LSRES",C_MSG->pcpmsg,-9,1);
return(-9);
bad_subsamp:
sprintf(C_MSG->pcpmsg,"Error getting sub-sample information for %s",
from);
u_error("INITPCSI-SUBSAMP",C_MSG->pcpmsg,-10,1);
return(-10);
bad_subline:
sprintf(C_MSG->pcpmsg,"Error getting sub-line information for %s",
from);
u_error("INITPCSI-SUBLINE",C_MSG->pcpmsg,-11,1);
return(-11);
inc_error:
sprintf(C_MSG->pcpmsg,"Line and sample increment of file %s are not equal",
from);
u_error("INITPCSI-LINCSINC",C_MSG->pcpmsg,-12,1);
return(-12);
mosaic_group_error:
sprintf(C_MSG->pcpmsg,
"Mosaicked images must have all of groups: ISIS_INSTRUMENT, ISIS_GEOMETRY, ISIS_TARGET");
u_error("INITPCSI-MOSGRP",C_MSG->pcpmsg,-13,1);
return(-13);
level2_group_error:
sprintf(C_MSG->pcpmsg,
"Level 2 images must have all of groups: ISIS_INSTRUMENT, ISIS_GEOMETRY, ISIS_TARGET");
u_error("INITPCSI-LEV2GRP",C_MSG->pcpmsg,-14,1);
return(-14);
unknown_inst:
sprintf(C_MSG->pcpmsg,
"Unknown spacecraft/instrument found in %s",from);
u_error("INITPCSI-BILEV1",C_MSG->pcpmsg,-15,1);
return(-15);
labels_not_complete:
sprintf(C_MSG->pcpmsg,
"Necessary Level 1 keyword groups not found in %s",from);
u_error("INITPCSI-INCLAB",C_MSG->pcpmsg,-16,1);
return(-16);
haphen_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function HAPHEN: wh,theta,b0,hh,hg1,hg2");
u_error("INITPCSI-HAPHEN",C_MSG->pcpmsg,-17,1);
return(-17);
hapleg_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function HAPLEG: wh,theta,b0,hh,bh,ch");
u_error("INITPCSI-HAPLEG",C_MSG->pcpmsg,-18,1);
return(-18);
haph_s_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function HAPH_S: wh,hg1,hg2");
u_error("INITPCSI-HAPH_S",C_MSG->pcpmsg,-19,1);
return(-19);
hapl_s_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function HAPL_S: wh,bh,ch");
u_error("INITPCSI-HAPL_S",C_MSG->pcpmsg,-20,1);
return(-20);
min_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function MIN: k");
u_error("INITPCSI-MIN",C_MSG->pcpmsg,-21,1);
return(-21);
lunlam_error:
sprintf(C_MSG->pcpmsg,
"The following values must be provided for function LUNLAM: l");
u_error("INITPCSI-LUNLAM",C_MSG->pcpmsg,-22,1);
return(-22);
zout_remove_error:
sprintf(C_MSG->pcpmsg,
"Unable to delete pre-existing ZOUT file");
u_error("INITPCSI-ZOUTREM",C_MSG->pcpmsg,-23,1);
return(-23);
flspec_error:
sprintf(C_MSG->pcpmsg,
"The ZOUT filename is invalid");
u_error("INITPCSI-FLSPEC",C_MSG->pcpmsg,-24,1);
return(-24);
to_file_error:
sprintf(C_MSG->pcpmsg,
"TO file was not specified by user");
u_error("INITPCSI-TOERR",C_MSG->pcpmsg,-25,1);
return(-25);
to_file_exist:
sprintf(C_MSG->pcpmsg,
"TO file already exists");
u_error("INITPCSI-TOEXIST",C_MSG->pcpmsg,-26,1);
return(-26);
to_file_invalid:
sprintf(C_MSG->pcpmsg,
"TO file name invalid");
u_error("INITPCSI-TOINV",C_MSG->pcpmsg,-27,1);
return(-27);
cube_open_error:
sprintf(C_MSG->pcpmsg,"Error opening input cube file %s",from);
u_error("INITPCSI-OPENERR",C_MSG->pcpmsg,-28,1);
return(-28);
invalid_cube:
sprintf(C_MSG->pcpmsg,"%s is not a standard cube file",from);
u_error("INITPCSI-INVCUBE",C_MSG->pcpmsg,-29,1);
return(-29);
bad_sys_keys:
sprintf(C_MSG->pcpmsg,"Error obtaining system keywords from %s",from);
u_error("INITPCSI-SYSKEYS",C_MSG->pcpmsg,-30,1);
return(-30);
parse_sfrom_error:
sprintf(C_MSG->pcpmsg,"Unable to parse SFROM parameter %s for %s",sfrom,from);
u_error("INITPCSI-PRSSFROM",C_MSG->pcpmsg,-31,1);
return(-31);
sfrom_error:
sprintf(C_MSG->pcpmsg,"Unable to apply SFROM %s to input file %s",sfrom,from);
u_error("INITPCSI-SFROMERR",C_MSG->pcpmsg,-32,1);
return(-32);
clat_error:
sprintf(C_MSG->pcpmsg,"Unable to get clat from labels of %s",from);
u_error("INITPCSI-CLAT",C_MSG->pcpmsg,-33,1);
return(-33);
clon_error:
sprintf(C_MSG->pcpmsg,"Unable to get clon from labels of %s",from);
u_error("INITPCSI-CLON",C_MSG->pcpmsg,-34,1);
return(-34);
}
#define PROJ_PARMS__ \ double K, c, hlf_e, kR, cosp0, sinp0; #define PJ_LIB__ #include "projects.h" PROJ_HEAD(somerc, "Swiss. Obl. Mercator") "\n\tCyl, Ell\n\tFor CH1903"; #define EPS 1.e-10 #define NITER 6 FORWARD(e_forward); double phip, lamp, phipp, lampp, sp, cp; sp = P->e * sin(lp.phi); phip = 2.* atan( exp( P->c * ( log(tan(FORTPI + 0.5 * lp.phi)) - P->hlf_e * log((1. + sp)/(1. - sp))) + P->K)) - HALFPI; lamp = P->c * lp.lam; cp = cos(phip); phipp = aasin(P->ctx,P->cosp0 * sin(phip) - P->sinp0 * cp * cos(lamp)); lampp = aasin(P->ctx,cp * sin(lamp) / cos(phipp)); xy.x = P->kR * lampp; xy.y = P->kR * log(tan(FORTPI + 0.5 * phipp)); return (xy); } INVERSE(e_inverse); /* ellipsoid & spheroid */ double phip, lamp, phipp, lampp, cp, esp, con, delp; int i; phipp = 2. * (atan(exp(xy.y / P->kR)) - FORTPI); lampp = xy.x / P->kR; cp = cos(phipp); phip = aasin(P->ctx,P->cosp0 * sin(phipp) + P->sinp0 * cp * cos(lampp)); lamp = aasin(P->ctx,cp * sin(lampp) / cos(phip));
vector<float> CircularBuffer::creatHistogram(int partitionN){
vector<float> histogram(8 * partitionN);
float angle;
float mod;
int frameN = maxSize/partitionN;
for(int j = 0; j < partitionN; ++j){
for(int i = j * frameN; i < (j + 1) * frameN; ++i){
int pos = startPoint + i;
if(pos >= maxSize)
pos = pos - maxSize;
if(!zero(v[pos])){
mod = getMod(v[pos]);
if(v[pos].first == 0 && v[pos].second > 0){//positive y direction
histogram[2 + 8 * j] += mod;
continue;
}else if(v[pos].first == 0 && v[pos].second < 0){//negative y direction
histogram[6 + 8 * j] += mod;
continue;
}else if(v[pos].second == 0 && v[pos].first > 0){//positive x direction
histogram[0 + 8 * j] += mod;
continue;
}else{//negative x direction
histogram[4 + 8 * j] += mod;
continue;
}
angle = atan((v[pos].second*1.0)/v[pos].first);
if(v[pos].first > 0 && v[pos].second > 0){//positive x and positive y region
if(angle <= 3.1415926/4){
histogram[0 + 8 * j] += mod;
continue;
}
if(angle > 3.1415926/4 && angle < 3.1415926/2){
histogram[1 + 8 * j] += mod;
continue;
}
}else if(v[pos].first > 0 && v[pos].second < 0){//positive x and negative y region
if(angle > 3.1415926/4){
histogram[7 + 8 * j] += mod;
continue;
}
if(angle > -3.1415926/2 && angle < -3.1415926/4){
histogram[6 + 8 * j] += mod;
continue;
}
}else if(v[pos].first < 0 && v[pos].second > 0){//negative x and positive y region
if(angle > -3.1415926/4){
histogram[3 + 8 * j] += mod;
continue;
}
if(angle > -3.1415926/2 && angle < -3.1415926/4){
histogram[2 + 8 * j] += mod;
continue;
}
}else if(v[pos].first < 0 && v[pos].second < 0){//negative x and negative y region
if(angle <= 3.1415926/4){
histogram[4 + 8 * j] += mod;
continue;
}
if(angle > 3.1415926/4 && angle < 3.1415926/2){
histogram[5 + 8 * j] += mod;
continue;
}
}
}//end of if
}//end of for
}
for(int i = 0; i < 8 * partitionN; ++i){
histogram[i] /= frameN * 1.0;
}
return histogram;
}
float Math::arctanDegrees(float i){
return atan(i) * (180/PI);
}
void ThreadSSAO(void *ptr)
{
sSSAOparams *param;
param = (sSSAOparams*) ptr;
int quality = param->quality;
double persp = param->persp;
int threadNo = param->threadNo;
cImage *image = param->image;
int width = image->GetWidth();
int height = image->GetHeight();
int progressive = param->progressive;
bool quiet = param->quiet;
double *cosinus = new double[quality];
double *sinus = new double[quality];
for (int i = 0; i < quality; i++)
{
sinus[i] = sin((double) i / quality * 2.0 * M_PI);
cosinus[i] = cos((double) i / quality * 2.0 * M_PI);
}
double scale_factor = (double) width / (quality * quality) / 2.0;
double aspectRatio = (double) width / height;
enumPerspectiveType perspectiveType = param->perspectiveType;
double fov = param->persp;
sImageAdjustments *imageAdjustments = image->GetImageAdjustments();
double intensity = imageAdjustments->globalIlum;
for (int y = threadNo * progressive; y < height; y += NR_THREADS * progressive)
{
for (int x = 0; x < width; x += progressive)
{
double z = image->GetPixelZBuffer(x, y);
unsigned short opacity16 = image->GetPixelOpacity(x, y);
double opacity = opacity16 / 65535.0;
double total_ambient = 0;
if (z < 1e19)
{
//printf("SSAO point on object\n");
double x2, y2;
if (perspectiveType == fishEye)
{
x2 = M_PI * ((double) x / width - 0.5) * aspectRatio;
y2 = M_PI * ((double) y / height - 0.5);
double r = sqrt(x2 * x2 + y2 * y2);
if(r != 0.0)
{
x2 = x2 / r * sin(r * fov) * z;
y2 = y2 / r * sin(r * fov) * z;
}
}
else if(perspectiveType == equirectangular)
{
x2 = M_PI * ((double) x / width - 0.5) * aspectRatio; //--------- do sprawdzenia
y2 = M_PI * ((double) y / height - 0.5);
x2 = sin(fov * x2) * cos(fov * y2) * z;
y2 = sin(fov * y2) * z;
}
else
{
x2 = ((double) x / width - 0.5) * aspectRatio;
y2 = ((double) y / height - 0.5);
x2 = x2 * z * persp;
y2 = y2 * z * persp;
}
double ambient = 0;
for (int angle = 0; angle < quality; angle++)
{
double ca = cosinus[angle];
double sa = sinus[angle];
double max_diff = -1e50;
for (double r = 1.0; r < quality; r += 1.0)
{
double rr = r * r * scale_factor;
double xx = x + rr * ca;
double yy = y + rr * sa;
if ((int) xx == (int) x && (int) yy == (int) y) continue;
if (xx < 0 || xx > width - 1 || yy < 0 || yy > height - 1) continue;
double z2 = image->GetPixelZBuffer(xx, yy);
double xx2, yy2;
if (perspectiveType == fishEye)
{
xx2 = M_PI * ((double) xx / width - 0.5) * aspectRatio;
yy2 = M_PI * ((double) yy / height - 0.5);
double r2 = sqrt(xx2 * xx2 + yy2 * yy2);
if(r != 0.0)
{
xx2 = xx2 / r2 * sin(r2 * fov) * z2;
yy2 = yy2 / r2 * sin(r2 * fov) * z2;
}
}
else if (perspectiveType == equirectangular)
{
xx2 = M_PI * (xx / width - 0.5) * aspectRatio; //--------- do sprawdzenia
yy2 = M_PI * (yy / height - 0.5);
xx2 = sin(fov * xx2) * cos(fov * yy2) * z2;
yy2 = sin(fov * yy2) * z2;
}
else
{
xx2 = (xx / width - 0.5) * aspectRatio;
yy2 = (yy / height - 0.5);
xx2 = xx2 * (z2 * persp);
yy2 = yy2 * (z2 * persp);
}
double dx = xx2 - x2;
double dy = yy2 - y2;
double dz = z2 - z;
double dr = sqrt(dx * dx + dy * dy);
double diff = -dz / dr;
if (diff > max_diff) max_diff = diff;
}
double max_angle = atan(max_diff);
ambient += -max_angle / M_PI + 0.5;
}
total_ambient = ambient / quality;
if (total_ambient < 0) total_ambient = 0;
}
sRGB8 colour = image->GetPixelColor(x, y);
sRGB16 pixel = image->GetPixelImage16(x, y);
double R = pixel.R + 65535.0 * colour.R / 256.0 * total_ambient * intensity * (1.0 - opacity);
double G = pixel.G + 65535.0 * colour.G / 256.0 * total_ambient * intensity * (1.0 - opacity);
double B = pixel.B + 65535.0 * colour.B / 256.0 * total_ambient * intensity * (1.0 - opacity);
if (R > 65535) R = 65535;
if (G > 65535) G = 65535;
if (B > 65535) B = 65535;
for(int sqY =0; sqY<progressive; sqY++)
{
for(int sqX =0; sqX<progressive; sqX++)
{
pixel.R = R;
pixel.G = G;
pixel.B = B;
image->PutPixelImage16(x + sqX, y + sqY, pixel);
}
}
}
double percentDone = (double) y / height * 100.0;
if(!quiet) printf("Rendering Screen Space Ambient Occlusion. Done %.2f%% \r", percentDone);
fflush(stdout);
param->done++;
if (!isPostRendering) break;
}
delete[] sinus;
delete[] cosinus;
}
float Math::arctanRadians(float i){
return atan(i);
}
/*< SUBROUTINE INTHP(A, B, D, F, M, P, EPS, INF, QUADR) >*/
/* Subroutine */ int inthp(doublereal *a, doublereal *b, doublereal *d__,
void *f, integer *m, doublereal *p, doublereal *eps, integer *inf,
doublereal *quadr)
{
/* System generated locals */
doublereal d__1;
/* Builtin functions */
double atan(doublereal), sqrt(doublereal), exp(doublereal), R_pow(
doublereal *, doublereal *), log(doublereal);
/* Local variables */
static doublereal alfa, exph, exph0, c__, h__;
static integer i__, k, l, n;
static doublereal s, t, u, v, w, c0, e1, h0, h1;
static integer i1, l1, m1, m2, n1;
static doublereal s1, v0, v1, v2, w1, w2, w3, w4, ba, pi, sr, sq2, cor,
sum;
static logical inf1, inf2;
static doublereal eps3, sum1, sum2;
static double *tmp;
static char *mode[1], *ss[1];
static long length[1];
static void *args[1];
static double zz[1];
/* THIS SUBROUTINE COMPUTES INTEGRAL OF FUNCTIONS WHICH */
/* MAY HAVE SINGULARITIES AT ONE OR BOTH END-POINTS OF AN */
/* INTERVAL (A,B), SEE [1, 2]. IT CONTAINS FOUR DIFFERENT */
/* QUADRATURE ROUTINES: ONE OVER A FINITE INTERVAL (A,B), */
/* TWO OVER (A,+INFINITY), AND ONE OVER (-INFINITY,+INFINITY). */
/* OF THE TWO FORMULAS OVER (A,+INFINITY), THE FIRST (INF=2 */
/* BELOW) IS MORE SUITED TO NON-OSCILLATORY INTEGRANDS, WHILE */
/* THE SECOND (INF=3) IS MORE SUITED TO OSCILLATORY INTEGRANDS. */
/* THE USER SUPPLIES THE INTEGRAND FUNCTION, HE SPECIFIES THE */
/* INTERVAL, AS WELL AS THE RELATIVE ERROR TO WHICH THE INTE- */
/* GRAL IS TO BE EVALUATED. */
/* THE FORMULAS ARE OPTIMAL IN CERTAIN HARDY SPACES H(P,DD), */
/* SEE [1, 2]. HERE DD IS AN OPEN DOMAIN IN THE COMPLEX PLANE, */
/* A AND B BELONG TO THE BOUNDARY OF DD AND H(P,DD), P.GT.1, IS */
/* THE SET OF ALL ANALYTIC FUNCTONS IN DD WHOSE P-TH NORM DEFI- */
/* NED AS IN [2] IS FINITE. */
/* IF THE USER IS UNABLE TO SPECIFY THE PARAMETERS P AND D */
/* OF THE SPACE H(P,DD) TO WHICH HIS INTEGRAND BELONGS, THE */
/* ALGORITHM TERMINATES ACCORDING TO A HEURISTIC CRITERION, SEE */
/* [2] AND COMMENTS TO EPS. */
/* IF THE USER CAN SPECIFY THE PARAMETERS P AND D OF THE */
/* SPACE H(P,DD) TO WHICH HIS INTEGRAND BELONGS, THE ALGORITHM */
/* TERMINATES WITH AN ANSWER HAVING A GUARANTEED ACCURACY ( DE- */
/* TEMINISTIC CRITERION, SEE [1, 2] AND COMMENTS TO EPS). */
/* INPUT PARAMETERS */
/* A = LOWER LIMIT OF INTEGRATION (SEE COMMENTS TO INF). */
/* B = UPPER LIMIT OF INTEGRATION (SEE COMMENTS TO INF). */
/* D = A PARAMETER OF THE CLASS H(P,DD) (SEE COMMENTS TO */
/* INF). */
/* USER SETS D: */
/* HEURISTIC TERMINATION */
/* = ANY REAL NUMBER */
/* DETERMINISTIC TERMINATION */
/* = A NUMBER IN THE RANGE 0.LT.D.LE.PI/2. */
/* F = A NAME OF AN EXTERNAL INTEGRAND FUNCTION TO BE */
/* SUPPLIED BY THE USER. F(X) COMPUTES THE VALUE OF */
/* A FUNCTION F AT A POINT X. THE STATEMENT */
/* ...EXTERNAL F... MUST APPEAR IN THE MAIN PROGRAM. */
/* M = MAXIMAL NUMBER OF FUNCTION EVALUATIONS ALLOWED IN */
/* THE COMPUTATIONS, M.GE.3.( ALTERED ON EXIT ). */
/* P = 0, 1, .GT.1 A PARAMETER OF THE CLASS H(P,DD). */
/* USER SETS P: */
/* = 0 - HEURISTIC TERMINATION. */
/* = 1 - DETERMINISTIC TERMINATION WITH THE INFINITY */
/* NORM. */
/* .GT.1 -DETERMINISTIC TERMINATION WITH THE P-TH NORM. */
/* EPS = A REAL NUMBER - THE RELATIVE ERROR BOUND - SEE */
/* REMARKS BELOW. ( ALTERED ON EXIT ). */
/* INF = 1, 2, 3, 4 - INFORMATION PARAMETER. ( ALTERED ON EXIT ). */
/* = 1 SIGNIFIES AN INFINITE INTERVAL (A,B)=REAL LINE, */
/* A AND B ANY NUMBERS. */
/* DETERMINISTIC TERMINATION - */
/* DD=STRIP(Z:ABS(IM(Z)).LT.D). */
/* = 2 SIGNIFIES A SEMI-INFINITE INTERVAL (A, +INFINITY) */
/* USER SUPPLIES A, B ANY NUMBER. */
/* QUADRATURE SUITED TO NON-OSCILLATORY INTEGRANDS. */
/* DETERMINISTIC TERMINATION - */
/* DD=SECTOR(Z:ABS(ARG(Z-A)).LT.D). */
/* = 3 SIGNIFIES A SEMI INFINITE INTERVAL (A,+INFINITY) */
/* USER SUPPLIES A, B ANY NUMBER. */
/* QUADRATURE SUITED TO OSCILLATORY INTEGRANDS. */
/* DETERMINISTIC TERMINATION - */
/* DD=REGION(Z:ABS(ARG(SINH(Z-A))).LT.D). */
/* = 4 SIGNIFIES A FINITE INTERVAL (A,B). */
/* USER SUPPLIES A AND B. */
/* DETERMINISTIC TERMINATION - */
/* DD=LENS REGION(Z:ABS(ARG((Z-A)/(B-Z))).LT.D). */
/* OUTPUT PARAMETERS */
/* M = THE NUMBER OF FUNCTION EVALUATIONS USED IN THE */
/* QUADRATURE. */
/* EPS = THE RELATIVE ERROR BOUND (SEE REMARKS BELOW). */
/* DETERMINISTIC TERMINATION */
/* = THE RELATIVE ERROR REXA BOUND, I.E., */
/* REXA(F,M(OUTPUT)) .LE. EPS. */
/* HEURISTIC TERMINATION */
/* = MAX(EPS(INPUT),MACHEP). */
/* INF = 0, 1 - DETERMINISTIC TERMINATION */
/* = 0 COMPUTED QUADRATURE QCOM(F,M(EPS)), SEE REMARKS */
/* BELOW. */
/* = 1 COMPUTED QUADRATURE QCOM(F,M1), SEE REMARKS */
/* BELOW. */
/* INF = 2, 3, 4 - HEURISTIC TERMINATION. */
/* = 2 INTEGRATION COMPLETED WITH EPS=MAX(EPS(INPUT), */
/* MACHEP). WE CAN EXPECT THE RELATIVE ERROR */
/* REXA TO BE OF THE ORDER OF EPS (FOR SOME P.GE.1). */
/* = 3 INTEGRATION NOT COMPLETED. ATTEMPT TO EXCEED THE */
/* MAXIMAL ALLOWED NUMBER OF FUNCTION EVALUATIONS M. */
/* TRUNCATION CONDITIONS (SEE [2]) SATISFIED. QUADR */
/* SET TO BE EQUAL TO THE LAST TRAPEZOIDAL APPRO- */
/* XIMATION. IT IS LIKELY THAT QUADR APPROXIMATES THE */
/* INTEGRAL IF M IS LARGE. */
/* = 4 INTEGRATION NOT COMPLETED. ATTEMPT TO EXCEED THE */
/* MAXIMAL ALLOWED NUMBER OF FUNCTION EVALUATIONS M. */
/* TRUNCATION CONDITIONS (SEE [2]) NOT SATISFIED. */
/* QUADR SET TO BE EQUAL TO THE COMPUTED TRAPEZOIDAL */
/* APPROXIMATION. IT IS UNLIKELY THAT QUADR APPROXIMATES */
/* THE INTEGRAL. */
/* INF = 10, 11, 12, 13 - INCORRECT INPUT */
/* = 10 M.LT.3. */
/* = 11 P DOES NOT SATISFY P=0, P=1 OR P.GT.1 OR IN THE */
/* CASE OF DETERMINISTIC TERMINATION D DOES NOT */
/* SATISFY 0.LT.D.LE.PI/2. */
/* = 12 A.GE.B IN CASE OF A FINITE INTERVAL. */
/* = 13 INF NOT EQUAL TO 1, 2, 3, OR 4. */
/* QUADR = THE COMPUTED VALUE OF QUADRATURE. */
/* REMARKS: */
/* LET QEXA(F,M) ( QCOM(F,M) ) BE THE EXACT (COMPUTED) */
/* VALUE OF THE QUADRATURE WITH M FUNCTION EVALUATIONS, */
/* AND LET REXA(F,M) ( RCOM(F,M) ) BE THE RELATIVE ERROR */
/* OF QEXA (QCOM) ,I.E., */
/* REXA(F,M)=ABS(INTEGRAL(F)-QEXA(F,M))/NORM(F), */
/* RCOM(F,M)=ABS(INTEGRAL(F)-QCOM(F,M))/NORM(F), */
/* WITH THE NOTATION 0/0=0. */
/* DUE TO THE ROUNDOFF ONE CANNOT EXPECT THE ERROR */
/* RCOM TO BE LESS THAN THE RELATIVE MACHINE PRECISION */
/* MACHEP. THEREFORE THE INPUT VALUE OF EPS IS CHANGED */
/* ACCORDING TO THE FORMULA */
/* EPS=MAX(EPS,MACHEP). */
/* DETERMINISTIC TERMINATON CASE */
/* THE NUMBER OF FUNCTON EVALUATIONS M(EPS) IS COMPUTED */
/* SO THAT THE ERROR REXA IS NO GREATER THAN EPS,I.E., */
/* (*) REXA(F,M(EPS)) .LE. EPS . */
/* IF M(EPS).LE.M THEN THE QUADRATURE QCOM(F,M(EPS)) IS COM- */
/* PUTED. OTHERWISE, WHICH MEANS THAT EPS IS TOO SMALL WITH */
/* RESPECT TO M, THE QUADRATURE QCOM(F,M1) IS COMPUTED, WHERE */
/* M1=2*INT((M-1)/2)+1. IN THIS CASE EPS IS CHANGED TO THE */
/* SMALLEST NUMBER FOR WHICH THE ESTIMATE (*) HOLDS WITH */
/* M(EPS)=M1 FUNCTION EVALUATIONS. */
/* HEURISTIC TERMINATION CASE */
/* WE CAN EXPECT THE RELATIVE ERROR REXA TO BE OF THE */
/* ORDER OF EPS, SEE [2]. IF EPS IS TOO SMALL WITH RESPECT */
/* TO M THEN THE QUADRATURE QCOM(F,M) IS COMPUTED. */
/* ROUNDOFF ERRORS */
/* IN BOTH DETERMINISTIC AND HEURISTIC CASES THE ROUND- */
/* OFF ERROR */
/* ROFF=ABS(QEXA(F,M)-QCOM(F,M)) */
/* CAN BE ESTIMATED BY */
/* (**) ROFF .LE. 3*C1*R*MACHEP, */
/* WHERE R=QCOM(ABS(F),M)+(1+2*C2)/3*SUM(W(I),I=1,2,...M) */
/* AND C1 IS OF THE ORDER OF UNITY, C1=1/(1-3*MACHEP), W(I) */
/* ARE THE WEIGHTS OF THE QUADRATURE, SEE [2], AND C2 IS */
/* A CONSTANT ESTIMATING THE ACCURACY OF COMPUTING FUNCTION */
/* VALUES, I.E., */
/* ABS(EXACT(F(X))-COMPUTED(F(X))).LE.C2*MACHEP. */
/* IF THE INTEGRAND VALUES ARE COMPUTED INACCURATELY, I.E., */
/* C2 IS LARGE, THEN THE ESTIMATE (**) IS LARGE AND ONE CAN */
/* EXPECT THE ACTUAL ERROR ROFF TO BE LARGE. NUMERICAL TESTS */
/* INDICATE THAT THIS HAPPENS ESPECIALLY WHEN THE INTEGRAND */
/* IS EVALUATED INACCURATELY NEAR A SINGULARITY. THE WAYS OF */
/* CIRCUMVENTING SUCH PITFALLS ARE EXPLAINED IN [2]. */
/* REFERENCES: */
/* [1] SIKORSKI,K., OPTIMAL QUADRATURE ALGORITHMS IN HP */
/* SPACES, NUM. MATH., 39, 405-410 (1982). */
/* [2] SIKORSKI,K., STENGER,F., OPTIMAL QUADRATURES IN */
/* HP SPACES, ACM TOMS. */
/* modified by J.K. Lindsey for R September 1998 */
/*< implicit none >*/
/*< INTEGER I, I1, INF, K, L, L1, M, M1, M2, N, N1 >*/
/*< >*/
/*< >*/
/*< double precision V2, W, W1, W2, W3, W4 >*/
/*< LOGICAL INF1, INF2 >*/
mode[0] = "double";
length[0] = 1;
args[0] = (void *)(zz);
/*< PI = 4.*ATAN(1.0) >*/
pi = atan(1.f) * 4.f;
/* CHECK THE INPUT DATA */
/*< >*/
if (*inf != 1 && *inf != 2 && *inf != 3 && *inf != 4) {
goto L300;
}
/*< IF (M.LT.3) GO TO 270 >*/
if (*m < 3) {
goto L270;
}
/*< IF (P.LT.1. .AND. P.NE.0.) GO TO 280 >*/
if (*p < 1.f && *p != 0.f) {
goto L280;
}
/*< IF (P.GE.1. .AND. (D.LE.0. .OR. D.GT.PI/2.)) GO TO 280 >*/
if (*p >= 1.f && (*d__ <= 0.f || *d__ > pi / 2.f)) {
goto L280;
}
/*< IF (INF.EQ.4 .AND. A.GE.B) GO TO 290 >*/
if (*inf == 4 && *a >= *b) {
goto L290;
}
/*< SQ2 = SQRT(2.0) >*/
sq2 = sqrt(2.f);
/*< I1 = INF - 2 >*/
i1 = *inf - 2;
/*< BA = B - A >*/
ba = *b - *a;
/*< N1 = 0 >*/
n1 = 0;
/* COMPUTE THE RELATIVE MACHINE PRECISION AND CHECK */
/* THE VALUE OF EPS. CAUTION...THIS LOOP MAY NOT WORK ON A */
/* MACHINE THAT HAS AN ACCURATED ARITHMETIC PROCESS COMPARED */
/* TO THE STORAGE PRECISION. THE VALUE OF U MAY NEED TO BE */
/* SIMPLY DEFINED AS THE RELATIVE ACCURACY OF STORAGE PRECISION. */
/*< U = 1. >*/
u = 1.f;
/*< 10 U = U/10. >*/
L10:
u /= 10.f;
/*< T = 1. + U >*/
t = u + 1.f;
/*< IF (1..NE.T) GO TO 10 >*/
if (1.f != t) {
goto L10;
}
/*< U = U*10. >*/
u *= 10.f;
/*< IF (EPS.LT.U) EPS = U >*/
if (*eps < u) {
*eps = u;
}
/*< IF (P.EQ.0.) GO TO 40 >*/
if (*p == 0.f) {
goto L40;
}
/* SET UP DATA FOR THE DETERMINISTIC TERMINATION */
/*< IF (P.EQ.1.) ALFA = 1. >*/
if (*p == 1.f) {
alfa = 1.f;
}
/*< IF (P.GT.1.) ALFA = (P-1.)/P >*/
if (*p > 1.f) {
alfa = (*p - 1.f) / *p;
}
/*< C = 2.*PI/(1.-1./EXP(PI*SQRT(ALFA))) + 4.**ALFA/ALFA >*/
c__ = pi * 2.f / (1.f - 1.f / exp(pi * sqrt(alfa))) + R_pow(&c_b8, &alfa)
/ alfa;
/*< W = dLOG(C/EPS) >*/
w = log(c__ / *eps);
/*< W1 = 1./(PI*PI*ALFA)*W*W >*/
w1 = 1.f / (pi * pi * alfa) * w * w;
/*< N = INT(W1) >*/
n = (integer) w1;
/*< IF (W1.GT.FLOAT(N)) N = N + 1 >*/
if (w1 > (real) n) {
++n;
}
/*< IF (W1.EQ.0.) N = 1 >*/
if (w1 == 0.f) {
n = 1;
}
/*< N1 = 2*N + 1 >*/
n1 = (n << 1) + 1;
/*< SR = SQRT(ALFA*FLOAT(N)) >*/
sr = sqrt(alfa * (real) n);
/*< IF (N1.LE.M) GO TO 20 >*/
if (n1 <= *m) {
goto L20;
}
/* EPS TOO SMALL WITH RESPECT TO M. COMPUTE THE NEW EPS */
/* GUARANTEED BY THE VALUE OF M. */
/*< N1 = 1 >*/
n1 = 1;
/*< N = INT(FLOAT((M-1)/2)) >*/
n = (integer) ((real) ((*m - 1) / 2));
/*< SR = SQRT(ALFA*FLOAT(N)) >*/
sr = sqrt(alfa * (real) n);
/*< M = 2*N + 1 >*/
*m = (n << 1) + 1;
/*< EPS = C/EXP(PI*SR) >*/
*eps = c__ / exp(pi * sr);
/*< GO TO 30 >*/
goto L30;
/*< 20 M = N1 >*/
L20:
*m = n1;
/*< N1 = 0 >*/
n1 = 0;
/*< 30 H = 2.*D/SR >*/
L30:
h__ = *d__ * 2.f / sr;
/*< SUM2 = 0. >*/
sum2 = 0.f;
/*< L1 = N >*/
l1 = n;
/*< K = N >*/
k = n;
/*< INF1 = .FALSE. >*/
inf1 = FALSE_;
/*< INF2 = .FALSE. >*/
inf2 = FALSE_;
/*< H0 = H >*/
h0 = h__;
/*< GO TO 50 >*/
goto L50;
/* SET UP DATA FOR THE HEURISTIC TERMINATION */
/*< 40 H = 1. >*/
L40:
h__ = 1.f;
/*< H0 = 1. >*/
h0 = 1.f;
/*< EPS3 = EPS/3. >*/
eps3 = *eps / 3.f;
/*< SR = SQRT(EPS) >*/
sr = sqrt(*eps);
/*< V1 = EPS*10. >*/
v1 = *eps * 10.f;
/*< V2 = V1 >*/
v2 = v1;
/*< M1 = M - 1 >*/
m1 = *m - 1;
/*< N = INT(FLOAT(M1/2)) >*/
n = (integer) ((real) (m1 / 2));
/*< M2 = N >*/
m2 = n;
/*< L1 = 0 >*/
l1 = 0;
/*< INF1 = .TRUE. >*/
inf1 = TRUE_;
/*< INF2 = .FALSE. >*/
inf2 = FALSE_;
/* INITIALIZE THE QUADRATURE */
/*< 50 I = 0 >*/
L50:
i__ = 0;
/*< IF (INF.EQ.1) SUM = F(0.d0) >*/
if (*inf == 1) {
/* sum = (*f)(&c_b12);*/
zz[0]=c_b12;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
sum=tmp[0];
}
/*< IF (INF.EQ.2) SUM = F(A+1.) >*/
if (*inf == 2) {
d__1 = *a + 1.f;
/* sum = (*f)(&d__1);*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
sum=tmp[0];
}
/*< IF (INF.EQ.3) SUM = F(A+dLOG(1.+SQ2))/SQ2 >*/
if (*inf == 3) {
d__1 = *a + log(sq2 + 1.f);
/* sum = (*f)(&d__1) / sq2;*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
sum=tmp[0]/sq2;
}
/*< IF (INF.EQ.4) SUM = F((A+B)/2.)/4.*BA >*/
if (*inf == 4) {
d__1 = (*a + *b) / 2.f;
/* sum = (*f)(&d__1) / 4.f * ba;*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
sum=tmp[0] / 4.f * ba;
}
/* COMPUTE WEIGHTS, NODES AND FUNCTION VALUES */
/*< 60 EXPH = EXP(H) >*/
L60:
exph = exp(h__);
/*< EXPH0 = EXP(H0) >*/
exph0 = exp(h0);
/*< H1 = H0 >*/
h1 = h0;
/*< E1 = EXPH0 >*/
e1 = exph0;
/*< U = 0. >*/
u = 0.f;
/*< COR = 0. >*/
cor = 0.f;
/*< 70 IF (I1) 80, 90, 100 >*/
L70:
if (i1 < 0) {
goto L80;
} else if (i1 == 0) {
goto L90;
} else {
goto L100;
}
/*< 80 V = F(H1) >*/
L80:
/* v = (*f)(&h1);*/
zz[0]=h1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
v=tmp[0];
/*< H1 = H1 + H >*/
h1 += h__;
/*< GO TO 150 >*/
goto L150;
/*< 90 V = E1*F(A+E1) >*/
L90:
d__1 = *a + e1;
/* v = e1 * (*f)(&d__1);*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
v=e1 * tmp[0];
/*< E1 = E1*EXPH >*/
e1 *= exph;
/*< GO TO 150 >*/
goto L150;
/*< 100 IF (INF.EQ.4) GO TO 140 >*/
L100:
if (*inf == 4) {
goto L140;
}
/*< W1 = SQRT(E1+1./E1) >*/
w1 = sqrt(e1 + 1.f / e1);
/*< W2 = SQRT(E1) >*/
w2 = sqrt(e1);
/*< IF (E1.LT.0.1) GO TO 110 >*/
if (e1 < .1f) {
goto L110;
}
/*< S = dLOG(E1+W1*W2) >*/
s = log(e1 + w1 * w2);
/*< GO TO 130 >*/
goto L130;
/*< 110 W3 = E1 >*/
L110:
w3 = e1;
/*< W4 = E1*E1 >*/
w4 = e1 * e1;
/*< C0 = 1. >*/
c0 = 1.f;
/*< S = E1 >*/
s = e1;
/*< S1 = E1 >*/
s1 = e1;
/*< T = 0. >*/
t = 0.f;
/*< 120 C0 = -C0*(0.5+T)*(2.*T+1.)/(2.*T+3.)/(T+1.) >*/
L120:
c0 = -c0 * (t + .5f) * (t * 2.f + 1.f) / (t * 2.f + 3.f) / (t + 1.f);
/*< T = T + 1. >*/
t += 1.f;
/*< W3 = W3*W4 >*/
w3 *= w4;
/*< S = S + C0*W3 >*/
s += c0 * w3;
/*< IF (S.EQ.S1) GO TO 130 >*/
if (s == s1) {
goto L130;
}
/*< S1 = S >*/
s1 = s;
/*< GO TO 120 >*/
goto L120;
/*< 130 V = W2/W1*F(A+S) >*/
L130:
d__1 = *a + s;
/* v = w2 / w1 * (*f)(&d__1);*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
v = w2 / w1 * tmp[0];
/*< E1 = E1*EXPH >*/
e1 *= exph;
/*< GO TO 150 >*/
goto L150;
/*< 140 W1 = E1 + 1. >*/
L140:
w1 = e1 + 1.f;
/*< V = E1/W1/W1*F((A+B*E1)/W1)*BA >*/
d__1 = (*a + *b * e1) / w1;
/* v = e1 / w1 / w1 * (*f)(&d__1) * ba;*/
zz[0]=d__1;
call_R(f, 1L, args, mode, length, 0L, 1L, ss);
tmp=(double *)ss[0];
v = e1 / w1 / w1 * tmp[0] * ba;
/*< E1 = E1*EXPH >*/
e1 *= exph;
/* SUMMATION ALGORITHM */
/*< 150 I = I + 1 >*/
L150:
++i__;
/*< SUM1 = U + V >*/
sum1 = u + v;
/*< IF (ABS(U).LT.ABS(V)) GO TO 160 >*/
if (abs(u) < abs(v)) {
goto L160;
}
/*< COR = V - (SUM1-U) + COR >*/
cor = v - (sum1 - u) + cor;
/*< GO TO 170 >*/
goto L170;
/*< 160 COR = U - (SUM1-V) + COR >*/
L160:
cor = u - (sum1 - v) + cor;
/*< 170 U = SUM1 >*/
L170:
u = sum1;
/*< IF (I.LT.L1) GO TO 70 >*/
if (i__ < l1) {
goto L70;
}
/* SWITCH TO CHECK TRUNCATION CONDITION ( HEURISTIC */
/* TERMINATION) */
/*< IF (INF1) GO TO 190 >*/
if (inf1) {
goto L190;
}
/* SWITCH TO COMPUTE THE MIDORDINATE APPROXIMATION */
/* ( HEURISTIC TERMINATION ) OR TO STOP ( DETERMINIS- */
/* TIC TERMINATION) */
/*< IF (INF2) GO TO 210 >*/
if (inf2) {
goto L210;
}
/* SET UP PARAMETERS TO CONTINUE SUMMATION */
/*< L1 = K >*/
l1 = k;
/*< 180 INF2 = .TRUE. >*/
L180:
inf2 = TRUE_;
/*< I = 0. >*/
i__ = 0.f;
/*< EXPH = 1./EXPH >*/
exph = 1.f / exph;
/*< H0 = -H0 >*/
h0 = -h0;
/*< E1 = 1./EXPH0 >*/
e1 = 1.f / exph0;
/*< H1 = H0 >*/
h1 = h0;
/*< H = -H >*/
h__ = -h__;
/*< GO TO 70 >*/
goto L70;
/* TRUNCATION CONDITION */
/*< 190 V0 = V1 >*/
L190:
v0 = v1;
/*< V1 = V2 >*/
v1 = v2;
/*< V2 = ABS(V) >*/
v2 = abs(v);
/*< IF (V0+V1+V2.LE.EPS3) GO TO 200 >*/
if (v0 + v1 + v2 <= eps3) {
goto L200;
}
/*< IF (I.LT.M2) GO TO 70 >*/
if (i__ < m2) {
goto L70;
}
/*< N1 = 5 >*/
n1 = 5;
/*< 200 IF (INF2) K = I >*/
L200:
if (inf2) {
k = i__;
}
/*< IF (.NOT.INF2) L = I >*/
if (! inf2) {
l = i__;
}
/*< V1 = 10.*EPS >*/
v1 = *eps * 10.f;
/*< V2 = V1 >*/
v2 = v1;
/*< M2 = M1 - L >*/
m2 = m1 - l;
/*< IF (.NOT.INF2) GO TO 180 >*/
if (! inf2) {
goto L180;
}
/* N1=5 - TRUNCATION CONDITION NOT SATISFIED */
/*< IF (N1.EQ.5) GO TO 260 >*/
if (n1 == 5) {
goto L260;
}
/* TRUNCATION CONDITION SATISFIED, SUM2=TRAPEZOIDAL */
/* APPROXIMATION */
/*< SUM2 = SUM1 + COR + SUM >*/
sum2 = sum1 + cor + sum;
/*< M2 = 2*(K+L) >*/
m2 = (k + l) << 1;
/* CHECK THE NUMBER OF FUNCTION EVALUATIONS */
/*< IF (M2.GT.M1) GO TO 240 >*/
if (m2 > m1) {
goto L240;
}
/* INITIALIZE ITERATION */
/*< INF1 = .FALSE. >*/
inf1 = FALSE_;
/*< INF2 = .FALSE. >*/
inf2 = FALSE_;
/*< L1 = L >*/
l1 = l;
/*< I = 0 >*/
i__ = 0;
/*< H = -H >*/
h__ = -h__;
/*< H0 = H/2. >*/
h0 = h__ / 2.f;
/*< GO TO 60 >*/
goto L60;
/* P.GE.1 = DETERMINISTIC TERMINATION */
/*< 210 IF (P.GE.1.) GO TO 220 >*/
L210:
if (*p >= 1.f) {
goto L220;
}
/* COMPUTE THE MIDORDINATE APPROXIMATION SUM1 */
/*< H = -H >*/
h__ = -h__;
/*< SUM1 = (SUM1+COR)*H >*/
sum1 = (sum1 + cor) * h__;
/*< W1 = (SUM1+SUM2)/2. >*/
w1 = (sum1 + sum2) / 2.f;
/* TERMINATION CONDITION */
/*< IF (ABS(SUM1-SUM2).LE.SR) GO TO 230 >*/
if ((d__1 = sum1 - sum2, abs(d__1)) <= sr) {
goto L230;
}
/* SET UP DATA FOR THE NEXT ITERATION */
/*< M2 = 2*M2 >*/
m2 <<= 1;
/*< IF (M2.GT.M1) GO TO 250 >*/
if (m2 > m1) {
goto L250;
}
/*< I = 0 >*/
i__ = 0;
/*< K = 2*K >*/
k <<= 1;
/*< L = 2*L >*/
l <<= 1;
/*< L1 = L >*/
l1 = l;
/*< H = H/2. >*/
h__ /= 2.f;
/*< H0 = H/2. >*/
h0 = h__ / 2.f;
/*< SUM2 = W1 >*/
sum2 = w1;
/*< INF2 = .FALSE. >*/
inf2 = FALSE_;
/*< GO TO 60 >*/
goto L60;
/* FINAL RESULTS */
/*< 220 QUADR = -H*(SUM1+COR+SUM) >*/
L220:
*quadr = -h__ * (sum1 + cor + sum);
/*< INF = N1 >*/
*inf = n1;
/*< RETURN >*/
return 0;
/*< 230 QUADR = W1 >*/
L230:
*quadr = w1;
/*< INF = 2 >*/
*inf = 2;
/*< M = M2 + 1 >*/
*m = m2 + 1;
/*< RETURN >*/
return 0;
/*< 240 QUADR = SUM2 >*/
L240:
*quadr = sum2;
/*< INF = 3 >*/
*inf = 3;
/*< M = K + L + 1 >*/
*m = k + l + 1;
/*< RETURN >*/
return 0;
/*< 250 QUADR = W1 >*/
L250:
*quadr = w1;
/*< INF = 3 >*/
*inf = 3;
/*< M = M2/2 + 1 >*/
*m = m2 / 2 + 1;
/*< RETURN >*/
return 0;
/*< 260 QUADR = U + COR + SUM >*/
L260:
*quadr = u + cor + sum;
/*< INF = 4 >*/
*inf = 4;
/*< M = K + L + 1 >*/
*m = k + l + 1;
/*< RETURN >*/
return 0;
/*< 270 INF = 10 >*/
L270:
*inf = 10;
/*< RETURN >*/
return 0;
/*< 280 INF = 11 >*/
L280:
*inf = 11;
/*< RETURN >*/
return 0;
/*< 290 INF = 12 >*/
L290:
*inf = 12;
/*< RETURN >*/
return 0;
/*< 300 INF = 13 >*/
L300:
*inf = 13;
/*< RETURN >*/
return 0;
/*< END >*/
} /* inthp_ */
double GusUtils::Atan( double angle )
{
return atan( angle );
}
static void generate_cone(const float radius,
const float height,
const int numslices,
const unsigned int flags,
SoShape * const shape,
SoAction * const action) {
int i;
int slices = numslices;
if (slices > 128) slices = 128;
if (slices < 4) slices = 4;
float h2 = height * 0.5f;
// put coordinates on the stack
SbVec3f coords[129];
SbVec3f normals[130];
SbVec2f texcoords[129];
sogenerate_generate_3d_circle(coords, slices, radius, -h2);
coords[slices] = coords[0];
double a = atan(height/radius);
sogenerate_generate_3d_circle(normals, slices, (float) sin(a), (float) cos(a));
normals[slices] = normals[0];
normals[slices+1] = normals[1];
int matnr = 0;
SoPrimitiveVertex vertex;
SoConeDetail sideDetail;
SoConeDetail bottomDetail;
sideDetail.setPart(SoCone::SIDES);
bottomDetail.setPart(SoCone::BOTTOM);
// FIXME: the texture coordinate generation for cone sides is of
// sub-par quality. The textures comes out looking "skewed" and
// "compressed". 20010926 mortene.
if (flags & SOGEN_GENERATE_SIDE) {
vertex.setDetail(&sideDetail);
vertex.setMaterialIndex(matnr);
shape->beginShape(action, SoShape::TRIANGLES);
i = 0;
float t = 1.0;
float delta = 1.0f / slices;
while (i < slices) {
vertex.setTextureCoords(SbVec2f(t - delta*0.5f, 1.0f));
vertex.setNormal((normals[i] + normals[i+1])*0.5f);
vertex.setPoint(SbVec3f(0.0f, h2, 0.0f));
shape->shapeVertex(&vertex);
vertex.setTextureCoords(SbVec2f(t, 0.0f));
vertex.setNormal(normals[i]);
vertex.setPoint(coords[i]);
shape->shapeVertex(&vertex);
vertex.setTextureCoords(SbVec2f(t-delta, 0.0f));
vertex.setNormal(normals[i+1]);
vertex.setPoint(coords[i+1]);
shape->shapeVertex(&vertex);
i++;
t -= delta;
}
if (flags & SOGEN_MATERIAL_PER_PART) matnr++;
shape->endShape();
}
if (flags & SOGEN_GENERATE_BOTTOM) {
vertex.setDetail(&bottomDetail);
vertex.setMaterialIndex(matnr);
sogenerate_generate_2d_circle(texcoords, slices, 0.5f);
texcoords[slices] = texcoords[0];
shape->beginShape(action, SoShape::TRIANGLE_FAN);
vertex.setNormal(SbVec3f(0.0f, -1.0f, 0.0f));
for (i = slices-1; i >= 0; i--) {
vertex.setTextureCoords(texcoords[i]+SbVec2f(0.5f, 0.5f));
vertex.setPoint(coords[i]);
shape->shapeVertex(&vertex);
}
shape->endShape();
}
}
void CartTransformation::wgsToJTSK(const Point3DGeographic <T> *p1, Point3DCartesian <T> * p2)
{
//Deg => rad
const T PI = 4.0 * atan(1.0), Ro = 57.29577951;
//WGS-84
const T A_WGS = 6378137.0000, B_WGS = 6356752.3142;
const T E2_WGS = (A_WGS * A_WGS - B_WGS * B_WGS) / (A_WGS * A_WGS);
//Bessel
const T A_Bes = 6377397.1550, B_Bes = 6356078.9633;
const T E2_Bes = (A_Bes * A_Bes - B_Bes * B_Bes) / (A_Bes * A_Bes), E_Bes = sqrt(E2_Bes);
//Scale, Translation, Rotation
const T m = -3.5623e-6;
const T X0 = -570.8285, Y0 = -85.6769, Z0 = -462.8420;
const T OMX = 4.9984 / 3600 / Ro, OMY = 1.5867 / 3600 / Ro, OMZ = 5.2611 / 3600 / Ro;
//JTSK
const T FI0 = 49.5 / Ro;
const T U0 = (49.0 + 27.0 / 60 + 35.84625 / 3600) / Ro;
const T ALFA = 1.000597498372;
const T LA_FERRO = (17.0 + 40.0 / 60) / Ro;
const T K = 0.9965924869;
const T R = 6380703.6105;
const T UK = (59.0 + 42.0 / 60 + 42.6969 / 3600) / Ro;
const T VK = (42.0 + 31.0 / 60 + 31.41725 / 3600) / Ro;
const T Ro0 = 1298039.0046;
const T S0 = 78.5 / Ro;
//Point parameters
T W = sqrt(1 - E2_WGS * sin(p1->getLat() / Ro) * sin(p1->getLat() / Ro));
T M = A_WGS * (1 - E2_WGS) / (W * W * W);
T N = A_WGS / W;
//Transformation (B,L,H)WGS => (X,Y,Z)WGS
T X_WGS = (N + p1->getH()) * cos(p1->getLat() / Ro) * cos(p1->getLon() / Ro);
T Y_WGS = (N + p1->getH()) * cos(p1->getLat() / Ro) * sin(p1->getLon() / Ro);
T Z_WGS = (N * (1 - E2_WGS) + p1->getH()) * sin(p1->getLat() / Ro);
//Transformation (X,Y,Z)WGS =>(X,Y,Z)Bes
T X_Bes = X0 + (m + 1) * (X_WGS + Y_WGS * OMZ - Z_WGS * OMY);
T Y_Bes = Y0 + (m + 1) * (-X_WGS * OMZ + Y_WGS + Z_WGS * OMX);
T Z_Bes = Z0 + (m + 1) * (X_WGS * OMY - Y_WGS * OMX + Z_WGS);
//Transformation (X,Y,Z)Bes => (BLH)Bess
T rad = sqrt(X_Bes * X_Bes + Y_Bes * Y_Bes);
T la = 2 * atan(Y_Bes / (rad + X_Bes));
T p = atan((A_Bes * Z_Bes) / (B_Bes * rad));
T t = (Z_Bes + E2_Bes * A_Bes * A_Bes / B_Bes * pow(sin(p), 3)) / (rad - E2_Bes * A_Bes * pow(cos(p), 3));
T fi = atan(t);
T H = sqrt(1 + t * t) * (rad - A_Bes / sqrt(1 + (1 - E2_Bes) * t * t));
//Transformation (fi,la)Bes => (u,v)sphere (Gauss conformal projection)
la = la + LA_FERRO;
T ro = 1 / K * pow(tan(fi / 2 + PI / 4) * pow((1 - E_Bes * sin(fi)) / (1 + E_Bes * sin(fi)), E_Bes / 2), ALFA);
T u = 2 * atan(ro) - PI / 2;
T v = ALFA * la;
//Transformation (u,v)sphere => (s,d)sphere
T s = asin(sin(UK) * sin(u) + cos(UK) * cos(u) * cos(VK - v));
T d = asin(sin(VK - v) * cos(u) / cos(s));
//Transformation (s,d)sphere => (Ro,Eps)plane (Lambert conformal projection)
T n = sin(S0);
T Ro_JTSK = Ro0 * pow((tan((S0 / 2 + PI / 4)) / (tan(s / 2 + PI / 4))), n);
T eps_JTSK = n * d;
//(Ro, eps) => (x,y)
T X_JTSK = Ro_JTSK * cos(eps_JTSK);
T Y_JTSK = Ro_JTSK * sin(eps_JTSK);
//Set computed parameters to the projected point
if (p1->getPointLabel() != NULL)
{
p2->setPointLabel(p1->getPointLabel());
}
p2->setX(X_JTSK);
p2->setY(Y_JTSK);
p2->setZ(H);
}
bool QgsNorthArrowPlugin::calculateNorthDirection()
{
QgsMapCanvas& mapCanvas = *( qGisInterface->mapCanvas() );
bool goodDirn = false;
if ( mapCanvas.layerCount() > 0 )
{
QgsCoordinateReferenceSystem outputCRS = mapCanvas.mapRenderer()->destinationSrs();
if ( outputCRS.isValid() && !outputCRS.geographicFlag() )
{
// Use a geographic CRS to get lat/long to work out direction
QgsCoordinateReferenceSystem ourCRS;
ourCRS.createFromProj4( "+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs" );
assert( ourCRS.isValid() );
QgsCoordinateTransform transform( outputCRS, ourCRS );
QgsRectangle extent = mapCanvas.extent();
QgsPoint p1( extent.center() );
// A point a bit above p1. XXX assumes that y increases up!!
// May need to involve the maptopixel transform if this proves
// to be a problem.
QgsPoint p2( p1.x(), p1.y() + extent.height() * 0.25 );
// project p1 and p2 to geographic coords
try
{
p1 = transform.transform( p1 );
p2 = transform.transform( p2 );
}
catch ( QgsException &e )
{
Q_UNUSED( e );
// just give up
QgsDebugMsg( "North Arrow: Transformation error, quitting" );
return false;
}
// Work out the value of the initial heading one takes to go
// from point p1 to point p2. The north direction is then that
// many degrees anti-clockwise or vertical.
// Take some care to not divide by zero, etc, and ensure that we
// get sensible results for all possible values for p1 and p2.
goodDirn = true;
double angle = 0.0;
// convert to radians for the equations below
p1.multiply( PI / 180.0 );
p2.multiply( PI / 180.0 );
double y = sin( p2.x() - p1.x() ) * cos( p2.y() );
double x = cos( p1.y() ) * sin( p2.y() ) -
sin( p1.y() ) * cos( p2.y() ) * cos( p2.x() - p1.x() );
if ( y > 0.0 )
{
if ( x > TOL )
angle = atan( y / x );
else if ( x < -TOL )
angle = PI - atan( -y / x );
else
angle = 0.5 * PI;
}
else if ( y < 0.0 )
{
if ( x > TOL )
angle = -atan( -y / x );
else if ( x < -TOL )
angle = atan( y / x ) - PI;
else
angle = 1.5 * PI;
}
else
{
if ( x > TOL )
angle = 0.0;
else if ( x < -TOL )
angle = PI;
else
{
angle = 0.0; // p1 = p2
goodDirn = false;
}
}
// And set the angle of the north arrow. Perhaps do something
// different if goodDirn = false.
mRotationInt = static_cast<int>( round( fmod( 360.0 - angle * 180.0 / PI, 360.0 ) ) );
}
else
{
// For geographic CRS and for when there are no layers, set the
// direction back to the default
mRotationInt = 0;
}
}
return goodDirn;
}
float
atanf (float x)
{
return (float) atan (x);
}
// Returns the angle between two Vecs
double angle_between(Vec3D<Type> v){
return(atan(dot_product(v)/(magnitude() * v.magnitude())));
}
float
atanf (float x)
{
return (float) atan ((double) x);
}
int scene::loadCamera(){
cout << "Loading Camera ..." << endl;
camera newCamera;
float fovy;
//load static properties
for(int i=0; i<4; i++){
string line;
utilityCore::safeGetline(fp_in,line);
vector<string> tokens = utilityCore::tokenizeString(line);
if(strcmp(tokens[0].c_str(), "RES")==0){
newCamera.resolution = glm::vec2(atoi(tokens[1].c_str()), atoi(tokens[2].c_str()));
}else if(strcmp(tokens[0].c_str(), "FOVY")==0){
fovy = atof(tokens[1].c_str());
}else if(strcmp(tokens[0].c_str(), "ITERATIONS")==0){
newCamera.iterations = atoi(tokens[1].c_str());
}else if(strcmp(tokens[0].c_str(), "FILE")==0){
newCamera.imageName = tokens[1];
}
}
//load time variable properties (frames)
int frameCount = 0;
string line;
utilityCore::safeGetline(fp_in,line);
vector<glm::vec3> positions;
vector<glm::vec3> views;
vector<glm::vec3> ups;
while (!line.empty() && fp_in.good()){
//check frame number
vector<string> tokens = utilityCore::tokenizeString(line);
if(strcmp(tokens[0].c_str(), "frame")!=0 || atoi(tokens[1].c_str())!=frameCount){
cout << "ERROR: Incorrect frame count!" << endl;
return -1;
}
//load camera properties
for(int i=0; i<3; i++){
//glm::vec3 translation; glm::vec3 rotation; glm::vec3 scale;
utilityCore::safeGetline(fp_in,line);
tokens = utilityCore::tokenizeString(line);
if(strcmp(tokens[0].c_str(), "EYE")==0){
positions.push_back(glm::vec3(atof(tokens[1].c_str()), atof(tokens[2].c_str()), atof(tokens[3].c_str())));
}else if(strcmp(tokens[0].c_str(), "VIEW")==0){
views.push_back(glm::vec3(atof(tokens[1].c_str()), atof(tokens[2].c_str()), atof(tokens[3].c_str())));
}else if(strcmp(tokens[0].c_str(), "UP")==0){
ups.push_back(glm::vec3(atof(tokens[1].c_str()), atof(tokens[2].c_str()), atof(tokens[3].c_str())));
}
}
frameCount++;
utilityCore::safeGetline(fp_in,line);
}
newCamera.frames = frameCount;
//move frames into CUDA readable arrays
newCamera.positions = new glm::vec3[frameCount];
newCamera.views = new glm::vec3[frameCount];
newCamera.ups = new glm::vec3[frameCount];
for(int i=0; i<frameCount; i++){
newCamera.positions[i] = positions[i];
newCamera.views[i] = views[i];
newCamera.ups[i] = ups[i];
}
//calculate fov based on resolution
float yscaled = tan(fovy*(PI/180));
float xscaled = (yscaled * newCamera.resolution.x)/newCamera.resolution.y;
float fovx = (atan(xscaled)*180)/PI;
newCamera.fov = glm::vec2(fovx, fovy);
renderCam = newCamera;
//set up render camera stuff
renderCam.image = new glm::vec3[(int)renderCam.resolution.x*(int)renderCam.resolution.y];
renderCam.rayList = new ray[(int)renderCam.resolution.x*(int)renderCam.resolution.y];
for(int i=0; i<renderCam.resolution.x*renderCam.resolution.y; i++){
renderCam.image[i] = glm::vec3(0,0,0);
}
cout << "Loaded " << frameCount << " frames for camera!" << endl;
return 1;
}
//
// Do single unary node folding
//
// Returns a new node representing the result.
//
TIntermTyped* TIntermConstantUnion::fold(TOperator op, const TType& returnType) const
{
// First, size the result, which is mostly the same as the argument's size,
// but not always, and classify what is componentwise.
// Also, eliminate cases that can't be compile-time constant.
int resultSize;
bool componentWise = true;
int objectSize = getType().computeNumComponents();
switch (op) {
case EOpDeterminant:
case EOpAny:
case EOpAll:
case EOpLength:
componentWise = false;
resultSize = 1;
break;
case EOpEmitStreamVertex:
case EOpEndStreamPrimitive:
// These don't actually fold
return 0;
case EOpPackSnorm2x16:
case EOpPackUnorm2x16:
case EOpPackHalf2x16:
componentWise = false;
resultSize = 1;
break;
case EOpUnpackSnorm2x16:
case EOpUnpackUnorm2x16:
case EOpUnpackHalf2x16:
componentWise = false;
resultSize = 2;
break;
case EOpNormalize:
componentWise = false;
resultSize = objectSize;
break;
default:
resultSize = objectSize;
break;
}
// Set up for processing
TConstUnionArray newConstArray(resultSize);
const TConstUnionArray& unionArray = getConstArray();
// Process non-component-wise operations
switch (op) {
case EOpLength:
case EOpNormalize:
{
double sum = 0;
for (int i = 0; i < objectSize; i++)
sum += unionArray[i].getDConst() * unionArray[i].getDConst();
double length = sqrt(sum);
if (op == EOpLength)
newConstArray[0].setDConst(length);
else {
for (int i = 0; i < objectSize; i++)
newConstArray[i].setDConst(unionArray[i].getDConst() / length);
}
break;
}
// TODO: 3.0 Functionality: unary constant folding: the rest of the ops have to be fleshed out
case EOpPackSnorm2x16:
case EOpPackUnorm2x16:
case EOpPackHalf2x16:
case EOpUnpackSnorm2x16:
case EOpUnpackUnorm2x16:
case EOpUnpackHalf2x16:
case EOpDeterminant:
case EOpMatrixInverse:
case EOpTranspose:
case EOpAny:
case EOpAll:
return 0;
default:
assert(componentWise);
break;
}
// Turn off the componentwise loop
if (! componentWise)
objectSize = 0;
// Process component-wise operations
for (int i = 0; i < objectSize; i++) {
switch (op) {
case EOpNegative:
switch (getType().getBasicType()) {
case EbtDouble:
case EbtFloat: newConstArray[i].setDConst(-unionArray[i].getDConst()); break;
case EbtInt: newConstArray[i].setIConst(-unionArray[i].getIConst()); break;
case EbtUint: newConstArray[i].setUConst(static_cast<unsigned int>(-static_cast<int>(unionArray[i].getUConst()))); break;
default:
return 0;
}
break;
case EOpLogicalNot:
case EOpVectorLogicalNot:
switch (getType().getBasicType()) {
case EbtBool: newConstArray[i].setBConst(!unionArray[i].getBConst()); break;
default:
return 0;
}
break;
case EOpBitwiseNot:
newConstArray[i] = ~unionArray[i];
break;
case EOpRadians:
newConstArray[i].setDConst(unionArray[i].getDConst() * pi / 180.0);
break;
case EOpDegrees:
newConstArray[i].setDConst(unionArray[i].getDConst() * 180.0 / pi);
break;
case EOpSin:
newConstArray[i].setDConst(sin(unionArray[i].getDConst()));
break;
case EOpCos:
newConstArray[i].setDConst(cos(unionArray[i].getDConst()));
break;
case EOpTan:
newConstArray[i].setDConst(tan(unionArray[i].getDConst()));
break;
case EOpAsin:
newConstArray[i].setDConst(asin(unionArray[i].getDConst()));
break;
case EOpAcos:
newConstArray[i].setDConst(acos(unionArray[i].getDConst()));
break;
case EOpAtan:
newConstArray[i].setDConst(atan(unionArray[i].getDConst()));
break;
case EOpDPdx:
case EOpDPdy:
case EOpFwidth:
case EOpDPdxFine:
case EOpDPdyFine:
case EOpFwidthFine:
case EOpDPdxCoarse:
case EOpDPdyCoarse:
case EOpFwidthCoarse:
// The derivatives are all mandated to create a constant 0.
newConstArray[i].setDConst(0.0);
break;
case EOpExp:
newConstArray[i].setDConst(exp(unionArray[i].getDConst()));
break;
case EOpLog:
newConstArray[i].setDConst(log(unionArray[i].getDConst()));
break;
case EOpExp2:
{
const double inv_log2_e = 0.69314718055994530941723212145818;
newConstArray[i].setDConst(exp(unionArray[i].getDConst() * inv_log2_e));
break;
}
case EOpLog2:
{
const double log2_e = 1.4426950408889634073599246810019;
newConstArray[i].setDConst(log2_e * log(unionArray[i].getDConst()));
break;
}
case EOpSqrt:
newConstArray[i].setDConst(sqrt(unionArray[i].getDConst()));
break;
case EOpInverseSqrt:
newConstArray[i].setDConst(1.0 / sqrt(unionArray[i].getDConst()));
break;
case EOpAbs:
if (unionArray[i].getType() == EbtDouble)
newConstArray[i].setDConst(fabs(unionArray[i].getDConst()));
else if (unionArray[i].getType() == EbtInt)
newConstArray[i].setIConst(abs(unionArray[i].getIConst()));
else
newConstArray[i] = unionArray[i];
break;
case EOpSign:
#define SIGN(X) (X == 0 ? 0 : (X < 0 ? -1 : 1))
if (unionArray[i].getType() == EbtDouble)
newConstArray[i].setDConst(SIGN(unionArray[i].getDConst()));
else
newConstArray[i].setIConst(SIGN(unionArray[i].getIConst()));
break;
case EOpFloor:
newConstArray[i].setDConst(floor(unionArray[i].getDConst()));
break;
case EOpTrunc:
if (unionArray[i].getDConst() > 0)
newConstArray[i].setDConst(floor(unionArray[i].getDConst()));
else
newConstArray[i].setDConst(ceil(unionArray[i].getDConst()));
break;
case EOpRound:
newConstArray[i].setDConst(floor(0.5 + unionArray[i].getDConst()));
break;
case EOpRoundEven:
{
double flr = floor(unionArray[i].getDConst());
bool even = flr / 2.0 == floor(flr / 2.0);
double rounded = even ? ceil(unionArray[i].getDConst() - 0.5) : floor(unionArray[i].getDConst() + 0.5);
newConstArray[i].setDConst(rounded);
break;
}
case EOpCeil:
newConstArray[i].setDConst(ceil(unionArray[i].getDConst()));
break;
case EOpFract:
{
double x = unionArray[i].getDConst();
newConstArray[i].setDConst(x - floor(x));
break;
}
case EOpIsNan:
{
newConstArray[i].setBConst(isNan(unionArray[i].getDConst()));
break;
}
case EOpIsInf:
{
newConstArray[i].setBConst(isInf(unionArray[i].getDConst()));
break;
}
// TODO: 3.0 Functionality: unary constant folding: the rest of the ops have to be fleshed out
case EOpSinh:
case EOpCosh:
case EOpTanh:
case EOpAsinh:
case EOpAcosh:
case EOpAtanh:
case EOpFloatBitsToInt:
case EOpFloatBitsToUint:
case EOpIntBitsToFloat:
case EOpUintBitsToFloat:
default:
return 0;
}
}
TIntermConstantUnion *newNode = new TIntermConstantUnion(newConstArray, returnType);
newNode->getWritableType().getQualifier().storage = EvqConst;
newNode->setLoc(getLoc());
return newNode;
}
static void dbDelaz(float *slat, float *slon, float *elat, float *elon,
float *delt, float *dist, float *azim, float *bazim)
{
/* Builtin functions */
/* double tan(), atan(), sin(), cos(), acos(), sqrt(); */
/* Local variables */
static double a, b, c__, z__, celon, elatr, cslon, elonr, selon, a1,
b1, c1, slatr, slonr, sslon, cd, ceclat, bz, cz, eclatr, seclat,
csclat, sclatr, ssclat, aaa, cbz;
/* cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
/* This subroutine calculates the four quantities that depend on the c */
/* relative locations on the globe of epicenter and station. c */
/* Output: delt(angular separation id degrees), dist(geodesic distancec */
/* in kilometers), azim(epicenter-to-station azimuth, in degrees N c */
/* thru E), and bazim(station-to-epicenter azimuth, in degrees N thru E */
/* Input: slat(station latitude), slon(station longitude), elat(epi- c */
/* center latitude), and elon(epicenter longitude). Latitudes(which c */
/* are input as geographic latitudes) are to be given from 0 to 90 c */
/* degrees, +indicating North, and - indicating South. Longitudes c */
/* are to be given from 0 to 180 degrees, + indicating East and - c */
/* indicating West, with 0=Greenwich Meridian. c */
/* cccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc */
/* converting input angles from degrees to radians */
/* (suffix r always denotes radian measure) */
slatr = *slat * (float).01745329;
slonr = *slon * (float).01745329;
elatr = *elat * (float).01745329;
elonr = *elon * (float).01745329;
/* converting geographic latitudes to geocentric latitudes */
aaa = tan(slatr) * (float).996647;
slatr = atan(aaa);
aaa = tan(elatr) * (float).996647;
elatr = atan(aaa);
/* At this point latitudes are converted into colatitudes, which run c
*/
/* from 0 at North Pole to 180 at South Pole. Also, longitudes are c
*/
/* re-evaluated so that they run from 0 to 360 from Greenwich Eastward */
/* sclatr=station colatitude eclatr=epicenter colatitude (radians) */
sclatr = (float)1.570796327 - slatr;
if (slonr >= 0.) {
goto L20;
} else {
goto L10;
}
L10:
slonr += (float)6.283185307;
L20:
eclatr = (float)1.570796327 - elatr;
if (elonr >= 0.) {
goto L40;
} else {
goto L30;
}
L30:
elonr += (float)6.283185307;
L40:
seclat = sin(eclatr);
ceclat = cos(eclatr);
selon = sin(elonr);
celon = cos(elonr);
ssclat = sin(sclatr);
csclat = cos(sclatr);
sslon = sin(slonr);
cslon = cos(slonr);
/* calculation of direction cosines of station (a1,b1,c1) and of c
*/
/* epicenter (a,b,c). For this calculation, x-axis is thru Greenwich c
*/
/* Meridian, y-axis at 90E longitude, and z-axis thru North Pole. c
*/
a = seclat * celon;
b = seclat * selon;
c__ = ceclat;
a1 = ssclat * cslon;
b1 = ssclat * sslon;
c1 = csclat;
/* Now you have all the info necessary to compute delt, dist, azim, */
/* and bazim. For reference: cosine of delta=cd, cosine of azimuth=cz
*/
/* azim. in radians=z, cosine of backazimuth=cbz, and back-azimuth */
/* in radians=bz. */
cd = a * a1 + b * b1 + c__ * c1;
*delt = acos(cd) * (float)57.29577951;
*dist = *delt * (float).01745329252 * (float)6371.;
/* computation of cz and z. The following formula is derivable */
/* from Bullen orig via analytic geometry. */
cz = (seclat * csclat - ceclat * ssclat * (celon * cslon + selon * sslon))
/ sqrt((float)1. - cd * cd);
z__ = acos(cz);
/* The following test determines whether z should be > orig < 180 degrees.
*/
if (sslon * celon - cslon * selon < (float)0.) {
z__ = (float)6.283185307 - z__;
}
*azim = z__ * (float)57.29577951;
/* computation of cbz and bz. This is accomplished by switching the */
/* roles of station and epicenter in the previous formula. */
cbz = (ssclat * ceclat - csclat * seclat * (cslon * celon + sslon * selon)
) / sqrt((float)1. - cd * cd);
bz = acos(cbz);
/* The following test determines whether bz should be > orig < 180 degrees.
*/
if (selon * cslon - celon * sslon < (float)0.) {
bz = (float)6.283185307 - bz;
}
*bazim = bz * (float)57.29577951;
}
void dsmcCLLWallPatch::controlParticle(dsmcParcel& p, dsmcParcel::trackingData& td)
{
measurePropertiesBeforeControl(p);
vector& U = p.U();
label typeId = p.typeId();
scalar& ERot = p.ERot();
label& vibLevel = p.vibLevel();
vector nw = p.normal();
nw /= mag(nw);
// Normal velocity magnitude
scalar U_dot_nw = U & nw;
// Wall tangential velocity (flow direction)
vector Ut = U - U_dot_nw*nw;
Random& rndGen(cloud_.rndGen());
while (mag(Ut) < SMALL)
{
// If the incident velocity is parallel to the face normal, no
// tangential direction can be chosen. Add a perturbation to the
// incoming velocity and recalculate.
U = vector
(
U.x()*(0.8 + 0.2*rndGen.scalar01()),
U.y()*(0.8 + 0.2*rndGen.scalar01()),
U.z()*(0.8 + 0.2*rndGen.scalar01())
);
U_dot_nw = U & nw;
Ut = U - U_dot_nw*nw;
}
// Wall tangential unit vector
vector tw1 = Ut/mag(Ut);
// Info << "tw1 = " << tw1 << endl;
// Other tangential unit vector
vector tw2 = nw^tw1;
const scalar& T = temperature_;
scalar mass = cloud_.constProps(typeId).mass();
scalar rotationalDof = cloud_.constProps(typeId).rotationalDegreesOfFreedom();
scalar vibrationalDof = cloud_.constProps(typeId).vibrationalDegreesOfFreedom();
scalar characteristicVibrationalTemperature = cloud_.constProps(typeId).thetaV();
const scalar& alphaT = tangentialAccommodationCoefficient_*(2.0 - tangentialAccommodationCoefficient_);
const scalar& alphaN = normalAccommodationCoefficient_;
const scalar& alphaR = rotationalEnergyAccommodationCoefficient_;
const scalar& alphaV = vibrationalEnergyAccommodationCoefficient_;
scalar mostProbableVelocity = sqrt(2.0*physicoChemical::k.value()*T/mass);
//normalising the incident velocities
vector normalisedTangentialVelocity = Ut/mostProbableVelocity;
scalar normalisedNormalVelocity = U_dot_nw/mostProbableVelocity;
//normal random number components
scalar thetaNormal = 2.0*pi*rndGen.scalar01();
scalar rNormal = sqrt(-alphaN*log(rndGen.scalar01()));
//tangential random number components
scalar thetaTangential1 = 2.0*pi*rndGen.scalar01();
scalar rTangential1 = sqrt(-alphaT*log(rndGen.scalar01()));
scalar thetaTangential2 = 2.0*pi*rndGen.scalar01();
scalar rTangential2 = sqrt(-alphaT*log(rndGen.scalar01()));
//selecting the reflected thermal velocities
scalar normalisedIncidentTangentialVelocity1 = mag(normalisedTangentialVelocity);
scalar um = sqrt(1.0-alphaN)*normalisedNormalVelocity;
scalar normalVelocity = sqrt(
(rNormal*rNormal)
+ (um*um)
+ 2.0*rNormal*um*cos(thetaNormal)
);
scalar tangentialVelocity1 = (sqrt(1.0 - alphaT)*mag(normalisedIncidentTangentialVelocity1)
+ rTangential1*cos(thetaTangential1));
scalar tangentialVelocity2 = rTangential1*sin(thetaTangential1);
U =
mostProbableVelocity
*(
tangentialVelocity1*tw1
+ tangentialVelocity2*tw2
- normalVelocity*nw
);
if( (p.position().x() > 0.002) && ( p.position().x() < 0.0022) )
{
if(Pstream::parRun())
{
Pout << "Scattering angle, 0 mm = " << atan(U.y()/U.x()) << endl;
}
else
{
scalar angle=0;
if((tangentialVelocity1*tw1).x()>0)
angle = acos( ( (normalVelocity*nw + tangentialVelocity1*tw1) & tangentialVelocity1*tw1 )/mag((normalVelocity*nw+tangentialVelocity1*tw1) * tangentialVelocity1*tw1) );
if((tangentialVelocity1*tw1).x()<0)
angle = acos( ( (normalVelocity*nw - tangentialVelocity1*tw1) & tangentialVelocity1*tw1 )/mag((normalVelocity*nw+tangentialVelocity1*tw1) * tangentialVelocity1*tw1) );
Info << "Scattering angle, 0 mm = " << angle << endl;
}
}
if( (p.position().x() > 0.0068) && ( p.position().x() < 0.0072 ) )
{
if(Pstream::parRun())
{
Pout << "Scattering angle, 5 mm = " << atan(U.y()/U.x()) << endl;
}
else
{
scalar angle=0;
if((tangentialVelocity1*tw1).x()>0)
angle = acos( ( (normalVelocity*nw + tangentialVelocity1*tw1) & tangentialVelocity1*tw1 )/mag((normalVelocity*nw+tangentialVelocity1*tw1) * tangentialVelocity1*tw1) );
if((tangentialVelocity1*tw1).x()<0)
angle = acos( ( (normalVelocity*nw - tangentialVelocity1*tw1) & tangentialVelocity1*tw1 )/mag((normalVelocity*nw+tangentialVelocity1*tw1) * tangentialVelocity1*tw1) );
Info << "Scattering angle, 5 mm = " << angle << endl;
}
}
vector uWallNormal = (velocity_ & nw) * nw;
vector uWallTangential1 = (velocity_ & tw1) * tw1;
vector uWallTangential2 = (velocity_ & tw2) * tw2;
vector UNormal = ((U & nw) * nw) + uWallNormal*alphaN;
vector UTangential1 = (U & tw1) * tw1 + uWallTangential1*alphaT;
vector UTangential2 = (U & tw2) * tw2 + uWallTangential2*alphaT;
U = UNormal + UTangential1 + UTangential2;
//selecting rotational energy, this is Lord's extension to rotational degrees of freedom
if(rotationalDof == 2)
{
scalar om = sqrt( (ERot*(1.0 - alphaR)) / (physicoChemical::k.value()*T));
scalar rRot = sqrt(-alphaR*(log(max(1.0 - rndGen.scalar01(), VSMALL))));
scalar thetaRot = 2.0*pi*rndGen.scalar01();
ERot = physicoChemical::k.value()*T*((rRot*rRot) + (om*om) + (2.0*rRot*om*cos(thetaRot)));
}
if(rotationalDof == 3) //polyatomic case, see Bird's DSMC2.FOR code
{
scalar X = 0.0;
scalar A = 0.0;
do
{
X = 4.0*rndGen.scalar01();
A = 2.7182818*X*X*exp(-(X*X));
} while (A < rndGen.scalar01());
scalar om = sqrt( (ERot*(1.0 - alphaR)) / (physicoChemical::k.value()*T));
scalar rRot = sqrt(-alphaR)*X;
scalar thetaRot = 2.0*rndGen.scalar01() - 1.0;
ERot = physicoChemical::k.value()*T*((rRot*rRot) + (om*om) + (2.0*rRot*om*cos(thetaRot)));
}
// //selecting vibrational energy, this is lord's extension to vibrational degrees of freedom
//
// if(vibrationalDof > VSMALL)
// {
// scalar EVibStar = -log(1.0 - rndGen.scalar01() + rndGen.scalar01()*exp(-characteristicVibrationalTemperature*physicoChemical::k.value()));
//
// EVib += EVibStar;
//
// scalar vm = sqrt(1.0-alphaV)*sqrt(EVib);
//
// scalar thetaVib = 2.0*pi*rndGen.scalar01();
//
// scalar rVib = sqrt(-alphaV*log(rndGen.scalar01()));
//
// EVib = (
// (rVib*rVib)
// + (vm*vm)
// + 2.0*rVib*vm*cos(thetaVib)
// );
//
// label iPost = EVib / (characteristicVibrationalTemperature*physicoChemical::k.value()); //post interaction vibrational quantum level
//
// EVib = iPost * characteristicVibrationalTemperature*physicoChemical::k.value();
// }
// EVib = cloud_.equipartitionVibrationalEnergy(T, vibrationalDof, typeId);
measurePropertiesAfterControl(p, 0.0);
}
CAMLprim value caml_atan_float(value f)
{
return caml_copy_double(atan(Double_val(f)));
}
PExpVal TExpBi::GetBiFuncVal(
const TExpBiId& ExpBiId, const TExpValV& ArgValV, const PExpEnv& ExpEnv){
TExpBiArgType ExpBiArgType=TExpBi::GetExpBiArgType(ExpBiId);
int Args=ArgValV.Len();
double ArgFlt1=0; double ArgFlt2=0;
switch (ExpBiArgType){
case ebatUndef: Fail; break;
case ebatVoid:
AssertArgs(0, Args); break;
case ebatFlt:
AssertArgs(1, Args);
AssertArgValType(evtFlt, ArgValV[0]);
ArgFlt1=ArgValV[0]->GetFltVal(); break;
case ebatFltFlt:
AssertArgs(2, Args);
AssertArgValType(evtFlt, ArgValV[0]);
AssertArgValType(evtFlt, ArgValV[1]);
ArgFlt1=ArgValV[0]->GetFltVal();
ArgFlt2=ArgValV[1]->GetFltVal(); break;
default: Fail;
}
PExpVal ExpVal;
switch (ExpBiId){
// trigonometric funcions
case ebi_Sin: ExpVal=TExpVal::New(sin(ArgFlt1)); break;
case ebi_Cos: ExpVal=TExpVal::New(cos(ArgFlt1)); break;
case ebi_Tan: ExpVal=TExpVal::New(tan(ArgFlt1)); break;
case ebi_ASin: ExpVal=TExpVal::New(asin(ArgFlt1)); break;
case ebi_ACos: ExpVal=TExpVal::New(acos(ArgFlt1)); break;
case ebi_ATan: ExpVal=TExpVal::New(atan(ArgFlt1)); break;
case ebi_SinH: ExpVal=TExpVal::New(sinh(ArgFlt1)); break;
case ebi_CosH: ExpVal=TExpVal::New(cosh(ArgFlt1)); break;
case ebi_TanH: ExpVal=TExpVal::New(tanh(ArgFlt1)); break;
// exponential functions
case ebi_Pow: ExpVal=TExpVal::New(pow(ArgFlt1, ArgFlt2)); break;
case ebi_Exp: ExpVal=TExpVal::New(exp(ArgFlt1)); break;
case ebi_Sqr: ExpVal=TExpVal::New(TMath::Sqr(ArgFlt1)); break;
case ebi_Sqrt: ExpVal=TExpVal::New(sqrt(ArgFlt1)); break;
case ebi_Log: ExpVal=TExpVal::New(log(ArgFlt1)); break;
case ebi_Log10: ExpVal=TExpVal::New(log10(ArgFlt1)); break;
// number manipulation functions
case ebi_Ceil: ExpVal=TExpVal::New(ceil(ArgFlt1)); break;
case ebi_Floor: ExpVal=TExpVal::New(floor(ArgFlt1)); break;
case ebi_Int:{
double Int; modf(ArgFlt1, &Int);
ExpVal=TExpVal::New(Int); break;}
case ebi_Frac:{
double Frac, Int; Frac=modf(ArgFlt1, &Int);
ExpVal=TExpVal::New(Frac); break;}
case ebi_Abs: ExpVal=TExpVal::New(fabs(ArgFlt1)); break;
// random deviates
case ebi_UniDev: ExpVal=TExpVal::New(ExpEnv->GetRnd().GetUniDev()); break;
case ebi_NrmDev: ExpVal=TExpVal::New(ExpEnv->GetRnd().GetNrmDev()); break;
case ebi_ExpDev: ExpVal=TExpVal::New(ExpEnv->GetRnd().GetExpDev()); break;
case ebi_GamDev:{
int ArgInt1=int(ArgFlt1);
ExpVal=TExpVal::New(ExpEnv->GetRnd().GetGammaDev(ArgInt1)); break;}
case ebi_PoiDev:{
ExpVal=TExpVal::New(ExpEnv->GetRnd().GetPoissonDev(ArgFlt1)); break;}
case ebi_BinDev:{
int ArgInt2=int(ArgFlt2);
ExpVal=TExpVal::New(ExpEnv->GetRnd().GetBinomialDev(ArgFlt1, ArgInt2)); break;}
case ebi_UniDevStep:{
int ArgInt1=int(ArgFlt1); if (ArgInt1<0){ArgInt1=0;}
int ArgInt2=int(ArgFlt2);
ExpVal=TExpVal::New(TRnd::GetUniDevStep(ArgInt1, ArgInt2)); break;}
case ebi_NrmDevStep:{
int ArgInt1=int(ArgFlt1); if (ArgInt1<0){ArgInt1=0;}
int ArgInt2=int(ArgFlt2);
ExpVal=TExpVal::New(TRnd::GetNrmDevStep(ArgInt1, ArgInt2)); break;}
case ebi_ExpDevStep:{
int ArgInt1=int(ArgFlt1); if (ArgInt1<0){ArgInt1=0;}
int ArgInt2=int(ArgFlt2);
ExpVal=TExpVal::New(TRnd::GetExpDevStep(ArgInt1, ArgInt2)); break;}
default: TExcept::Throw("Invalid function.");
}
return ExpVal;
}
void Determine::JudgementMouse(CvSeq* circles){
switch (circles->total){
case 0:
OneFinger++;
//for mouse drag judgement,clear variables after being used
if(OneFinger==3){
MouseMoveFlag=0;
if(LeftButton){
mouse_event(MOUSEEVENTF_LEFTUP,0,0,0,0);
LeftButton=false;
}
OneFinger=0;
}
//left click or double clicks action in interval based on one finger relative action
if(Temp==1 || Temp==2){
mouse.LeftClick();
MouseMoveFlag=1800;//for mouse drag judgement
}
Initial();
break;
case 1:
if(CaseFlag!=1)
Initial();
CaseFlag=1;
for(int i=0;i<circles->total;i++){
float* p=(float*)cvGetSeqElem(circles,0);
X1[0]=cvRound(p[0]);
X1[1]=cvRound(p[1]);
}
Temp++;
if(Temp==1){
CooX=X1[0];
CooY=X1[1];
}
MouseMoveFlag++;
if(MouseMoveFlag==1801){
mouse_event(MOUSEEVENTF_LEFTDOWN, 0, 0, 0, 0);//press left button and not release
LeftButton=true;
GetCursorPos(&pt);//get current mouse pointer location
SetCursorPos(pt.x+X1[0]-CooX,pt.y+CooY-X1[1]);//set mouse pointer to a new coordinate
CooX=X1[0];
CooY=X1[1];//replace previous location with current one
MouseMoveFlag=1800;
break;
}
else{
GetCursorPos(&pt);
SetCursorPos(pt.x+X1[0]-CooX,pt.y+CooY-X1[1]);
CooX=X1[0];
CooY=X1[1];
MouseMoveFlag=0;
break;
}
case 2:
if(CaseFlag!=2)
Initial();
CaseFlag=2;
for(int i=0;i<circles->total;i++){
float* p=(float*)cvGetSeqElem(circles,i);
X2[i][0]=cvRound(p[0]);
X2[i][1]=cvRound(p[1]);
}
Temp++;
//initial,calculate the distance between two points and the angle relative to X axis
if(Temp==1){
InitialDistance=(double)(pow((X2[0][0]-X2[1][0]),2)+pow((X2[0][1]-X2[1][1]),2));
InitialAngle=(X2[0][1]+X2[1][1])/(X2[0][0]+X2[1][0]);
InitialAngle=atan(InitialAngle);//angle
InitialDistance=sqrt(InitialDistance);//distance
}
else{
double ProcessedDistance=(double)(pow((X2[0][0]-X2[1][0]),2)+pow((X2[0][1]-X2[1][1]),2));
//calculate the distance of two points in following frame
ProcessedDistance=sqrt(ProcessedDistance);
ProcessedAngle=(X2[0][1]+X2[1][1])/(X2[0][0]+X2[1][0]);//distance
ProcessedAngle=atan(ProcessedAngle);//angle
//calculate two angles in vector
if((InitialAngle-ProcessedAngle)>(3.1415926/20)){
gesture.RotateClockWise();
InitialAngle=ProcessedAngle;
break;
}
//calculate two angles in vector,picture clockwise
if((ProcessedAngle-InitialAngle)>(3.1415926/20)){
gesture.RotateAntiClockWise();
InitialAngle=ProcessedAngle;
break;
}
//picture zoom out
//calclate the distance, only when difference value greater than a certain number will following codes run
if((ProcessedDistance+80)<InitialDistance){
gesture.ZoomIn();
InitialDistance=ProcessedDistance;
break;
}
//picture zoom in
if((ProcessedDistance-80)>InitialDistance){
gesture.ZoomOut();
InitialDistance=ProcessedDistance;
break;
}
//double click
if(abs(ProcessedDistance-InitialDistance)<10 && Temp==6){
mouse.RightClick();
break;
}
}
break;
case 3:
if(CaseFlag!=3)
Initial();//clear all used variable when switch among different fingers action
CaseFlag=3;
ProcessedDistance=0;
for(int i=0;i<circles->total;i++){
float* p = (float*)cvGetSeqElem(circles,i);//get each point's coordinate
ProcessedDistance=ProcessedDistance+cvRound(p[0]);//sum three fingers' X asix value
}
Temp++;
if (Temp==1){
InitialDistance=ProcessedDistance;//initial
}
else{
Judge=WindowsJudge();//distinguish what kind of this window is
if(Judge==2){//if browser
if((ProcessedDistance-180)>InitialDistance){//tag forward
gesture.TagForward();
InitialDistance=ProcessedDistance;//replace initial sum with current sum of x axis
break;
}
if((ProcessedDistance+180)<InitialDistance){//tag back
gesture.TagBack();
InitialDistance=ProcessedDistance;
break;
}
}
//if current window is picture viewer
else if(Judge==1){
if((ProcessedDistance-180)>InitialDistance){//change to next picture
gesture.NextPicture();
InitialDistance=ProcessedDistance;
break;
}
if((ProcessedDistance+180)<InitialDistance){//change to previous picture
gesture.PreviousPicture();
InitialDistance=ProcessedDistance;
break;
}
}
}
break;
case 4:
if(CaseFlag!=4)
Initial();
CaseFlag=4;
ProcessedDistance=0;
for(int i=0;i<circles->total;i++){
float* p = (float*)cvGetSeqElem(circles,i);
ProcessedDistance=ProcessedDistance+cvRound(p[1]);//sum up four fingers' Y axis value
}
Temp++;
if (Temp==1){
InitialDistance=ProcessedDistance;//initial
}
else{
//movement should greater than a certain distance from bottom to top
if((ProcessedDistance-160)>InitialDistance){
gesture.UndoMinimizeAll();//call certain modual
InitialDistance=ProcessedDistance;
break;
}
//move from top to bottom
if((ProcessedDistance+160)<InitialDistance){
InitialDistance=ProcessedDistance;
gesture.MinimizeAll();
break;
}
}
break;
default:
Initial();//do not support more than 4 fingers' actions
break;
}
}
void TRACK::DrawShortNetname( EDA_DRAW_PANEL* panel,
wxDC* aDC, GR_DRAWMODE aDrawMode, EDA_COLOR_T aBgColor )
{
if( ! panel )
return;
/* we must filter tracks, to avoid a lot of texts.
* - only tracks with a length > 10 * thickness are eligible
* and, of course, if we are not printing the board
*/
DISPLAY_OPTIONS* displ_opts = (DISPLAY_OPTIONS*)panel->GetDisplayOptions();
if( displ_opts->m_DisplayNetNamesMode == 0 || displ_opts->m_DisplayNetNamesMode == 1 )
return;
#define THRESHOLD 10
int len = KiROUND( GetLineLength( m_Start, m_End ) );
if( len < THRESHOLD * m_Width )
return;
// no room to display a text inside track
if( aDC->LogicalToDeviceXRel( m_Width ) < MIN_TEXT_SIZE )
return;
if( GetNetCode() == NETINFO_LIST::UNCONNECTED )
return;
NETINFO_ITEM* net = GetNet();
if( net == NULL )
return;
int textlen = net->GetShortNetname().Len();
if( textlen > 0 )
{
// calculate a good size for the text
int tsize = std::min( m_Width, len / textlen );
int dx = m_End.x - m_Start.x ;
int dy = m_End.y - m_Start.y ;
wxPoint tpos = m_Start + m_End;
tpos.x /= 2;
tpos.y /= 2;
// Calculate angle: if the track segment is vertical, angle = 90 degrees
// If horizontal 0 degrees, otherwise compute it
double angle; // angle is in 0.1 degree
if( dy == 0 ) // Horizontal segment
{
angle = 0;
}
else
{
if( dx == 0 ) // Vertical segment
{
angle = 900;
}
else
{
/* atan2 is *not* the solution here, since it can give upside
down text. We want to work only in the first and fourth quadrant */
angle = RAD2DECIDEG( -atan( double( dy ) / double( dx ) ) );
}
}
LAYER_ID curr_layer = ( (PCB_SCREEN*) panel->GetScreen() )->m_Active_Layer;
if( ( aDC->LogicalToDeviceXRel( tsize ) >= MIN_TEXT_SIZE )
&& ( !(!IsOnLayer( curr_layer )&& displ_opts->m_ContrastModeDisplay) ) )
{
if( (aDrawMode & GR_XOR) == 0 )
GRSetDrawMode( aDC, GR_COPY );
tsize = (tsize * 7) / 10; // small reduction to give a better look
DrawGraphicHaloText( panel->GetClipBox(), aDC, tpos,
aBgColor, BLACK, WHITE, net->GetShortNetname(), angle,
wxSize( tsize, tsize ),
GR_TEXT_HJUSTIFY_CENTER, GR_TEXT_VJUSTIFY_CENTER,
tsize / 7,
false, false );
}
}
}
void FFT::fftCurve()
{
int n2 = d_n/2;
double *amp = (double *)malloc(d_n*sizeof(double));
double *result = (double *)malloc(2*d_n*sizeof(double));
if(!amp || !result){
memoryErrorMessage();
return;
}
double df = 1.0/(double)(d_n*d_sampling);//frequency sampling
double aMax = 0.0;//max amplitude
if(!d_inverse){
gsl_fft_real_workspace *work = gsl_fft_real_workspace_alloc(d_n);
gsl_fft_real_wavetable *real = gsl_fft_real_wavetable_alloc(d_n);
if(!work || !real){
memoryErrorMessage();
return;
}
gsl_fft_real_transform(d_y, 1, d_n, real, work);
gsl_fft_halfcomplex_unpack (d_y, result, 1, d_n);
gsl_fft_real_wavetable_free(real);
gsl_fft_real_workspace_free(work);
} else {
gsl_fft_real_unpack (d_y, result, 1, d_n);
gsl_fft_complex_wavetable *wavetable = gsl_fft_complex_wavetable_alloc (d_n);
gsl_fft_complex_workspace *workspace = gsl_fft_complex_workspace_alloc (d_n);
if(!workspace || !wavetable){
memoryErrorMessage();
return;
}
gsl_fft_complex_inverse (result, 1, d_n, wavetable, workspace);
gsl_fft_complex_wavetable_free (wavetable);
gsl_fft_complex_workspace_free (workspace);
}
if (d_shift_order){
for(int i = 0; i < d_n; i++){
d_x[i] = (i - n2)*df;
int j = i + d_n;
double aux = result[i];
result[i] = result[j];
result[j] = aux;
}
} else {
for(int i = 0; i < d_n; i++)
d_x[i] = i*df;
}
for(int i = 0; i < d_n; i++) {
int i2 = 2*i;
double real_part = result[i2];
double im_part = result[i2+1];
double a = sqrt(real_part*real_part + im_part*im_part);
amp[i]= a;
if (a > aMax)
aMax = a;
}
ApplicationWindow *app = (ApplicationWindow *)parent();
QLocale locale = app->locale();
int prec = app->d_decimal_digits;
for (int i = 0; i < d_n; i++){
int i2 = 2*i;
d_result_table->setText(i, 0, locale.toString(d_x[i], 'g', prec));
d_result_table->setText(i, 1, locale.toString(result[i2], 'g', prec));
d_result_table->setText(i, 2, locale.toString(result[i2 + 1], 'g', prec));
if (d_normalize)
d_result_table->setText(i, 3, locale.toString(amp[i]/aMax, 'g', prec));
else
d_result_table->setText(i, 3, locale.toString(amp[i], 'g', prec));
d_result_table->setText(i, 4, locale.toString(atan(result[i2 + 1]/result[i2]), 'g', prec));
}
free(amp);
free(result);
}
//switch between mouse and gesture
void Recognition::MouseGestureSwitch(CvSeq* TouchBlobList){
switch (TouchBlobList->total){
//initial, button release,left click,two left click is double click
case 0:
OneFinger++;// a counter
//for mouse drag,clear variables after being used
if(OneFinger==3){
MouseDragFlag=0;
if(LeftButton){
mouse_event(MOUSEEVENTF_LEFTUP,0,0,0,0);//release left button
LeftButton=false;//flag, left button not pressed
}
OneFinger=0;
}
//left click or double clicks action in interval based on one finger relative action
if(Counter==1 || Counter==2){
mouse.LeftClick();
MouseDragFlag=FlagForMouseDrag;//for mouse drag Recognition
}
Initial();
break;
//mouse drag,mouse move
case 1:
//if switch from another case,first initialize variables
if(CaseFlag!=1)
Initial();
CaseFlag=1;//set CaseFlag equal to 1
for(int i=0;i<TouchBlobList->total;i++){
float* p=(float*)cvGetSeqElem(TouchBlobList,0);
PointForCaseOne.x=cvRound(p[0]);
PointForCaseOne.y=cvRound(p[1]);
}//evaluate
Counter++;
//when first point comes,cursor location is current point location
if(Counter==1){
CursorInitialPosition.x=PointForCaseOne.x;
CursorInitialPosition.y=PointForCaseOne.y;
}
//this is for mouse drag
MouseDragFlag++;
if(MouseDragFlag==(FlagForMouseDrag+1)){
mouse_event(MOUSEEVENTF_LEFTDOWN, 0, 0, 0, 0);//press left button and not release
LeftButton=true;
GetCursorPos(&pt);//get current mouse pointer location
//set cursorlocation according to point relative movement
SetCursorPos(pt.x+PointForCaseOne.x-CursorInitialPosition.x,pt.y+CursorInitialPosition.y-PointForCaseOne.y);
CursorInitialPosition.x=PointForCaseOne.x;
CursorInitialPosition.y=PointForCaseOne.y;//replace previous location with current one
MouseDragFlag=FlagForMouseDrag;
break;
}
else{
//mouse movement, set sursor location according to point relatice movement
GetCursorPos(&pt);
SetCursorPos(pt.x+PointForCaseOne.x-CursorInitialPosition.x,pt.y+CursorInitialPosition.y-PointForCaseOne.y);
CursorInitialPosition.x=PointForCaseOne.x;
CursorInitialPosition.y=PointForCaseOne.y;
MouseDragFlag=0;
break;
}
//right click,clockwise,anticlockwise,zoom in,zoom out
case 2:
if(CaseFlag!=2)//if switch from another case,first initialize variables
Initial();
CaseFlag=2;
for(int i=0;i<TouchBlobList->total;i++){
float* p=(float*)cvGetSeqElem(TouchBlobList,i);
PointForCaseTwo[i].x=cvRound(p[0]);
PointForCaseTwo[i].y=cvRound(p[1]);
}//evaluate
Counter++;
//when first point comes,initialize,calculate the distance between two points and the angle relative to X axis
if(Counter==1){
InitialDistance=(double)(pow(double(PointForCaseTwo[0].x-PointForCaseTwo[1].x),2)+pow(double(PointForCaseTwo[0].y-PointForCaseTwo[1].y),2));
InitialAngle=(PointForCaseTwo[0].y+PointForCaseTwo[1].y)/(PointForCaseTwo[0].x+PointForCaseTwo[1].x);
InitialAngle=atan(InitialAngle);//angle
InitialDistance=sqrt(InitialDistance);//distance
}
else{
double ProcessedDistance=(double)(pow(double(PointForCaseTwo[0].x-PointForCaseTwo[1].x),2)+pow(double(PointForCaseTwo[0].y-PointForCaseTwo[1].y),2));
//calculate the distance of two points in following frame
ProcessedDistance=sqrt(ProcessedDistance);
ProcessedAngle=(PointForCaseTwo[0].y+PointForCaseTwo[1].y)/(PointForCaseTwo[0].x+PointForCaseTwo[1].x);
ProcessedAngle=atan(ProcessedAngle);//angle
//calculate two angles in vector,only beyond a certain angle will work
if((InitialAngle-ProcessedAngle)>(3.1415926/40)){
gesture.RotateClockWise();
InitialAngle=ProcessedAngle;
break;
}else if((ProcessedAngle-InitialAngle)>(3.1415926/40)){//calculate two angles in vector,picture clockwise
gesture.RotateAntiClockWise();
InitialAngle=ProcessedAngle;
break;
}else if((ProcessedDistance+160)<InitialDistance){//picture zoom out
//calclate the distance, only when difference value greater than a certain number will following codes run
gesture.ZoomIn();
InitialDistance=ProcessedDistance;
break;
}else if((ProcessedDistance-160)>InitialDistance){//picture zoom in
gesture.ZoomOut();
InitialDistance=ProcessedDistance;
break;
}else if(abs(ProcessedDistance-InitialDistance)<5 && Counter==6){//double click
mouse.RightClick();
break;
}
}
break;
//prevous or next picture,tag back and forwards
case 3:
if(CaseFlag!=3)
Initial();//clear all used variable when switch among different fingers action
CaseFlag=3;
ProcessedDistance=0;
for(int i=0;i<TouchBlobList->total;i++){
float* p = (float*)cvGetSeqElem(TouchBlobList,i);//get each point's coordinate
ProcessedDistance=ProcessedDistance+cvRound(p[0]);//sum three fingers' X asix value
}
Counter++;
if (Counter==1){
InitialDistance=ProcessedDistance;//initial
}
else{
WindowType=WindowsRecognition();//Recognition what kind of this window is
if(WindowType==2){//if browser
if((ProcessedDistance-180)>InitialDistance){//tag forward
gesture.TagForward();
InitialDistance=ProcessedDistance;//replace initial sum with current sum of x axis
break;
}
if((ProcessedDistance+180)<InitialDistance){//tag back
gesture.TagBack();
InitialDistance=ProcessedDistance;
break;
}
}
//if current window is picture viewer
else if(WindowType==1){
if((ProcessedDistance-180)>InitialDistance){//change to next picture
gesture.NextPicture();
InitialDistance=ProcessedDistance;
break;
}
if((ProcessedDistance+180)<InitialDistance){//change to previous picture
gesture.PreviousPicture();
InitialDistance=ProcessedDistance;
break;
}
}
}
break;
case 4:
//if jump from another case,fist initialize
if(CaseFlag!=4)
Initial();
CaseFlag=4;//set CaseFlag equal to 4
ProcessedDistance=0;
for(int i=0;i<TouchBlobList->total;i++){
float* p = (float*)cvGetSeqElem(TouchBlobList,i);
ProcessedDistance=ProcessedDistance+cvRound(p[1]);//sum up four fingers' Y axis value
}
Counter++;
if (Counter==1){
InitialDistance=ProcessedDistance;//initialize at begin
}
else{
//movement should greater than a certain distance from bottom to top
if((ProcessedDistance-160)>InitialDistance){
gesture.UndoMinimizeAll();//call certain function
InitialDistance=ProcessedDistance;
break;
}
//move from top to bottom
if((ProcessedDistance+160)<InitialDistance){
InitialDistance=ProcessedDistance;
gesture.MinimizeAll();//call certain function
break;
}
}
break;
//do not support more than 4 fingers' actions
default:
Initial();
break;
}
}