Example #1
0
std::vector<sf::Vector2f> BouncyThing::getAxes() {
	//returns a normal to all the sides of the shape
	axes.clear();	
	sf::Vector2f temp;
	float length;
	int points = shape.getPointCount();

	for (int i = 0; i < points; i++) {

		//temp = sf::Vector2f(-(corners[i + 1].y - corners[i].y), corners[i + 1].x - corners[i].x);
		
		if (points - 1 > i) {
			temp = sf::Vector2f(-(corners[i + 1].y - corners[i].y), corners[i + 1].x - corners[i].x);
			length = (float)(sqrt(temp.x * temp.x + temp.y * temp.y));
			axes.push_back(sf::Vector2f(temp.x / length, temp.y / length));
		}
		else {
			temp = sf::Vector2f(-(corners[points - 1].y - corners[0].y), corners[points - 1].x - corners[0].x);
			length = (float)(sqrt(temp.x * temp.x + temp.y * temp.y));
			axes.push_back(sf::Vector2f(temp.x / length, temp.y / length));
		}
		

	}

	return axes;
}
geometry_msgs::Pose MoveitPlanningInterface::convToPose(Vector3f plane_normal, Vector3f major_axis, Vector3f centroid) {
	geometry_msgs::Pose pose;
	Affine3d Affine_des_gripper;
	Vector3d xvec_des,yvec_des,zvec_des,origin_des;
	
	float temp;
	temp = major_axis[0];
	major_axis[0] = major_axis[1];
	major_axis[1] = temp;

	Vector3f rotation = major_axis;
	major_axis[0] = (sqrt(3)/2)*rotation[0] - rotation[1]/2;
	major_axis[1] = (sqrt(3)/2)*rotation[1] + rotation[0]/2;

	Matrix3d Rmat;
	for (int i=0;i<3;i++) {
		origin_des[i] = centroid[i]; // convert to double precision
		zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
		xvec_des[i] = major_axis[i];
	}
//	origin_des[2]+=0.02; //raise up 2cm
	yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
	Rmat.col(0)= xvec_des;
	Rmat.col(1)= yvec_des;
	Rmat.col(2)= zvec_des;
	Affine_des_gripper.linear()=Rmat;
	Affine_des_gripper.translation()=origin_des;
	
	//convert des pose from Affine to geometry_msgs::PoseStamped
	pose = transformEigenAffine3dToPose(Affine_des_gripper);
	return pose;
}
Example #3
0
File: _math.c Project: 10sr/cpython
double
_Py_acosh(double x)
{
    if (Py_IS_NAN(x)) {
        return x+x;
    }
    if (x < 1.) {                       /* x < 1;  return a signaling NaN */
        errno = EDOM;
#ifdef Py_NAN
        return Py_NAN;
#else
        return (x-x)/(x-x);
#endif
    }
    else if (x >= two_pow_p28) {        /* x > 2**28 */
        if (Py_IS_INFINITY(x)) {
            return x+x;
        }
        else {
            return log(x)+ln2;          /* acosh(huge)=log(2x) */
        }
    }
    else if (x == 1.) {
        return 0.0;                     /* acosh(1) = 0 */
    }
    else if (x > 2.) {                  /* 2 < x < 2**28 */
        double t = x*x;
        return log(2.0*x - 1.0 / (x + sqrt(t - 1.0)));
    }
    else {                              /* 1 < x <= 2 */
        double t = x - 1.0;
        return m_log1p(t + sqrt(2.0*t + t*t));
    }
}
Example #4
0
  /*
     Converts an image to 8-bit grayscale and Gaussian-smooths it.  The image is
     optionally doubled in size prior to smoothing.

     @param img input image
     @param img_dbl if true, image is doubled in size prior to smoothing
     @param sig total std of Gaussian smoothing
   */
  void FAST_Laplace::createBaseImage(const cv::Mat& src, cv::Mat& dst,
					                             const bool img_dbl, const double sig)
  {
    cv::Mat grey, dbl;
    double sig_diff;
    cv::Mat fimg;
    src.convertTo( fimg, CV_64FC1 );
    if (src.channels() == 3)
      cv::cvtColor(fimg * 1.0 / 255, grey, CV_BGR2GRAY);
    else if (src.channels() == 1)
      grey = fimg * 1.0 / 255;
    else
    {
	    std::cout << "not an rgb or greyscale image\n";
	    exit (EXIT_FAILURE);
    }

    if (img_dbl)
    {
	    sig_diff = sqrt(sig * sig - 0.5 * 0.5 * 4);

    	cv::resize(grey, dbl, cv::Size(), 2, 2, CV_INTER_CUBIC);
	    cv::GaussianBlur(dbl, dbl, cv::Size(), sig_diff);
	    dbl.convertTo(dst, CV_32FC1);
	    return;
    }
    else
    {
	    sig_diff = sqrt(sig * sig - 0.5 * 0.5);
	    cv::GaussianBlur(grey, grey, cv::Size(), sig_diff);
	    grey.convertTo(dst, CV_32FC1);
	    return;
    }
  }
