Example #1
0
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;
}
Example #2
0
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;
}
Example #4
0
double
g( double x ) {
	return +1.0 + ( atan( 1.0 + x ) + atan( 1.0 - x ) ) / M_PI;
}
Example #5
0
/* 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;
}
Example #6
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; 
  } 
Example #7
0
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); 
}
Example #8
0
#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));
Example #9
0
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;
}
Example #10
0
float Math::arctanDegrees(float i){
	return atan(i) * (180/PI);
}
Example #11
0
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;

}
Example #12
0
float Math::arctanRadians(float i){
	return atan(i);
}
Example #13
0
/*<       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_ */
Example #14
0
	double GusUtils::Atan( double angle )
	{
		return atan( angle );
	}
Example #15
0
  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);
}
Example #17
0
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;
}
Example #18
0
float
atanf (float x)
{
  return (float) atan (x);
}
Example #19
0
 // 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);
}
Example #21
0
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;
}
Example #22
0
//
// 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;
}
Example #23
0
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);
}
Example #25
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 );
        }
    }
}
Example #29
0
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;
		}		
}