Example #5
0
double Angle::compute(const Vector& v1,const Vector& v2,Vector& d1,Vector& d2)const{
    const double dp(dotProduct(v1,v2));
    const Vector& dp_dv1(v2);
    const Vector& dp_dv2(v1);
    const double sv1(v1.modulo2());
    const double sv2(v2.modulo2());
    const Vector dsv1_dv1(2*v1);
    const Vector dsv2_dv2(2*v2);
    const double nn(1.0/sqrt(sv1*sv2));
    const Vector dnn_dv1(-0.5*nn/sv1*dsv1_dv1);
    const Vector dnn_dv2(-0.5*nn/sv2*dsv2_dv2);

    const double dpnn(dp*nn);

    if(dpnn>=1.0-epsilon){
      d1=Vector(0.0,0.0,0.0);
      d2=Vector(0.0,0.0,0.0);
      return 0.0;
    }
    if(dpnn<=-1.0+epsilon){
      d1=Vector(0.0,0.0,0.0);
      d2=Vector(0.0,0.0,0.0);
      return pi;
    }
    const Vector ddpnn_dv1(dp*dnn_dv1+dp_dv1*nn);
    const Vector ddpnn_dv2(dp*dnn_dv2+dp_dv2*nn);

    const double x(-1.0/sqrt(1-dpnn*dpnn));

    d1=x*ddpnn_dv1;
    d2=x*ddpnn_dv2;


    return std::acos(dpnn);
}
int 
DwtHaar1D::calApproxFinalOnHost()
{
    // Copy inData to hOutData
    cl_float *tempOutData = (cl_float*)malloc(signalLength * sizeof(cl_float));
    CHECK_ALLOCATION(tempOutData, "Failed to allocate host memory. (tempOutData)");

    memcpy(tempOutData, inData, signalLength * sizeof(cl_float));

    for(cl_uint i = 0; i < signalLength; ++i)
    {
        tempOutData[i] = tempOutData[i] / sqrt((float)signalLength);
    }

    cl_uint length = signalLength;
    while(length > 1u)
    {
        for(cl_uint i = 0; i < length / 2; ++i)
        {
            cl_float data0 = tempOutData[2 * i];
            cl_float data1 = tempOutData[2 * i + 1];

            hOutData[i] = (data0 + data1) / sqrt((float)2);
            hOutData[length / 2 + i] = (data0 - data1) / sqrt((float)2);
        }
        // Copy inData to hOutData
        memcpy(tempOutData, hOutData, signalLength * sizeof(cl_float));

        length >>= 1;
    }

    FREE(tempOutData);
    return SDK_SUCCESS;
}
Example #7
0
//normalize vector
Vector* Vector::normalize()
{
	Vector *vnew=new Vector(x,y);
	vnew->x=x/sqrt(x*x+y*y);	
	vnew->y=y/sqrt(x*x+y*y);
	return vnew;
}
Example #8
0
static double mysqrt(double x)
{
	if(x<0)
		return -sqrt(-x);
	else
		return sqrt(x);
}
Example #9
0
double higs1(double sw, double mw, double ee, double sb, 
	double cb, double hk, double hl, double xvev, double hks, double hls,
	double dlh2)
{
	double neum[32][32];
	int n=3;
	int i, j, f=1;
	double cw=sqrt(1.0-sw*sw);
	double vev=sqrt(2.0)*mw*sw/ee;
	
	neum[0][0]= 1.0/cw/cw*mw*mw*cb*cb+hl*hls/cb*sb*xvev+hk*hl/cb*sb*xvev*xvev;
	neum[1][1]= 1.0/cw/cw*mw*mw*sb*sb+hl*hls/sb*cb*xvev+hk*hl/sb*cb*xvev*xvev
			+2.0*vev*vev*sb*sb*dlh2;
	neum[2][2]=4*hk*hk*xvev*xvev+vev*vev*hl*hls/xvev*cb*sb+hk*hks*xvev;
	
	neum[0][1]=neum[1][0]= -1.0/cw/cw*mw*mw*cb*sb+2.0*vev*vev*cb*sb*hl*hl
			-xvev*hl*(hls+xvev*hk);
	neum[0][2]=neum[2][0]=vev*(2.0*hl*hl*xvev*cb-2*hk*hl*xvev*sb-hl*hls*sb);
	
	neum[1][2]=neum[2][1]=vev*(2.0*hl*hl*xvev*sb-2*hk*hl*xvev*cb-hl*hls*cb);

	
	if(dia_(&n,(double *)neum,(double *)hisr, (double *)hisz))
	{
		puts("CP-even higgs mass matrix diagonalization failed!");
		return 0;
	}

	return 1.0;
}
Example #10
0
int solve_quadratic_eq(const double (*coeffs)[3], double (*roots)[4])
{
	// find discriminant and then calculate roots
	double d = discriminant(coeffs);
	
	int retCode = 0;
	if (d > zero_tol) // two real roots
	{
		double d1 = (-(*coeffs)[1]+sqrt(d))/(2.0* (*coeffs)[0]);
		double d2 = (-(*coeffs)[1]-sqrt(d))/(2.0* (*coeffs)[0]);
		(*roots)[0] = d1;
		(*roots)[1] = d2;
	} else if (d < -zero_tol) // complex roots
	{
		double r = (-(*coeffs)[1])/(2.0*(*coeffs)[0]);
		double w = (sqrt(d)/(2.0*(*coeffs)[0]));
		(*roots)[0] = r;
		(*roots)[1] = w; // first root is x1 = r + iw, real part is roots[0], imaginary is roots[1]
		(*roots)[2] = r;
		(*roots)[3] = -w; // second root is x2 = r - iw, real part is roots[2], imaginary is roots[2]
		retCode = 1;
	} else // two identical real roots
	{
		double r = (-(*coeffs)[1])/(2.0*(*coeffs)[0]);
		(*roots)[0] = r;
		(*roots)[1] = r;
	}
	return retCode;
}
Example #11
0
double lineProfile(const double vy, const double x, const double vz, void * params){

	struct LPPar * par = (struct LPPar*) params;
	double Ve = par->Ve;
	double *Rz = par->Rz;
	double vx = par->vx;

	double R = sqrt(pow(Rz[0],2) + pow(x * (ar[NR-1]),2)), z = Rz[1], y = Rz[0];
	// double vphi = sgn<double>(vx)*sqrt(vx*vx+vy*vy) * sin(atan(y/x));
	// double vr = vy;
	double v_abs = sqrt(vx*vx + vy*vy);
	double alpha = acos(vx/v_abs), theta = atan2(y, x);
	double vr = v_abs * cos(alpha+theta), vphi = v_abs * sin(alpha+theta);

	//Rz[0]=R;
	//Rz[2]=(*p)(R,z);
	//Ve=sqrt(-2*((*p)(R,z)-(*p)(100 * ar[NR-1], 100 * ar[NR-1])));

	/* rescaling the integral to [0,1]x[-1,1]x[0,1] */
	double V[3] = {1e-6+Ve*vr, 1e-6+Ve*vphi, 1e-6+Ve*vz};
	double jacob = Ve*Ve*(fabs(x) * pow(ar[NR-1],2))/R;

	//if (x<0.)
	//	printf("%e %e %e %e %e %e\n",vr,vphi,vz,vx,vy,Ve);

	return jacob*df(Rz,V);
}
Example #12
0
void thermo_heat_up()
{
  int j;
  if(thermo_switch & THERMO_LANGEVIN) {
    langevin_pref2_buffer          = langevin_pref2;
    langevin_pref2 *= sqrt(3);
#ifndef ROTATIONAL_INERTIA
    langevin_pref2_rotation_buffer = langevin_pref2_rotation;
    langevin_pref2_rotation *= sqrt(3);
#else
    for ( j = 0 ; j < 3 ; j++)
    {
    	langevin_pref2_rotation_buffer[j] = langevin_pref2_rotation[j];
    	langevin_pref2_rotation[j] *= sqrt(3);
    }
#endif
#ifdef MULTI_TIMESTEP
    langevin_pref2_small_buffer    = langevin_pref2_small;
    langevin_pref2_small          *= sqrt(3);
#endif
  }
#ifdef DPD
  else if (thermo_switch & THERMO_DPD){dpd_heat_up();}
#endif
#ifdef INTER_DPD
  else if (thermo_switch & THERMO_INTER_DPD) {inter_dpd_heat_up();}
#endif
}
Example #13
0
void thermo_init_npt_isotropic()
{
  if (nptiso.piston != 0.0) {
#if defined (FLATNOISE)
    nptiso_pref1 = -nptiso_gamma0*0.5 * time_step;
#ifdef MULTI_TIMESTEP
    if (smaller_time_step > 0.) 
      nptiso_pref2 = sqrt(12.0*temperature*nptiso_gamma0*time_step) * smaller_time_step;
    else
#endif
      nptiso_pref2 = sqrt(12.0*temperature*nptiso_gamma0*time_step) * time_step;
    nptiso_pref3 = -nptiso_gammav*(1.0/nptiso.piston)*0.5*time_step;
    nptiso_pref4 = sqrt(12.0*temperature*nptiso_gammav*time_step);
#elif defined (GAUSSRANDOMCUT) || defined (GAUSSRANDOM)
    nptiso_pref1 = -nptiso_gamma0*0.5 * time_step;
    nptiso_pref2 = sqrt(1.0*temperature*nptiso_gamma0*time_step) * time_step;
    nptiso_pref3 = -nptiso_gammav*(1.0/nptiso.piston)*0.5*time_step;
    nptiso_pref4 = sqrt(1.0*temperature*nptiso_gammav*time_step);
#else
#error No Noise defined
#endif
    THERMO_TRACE(fprintf(stderr,"%d: thermo_init_npt_isotropic: nptiso_pref1=%f, nptiso_pref2=%f, nptiso_pref3=%f, nptiso_pref4=%f \n",this_node,nptiso_pref1,nptiso_pref2,nptiso_pref3,nptiso_pref4));
  }
  else {
    thermo_switch = ( thermo_switch ^ THERMO_NPT_ISO );
    THERMO_TRACE(fprintf(stderr,"%d: thermo_init_npt_isotropic: switched off nptiso (piston=%f; thermo_switch=%d) \n",this_node,nptiso.piston,thermo_switch));
  }
}
Example #14
0
void
quat_q_from_R (quaternion_t q, const double *R, const int ldim)
{
  double c2;

  c2 = ((R[0] + R[ldim + 1] + R[2*ldim + 2]) + 1.0) / 4.0;
  if (c2 < 0.0 || 1.0 < c2)
    {
      quat_copy (q, quat_one);
      return;
    }

  quat_re (q) = sqrt (c2);
  if (quat_re (q) > QUAT_EPS)
    {
      quat_im (q, 0) = (R[ldim + 2] - R[2*ldim + 1]) / 4.0 / quat_re (q);
      quat_im (q, 1) = (R[2*ldim + 0] - R[2]) / 4.0 / quat_re (q);
      quat_im (q, 2) = (R[1] - R[ldim]) / 4.0 / quat_re (q);
    }
  else
    {
      quat_im (q, 0) = sqrt((R[0] + 1.0)/2.0 - c2);
      quat_im (q, 1) = sqrt((R[ldim + 1] + 1.0)/2.0 - c2);
      quat_im (q, 2) = sqrt((R[2*ldim + 2] + 1.0)/2.0 - c2);
    }

  quat_normalize (q);
}
Example #15
0
int g1(double* a, double* b, double* cterm, double* sterm, double* sig)
{
  /* System generated locals */
  double d;
 
  static double xr, yr;
 
 
  if (nnls_abs(*a) > nnls_abs(*b)) {
    xr = *b / *a;
    /* Computing 2nd power */
    d = xr;
    yr = sqrt(d * d + 1.);
    d = 1. / yr;
    *cterm = d_sign(d, *a);
    *sterm = *cterm * xr;
    *sig = nnls_abs(*a) * yr;
    return 0;
  }
  if (*b != 0.) {
    xr = *a / *b;
    /* Computing 2nd power */
    d = xr;
    yr = sqrt(d * d + 1.);
    d = 1. / yr;
    *sterm = d_sign(d, *b);
    *cterm = *sterm * xr;
    *sig = nnls_abs(*b) * yr;
    return 0;
  }
  *sig = 0.;
  *cterm = 0.;
  *sterm = 1.;
  return 0;
} /* g1_ */
Example #16
0
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  float sqr_threshold = static_cast<float> (radius_max_ * radius_max_);
  distances.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
      // Need to estimate sqrt here to keep MSAC and friends general
      distances[i] = sqrt (sqr_distance);
    else
      // Penalize outliers by doubling the distance
      distances[i] = 2 * sqrt (sqr_distance);
  }
}
Example #17
0
int resoudre2(int a,int b,int c, float *x1, float *x2)
{
  int delta;
  printf("%d*x^2+%d*x+%d\n",a,b,c);
  if(a==0)
   {
    if(b == 0)
      return 0;
    else
     {
       *x1 = (float)(-c/b);
       return 1;
     }
  }
  else
  {
    delta = pow(b,2)-4*a*c;
    if(delta==0)
     {
      *x1=(float)(-b)/(2*a);
      return 1;
     }
    else if(delta>0)
     {
      *x1=(-b-sqrt(delta))/(2*a);
      *x2=(-b+sqrt(delta))/(2*a);
      return 2;
     }
     else
      return 0;
   }
}
Example #18
0
void QR::Normalize(int col) {
 cd d = zero, p;
 CDArray nrm;
 if(col == 0) {
  coeffs_.reserve(r_.size());
  for(int i = 0; i < r_.size(); ++i) {
   coeffs_.push_back(zero);
  }
 }
 for(int i = col; i < coeffs_.size(); ++i) {
  d += (r_[i][col] * r_[i][col]);
 }
 p = sqrt(d);
 d = zero;
 nrm.reserve(coeffs_.size() - col);
 for(int i = col; i < coeffs_.size(); ++i) {
  nrm.push_back(r_[i][col] / p);
  d += (nrm[i - col] * nrm[i - col]);
 }
 d = sqrt(d);
 if(nrm[0].real() > 0.) {
  d *= mone;
 } 
 for(int i = 0; i < coeffs_.size(); ++i) {
  if(i < col) {
   coeffs_[i] = zero;
  } else if(i == col) {
   coeffs_[i] = sqrt((one - (nrm[i - col] / d)) / two);
   p = mone * coeffs_[i] * d;
  } else {
   coeffs_[i] = nrm[i - col] / p / two; 
  }
 }
}
Example #19
0
//stereo bus
double *maxiMix::stereo(double input,double two[2],double x) {
	if (x>1) x=1;
	if (x<0) x=0;
	two[0]=input*sqrt(1.0-x);
	two[1]=input*sqrt(x);
	return(two);
} 
Example #20
0
bool InstrVAcc::Redraw2D (SURFHANDLE surf)
{
	int i;
	float yofs;
	VECTOR3 V;
	double vspd;
	double t = oapiGetSimTime();
	if (vessel->GetAirspeedVector (FRAME_HORIZON, V)) {
		vspd = V.y; // unit is 10m
	} else {
		vspd = 0.0;
	}
	if (t > pt) {
		const double yscale = 6.2455;
		double vacc = (vspd-pvspd)/(t-pt);
		yofs = -(float)((vacc >= 0.0 ? sqrt(min(40,vacc)) : -sqrt(min(40,-vacc))) * yscale);
		static const float y0[3] = {vtape_ycnt, vtape_ycnt-6, vtape_ycnt+6};
		for (i = 0; i < 3; i++)
			grp->Vtx[vtxofs+i].y = y0[i] + yofs;
	}
	pvspd = vspd;
	pt = t;

	return false;
}
Example #21
0
/*
 * Get the spectrum of the oscillator for the UI
 */
void OscilGen::getspectrum(int n, REALTYPE *spc, int what)
{
    if(n > OSCIL_SIZE / 2)
        n = OSCIL_SIZE / 2;

    for(int i = 1; i < n; i++) {
        if(what == 0)
            spc[i - 1] = sqrt(oscilFFTfreqs.c[i] * oscilFFTfreqs.c[i]
                              + oscilFFTfreqs.s[i] * oscilFFTfreqs.s[i]);
        else {
            if(Pcurrentbasefunc == 0)
                spc[i - 1] = ((i == 1) ? (1.0) : (0.0));
            else
                spc[i - 1] = sqrt(basefuncFFTfreqs.c[i] * basefuncFFTfreqs.c[i]
                                  + basefuncFFTfreqs.s[i]
                                  * basefuncFFTfreqs.s[i]);
        }
    }

    if(what == 0) {
        for(int i = 0; i < n; i++)
            outoscilFFTfreqs.s[i] = outoscilFFTfreqs.c[i] = spc[i];
        for(int i = n; i < OSCIL_SIZE / 2; i++)
            outoscilFFTfreqs.s[i] = outoscilFFTfreqs.c[i] = 0.0;
        adaptiveharmonic(outoscilFFTfreqs, 0.0);
        for(int i = 0; i < n; i++)
            spc[i] = outoscilFFTfreqs.s[i];
        adaptiveharmonicpostprocess(spc, n - 1);
    }
}
SphereInterpolator::SphereInterpolator(const string& lutPath)
{
  m_ValidState = true;
  if (lutPath.length()==0)
  {
    if (!LoadLookuptables())
    {
      m_ValidState = false;
      return;
    }
  }
  else
  {
    if (!LoadLookuptables(lutPath))
    {
      m_ValidState = false;
      return;
    }
  }

  size = 301;
  sN = (size-1)/2;
  nverts = ODF_SAMPLING_SIZE;
  beta = 0.5;

  inva = (sqrt(1+beta)-sqrt(beta));
  b = 1/(1-sqrt(1/beta + 1));
}
void f1(int a,int b,int c)
{
	float x,y;
	x=(-b+sqrt(b*b-4*a*c))/(2*a);
	y=(-b-sqrt(b*b-4*a*c))/(2*a);
	printf("此方程根为%f,%f",x,y);
}
Example #24
0
bool Sphere::rayIntersection(
    const Ray &ray,
    Vector3D *intersectionPoint,
    Vector3D *normal) const {
  // (p + t * d - c) ^ 2 = r ^ 2
  Vector3D pmc = ray.point - center_;
  double a = Vector3D::dot(ray.direction, ray.direction);
  double b = 2 * Vector3D::dot(ray.direction, pmc);
  double c = Vector3D::dot(pmc, pmc) - radius_ * radius_;

  double disc = b * b - 4 * a * c;
  if (disc < 0.0) {
    return false;
  }

  double t1 = (- b - sqrt(disc)) / (2 * a);
  double t2 = (- b + sqrt(disc)) / (2 * a);
  if (t2 < EPS) {
    return false;
  } else {
    double t = t2;
    if (t1 > EPS) {
      t = t1;
    }
    *intersectionPoint = ray.point + t * ray.direction;
    *normal = *intersectionPoint - center_;
    return true;
  }
}
Example #25
0
void ComputeForces(Particle *particles, Particle *recvbuf,int rlen,float *max_f){
int i, j;
float xi, yi, mi, rx, ry, mj, r, fx, fy;
float xnew, ynew, rmin;

/* Compute forces (2D only) */
for (i=0; i<BODIES; i++) {
    rmin = 100.0;
    xi   = particles[i].x;
    yi   = particles[i].y;
    fx   = 0.0;
    fy   = 0.0;
    for (j=0; j<rlen; j++) {
	rx = xi - recvbuf[j].x;
	ry = yi - recvbuf[j].y;
	mj = recvbuf[j].m;
	r  = rx * rx + ry * ry;
	/* ignore overlap and same particle */
	if (r == 0.0) continue;
	if (r < rmin) rmin = r;
	/* compute forces */
	r  = r * sqrt(r);
	fx += mj * rx / r;
	fy += mj * ry / r;
	}
    particles[i].fx -= fx;
    particles[i].fy -= fy;
    /* Compute a rough estimate of (1/m)|df / dx| */
    fx = sqrt(fx*fx + fy*fy)/rmin;
    if (fx > *max_f) *max_f = fx;
    }
}
Example #26
0
/**
 * Deconvolves a Gaussian "beam" from a Gaussian component.  
 * Routine translated from the AIPSish APL/SUB/DECONV.FOR/DECONV  
 * \param fmaj    Convolved major axis 
 * \param fmin    Convolved minor axis 
 * \param fpa     Convolved position angle of major axis 
 * \param cmaj    Beam major axis 
 * \param cmin    Beam minor axis 
 * \param cpa     Beam position angle of major axis 
 * \param rmaj    [out] Actual major axis; = 0 => unable to fit 
 * \param rmin    [out] Actual  minor axis; = 0 => unable to fit 
 * \param rpa     [out] Actual position angle of major axis 
 */
void ObitConvUtilDeconv (ofloat fmaj, ofloat fmin, ofloat fpa, 
			 ofloat cmaj, ofloat cmin, ofloat cpa, 
			 ofloat *rmaj, ofloat *rmin, ofloat *rpa)
{
  ofloat      cmj2, cmn2, fmj2, fmn2, sinc, cosc, rhoc, 
    sigic2, det, rhoa, lfpa, lcpa, konst = 28.647888;
  olong csux;

  /* Get useful constants */
  csux = (olong) ((fpa+900.0)/180.0);
  lfpa = (fpa+900.0) - csux*180.0;
  csux = (olong) ((cpa+900.0)/180.0);
  lcpa = (cpa+900.0) - csux*180.0;

  cmj2 = cmaj * cmaj;
  cmn2 = cmin * cmin;
  fmj2 = fmaj * fmaj;
  fmn2 = fmin * fmin;
  sinc = (lfpa - lcpa) / konst;
  cosc = cos (sinc);
  sinc = sin (sinc);

  /* Trigonometry now */
  rhoc = (fmj2 - fmn2) * cosc - (cmj2 - cmn2);
  if (rhoc == 0.0) {
    sigic2 = 0.0;
    rhoa = 0.0;
  } else {
    sigic2 = atan((fmj2 - fmn2) * sinc / rhoc);
    rhoa = ((cmj2 - cmn2) - (fmj2 - fmn2) * cosc) / (2.0 * cos (sigic2));
  } 
  (*rpa) = sigic2 * konst + lcpa;
  det = ((fmj2 + fmn2) -(cmj2 + cmn2)) / 2.0;
  (*rmaj) = det - rhoa;
  (*rmin) = det + rhoa;

  /* Swap to get major > minor */
  (*rmaj) = MAX (0.0, *rmaj);
  (*rmin) = MAX (0.0, *rmin);
  (*rmaj) = sqrt (fabs (*rmaj));
  (*rmin) = sqrt (fabs (*rmin));
  if (*rmaj < *rmin) {
    sinc = (*rmaj);
    (*rmaj) = (*rmin);
    (*rmin) = sinc;
    (*rpa) = (*rpa)+90.0;
  } 

  /* Fix up PA */
  csux = (olong) ((*rpa+900.0)/180.0);
  *rpa = (*rpa+900.0) - csux*180.0;
  if (*rmaj == 0.0) {
    (*rpa) = 0.0;
  } else if (*rmin == 0.0) {
    if ((fabs(*rpa-lfpa) > 45.0)  &&  (fabs(*rpa-lfpa) < 135.0)) {
      csux = (olong) ((*rpa+450.0)/180.0);
      *rpa = (*rpa+450.0) - csux*180.0;
    }
  } 
} /* end of routine ObitConvUtilDeconv */ 
Example #27
0
MatrixDouble hexagonalLattice(int n, int n1) {
	MatrixDouble coord;
	int i=0, l=0, j =0;
	int n_pro_level = n1;
	int sum = n1;
	int dx=0;
	double y=0;

	coord.resize(n);
	while (i<n) {
		if (i==(sum)) {	// Uebergang zu dem naechsten Level
			l++;
			if (sum<=n/2) {
				dx += l%2;
				n_pro_level = n_pro_level + l%2;
			}
			else {
				dx -= l%2;
				n_pro_level = n_pro_level - l%2;
			}
			sum += n_pro_level;
			y = y+ (1+(l+1)%2)*1/2.;
		}

		coord[i].resize(2);
		coord[i][0] = -dx*sqrt(3)/2.+j* sqrt(3);
		coord[i][1] = y;

		j = (j+1)%n_pro_level;
		i++;
	}

	return coord;
}
Double_t dijet_PFNtuple::getSumHcalE_trackDiffEcal(int tagJet) const {
  Double_t sum=0;
  if (tagJet) {
    for (unsigned int i=0; i<tpfjet_had_id->size(); ++i) {
      int trackIdx= tpfjet_had_candtrackind->at(i);
      if ((tpfjet_had_id->at(i) == 0) && // charged hadron
	  (trackIdx>-1)    // has a track
	  && (tpfjet_had_ntwrs->at(i) == 0)  // has no recHits
	  ) {
	sum += sqrt( pow(tpfjet_candtrack_px->at(trackIdx),2) +
		     pow(tpfjet_candtrack_py->at(trackIdx),2) +
		     pow(tpfjet_candtrack_pz->at(trackIdx),2) )
	  - tpfjet_had_EcalE->at(i);
      }
    }
  }
  else {
    for (unsigned int i=0; i<ppfjet_had_id->size(); ++i) {
      int trackIdx= ppfjet_had_candtrackind->at(i);
      if ((ppfjet_had_id->at(i) == 0) && // charged hadron
	  (trackIdx>-1)    // has a track
	  && (ppfjet_had_ntwrs->at(i) == 0)  // has no recHits
	  ) {
	sum += sqrt( pow(ppfjet_candtrack_px->at(trackIdx),2) +
		     pow(ppfjet_candtrack_py->at(trackIdx),2) +
		     pow(ppfjet_candtrack_pz->at(trackIdx),2) )
	  - ppfjet_had_EcalE->at(i);
      }
    }
  }
 return sum;
}
Example #29
0
File: _math.c Project: 10sr/cpython
double
_Py_asinh(double x)
{
    double w;
    double absx = fabs(x);

    if (Py_IS_NAN(x) || Py_IS_INFINITY(x)) {
        return x+x;
    }
    if (absx < two_pow_m28) {           /* |x| < 2**-28 */
        return x;                       /* return x inexact except 0 */
    }
    if (absx > two_pow_p28) {           /* |x| > 2**28 */
        w = log(absx)+ln2;
    }
    else if (absx > 2.0) {              /* 2 < |x| < 2**28 */
        w = log(2.0*absx + 1.0 / (sqrt(x*x + 1.0) + absx));
    }
    else {                              /* 2**-28 <= |x| < 2= */
        double t = x*x;
        w = m_log1p(absx + t / (1.0 + sqrt(1.0 + t)));
    }
    return copysign(w, x);

}
Example #30
0
void Lgm_InitS( double S[14][14], int N ) {

    register int n, m;
    int          nm1, nm1_2;

    for ( n=0; n<=N; n++ ) {
        for ( m=0; m<=N; m++ ) {
            S[n][m] = 0.0;
        }
    }

    
    S[0][0] = 1.0;

    for ( m=0; m<=N; m++ ) {
        for ( n=1; n<=N; n++ ) {

            if ( m<=n){
                if ( m==0 ) {
                    S[n][0] = S[n-1][0]*(double)(2*n-1)/(double)n;
                } else if ( m==1) {
                    S[n][m] = S[n][m-1]*sqrt( (double)(2*(n-m+1))/((double)(n+m)) );
                } else {
                    S[n][m] = S[n][m-1]*sqrt( (double)(n-m+1)/((double)(n+m)) );
                }
            }

//printf("S[%d][%d] = %g\n", n, m, S[n][m] );
        }
    }
    
    

}