Example #1
0
void meCloseHoles::ComputePatchForHole(vtkPolyDataNormals* normals, vtkPoints* points, vtkCellArray* polys)
{
	vtkSmartPointer<vtkPointLocator> locator = vtkSmartPointer<vtkPointLocator>::New();
	locator->SetDataSet(this->input);
	locator->BuildLocator();

	int numEdgesInRegion = 0,
		nUsedIds,
		pointId,
		pointId1,
		pointId2;

	vtkSmartPointer<vtkCleanPolyData> clean = vtkSmartPointer<vtkCleanPolyData>::New();
	vtkIdList* ptIds = vtkIdList::New(),
		* ptIds1 = vtkIdList::New(),
		* ptIds2 = vtkIdList::New(),
		* ptIdsOrdered = vtkIdList::New(),
		* cellIds = vtkIdList::New(),
		* cellIdsOrdered = vtkIdList::New();

	vector<int> nptsInCorona,
		nptsUntilCorona;

	vector< vector<bool> > onlyOneCorrespondingPoint;
	vector< vector<double> > normalsAtPoint;

	////For each hole (region) the points are stored and sorted
	//for(int idRegion = 0; idRegion < numRegions;	idRegion++ )
	//{
	numEdgesInRegion = inputHole->GetNumberOfLines();
	clean->SetInput(inputHole);
	clean->Update();    

	//For each hole, point and connectivity lists (not sorted) are created.
	int npts = clean->GetOutput()->GetNumberOfPoints();
	clean->GetOutput()->BuildLinks(); //in order to sort
	clean->GetOutput()->BuildCells();
	clean->GetOutput()->Update();

	double center[3]={0,0,0},
		point[3],
		point1[3],
		point2[3];

	//points = vtkPoints::New();
	ptIdsOrdered->Reset();
	cellIdsOrdered->Reset();

	int idUsed = 0;
	for(int id=0; id< numEdgesInRegion; id++)
	{
		cellIdsOrdered->InsertNextId(idUsed);
		clean->GetOutput()->GetCellPoints(idUsed,ptIds);
		ptIdsOrdered->InsertNextId(ptIds->GetId(0));
		clean->GetOutput()->GetPointCells(ptIds->GetId(1),cellIds);
		idUsed = (cellIds->GetId(0) == idUsed ? cellIds->GetId(1) : idUsed=cellIds->GetId(0));
		clean->GetOutput()->GetPoint(ptIds->GetId(0),point);
		points->InsertNextPoint(point);
	}
	for(int id=0; id<npts; id++)
	{
		clean->GetOutput()->GetPoint(id,point);
		for(int i=0; i<3; i++) center[i]+=point[i];
	}
	for(int i=0; i<3; i++) center[i]/=npts;

	double normal[3]={0,0,0};
	double area=0,
		theta,
		R,
		a,
		l,
		b,
		mn;
	if(this->closeHoleAlgorithm == SMOOTH_TO_CENTER || this->closeHoleAlgorithm == SMOOTH_PATCH)
	{
		points->GetPoint(npts-1,point1);
		for(int i=0; i<3; i++) point1[i]-=center[i];
		for(int id=0; id< npts; id++)
		{
			points->GetPoint(id,point);
			for(int i=0; i<3; i++)
			{
				point[i]-=center[i];
				point2[i]=point[i]-point1[i];
			}
			for(int i=0; i<3; i++) normal[i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
			for(int i=0; i<3; i++) point1[i]=point2[i];
		}
		for(int i=0; i<3; i++) area+=pow(normal[i],2);
		area=sqrt(area);
		for(int i=0; i<3; i++) normal[i]/=area;
	}
	int numberOfCoronas=( this->closeHoleAlgorithm==SINGLE_CENTER_POINT ? 1 : npts/6+1 );
	if(this->closeHoleAlgorithm == SMOOTH_PATCH) numberOfCoronas*=5;
	nptsInCorona.clear();
	nptsInCorona.resize(numberOfCoronas);
	nptsUntilCorona.clear();
	nptsUntilCorona.resize(numberOfCoronas+1);
	nptsUntilCorona[0]=0;
	nptsInCorona[0]=npts;
	onlyOneCorrespondingPoint.clear();
	onlyOneCorrespondingPoint.resize(numberOfCoronas);
	onlyOneCorrespondingPoint[0].resize(npts);
	if(this->closeHoleAlgorithm == SMOOTH_PATCH)
	{
		normalsAtPoint.resize(npts);
		for(int id=0; id<npts; id++)
		{
			normalsAtPoint[id].clear();
			normalsAtPoint[id].resize(3);
			points->GetPoint(id,point);
			pointId=locator->FindClosestPoint(point);
			normals->GetOutput()->GetPointCells(pointId,cellIds);
			//				for(int i=0; i<3; i++) normalsAtPoint[id][i]=0;
			a=cellIds->GetNumberOfIds();
			for(int j=0; j<a; j++)
			{
				normals->GetOutput()->GetCellPoints(cellIds->GetId(j),ptIds);
				int sameId;
				if(ptIds->GetId(0)==pointId) sameId=0;
				else if(ptIds->GetId(1)==pointId) sameId=1;
				else sameId=2;
				this->input->GetPoint(ptIds->GetId((sameId+1)%3),point1);
				this->input->GetPoint(ptIds->GetId((sameId+2)%3),point2);
				for(int i=0; i<3; i++)
				{
					point1[i]-=point[i];
					point2[i]-=point[i];
				}
				for(int i=0; i<3; i++)
				{
					normalsAtPoint[id][i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
				}
				area=0;
				for(int i=0; i<3; i++) area+=pow(normalsAtPoint[id][i],2);
				area=sqrt(area);
				for(int i=0; i<3; i++) normalsAtPoint[id][i]/=area;
			}
		}
	}

	double typicalLengthSq=0,
		lengthSq;
	clean->GetOutput()->GetPoint(0,point);
	for(int i=0; i<3; i++) typicalLengthSq+=pow(point[i]-center[i],2);
	typicalLengthSq/=numberOfCoronas*numberOfCoronas;
	bool signChange = true;

	for(int c=1; c<numberOfCoronas; c++)
	{
		nptsUntilCorona[c]=nptsUntilCorona[c-1]+nptsInCorona[c-1];
		nUsedIds=0;
		if(this->closeHoleAlgorithm == SMOOTH_PATCH)
		{
			/*				double displacement=1e10;
			for(int id=0; id<nptsInCorona[c-1]; id++)
			{
			points->GetPoint(nptsUntilCorona[c-1]+id,point);
			a=0;
			b=0;
			mn=0;
			for(int i=0; i<3; i++)
			{
			point[i]-=center[i];
			a+=point[i]*normalsAtPoint[id][i];
			b+=point[i]*normal[i];
			mn+=normalsAtPoint[id][i]*normal[i];
			}
			l=(mn*a-b)/(1-pow(mn,2));
			if(firstDisplacement)
			{
			if(displacement>l) displacement=l;
			firstDisplacement = false;
			}
			else
			{
			if(displacement>l) displacement=l;
			} 
			}
			for(int i=0; i<3; i++)
			{
			center[i]+=displacement*normal[i];
			}*/
			for(int i=0; i<3; i++)
			{
				center[i]=0;
				for(int id=0; id<nptsInCorona[c-1]; id++) center[i]+=points->GetPoint(nptsUntilCorona[c-1]+id)[i];
				center[i]/=nptsInCorona[c-1];
			}
		}
		for(int id=0; id<nptsInCorona[c-1]; id++)
		{
			points->GetPoint(id+nptsUntilCorona[c-1],point1);
			points->GetPoint((id+1)%nptsInCorona[c-1]+nptsUntilCorona[c-1],point2);
			lengthSq=0;
			for(int i=0; i<3; i++) lengthSq+=pow(point1[i]-point2[i],2);
			onlyOneCorrespondingPoint[c-1][id]=
				((this->closeHoleAlgorithm==LINEAR_TO_CENTER || this->closeHoleAlgorithm==SMOOTH_TO_CENTER ) 
				&& lengthSq<typicalLengthSq && (id==0 || !onlyOneCorrespondingPoint[c-1][id-1]) 
				&& nptsInCorona[c-1]>3	// Added to correct bad behaviour in extrange contours.
				);
			if(!onlyOneCorrespondingPoint[c-1][id])
			{
				switch(this->closeHoleAlgorithm)
				{
				case RADIAL_TRIANGLES :
					for(int i=0; i<3; i++)
					{
						point[i]=(point1[i]*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1);
					}
					break;
				case LINEAR_TO_CENTER :
				case LINEAR_TO_CENTER_NONREDUCING_TRIANGLES :
					for(int i=0; i<3; i++)
					{
						point[i]=((point1[i]+point2[i])/2*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1);
					}
					break;
				case SMOOTH_TO_CENTER :
					a=0;
					l=0;
					for(int i=0; i<3; i++)
					{
						point[i]=(point1[i]+point2[i])/2-center[i];
						a+=point[i]*normal[i];
						l+=pow(point[i],2);
					}
					R=l/a/2;
					theta=asin(a/sqrt(l));
					theta=2*theta*(1.-1./(numberOfCoronas-c+1));
					if(fabs(theta)<0.001)
					{
						l=pow(a,2)/l;
						double fractionAngle=1.-1./(numberOfCoronas-c+1);
						for(int i=0; i<3; i++)
						{
							point[i]=pow(fractionAngle,2)*a*normal[i]+center[i]
							+fractionAngle/sqrt(1-l)*(point[i]-a*normal[i]);
						}
					}
					else
					{
						for(int i=0; i<3; i++)
						{
							point[i]=-R*cos(theta)*normal[i]
							+R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normal[i])
								+center[i]+R*normal[i];
						}
					}
					break;
				case SMOOTH_PATCH :
					points->GetPoint(nptsUntilCorona[c-1]+id,point);
					a=0;
					b=0;
					mn=0;
					for(int i=0; i<3; i++)
					{
						point[i]-=center[i];
						a+=point[i]*normalsAtPoint[id][i];
						b+=point[i]*normal[i];
						mn+=normalsAtPoint[id][i]*normal[i];
					}
					l=(mn*a-b)/(1-pow(mn,2));
					signChange = (l>0);

					a=0;
					l=0;
					b=0;
					mn=0;
					for(int i=0; i<3; i++)
					{
						point[i]=-(point1[i]+point2[i])/2+center[i];
						a+=point[i]*normalsAtPoint[id][i];
						l+=pow(point[i],2);
					}
					R=l/a/2;
					theta=asin(a/sqrt(l));
					double fractionAngle=1./(numberOfCoronas-c+1);
					if(signChange) fractionAngle*=-1;
					theta=2*theta*fractionAngle;
					if(fabs(theta)<0.001)
					{
						l=pow(a,2)/l;
						for(int i=0; i<3; i++)
						{
							point[i]=pow(fractionAngle,2)*a*normalsAtPoint[id][i]
							+center[i]-point[i]
							+fractionAngle/sqrt(1-l)*(point[i]-a*normalsAtPoint[id][i]);
						}
					}
					else
					{
						for(int i=0; i<3; i++)
						{
							point[i]=-R*cos(theta)*normalsAtPoint[id][i]
							+R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normalsAtPoint[id][i])
								+center[i]-point[i]+R*normalsAtPoint[id][i];
						}
					}
					for(int i=0; i<3; i++)
					{
						point1[i]-=point[i];
						point2[i]-=point[i];
					}
					for(int i=0; i<3; i++)
					{
						normalsAtPoint[nUsedIds][i]=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
					}
					area=0;
					for(int i=0; i<3; i++) area+=pow(normalsAtPoint[nUsedIds][i],2);
					area=sqrt(area);
					for(int i=0; i<3; i++) normalsAtPoint[nUsedIds][i]/=area;
					break;
				}
				nUsedIds++;
				points->InsertNextPoint(point);
			}
		}
		nptsInCorona[c]=nUsedIds;
		onlyOneCorrespondingPoint[c].resize(nUsedIds);
	}
	nptsUntilCorona[numberOfCoronas]=nptsUntilCorona[numberOfCoronas-1]+nptsInCorona[numberOfCoronas-1];
	points->InsertNextPoint(center);

//	polys = vtkCellArray::New(); //A polygon is created

	for(int c=0; c<numberOfCoronas-1; c++)
	{
		int id2=0;
		for(int id=0; id<nptsInCorona[c] ; id++)
		{
			if(onlyOneCorrespondingPoint[c][id])
			{
				ptIds1->Reset();
				ptIds1->InsertNextId(id+nptsUntilCorona[c]);
				ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
				ptIds1->InsertNextId(id2%nptsInCorona[c+1]+nptsUntilCorona[c+1]);
				polys->InsertNextCell(ptIds1);
			}
			else
			{
				ptIds1->Reset();
				ptIds1->InsertNextId(id+nptsUntilCorona[c]);
				ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
				ptIds1->InsertNextId(id2+nptsUntilCorona[c+1]);
				ptIds2->Reset();
				ptIds2->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
				ptIds2->InsertNextId(id2+nptsUntilCorona[c+1]);
				ptIds2->InsertNextId((id2+1)%nptsInCorona[c+1]+nptsUntilCorona[c+1]);
				polys->InsertNextCell(ptIds1);
				polys->InsertNextCell(ptIds2);
				id2++;
			}
		}
	}
	for(int id=0; id< nptsInCorona[numberOfCoronas-1]; id++)
	{
		ptIds1->Reset();
		ptIds1->InsertNextId(id+nptsUntilCorona[numberOfCoronas-1]);
		ptIds1->InsertNextId((id+1)%nptsInCorona[numberOfCoronas-1]+nptsUntilCorona[numberOfCoronas-1]);
		ptIds1->InsertNextId(nptsUntilCorona[numberOfCoronas]);
		polys->InsertNextCell(ptIds1);
	}
}
Example #2
0
/*
 * Use Girard's theorem to compute the area of a sky polygon.
 */
double Girard() {
  int i, j, ibad;

  double area;
  double lon, lat;

  Vec side[16];
  double ang[16];

  Vec tmp;

  double sumang, cosAng, sinAng;

  sumang = 0;

  if (nv < 3)
    return 0;

  if (DEBUG >= 4) {
    for (i = 0; i < nv; ++i) {
      lon = atan2(V[i].y, V[i].x) / DEG_TO_RADIANS;
      lat = asin(V[i].z) / DEG_TO_RADIANS;

      printf("Girard(): %3d [%13.6e,%13.6e,%13.6e] -> (%10.6f,%10.6f)\n", i,
             V[i].x, V[i].y, V[i].z, lon, lat);

      fflush(stdout);
    }
  }

  for (i = 0; i < nv; ++i) {
    Cross(&V[i], &V[(i + 1) % nv], &side[i]);

    Normalize(&side[i]);
  }

  for (i = 0; i < nv; ++i) {
    Cross(&side[i], &side[(i + 1) % nv], &tmp);

    sinAng = Normalize(&tmp);
    cosAng = -Dot(&side[i], &side[(i + 1) % nv]);

    // Remove center point of colinear segments

    ang[i] = atan2(sinAng, cosAng);

    if (DEBUG >= 4) {
      if (i == 0)
        printf("\n");

      printf("Girard(): angle[%d] = %13.6e -> %13.6e (from %13.6e / %13.6e)\n",
             i, ang[i], ang[i] - M_PI / 2., sinAng, cosAng);
      fflush(stdout);
    }

    // Direction changes of less than a degree can be tricky
    if (ang[i] > M_PI - 0.0175) {
      ibad = (i + 1) % nv;

      if (DEBUG >= 4) {
        printf("Girard(): ---------- Corner %d bad; "
               "Remove point %d -------------\n",
               i, ibad);
        fflush(stdout);
      }

      --nv;

      for (j = ibad; j < nv; ++j) {
        V[j].x = V[j + 1].x;
        V[j].y = V[j + 1].y;
        V[j].z = V[j + 1].z;
      }

      return (Girard());
    }

    sumang += ang[i];
  }

  area = sumang - (nv - 2.) * M_PI;

  if (mNaN(area) || area < 0.)
    area = 0.;

  if (DEBUG >= 4) {
    printf("\nGirard(): area = %13.6e [%d]\n\n", area, nv);
    fflush(stdout);
  }

  return area;
}
Example #3
0
/**
 * \brief Calculate distance between two points
 * This function uses an algorithm for an oblate spheroid earth model.
 * The algorithm is described here:
 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
 * \return Distance in meters
 */
double nmea_distance_ellipsoid(
  const nmeaPOS *from_pos,    /**< From position in radians */
  const nmeaPOS *to_pos,      /**< To position in radians */
  double *from_azimuth,       /**< (O) azimuth at "from" position in radians */
  double *to_azimuth          /**< (O) azimuth at "to" position in radians */
)
{
  /* All variables */
  double f, a, b, sqr_a, sqr_b;
  double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
  double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
  int remaining_steps;
  double sqr_u, A, B, delta_sigma;

  /* Check input */
  NMEA_ASSERT( from_pos != 0 );
  NMEA_ASSERT( to_pos != 0 );

  if (( from_pos->lat == to_pos->lat ) && ( from_pos->lon == to_pos->lon ) )
  { /* Identical points */
    if ( from_azimuth != 0 )
      *from_azimuth = 0;
    if ( to_azimuth != 0 )
      *to_azimuth = 0;
    return 0;
  } /* Identical points */

  /* Earth geometry */
  f = NMEA_EARTH_FLATTENING;
  a = NMEA_EARTH_SEMIMAJORAXIS_M;
  b = ( 1 - f ) * a;
  sqr_a = a * a;
  sqr_b = b * b;

  /* Calculation */
  L = to_pos->lon - from_pos->lon;
  phi1 = from_pos->lat;
  phi2 = to_pos->lat;
  U1 = atan(( 1 - f ) * tan( phi1 ) );
  U2 = atan(( 1 - f ) * tan( phi2 ) );
  sin_U1 = sin( U1 );
  sin_U2 = sin( U2 );
  cos_U1 = cos( U1 );
  cos_U2 = cos( U2 );

  /* Initialize iteration */
  sigma = 0;
  sin_sigma = sin( sigma );
  cos_sigma = cos( sigma );
  cos_2_sigmam = 0;
  sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
  sqr_cos_alpha = 0;
  lambda = L;
  sin_lambda = sin( lambda );
  cos_lambda = cos( lambda );
  delta_lambda = lambda;
  remaining_steps = 20;

  while (( delta_lambda > 1e-12 ) && ( remaining_steps > 0 ) )
  { /* Iterate */
    /* Variables */
    double tmp1, tmp2, sin_alpha, cos_alpha, C, lambda_prev;

    /* Calculation */
    tmp1 = cos_U2 * sin_lambda;
    tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
    sin_sigma = sqrt( tmp1 * tmp1 + tmp2 * tmp2 );
    cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
    sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
    cos_alpha = cos( asin( sin_alpha ) );
    sqr_cos_alpha = cos_alpha * cos_alpha;
    cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
    sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
    C = f / 16 * sqr_cos_alpha * ( 4 + f * ( 4 - 3 * sqr_cos_alpha ) );
    lambda_prev = lambda;
    sigma = asin( sin_sigma );
    lambda = L +
             ( 1 - C ) * f * sin_alpha
             * ( sigma + C * sin_sigma * ( cos_2_sigmam + C * cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) ) );
    delta_lambda = lambda_prev - lambda;
    if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
    sin_lambda = sin( lambda );
    cos_lambda = cos( lambda );
    remaining_steps--;
  }  /* Iterate */

  /* More calculation  */
  sqr_u = sqr_cos_alpha * ( sqr_a - sqr_b ) / sqr_b;
  A = 1 + sqr_u / 16384 * ( 4096 + sqr_u * ( -768 + sqr_u * ( 320 - 175 * sqr_u ) ) );
  B = sqr_u / 1024 * ( 256 + sqr_u * ( -128 + sqr_u * ( 74 - 47 * sqr_u ) ) );
  delta_sigma = B * sin_sigma * (
                  cos_2_sigmam + B / 4 * (
                    cos_sigma * ( -1 + 2 * sqr_cos_2_sigmam ) -
                    B / 6 * cos_2_sigmam * ( -3 + 4 * sin_sigma * sin_sigma ) * ( -3 + 4 * sqr_cos_2_sigmam )
                  ) );

  /* Calculate result */
  if ( from_azimuth != 0 )
  {
    double tan_alpha_1 = cos_U2 * sin_lambda / ( cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda );
    *from_azimuth = atan( tan_alpha_1 );
  }
  if ( to_azimuth != 0 )
  {
    double tan_alpha_2 = cos_U1 * sin_lambda / ( -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda );
    *to_azimuth = atan( tan_alpha_2 );
  }

  return b * A *( sigma - delta_sigma );
}
Example #4
0
void ParticleEffectEntityItem::stepSimulation(float deltaTime) {

    _particleMinBound = glm::vec3(-1.0f, -1.0f, -1.0f);
    _particleMaxBound = glm::vec3(1.0f, 1.0f, 1.0f);

    // update particles between head and tail
    for (quint32 i = _particleHeadIndex; i != _particleTailIndex; i = (i + 1) % _maxParticles) {
        _particleLifetimes[i] -= deltaTime;

        // if particle has died.
        if (_particleLifetimes[i] <= 0.0f || _lifespan == 0.0f) {
            // move head forward
            _particleHeadIndex = (_particleHeadIndex + 1) % _maxParticles;
        }
        else {
            float age = 1.0f - _particleLifetimes[i] / _lifespan;  // 0.0 .. 1.0
            updateRadius(i, age);
            updateColor(i, age);
            updateAlpha(i, age);
            integrateParticle(i, deltaTime);
            extendBounds(_particlePositions[i]);
        }
    }

    // emit new particles, but only if we are emmitting
    if (getIsEmitting() && _emitRate > 0.0f && _lifespan > 0.0f && _polarStart <= _polarFinish) {

        float timeLeftInFrame = deltaTime;
        while (_timeUntilNextEmit < timeLeftInFrame) {

            timeLeftInFrame -= _timeUntilNextEmit;
            _timeUntilNextEmit = 1.0f / _emitRate;

            // emit a new particle at tail index.
            quint32 i = _particleTailIndex;
            _particleLifetimes[i] = _lifespan;

            // Radius
            if (_radiusSpread == 0.0f) {
                _radiusStarts[i] = getRadiusStart();
                _radiusMiddles[i] =_particleRadius;
                _radiusFinishes[i] = getRadiusFinish();
            } else {
                float spreadMultiplier;
                if (_particleRadius > 0.0f) {
                    spreadMultiplier = 1.0f + randFloatInRange(-1.0f, 1.0f) * _radiusSpread / _particleRadius;
                } else {
                    spreadMultiplier = 1.0f;
                }
                _radiusStarts[i] = 
                    glm::clamp(spreadMultiplier * getRadiusStart(), MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS);
                _radiusMiddles[i] = 
                    glm::clamp(spreadMultiplier * _particleRadius, MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS);
                _radiusFinishes[i] = 
                    glm::clamp(spreadMultiplier * getRadiusFinish(), MINIMUM_PARTICLE_RADIUS, MAXIMUM_PARTICLE_RADIUS);
            }
            updateRadius(i, 0.0f);

            // Position, velocity, and acceleration
            if (_polarStart == 0.0f && _polarFinish == 0.0f && _emitDimensions.z == 0.0f) {
                // Emit along z-axis from position
                _particlePositions[i] = getPosition();
                _particleVelocities[i] = 
                    (_emitSpeed + randFloatInRange(-1.0f, 1.0f) * _speedSpread) * (_emitOrientation * Z_AXIS);
                _particleAccelerations[i] = _emitAcceleration + randFloatInRange(-1.0f, 1.0f) * _accelerationSpread;

            } else {
                // Emit around point or from ellipsoid
                // - Distribute directions evenly around point
                // - Distribute points relatively evenly over ellipsoid surface
                // - Distribute points relatively evenly within ellipsoid volume

                float elevationMinZ = sin(PI_OVER_TWO - _polarFinish);
                float elevationMaxZ = sin(PI_OVER_TWO - _polarStart);
                float elevation = asin(elevationMinZ + (elevationMaxZ - elevationMinZ) * randFloat());

                float azimuth;
                if (_azimuthFinish >= _azimuthStart) {
                    azimuth = _azimuthStart + (_azimuthFinish - _azimuthStart) * randFloat();
                } else {
                    azimuth = _azimuthStart + (TWO_PI + _azimuthFinish - _azimuthStart) * randFloat();
                }

                glm::vec3 emitDirection;

                if (_emitDimensions == glm::vec3()) {
                    // Point
                    emitDirection = glm::quat(glm::vec3(PI_OVER_TWO - elevation, 0.0f, azimuth)) * Z_AXIS;

                    _particlePositions[i] = getPosition();
                } else {
                    // Ellipsoid
                    float radiusScale = 1.0f;
                    if (_emitRadiusStart < 1.0f) {
                        float emitRadiusStart = glm::max(_emitRadiusStart, EPSILON);  // Avoid math complications at center
                        float randRadius = 
                            emitRadiusStart + randFloatInRange(0.0f, MAXIMUM_EMIT_RADIUS_START - emitRadiusStart);
                        radiusScale = 1.0f - std::pow(1.0f - randRadius, 3.0f);
                    }

                    glm::vec3 radiuses = radiusScale * 0.5f * _emitDimensions;
                    float x = radiuses.x * glm::cos(elevation) * glm::cos(azimuth);
                    float y = radiuses.y * glm::cos(elevation) * glm::sin(azimuth);
                    float z = radiuses.z * glm::sin(elevation);
                    glm::vec3 emitPosition = glm::vec3(x, y, z);
                    emitDirection = glm::normalize(glm::vec3(
                        radiuses.x > 0.0f ? x / (radiuses.x * radiuses.x) : 0.0f,
                        radiuses.y > 0.0f ? y / (radiuses.y * radiuses.y) : 0.0f,
                        radiuses.z > 0.0f ? z / (radiuses.z * radiuses.z) : 0.0f
                        ));

                    _particlePositions[i] = getPosition() + _emitOrientation * emitPosition;
                }

                _particleVelocities[i] =
                    (_emitSpeed + randFloatInRange(-1.0f, 1.0f) * _speedSpread) * (_emitOrientation * emitDirection);
                _particleAccelerations[i] = _emitAcceleration + randFloatInRange(-1.0f, 1.0f) * _accelerationSpread;
            }
            integrateParticle(i, timeLeftInFrame);
            extendBounds(_particlePositions[i]);

            // Color
            if (_colorSpread == xColor{ 0, 0, 0 }) {
                _colorStarts[i] = getColorStart();
                _colorMiddles[i] = getXColor();
                _colorFinishes[i] = getColorFinish();
            } else {
                xColor startColor = getColorStart();
                xColor middleColor = getXColor();
                xColor finishColor = getColorFinish();

                float spread = randFloatInRange(-1.0f, 1.0f);
                float spreadMultiplierRed = 
                    middleColor.red > 0 ? 1.0f + spread * (float)_colorSpread.red / (float)middleColor.red : 1.0f;
                float spreadMultiplierGreen = 
                    middleColor.green > 0 ? 1.0f + spread * (float)_colorSpread.green / (float)middleColor.green : 1.0f;
                float spreadMultiplierBlue = 
                    middleColor.blue > 0 ? 1.0f + spread * (float)_colorSpread.blue / (float)middleColor.blue : 1.0f;

                _colorStarts[i].red = (int)glm::clamp(spreadMultiplierRed * (float)startColor.red, 0.0f, 255.0f);
                _colorStarts[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)startColor.green, 0.0f, 255.0f);
                _colorStarts[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)startColor.blue, 0.0f, 255.0f);

                _colorMiddles[i].red = (int)glm::clamp(spreadMultiplierRed * (float)middleColor.red, 0.0f, 255.0f);
                _colorMiddles[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)middleColor.green, 0.0f, 255.0f);
                _colorMiddles[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)middleColor.blue, 0.0f, 255.0f);

                _colorFinishes[i].red = (int)glm::clamp(spreadMultiplierRed * (float)finishColor.red, 0.0f, 255.0f);
                _colorFinishes[i].green = (int)glm::clamp(spreadMultiplierGreen * (float)finishColor.green, 0.0f, 255.0f);
                _colorFinishes[i].blue = (int)glm::clamp(spreadMultiplierBlue * (float)finishColor.blue, 0.0f, 255.0f);
            }
            updateColor(i, 0.0f);

            // Alpha
            if (_alphaSpread == 0.0f) {
                _alphaStarts[i] = getAlphaStart();
                _alphaMiddles[i] = _alpha;
                _alphaFinishes[i] = getAlphaFinish();
            } else {
                float spreadMultiplier = 1.0f + randFloatInRange(-1.0f, 1.0f) * _alphaSpread / _alpha;
                _alphaStarts[i] = spreadMultiplier * getAlphaStart();
                _alphaMiddles[i] = spreadMultiplier * _alpha;
                _alphaFinishes[i] = spreadMultiplier * getAlphaFinish();
            }
            updateAlpha(i, 0.0f);

            _particleTailIndex = (_particleTailIndex + 1) % _maxParticles;

            // overflow! move head forward by one.
            // because the case of head == tail indicates an empty array, not a full one.
            // This can drop an existing older particle, but this is by design, newer particles are a higher priority.
            if (_particleTailIndex == _particleHeadIndex) {
                _particleHeadIndex = (_particleHeadIndex + 1) % _maxParticles;
            }
        }

        _timeUntilNextEmit -= timeLeftInFrame;
    }
}
Example #5
0
bool GeoAlgorithms::initVincenty(double aLat1, double aLon1, double aLat2, double aLon2)
{

   // Verify that input latitudes are between -90 and 90 and longitudes are
   // between -180 and 180
   if ((abs(aLat1) > 90) || (abs(aLat2) > 90) || (abs(aLon1) > 180)
        || (abs(aLon2) > 180))
   {
       return false;
   }

   // convert inputs in degrees to radians:
   aLat1 = aLat1 * 0.0174532925199433;
   aLon1 = aLon1 * 0.0174532925199433;
   aLat2 = aLat2 * 0.0174532925199433;
   aLon2 = aLon2 * 0.0174532925199433;

   // correct for errors at exact poles by adjusting 0.6 millimeters:
   if (abs(GeoConversions::PI_OVER_2-abs(aLat1)) < (1e-10))
   {
       aLat1 = getSign(aLat1) * (GeoConversions::PI_OVER_2 - (1e-10));
   }
   if (abs(GeoConversions::PI_OVER_2-abs(aLat2)) < (1e-10))
   {
       aLat2 = getSign(aLat2) * (GeoConversions::PI_OVER_2 - (1e-10));
   }

   // Ellipse CalcuaAltitudeions?
   mVincentyU1 = atan(m1MinF*tan(aLat1));
   mVincentyU2 = atan(m1MinF*tan(aLat2));
   aLon1 = getMod(aLon1, (GeoConversions::TWO_PI));
   aLon2 = getMod(aLon2, (GeoConversions::TWO_PI));
   mVincentyL = aLon2-aLon1;
   if (abs(mVincentyL) > PI)
   {
     mVincentyL = getSign(mVincentyL) * (GeoConversions::TWO_PI - abs(mVincentyL));
   }

   // Initialize Variables for Loop
   double sin_mVincentyU1 = sin(mVincentyU1);
   double cos_mVincentyU1 = cos(mVincentyU1);
   double sin_mVincentyU2 = sin(mVincentyU2);
   double cos_mVincentyU2 = cos(mVincentyU2);
   double sinU1_sinU2 = sin_mVincentyU1 * sin_mVincentyU2;
   double cosU1_sinU2 = cos_mVincentyU1 * sin_mVincentyU2;
   double sinU1_cosU2 = sin_mVincentyU1 * cos_mVincentyU2;
   double cosU1_cosU2 = cos_mVincentyU1 * cos_mVincentyU2;
   double sin_mVincentyLambda = 0;
   double cos_mVincentyLambda = 0;
   double cos_mVincentyAlpha = 0;
   double sin_mVincentySigma = 0;
   double cos_mVincentySigma = 0;
   double lLambdaOld = 0;
   long   lIterCount = 0;
   double lSinSigma = 0;
   double lCosSigma = 0;
   double c = 0;
   mVincentySigma = 0;
   mVincentyAlpha = 0;
   mVincentyCosToSigmaM = 0;
   mVincentyLambda = mVincentyL;

   // ?
   while ((!lIterCount) || abs((mVincentyLambda-lLambdaOld) > (1e-12)))
   {
      lIterCount += 1;
      if (lIterCount > 50)
      {
          mVincentyLambda = PI;
          break;
      }

      sin_mVincentyLambda = sin(mVincentyLambda);
      cos_mVincentyLambda = cos(mVincentyLambda);
      lLambdaOld = mVincentyLambda;
      lSinSigma = sqrt(pow(cos_mVincentyU2 * sin_mVincentyLambda, 2) +
         pow(cosU1_sinU2 - sinU1_cosU2 * cos_mVincentyLambda, 2));
      lCosSigma = sinU1_sinU2 + cosU1_cosU2 * cos_mVincentyLambda;
      mVincentySigma = atan2(lSinSigma, lCosSigma);
      sin_mVincentySigma = sin(mVincentySigma);
      cos_mVincentySigma = cos(mVincentySigma);
      mVincentyAlpha = asin(cosU1_cosU2 * sin_mVincentyLambda / sin_mVincentySigma);
      cos_mVincentyAlpha = cos(mVincentyAlpha);
      mVincentyCosToSigmaM = cos_mVincentySigma -  2 * sinU1_sinU2 / pow(cos_mVincentyAlpha, 2);
      c = mF/ 16 * pow(cos_mVincentyAlpha, 2) * (4 + mF * (4 - 3 * pow(cos_mVincentyAlpha, 2)));
      mVincentyLambda = mVincentyL + (1 - c) * mF * sin(mVincentyAlpha) * (mVincentySigma + c * sin_mVincentySigma *
         (mVincentyCosToSigmaM + c * cos_mVincentySigma * (-1 + 2 * pow(mVincentyCosToSigmaM, 2))));

      // Correct for convergence failure in the case of essentially
      // antipodal points
      if (mVincentyLambda > PI)
      {
          mVincentyLambda = PI;
          break;
      }
   }

   return true;
}
void Controller_StateMachine_v2::processTurn() {	// "during:"

	// 1º proyectar posicion sobre el plano r_ur, r_ur2
	Vector aux_ur2(3), aux_vector(3);
	aux_vector.copy(&r_ur);
	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, r_ur2), aux_vector );
	aux_ur2.substraction( &r_ur2, &aux_vector);
	jesus_library::unitarizeVector(aux_ur2); // vector ortogonal a r_ur; para formar base ortonormal en el plano r_ur, r_ur2

	Vector pos_act(3);
	pos_act.setValueData(xei,1);
	pos_act.setValueData(yei,2);
	pos_act.setValueData(zei,3);

	aux_vector.substraction( &pos_act, &c_pc);
	Vector pos_act_proy_ur(3),pos_act_proy_ur2(3);
	pos_act_proy_ur.copy(&r_ur);
	pos_act_proy_ur2.copy(&aux_ur2);

	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(r_ur, aux_vector), pos_act_proy_ur );
	jesus_library::multiplyDoubleVsVector( jesus_library::dotProduct(aux_ur2, aux_vector), pos_act_proy_ur2 );

	Vector pos_act_proy(3);
	pos_act_proy.addition( &pos_act_proy_ur, &pos_act_proy_ur2);
	cvg_double current_radius = jesus_library::normOfVector(pos_act_proy);

	#ifdef SM_STATEMACHINE_DEBUG
	std::cout << "c_pc = \n"; c_pc.mostrar();
	std::cout << "c_pinit = \n"; c_pinit.mostrar();
	std::cout << "c_pend = \n"; c_pend.mostrar();
	std::cout << "pos_act_proy = \n"; pos_act_proy.mostrar();
	#endif // SM_STATEMACHINE_DEBUG

	// 2º localizar pref
	Vector u_ro(3);
	u_ro.copy(&pos_act_proy);
	jesus_library::unitarizeVector(u_ro);
	aux_vector.copy(&u_ro);
	jesus_library::multiplyDoubleVsVector( c_Rt, aux_vector);
	Vector pos_ref(3);
	pos_ref.addition(&c_pc, &aux_vector);
	jesus_library::getVectorComponents(pos_ref, xrefo, yrefo, zrefo);
	cvg_double current_altitude_error = (zei-zrefo); // fabs(zei-zrefo);

	// 3º calcular velocidad de referencia
	Vector u_fi(3);
	jesus_library::crossProduct(u_fi, c_u0, u_ro);
	jesus_library::unitarizeVector(u_fi);
	Vector v_ref(3);
	v_ref.copy(&u_fi);
	jesus_library::multiplyDoubleVsVector(c_vc, v_ref);
	jesus_library::getVectorComponents(v_ref, vxfo, vyfo, vzfo);


//	// 4º Calcular pitcho, rollo
	derivBlock_vxfo.setInput( vxfo);
	dvxfo = derivBlock_vxfo.getOutput();
	derivBlock_vyfo.setInput( vyfo);
	dvyfo = derivBlock_vyfo.getOutput();
#ifdef SM_TRAJECTORYMODE_ACTIVATE_TILTFO
	double g = 9.81;
	pitchfo  = -asin( (cvg_double) dvxfo/g);
	rollfo   =  asin( (cvg_double) dvyfo/g);
	pitchfo *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF;
	rollfo  *= 1/SM_TRAJECTORYMODE_TILTFO_RAD2TILTREF;
#else
	pitchfo = 0.0;
	rollfo  = 0.0;
#endif
//  // This is a code that never work that was intended to stop the parrot in it's current position
//	double v_act = sqrt( vxei*vxei + vyei*vyei + vzei*vzei );
//	double act_req  = pow(v_act,2)/c_Rt;
//	double tilt_req = act_req/SM_STATEMACHINE_G;
//	aux_vector.copy(&u_ro);
//	jesus_library::multiplyDoubleVsVector( -tilt_req, aux_vector);
//	aux_vector.setValueData( 0.0, 3);
//
//	Vector parrot_ux(3), parrot_uy(3);
//	parrot_ux.setValueData( cos(yawei), 1);
//	parrot_ux.setValueData( sin(yawei), 2);
//	parrot_uy.setValueData(-sin(yawei), 1);
//	parrot_uy.setValueData( cos(yawei), 2);
//
//	pitchfo = -jesus_library::dotProduct( parrot_ux, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR;
//	rollfo  =  jesus_library::dotProduct( parrot_uy, aux_vector)*SM_STATEMACHINE_TILTCOMM_CORRECT_FACTOR;
//	pitchfo /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT;
//	rollfo  /= SM_STATEMACHINE_TILTCOMM_NORMALIZATION_CONSTANT;

	// 5º Calculo de alpha y comparacion con c_alim
	Vector aux_vector2(3);
	aux_vector.substraction(&c_pinit,&c_pc);
	jesus_library::unitarizeVector(aux_vector);
	cvg_double aux = jesus_library::dotProduct(u_ro, aux_vector);
	jesus_library::saturate( aux, -1, 1);
	jesus_library::crossProduct(aux_vector2, aux_vector, u_ro);
	cvg_double angle_sign = ( jesus_library::dotProduct(c_u0, aux_vector2) > 0.0 ) ? +1.0 : -1.0;
	cvg_double current_alpha = angle_sign*acos(aux);

	#ifdef SM_STATEMACHINE_DEBUG
	std::cout << "c_alim = " << c_alim << "; alpha = " << current_alpha << "\n";
	#endif // SM_STATEMACHINE_DEBUG

	if ( current_alpha > c_alim) {  // ended the turn
		c_nextState = SM_stateNames::STRAIGHT;
		c_changeState = true;
		return;
	} else {  // turn not ended, check safety zones using: current_radius, current_alpha
		if ( ( current_alpha < trajectory.traj_config.turnmode_safetyzone_negalpha_rad ) ||
				( fabs(current_radius - c_Rt) > trajectory.traj_config.turnmode_safetyzone_radius_m ) ||
				( fabs(current_altitude_error) > trajectory.traj_config.turnmode_safetyzone_altitude_m ) ) { // enter hover to prev checkpoint
#ifdef SM_STATEMACHINE_DEBUG
			std::cout << "pos_act_proy_ur < 0\n";
#endif // SM_STATEMACHINE_DEBUG
			trajectory[pr_checkpoint].convert2Vector(h_checkpoint);
			c_nextState = SM_stateNames::HOVER;
			h_stay_in_last_checkpoint = false;
			c_changeState = true;

			// In this case I have to redefine the state machine output
			jesus_library::getVectorComponents(h_checkpoint, xrefo, yrefo, zrefo);
			vxfo = 0.0;
			vyfo = 0.0;
			vzfo = 0.0;
			pitchfo = 0.0;
			rollfo  = 0.0;

			return;
		} else { // Continue turn, nothing else to do
			return;
		}
	}

}
Example #7
0
// LINE
void HTMesh::intersect(double ra1, double dec1, double ra2, double dec2,
                       BufNum bufNum)
{
    double x1, y1, z1, x2, y2, z2;
    
    //if (ra1 == 0.0 || ra1 == 180.00) ra1 += 0.1;
    //if (ra2 == 0.0 || ra2 == 180.00) ra2 -= 0.1;
    //if (dec1 == 0.0 ) dec1 += 0.1;
    //if (dec2 == 0.0 ) dec2 -= 0.1;

    // convert to Cartesian. Ugh.
    toXYZ( ra1, dec1, &x1, &y1, &z1);
    toXYZ( ra2, dec2, &x2, &y2, &z2);

    // Check if points are too close
    double len;
    len =  fabs(x1 - x2);
    len += fabs(y1 - y2);
    len += fabs(z1 - z2);

    if (htmDebug > 0 ) {
        printf("htmDebug = %d\n", htmDebug);
        printf("p1 = (%f, %f, %f)\n", x1, y1, z1);
        printf("p2 = (%f, %f, %f)\n", x2, y2, z2);
        printf("edge: %f (radians) %f (degrees)\n", edge, edge / degree2Rad);
        printf("len : %f (radians) %f (degrees)\n", len,  len  / degree2Rad);
    }

    if ( len < edge10 )
        return intersect( ra1, len, bufNum);

    // Cartesian cross product => perpendicular!.  Ugh.
    double cx = y1 * z2 - z1 * y2;
    double cy = z1 * x2 - x1 * z2;
    double cz = x1 * y2 - y1 * x2;

    if ( htmDebug > 0 ) printf("cp  = (%f, %f, %f)\n", cx, cy, cz);

    double norm =  edge10 / ( fabs(cx) + fabs(cy) + fabs(cz) );

    // give it length edge/10
    cx *= norm;
    cy *= norm;
    cz *= norm;

    if ( htmDebug > 0 ) printf("cpn  = (%f, %f, %f)\n", cx, cy, cz);

    // add it to (ra1, dec1)
    cx += x1;
    cy += y1;
    cz += z1;

    if ( htmDebug > 0 ) printf("cpf  = (%f, %f, %f)\n", cx, cy, cz);

    // back to spherical
    norm = sqrt( cx*cx + cy*cy + cz*cz);
    double ra0 = atan2( cy, cx )     / degree2Rad;
    double dec0 = asin( cz / norm )  / degree2Rad;

    if ( htmDebug > 0 ) printf("new ra, dec = (%f, %f)\n", ra0, dec0);

    SpatialVector p1(ra1, dec1);
    SpatialVector p0(ra0, dec0);
    SpatialVector p2(ra2, dec2);
    RangeConvex convex(&p1, &p0, &p2);

    if ( ! performIntersection(&convex, bufNum) )
        printf("In intersect(%f, %f, %f, %f)\n", ra1, dec1, ra2, dec2);
}
Example #8
0
		template <typename T> inline T acsc(const T &a) {
			return asin(1 / a);
		}
    void Pager::paintEvent(QPaintEvent * event)
    {
        QFrame::paintEvent(event);

        if (d->images.size() == 0) { return; }

        // Maximum bounding box of a single page image
        QSize box = clampedBoundingBox();

        // In parametric equation of a circle, the offset from the centre is:
        //     x = a sin t
        // Spread determines how the index of page maps to the above t.

        double h = (double) box.height();
        double w = (double) box.width();
        double a = (width() - w - d->marginLeft - d->marginRight - (d->orientation == Qt::Vertical && d->scrollBar->isVisible() ? d->scrollBar->width() : 0)) / 2.0;
        double _1 = asin(w/a);
        double s = spread();
        double t1 = _1 + s * ((_PI / 2) - _1);

        int limitGrace = 5;
        //int upperLimit = (int) (d->guiIndex + _PI / (2.0 * t1)) + limitGrace + 1; // FIXME shuffle
        int upperLimit = (int) (d->guiIndex + width() / w + 1);
        //int lowerLimit = (int) (d->guiIndex - _PI / (2.0 * t1)) - limitGrace; // FIXME shuffle
        int lowerLimit = (int) (d->guiIndex - width() / w - 1);
        QPainter painter(this);
        painter.setClipRect(rect().adjusted(0, 1, -1, 0));
        painter.setRenderHint(QPainter::Antialiasing, true);
        painter.setRenderHint(QPainter::TextAntialiasing, true);
        painter.setRenderHint(QPainter::SmoothPixmapTransform, true);
        QFont f(painter.font());
        f.setPixelSize(9);
        painter.setFont(f);
        painter.translate(width() / 2, 0);
        painter.translate(-box.width() / 2, 0);
        // FIXME shuffle
        double lower = a;
        double upper = (count() - 1) * w - lower;
        double clamp = qBound(lower, d->guiIndex * w, upper);
        painter.translate(-clamp, 0);
        // end FIXME shuffle
        painter.translate(0, d->marginTop);

        painter.setPen(Qt::NoPen);
        if (isActiveWindow())
        {
            painter.setPen(QApplication::palette().highlight().color());
            painter.setBrush(QApplication::palette().highlight());
        }
        else
        {
            int gray = qGray(QApplication::palette().highlight().color().rgb());
            painter.setPen(QColor(gray, gray, gray));
            painter.setBrush(QColor(gray, gray, gray));
        }

        double radius = 4.0;
        double transientWidth = 0.0;
        double transientHeight = 0.0;
        double transientLabelWidth = 0.0;
        double transientLabelBottom = 0.0;
        if (d->images.size() == 1)
        {
            QSize imageSize = d->images[0].size();
            imageSize.scale(box, Qt::KeepAspectRatio);
            transientWidth = imageSize.width();
            transientHeight = imageSize.height();
            if (d->drawLabels && !labelAt(0).isEmpty())
            {
                QRect labelRect(QPoint((box.width() - transientWidth) / 2.0, radius), QPoint(box.width() - (box.width() - transientWidth) / 2.0, 12 + radius));
                QString elidedLabel = painter.fontMetrics().elidedText(labelAt(0), Qt::ElideMiddle, labelRect.width());
                QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel));
                transientLabelWidth = (double) bb.width();
                transientLabelBottom = (double) bb.bottom();
            }
        }
        else
        {
            int previousIndex = clampIndex((int) floor(d->guiIndex));
            int nextIndex = clampIndex((int) ceil(d->guiIndex));
            QSize previousSize = box;
            if (!d->images[previousIndex].isNull())
            {
                previousSize = d->images[previousIndex].size();
                previousSize.scale(box, Qt::KeepAspectRatio);
            }
            QSize nextSize = box;
            if (!d->images[nextIndex].isNull())
            {
                nextSize = d->images[nextIndex].size();
                nextSize.scale(box, Qt::KeepAspectRatio);
            }
            transientWidth = previousSize.width() + (nextSize.width() - previousSize.width()) * (d->guiIndex - previousIndex);
            transientHeight = previousSize.height() + (nextSize.height() - previousSize.height()) * (d->guiIndex - previousIndex);
            if (d->drawLabels && !labelAt(previousIndex).isEmpty() && !labelAt(nextIndex).isEmpty())
            {
                double previousLabelWidth = 0.0;
                double previousLabelBottom = 0.0;
                {
                    QRect labelRect(0, box.height() + radius, previousSize.width(), 12);
                    QString elidedLabel = painter.fontMetrics().elidedText(labelAt(previousIndex), Qt::ElideMiddle, labelRect.width());
                    QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel));
                    previousLabelWidth = (double) bb.width();
                    previousLabelBottom = (double) bb.bottom();
                }
                double nextLabelWidth = 0.0;
                double nextLabelBottom = 0.0;
                {
                    QRect labelRect(0, box.height() + radius, nextSize.width(), 12);
                    QString elidedLabel = painter.fontMetrics().elidedText(labelAt(nextIndex), Qt::ElideMiddle, labelRect.width());
                    QRect bb(painter.fontMetrics().boundingRect(labelRect, Qt::AlignCenter, elidedLabel));
                    nextLabelWidth = (double) bb.width();
                    nextLabelBottom = (double) bb.bottom();
                }
                transientLabelWidth = previousLabelWidth + (nextLabelWidth - previousLabelWidth) * (d->guiIndex - previousIndex);
                transientLabelBottom = previousLabelBottom + (nextLabelBottom - previousLabelBottom) * (d->guiIndex - previousIndex);
            }
        }
        painter.drawRoundedRect(QRectF(d->guiIndex * w + (box.width() - transientWidth) / 2.0 - radius, box.height() - transientHeight - radius, transientWidth + radius * 2, transientHeight + radius * 2), radius, radius);
        if (d->drawLabels && transientLabelWidth > 0.0 && transientLabelBottom > 0.0)
        {
            painter.drawRoundedRect(QRectF(d->guiIndex * w + (box.width() - transientLabelWidth) / 2.0 - radius, box.height(), transientLabelWidth + radius * 2, transientLabelBottom - box.height() + radius / 2.0), radius, radius);
        }

        painter.setPen(Qt::NoPen);
        QPen pen(QColor(115, 115, 115));
        pen.setJoinStyle(Qt::MiterJoin);
        pen.setWidthF(0.5);
        painter.setPen(pen);
        painter.setBrush(Qt::NoBrush);

        static QVector< double > sin_ts; // FIXME Make a member, not a static variable
        sin_ts.resize(count());

        // Iterate through pages
        double offset = 0.0;
        for (int i = (int) ceil(d->guiIndex); i < count(); ++i)
        {
            if (i < lowerLimit || i > upperLimit) continue;

            double preModifier = d->indexPreModifiers.value(i, 0.0);
            double postModifier = d->indexPostModifiers.value(i, 0.0);
            offset += preModifier;
            if (preModifier >= 0.0)
            {
                double t = (i - (d->guiIndex - offset)) * t1;
                t = qBound(-_PI/2.0, t, _PI/2);
                sin_ts[i] = sin(t);
            }
            else
            {
                sin_ts[i] = 10.0;
            }
            offset += postModifier;
        }

        offset = 0.0;
        for (int i = (int) ceil(d->guiIndex) - 1; i >= 0; --i)
        {
            if (i < lowerLimit || i > upperLimit) continue;

            double preModifier = d->indexPreModifiers.value(i, 0.0);
            double postModifier = d->indexPostModifiers.value(i, 0.0);
            offset += postModifier;
            if (preModifier >= 0.0)
            {
                double t = (i - (d->guiIndex + offset)) * t1;
                t = qBound(-_PI/2.0, t, _PI/2);
                sin_ts[i] = sin(t);
            }
            else
            {
                sin_ts[i] = 10.0;
            }
            offset += preModifier;
        }

        for (int i = 0; i < d->guiIndex; ++i)
        {
            if (i < lowerLimit || i > upperLimit) continue;
            double sin_t = sin_ts.at(i);
            if (sin_t > 1.0) continue;

            double opacity = 1.0;
            if (d->transitionTimes.contains(i) && !d->transitionTimes[i].isNull())
            {
                opacity *= d->transitionTimes[i].elapsed() / 500.0;
            }
            if (i < lowerLimit + limitGrace)
            {
                // opacity *= (i - lowerLimit) / (double) limitGrace; // FIXME shuffle
            }
            painter.setOpacity(opacity);
            QPixmap image = d->images[i].isNull() ? QPixmap(":/images/pager_loading.png") : d->images[i];
            QSize size = image.size();
            size.scale(box, Qt::KeepAspectRatio);
            //double x = sin_t * (a + (w - size.width()) / 2.0) + (w - size.width()) / 2.0; // FIXME shuffle
            double x = w * i + (w - size.width()) / 2.0;
            double y = h - size.height();
            QRect rect((int) x, (int) y, size.width(), size.height());
            painter.drawPixmap(rect, image);
            painter.drawRect(rect);

            if (d->drawLabels)
            {
                QString label = labelAt(i);
                if (!label.isEmpty())
                {
                    QRect labelRect(rect.bottomLeft() + QPoint(0, radius), rect.bottomRight() + QPoint(0, 12 + radius));
                    QString elidedLabel = painter.fontMetrics().elidedText(label, Qt::ElideMiddle, rect.width());

                    painter.save();
                    painter.setPen(QColor(0, 0, 0));

                    QFontMetrics fm(painter.font());

                    QSize numSize = fm.boundingRect(elidedLabel).size();
                    double lozengeRadius = 1 + numSize.height() / 2.0;
                    numSize += QSize(numSize.height(), 0);
                    QRect numRect(0, 0, numSize.width() + 2, numSize.height() + 2);

                    numRect.moveCenter(rect.center());
                    numRect.moveBottom(rect.bottom() + radius + 1);

                    painter.setBrush(Qt::yellow);
                    painter.setPen(QColor(140, 140, 0));
                    painter.drawRoundedRect(numRect, lozengeRadius, lozengeRadius);

                    painter.setPen(Qt::black);
                    painter.drawText(numRect, Qt::AlignCenter, elidedLabel);

                    painter.restore();

                }
            }

            if (d->hasAnnotation.value(i, 0) > 0)
            {
                drawAnnotationTag(&painter, rect);
            }

            if (!d->spotlightsHidden && !d->searchHits.isEmpty())
            {
                drawSearchHits(&painter, d->searchHits.value(i, 0), rect);
            }

            painter.setOpacity(1.0);
        }

        for (int i = count() - 1; i >= d->guiIndex; --i)
        {
            if (i < lowerLimit || i > upperLimit) continue;
            double sin_t = sin_ts.at(i);
            if (sin_t > 1.0) continue;

            double opacity = 1.0;
            if (d->transitionTimes.contains(i) && !d->transitionTimes[i].isNull())
            {
                opacity *= d->transitionTimes[i].elapsed() / 500.0;
            }
            if (i > upperLimit - limitGrace)
            {
                // opacity *= (limitGrace + i - upperLimit) / (double) limitGrace; // FIXME shuffle
            }
            painter.setOpacity(opacity);
            QPixmap image = d->images[i].isNull() ? QPixmap(":/images/pager_loading.png") : d->images[i];
            QSize size = image.size();
            size.scale(box, Qt::KeepAspectRatio);
            //double x = sin_t * (a + (w - size.width()) / 2.0) + (w - size.width()) / 2.0; // FIXME shuffle
            double x = w * i + (w - size.width()) / 2.0;
            double y = h - size.height();
            QRect rect((int) x, (int) y, size.width(), size.height());
            painter.drawPixmap(rect, image);
            painter.drawRect(rect);

            if (d->drawLabels)
            {
                QString label = labelAt(i);
                if (!label.isEmpty())
                {
                    QRect labelRect(rect.bottomLeft() + QPoint(0, radius), rect.bottomRight() + QPoint(0, 12 + radius));
                    QString elidedLabel = painter.fontMetrics().elidedText(label, Qt::ElideMiddle, labelRect.width());
                    painter.save();
                    painter.setPen(QColor(200, 200, 200));
                    painter.setPen(QColor(0, 0, 0));
                    painter.drawText(labelRect, Qt::AlignCenter, elidedLabel);
                    painter.restore();
                }
            }

            if (d->hasAnnotation.value(i, 0) > 0)
            {
                drawAnnotationTag(&painter, rect);
            }

            if (!d->spotlightsHidden && !d->searchHits.isEmpty())
            {
                drawSearchHits(&painter, d->searchHits.value(i, 0), rect);
            }

            painter.setOpacity(1.0);
        }

        sin_ts.clear();
    }
Example #10
0
void XYZ2GEOM(double X, double Y, double Z, double *Lon, double *Lat)
{
    *Lon = asin(Z / sqrt(X * X + Y * Y + Z * Z)) * 180 / M_PI;
    *Lat = atan2(Y, X) * 180 / M_PI;
}
Example #11
0
static int math_asin (lua_State *L) {
  lua_pushnumber(L, asin(luaL_checknumber(L, 1)));
  return 1;
}
Example #12
0
static void fcn_asin(EvalState *c)
{
    c->rStackPtr->v.value = asin(c->rStackPtr->v.value);
}
Example #13
0
MINLINE float saasin(float fac)
{
	if(fac<= -1.0f) return (float)-M_PI/2.0f;
	else if(fac>=1.0f) return (float)M_PI/2.0f;
	else return (float)asin(fac);
}
Example #14
0
void LibAsin(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
    ReturnValue->Val->FP = asin(Param[0]->Val->FP);
}
bool TScriptInternalFunctions::runFunction(QString function, QStringList param, QString &result)
{
    QString fn = function.toUpper();
    if (fn == "ACOS") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(acos(v));
        return true;
    }

    if (fn == "ASIN") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(asin(v));
        return true;
    }

    if (fn == "ATAN") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(atan(v));
        return true;
    }

    if (fn == "BUILDTYPE") {
#ifdef STANDALONE
        result = "STANDALONE";
#endif
#ifdef PACKAGED
        result = "PACKAGED";
#endif
        return true;
    }

    if (fn == "CALC") {
        // Calculate an expression (e.g. 5+5)
        if (param.length() == 0)
            return false;
        QString expr = param[0];
        result = calc(expr);
        return true;
    }

    if (fn == "COS") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(cos(v));
        return true;
    }

    if (fn == "COLORAT") { // $ColorAt(@window, layer, x, y)
        if (param.length() < 3)
            return false;
        QString layer = "main";
        if (param.length() > 3) {
            layer = param[1];
            param.removeAt(1);
        }

        subwindow_t sw = getCustomWindow(param[0]);
        if (sw.type == WT_NOTHING)
            return false;
        int x = floor( param[1].toFloat() );
        int y = floor( param[2].toFloat() );

        result = sw.widget->picwinPtr()->colorAt(layer, x, y);
        return true;
    }

    if (fn == "CURWINTYPE") {
        // Returns the current target type (msg or channel)
        subwindow_t sw = winList->value(*activeWid);
        if (sw.type == WT_CHANNEL)
            result = "CHANNEL";
        else if (sw.type == WT_GRAPHIC)
            result = "GRAPHIC";
        else if (sw.type == WT_GWINPUT)
            result = "GRAPHICINPUT";
        else if (sw.type == WT_NOTHING)
            result = "NOTHIG";
        else if (sw.type == WT_PRIVMSG)
            result = "PRIVMSG";
        else if (sw.type == WT_STATUS)
            result = "STATUS";
        else if (sw.type == WT_TXTONLY)
            result = "TXTONLY";
        else
            result = "UNKNOWN";

        return true;
    }

    if (fn == "DLG") {
        // Returns information of a dialog or its objects
        // $dlg(dialog,object)
        if (param.count() == 2) {
            QString dlg = param[0];
            QString object = param[1];

            QHashIterator<QString,TCustomScriptDialog*> i(*dialogs);
            while (i.hasNext()) {
                i.next();
                if (i.key().toUpper() == dlg.toUpper()) {
                    result = i.value()->getLabel(object);
                    return true;
                }
            }
        }

        // $dlg(dialog,object,index)
        if (param.count() == 3) {
            QString dlg = param[0];
            QString object = param[1];
            QString index = param[2];

            QHashIterator<QString,TCustomScriptDialog*> i(*dialogs);
            while (i.hasNext()) {
                i.next();
                if (i.key().toUpper() == dlg.toUpper()) {
                    result = i.value()->getItem(object, index.toInt());
                    return true;
                }
            }
        }

        // Default
        return false;
    }

    if (fn == "FILE") {
        // Returns a file descriptor by opening a file for read and|or write
        // $file(file.name, rwb)
        // result: 0 cannot open, -1 not existing
        if (param.count() < 2) {
            result = "0";
            return true;
        }
        QString mode = param[1];

        bool read = false;
        bool write = false;
        bool binary = false;
        bool append = false;
        bool switchfail = false;
        for (int i = 0; i <= mode.length()-1; i++) {
            char c = mode[i].toLatin1();
                switch (c) {
                    case 'a':
                        append = true;
                        continue;
                    case 'r':
                        read = true;
                        continue;
                    case 'w':
                        write = true;
                        continue;
                    case 'b':
                        binary = true;
                        continue;
                    default:
                        switchfail = true;
                        break;
            }
        }

        if (switchfail == true) {
            result = "0";
            return true;
        }

        if ((read || write) == false)
            read = true;

        QIODevice::OpenMode om = 0;

        if (read)
            om |= QIODevice::ReadOnly;
        if (write)
            om |= QIODevice::WriteOnly;
        if (! binary)
            om |= QIODevice::Text;

        if (append)
            om |= QIODevice::Append;

        if (om == 0) {
            result = "0";
            return true;
        }

        QFile *f = new QFile(param[0]);

        if (! f->open(om)) {
            result = "0";
            return true;
        }

        t_sfile ts;
        ts.binary = binary;
        ts.read = read;
        ts.write = write;
        ts.fd = fdc;
        ts.file = f;

        files->insert(fdc, ts);
        result = QString::number(fdc++);
        return true;
    }

    if (fn == "FNEXIST") {
        // Checks if an actual function exists; This will NOT work on "internal" functions (these in here)
        if (param.count() != 1)
            return false;

        int idx = fnindex->value(param[0].toUpper(), -1);

        if (idx > -1)
            result = "1";
        else
            result = "0";
        return true;
    }

    if (fn == "GLUE") {
        // "glue" texts together.
        // $glue(hello,big,world) will return hellobigworld
        QString r;
        for (int i = 0; i <= param.length()-1; i++)
            r += param[i];

        result = r;
        return true;
    }

    if (fn == "HOSTMASK") {
        // Returns hostmask *!*@host.name of nickname if IAL got it.
        // Otherwise, if IAL doesn't, it returns nickname!*@* as hostmask.
        if (param.count() != 1) {
            result.clear();
            return false;
        }
        QString nickname = param[0];

        IConnection *con = conList->value(*activeConn);
        QString host = con->ial.getHost(nickname);
        if (host.isEmpty()) {
            host = nickname;
            host.append("!*@*");
        }
        else
            host.prepend("*!*@");

        result = host;

        return true;
    }

    if (fn == "IALHOSTMASK") {
        // Returns hostname of nickname if IAL got it, otherwise empty text.
        // Better off using $hostmask() instead.
        if (param.count() != 1) {
            result.clear();
            return false;
        }
        QString nickname = param[0];

        IConnection *con = conList->value(*activeConn);
        QString host = con->ial.getHost(nickname);

        if (! host.isEmpty())
            host.prepend("*!*@");

        result = host;

        return true;
    }

    if (fn == "LEN") {
        // Counts amount of letters in a given text
        if (param.count() == 0) {
            result = "0";
            return true;
        }
        result = QString::number( param.at(0).length() );
        return true;
    }

    if (fn == "NULL") {
        // Returns empty
        result.clear();
        return true;
    }

    if (fn == "PATH") {
        // $path(type)
        // Returns a file path to the given type.
        if (param.count() != 1)
            return false;

        QString type = param[0].toUpper();
        if (type == "CONFIG")
            result = CONF_PATH;
        if (type == "COMMON")
            result = COMMON_PATH;
        if (type == "EXEC")
            result = QApplication::applicationDirPath();
        if (type == "SKEL")
            result = SKEL_PATH;

        return true;
    }

    if (fn == "RAND") {
        // Pseudo-random number generator
        if (param.length() < 2)
            return false;
        int lo = param[0].toInt();
        int hi = param[1].toInt();
        result = rand(lo, hi);
        return true;
    }

    if (fn == "SIN") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(sin(v));
        return true;
    }

    if (fn == "SOCKBUFLEN") {
        // Returns amount of bytes left in sockread buffer
        if (param.length() < 1)
            return false;

        result = sockfactory->sockBufLen(param[0]);
        return true;
    }

    if (fn == "SOCKLIST") {
        // Find socket names.
        // $socklist(patt_*, pos)
        // If pos is zero, that will return amount of socket names the pattern matches.
        // If pos > 0, this will return an actual socket name it matches.
        if (param.length() < 2)
            return false;

        result = sockfactory->socklist(param[0], param[1].toInt());
        return true;
    }

    if (fn == "SSTR") {
        // Substring, returns text by given positions inside the text.
        // $SSTR(The text, start, end) end is optional.
        if (param.length() < 2)
            return false;

        int start = param[1].toInt();
        int stop = -1;
        if (param.length() >= 3)
            stop = param[2].toInt();

        QString text = param[0];
        result = sstr(text, start, stop);
        return true;
    }

    if (fn == "TAN") {
        if (param.length() < 1)
            return false;

        bool ok = false;
        double v = param[0].toDouble(&ok);

        if (!ok)
            v = 0;

        result = QString::number(tan(v));
        return true;
    }

    if (fn == "TARGET") {
        // Returns the current target to send messages to (msg or channel)
        subwindow_t sw = winList->value(*activeWid);
        result.clear();
        if ((sw.type == WT_CHANNEL) || (sw.type == WT_PRIVMSG))
            result = sw.widget->getTarget();
        return true;
    }

    if (fn == "TEXTWIDTH") {
        // Return text width in pixles by given font name and size
        // $textwidth(font, size, text)
        if (param.count() != 3)
            return false;

        QFont font(param[0]);
        font.setPixelSize(param[1].toInt());

        QFontMetrics fm(font);
        result = QString::number( fm.width(param[2]) );

        return true;
    }

    if (fn == "TOKEN") {
        // Get a text by tokens
        // $token(text here, position, token)  the token is in ascii number.
        // If the position is zero, this will count amount of text items separated by the given token
        if (param.length() < 3)
        return false;

        bool ok = false;

        QString tcnum = param[2]; // token character (ascii num)
        QChar tc = tcnum.toInt(&ok); // converted from string-number to actual number, into a character.

        if (ok == false)
            return false;

        int p = param[1].toInt(&ok); // Which position to use

        if (ok == false)
            return false;

        result = token(param[0], p, tc);

        return true;
    }

    if (fn == "VERSION") {
        // IIRC version.
        result = VERSION_STRING;
        return true;
    }

    // No functions were matching, return false as error.
    return false;
}
Example #16
0
File: floats.c Project: MassD/ocaml
CAMLprim value caml_asin_float_r(CAML_R, value f)
{
  return caml_copy_double_r(ctx,asin(Double_val(f)));
}
Example #17
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 #18
0
void HairpinSegment::layout()
      {
      if (hairpin()->useTextLine()) {
            if (parent())
                  rypos() += score()->styleS(StyleIdx::hairpinY).val() * spatium();
            TextLineSegment::layout();
            return;
            }

      QTransform t;
      qreal _spatium = spatium();
      qreal h1 = hairpin()->hairpinHeight().val() * spatium() * .5;
      qreal h2 = hairpin()->hairpinContHeight().val() * spatium() * .5;

      qreal len;
      qreal x = pos2().x();
      if (x < _spatium)             // minimum size of hairpin
            x = _spatium;
      qreal y = pos2().y();
      len     = sqrt(x * x + y * y);
      t.rotateRadians(asin(y/len));

      drawCircledTip =  hairpin()->hairpinCircledTip();
      circledTipRadius = 0;
      if( drawCircledTip )
        circledTipRadius  = 0.6 * _spatium * .5;
      if (hairpin()->hairpinType() == Hairpin::Type::CRESCENDO) {
            // crescendo
            switch (spannerSegmentType()) {
                  case SpannerSegmentType::SINGLE:
                  case SpannerSegmentType::BEGIN:
                        l1.setLine(.0 + circledTipRadius*2, .0, len, h1);
                        l2.setLine(.0 + circledTipRadius*2, .0, len, -h1);
                        circledTip.setX( 0 + circledTipRadius );
                        circledTip.setY( 0 );
                        break;
                  case SpannerSegmentType::MIDDLE:
                  case SpannerSegmentType::END:
                        drawCircledTip = false;
                        l1.setLine(.0,  h2, len, h1);
                        l2.setLine(.0, -h2, len, -h1);
                        break;
                  }
            }
      else {
            // decrescendo
            switch(spannerSegmentType()) {
                  case SpannerSegmentType::SINGLE:
                  case SpannerSegmentType::END:
                        l1.setLine(.0,  h1, len - circledTipRadius*2, 0.0);
                        l2.setLine(.0, -h1, len - circledTipRadius*2, 0.0);
                        circledTip.setX( len - circledTipRadius );
                        circledTip.setY( 0 );
                        break;
                  case SpannerSegmentType::BEGIN:
                  case SpannerSegmentType::MIDDLE:
                        drawCircledTip = false;
                        l1.setLine(.0,  h1, len, + h2);
                        l2.setLine(.0, -h1, len, - h2);
                        break;
                  }
            }
// Do Coord rotation
      l1 = t.map(l1);
      l2 = t.map(l2);
      if( drawCircledTip )
        circledTip = t.map(circledTip);


      QRectF r = QRectF(l1.p1(), l1.p2()).normalized() | QRectF(l2.p1(), l2.p2()).normalized();
      qreal w = point(score()->styleS(StyleIdx::hairpinLineWidth));
      setbbox(r.adjusted(-w*.5, -w*.5, w, w));
      if (parent())
            rypos() += score()->styleS(StyleIdx::hairpinY).val() * _spatium;
      adjustReadPos();
      }
CAAPhysicalJupiterDetails CAAPhysicalJupiter::Calculate(double JD)
{
  //What will be the return value
  CAAPhysicalJupiterDetails details;

  //Step 1
  double d = JD - 2433282.5;
  double T1 = d/36525;
  double alpha0 = 268.00 + 0.1061*T1;
  double alpha0rad = CAACoordinateTransformation::DegreesToRadians(alpha0);
  double delta0 = 64.50 - 0.0164*T1;
  double delta0rad = CAACoordinateTransformation::DegreesToRadians(delta0);

  //Step 2
  double W1 = CAACoordinateTransformation::MapTo0To360Range(17.710 + 877.90003539*d);
  double W2 = CAACoordinateTransformation::MapTo0To360Range(16.838 + 870.27003539*d);
  
  //Step 3
  double l0 = CAAEarth::EclipticLongitude(JD);
  double l0rad = CAACoordinateTransformation::DegreesToRadians(l0);
  double b0 = CAAEarth::EclipticLatitude(JD);
  double b0rad = CAACoordinateTransformation::DegreesToRadians(b0);
  double R = CAAEarth::RadiusVector(JD);

  //Step 4
  double l = CAAJupiter::EclipticLongitude(JD);
  double lrad = CAACoordinateTransformation::DegreesToRadians(l);
  double b = CAAJupiter::EclipticLatitude(JD);
  double brad = CAACoordinateTransformation::DegreesToRadians(b);
  double r = CAAJupiter::RadiusVector(JD);

  //Step 5
  double x = r*cos(brad)*cos(lrad) - R*cos(l0rad);
  double y = r*cos(brad)*sin(lrad) - R*sin(l0rad);
  double z = r*sin(brad) - R*sin(b0rad);
  double DELTA = sqrt(x*x + y*y + z*z);

  //Step 6
  l -= 0.012990*DELTA/(r*r);
  lrad = CAACoordinateTransformation::DegreesToRadians(l);

  //Step 7
  x = r*cos(brad)*cos(lrad) - R*cos(l0rad);
  y = r*cos(brad)*sin(lrad) - R*sin(l0rad);
  z = r*sin(brad) - R*sin(b0rad);
  DELTA = sqrt(x*x + y*y + z*z);

  //Step 8
  double e0 = CAANutation::MeanObliquityOfEcliptic(JD);
  double e0rad = CAACoordinateTransformation::DegreesToRadians(e0);

  //Step 9
  double alphas = atan2(cos(e0rad)*sin(lrad) - sin(e0rad)*tan(brad), cos(lrad));
  double deltas = asin(cos(e0rad)*sin(brad) + sin(e0rad)*cos(brad)*sin(lrad));

  //Step 10
  details.DS = CAACoordinateTransformation::RadiansToDegrees(asin(-sin(delta0rad)*sin(deltas) - cos(delta0rad)*cos(deltas)*cos(alpha0rad - alphas)));

  //Step 11
  double u = y*cos(e0rad) - z*sin(e0rad);
  double v = y*sin(e0rad) + z*cos(e0rad);
  double alpharad = atan2(u, x);
  double alpha = CAACoordinateTransformation::RadiansToDegrees(alpharad);
  double deltarad = atan2(v, sqrt(x*x + u*u));
  double delta = CAACoordinateTransformation::RadiansToDegrees(deltarad);
  double xi = atan2(sin(delta0rad)*cos(deltarad)*cos(alpha0rad - alpharad) - sin(deltarad)*cos(delta0rad), cos(deltarad)*sin(alpha0rad - alpharad));

  //Step 12
  details.DE = CAACoordinateTransformation::RadiansToDegrees(asin(-sin(delta0rad)*sin(deltarad) - cos(delta0rad)*cos(deltarad)*cos(alpha0rad - alpharad)));

  //Step 13
  details.Geometricw1 = CAACoordinateTransformation::MapTo0To360Range(W1 - CAACoordinateTransformation::RadiansToDegrees(xi) - 5.07033*DELTA);
  details.Geometricw2 = CAACoordinateTransformation::MapTo0To360Range(W2 - CAACoordinateTransformation::RadiansToDegrees(xi) - 5.02626*DELTA);

  //Step 14
  double C = 57.2958 * (2*r*DELTA + R*R - r*r - DELTA*DELTA)/(4*r*DELTA);
  if (sin(lrad - l0rad) > 0)
  {
    details.Apparentw1 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw1 + C);
    details.Apparentw2 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw2 + C);
  }
  else
  {
    details.Apparentw1 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw1 - C);
    details.Apparentw2 = CAACoordinateTransformation::MapTo0To360Range(details.Geometricw2 - C);
  }

  //Step 15
  double NutationInLongitude = CAANutation::NutationInLongitude(JD);
  double NutationInObliquity = CAANutation::NutationInObliquity(JD);
  e0 += NutationInObliquity/3600;
  e0rad = CAACoordinateTransformation::DegreesToRadians(e0);

  //Step 16
  alpha += 0.005693*(cos(alpharad)*cos(l0rad)*cos(e0rad) + sin(alpharad)*sin(l0rad))/cos(deltarad);
  alpha = CAACoordinateTransformation::MapTo0To360Range(alpha);
  alpharad = CAACoordinateTransformation::DegreesToRadians(alpha);
  delta += 0.005693*(cos(l0rad)*cos(e0rad)*(tan(e0rad)*cos(deltarad) - sin(alpharad)*sin(deltarad)) + cos(alpharad)*sin(deltarad)*sin(l0rad));
  deltarad = CAACoordinateTransformation::DegreesToRadians(delta);

  //Step 17
  double NutationRA = CAANutation::NutationInRightAscension(alpha/15, delta, e0, NutationInLongitude, NutationInObliquity);
  double alphadash = alpha + NutationRA/3600;
  double alphadashrad = CAACoordinateTransformation::DegreesToRadians(alphadash);
  double NutationDec = CAANutation::NutationInDeclination(alpha/15, delta, e0, NutationInLongitude, NutationInObliquity);
  double deltadash = delta + NutationDec/3600;
  double deltadashrad = CAACoordinateTransformation::DegreesToRadians(deltadash);
  NutationRA = CAANutation::NutationInRightAscension(alpha0/15, delta0, e0, NutationInLongitude, NutationInObliquity);
  double alpha0dash = alpha0 + NutationRA/3600;
  double alpha0dashrad = CAACoordinateTransformation::DegreesToRadians(alpha0dash);
  NutationDec = CAANutation::NutationInDeclination(alpha0/15, delta0, e0, NutationInLongitude, NutationInObliquity);
  double delta0dash = delta0 + NutationDec/3600;
  double delta0dashrad = CAACoordinateTransformation::DegreesToRadians(delta0dash);

  //Step 18
  details.P = CAACoordinateTransformation::MapTo0To360Range(CAACoordinateTransformation::RadiansToDegrees(atan2(cos(delta0dashrad)*sin(alpha0dashrad - alphadashrad), sin(delta0dashrad)*cos(deltadashrad) - cos(delta0dashrad)*sin(deltadashrad)*cos(alpha0dashrad - alphadashrad))));

  return details;
}
Example #20
0
void
pcl::CRHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Check if input was set
  if (!normals_)
  {
    PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  if (normals_->points.size () != surface_->points.size ())
  {
    PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  Eigen::Vector3f plane_normal;
  plane_normal[0] = -centroid_[0];
  plane_normal[1] = -centroid_[1];
  plane_normal[2] = -centroid_[2];
  Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
  plane_normal.normalize ();
  Eigen::Vector3f axis = plane_normal.cross (z_vector);
  double rotation = -asin (axis.norm ());
  axis.normalize ();

  int nbins = nbins_;
  int bin_angle = 360 / nbins;

  Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));

  pcl::PointCloud<pcl::PointNormal> grid;
  grid.points.resize (indices_->size ());

  for (size_t i = 0; i < indices_->size (); i++)
  {
    grid.points[i].getVector4fMap () = surface_->points[(*indices_)[i]].getVector4fMap ();
    grid.points[i].getNormalVector4fMap () = normals_->points[(*indices_)[i]].getNormalVector4fMap ();
  }

  pcl::transformPointCloudWithNormals (grid, grid, transformPC);

  //fill spatial data vector and the zero-initialize or "value-initialize" an array on c++, 
  // the initialization is made with () after the [nbins]
  kiss_fft_scalar * spatial_data = new kiss_fft_scalar[nbins]();
  

  float sum_w = 0, w = 0;
  int bin = 0;
  for (const auto &point : grid.points)
  {
    bin = static_cast<int> ((((atan2 (point.normal_y, point.normal_x) + M_PI) * 180 / M_PI) / bin_angle)) % nbins;
    w = std::sqrt (point.normal_y * point.normal_y + point.normal_x * point.normal_x);
    sum_w += w;
    spatial_data[bin] += w;
  }

  for (int i = 0; i < nbins; ++i)
    spatial_data[i] /= sum_w;

  kiss_fft_cpx * freq_data = new kiss_fft_cpx[nbins / 2 + 1];
  kiss_fftr_cfg mycfg = kiss_fftr_alloc (nbins, 0, NULL, NULL);
  kiss_fftr (mycfg, spatial_data, freq_data);

  output.points.resize (1);
  output.width = output.height = 1;

  output.points[0].histogram[0] = freq_data[0].r / freq_data[0].r; //dc
  int k = 1;
  for (int i = 1; i < (nbins / 2); i++, k += 2)
  {
    output.points[0].histogram[k] = freq_data[i].r / freq_data[0].r;
    output.points[0].histogram[k + 1] = freq_data[i].i / freq_data[0].r;
  }

  output.points[0].histogram[nbins - 1] = freq_data[nbins / 2].r / freq_data[0].r; //nyquist

  delete[] spatial_data;
  delete[] freq_data;

}
Example #21
0
kmScalar kmQuaternionGetYaw(const kmQuaternion* q) {
    float result = asin(-2 * (q->x * q->z - q->w * q->y));
    return result;
}
Example #22
0
void MadgwickAHRSupdate(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz, attitude_t * angle)
{
  float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;
    
    float hx, hy, bx, bz;
    float halfwx, halfwy, halfwz;
    
    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
    
    // Reference direction of Earth's magnetic field
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    bx = sqrt(hx * hx + hy * hy);
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
    
    // Estimated direction of magnetic field
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
    
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (my * halfwz - mz * halfwy);
    halfey = (mz * halfwx - mx * halfwz);
    halfez = (mx * halfwy - my * halfwx);


 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;
    
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }
  
  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
  
  
    /* output data */
    angle->Y = atan2(2 * q1 * q2 + 2 * q0 * q3, q0*q0+q1*q1-q2*q2-q3*q3)* 57.3; 
    angle->P = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;																			// pitcho???
    angle->R = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
    
}
Example #23
0
static void traverse_loop(struct loop *lp, struct connection *anchor_connection)
/*
*   This is the workhorse of the display program. The algorithm is
*   recursive based on processing individual loops. Each base pairing
*   region is displayed using the direction given by the circle diagram,
*   and the connections between the regions is drawn by equally spaced
*   points. The radius of the loop is set to minimize the square error
*   for lengths between sequential bases in the loops. The "correct"
*   length for base links is 1. If the least squares fitting of the
*   radius results in loops being less than 1/2 unit apart, then that
*   segment is extruded. 
*   
*   The variable, anchor_connection, gives the connection to the loop
*   processed in an previous level of recursion.
*/

{
  double xs,ys,xe,ye,xn,yn,angleinc,r;
  double radius,xc,yc,xo,yo,astart,aend,a;
  struct connection *cp,*cpnext,**cpp,*acp,*cpprev;
  int i,j,n,ic;
  double da,maxang;
  int count,icstart,icend,icmiddle,icroot;
  logical done,done_all_connections,rooted;
  int sign;
  double midx,midy,nrx,nry,mx,my,vx,vy,dotmv,nmidx,nmidy;
  int icstart1,icup,icdown,icnext,direction;
  double dan,dx,dy,rr;
  double cpx,cpy,cpnextx,cpnexty,cnx,cny,rcn,rc,lnx,lny,rl,ac,acn,sx,sy,dcp;
  int imaxloop;
    
  angleinc = 2 * pi / (nbase+1);
  acp = NULL;
  icroot = -1;
  for (cpp=lp->connections, ic = 0; (cp = *cpp); cpp++, ic++) {
    /*	xs = cos(angleinc*cp->start);
	ys = sin(angleinc*cp->start);
	xe = cos(angleinc*cp->end);
	ye = sin(angleinc*cp->end); */
    xs = -sin(angleinc*cp->start);
    ys = cos(angleinc*cp->start);
    xe = -sin(angleinc*cp->end);
    ye = cos(angleinc*cp->end);
    xn = ye-ys;
    yn = xs-xe;
    r = sqrt(xn*xn + yn*yn);
    cp->xrad = xn/r;
    cp->yrad = yn/r;
    cp->angle = atan2(yn,xn);
    if (cp->angle < 0.0) cp->angle += 2*pi;
    if (anchor_connection != NULL &&
	anchor_connection->region == cp->region) {
      acp = cp;
      icroot = ic;
    }
  }
	
 set_radius:
  determine_radius(lp,lencut);
  radius = lp->radius;
  if (anchor_connection == NULL) xc = yc = 0.0;
  else {
    xo = (bases[acp->start].x+bases[acp->end].x) / 2.0;
    yo = (bases[acp->start].y+bases[acp->end].y) / 2.0;
    xc = xo - radius * acp->xrad;
    yc = yo - radius * acp->yrad;
  }
	
  /*
   *   The construction of the connectors will proceed in blocks of
   *   connected connectors, where a connected connector pairs means
   *   two connectors that are forced out of the drawn circle because they
   *   are too close together in angle.
   */

  /*
   *   First, find the start of a block of connected connectors
   */

  if (icroot == -1) 
    icstart = 0;
  else icstart = icroot;
  cp = lp->connections[icstart];
  count = 0;
  if (debug) printf("Now processing loop %d\n",lp->number);
  done = false;
  do {
    j = icstart - 1;
    if (j < 0) j = lp->nconnection - 1;
    cpprev = lp->connections[j];
    if (!connected_connection(cpprev,cp)) {
      done = true;
    }
    else {
      icstart = j;
      cp = cpprev;
    }
    if (++count > lp->nconnection) {
      /*
       *  Here everything is connected. Break on maximum angular separation
       *  between connections. 
       */
      maxang = -1.0;
      for (ic = 0;  ic < lp->nconnection;  ic++) {
	j = ic + 1;
	if (j >= lp->nconnection) j = 0;
	cp = lp->connections[ic];
	cpnext = lp->connections[j];
	ac = cpnext->angle - cp->angle;
	if (ac < 0.0) ac += 2*pi;
	if (ac > maxang) {
	  maxang = ac;
	  imaxloop = ic;
	}
      }
      icend = imaxloop;
      icstart = imaxloop + 1;
      if (icstart >= lp->nconnection) icstart = 0;
      cp = lp->connections[icend];
      cp->broken = true;
      done = true;
    }
  } while    (!done);
  done_all_connections = false;
  icstart1 = icstart;
  if (debug) printf("Icstart1 = %d\n",icstart1);
  while (!done_all_connections) {
    count = 0;
    done = false;
    icend = icstart;
    rooted = false;
    while (!done) {
      cp = lp->connections[icend];
      if (icend == icroot) rooted = true;
      j = icend + 1;
      if (j >= lp->nconnection) {
	j = 0;
      }
      cpnext = lp->connections[j];
      if (connected_connection(cp,cpnext)) {
	if (++count >= lp->nconnection) break;
	icend = j;
      }
      else {
	done = true;
      }
    }
    icmiddle = find_ic_middle(icstart,icend,anchor_connection,acp,lp);
    ic = icup = icdown = icmiddle;
    if (debug)
      printf("IC start = %d  middle = %d  end = %d\n",
	     icstart,icmiddle,icend);
    done = false;
    direction = 0;
    while (!done) {
      if (direction < 0) {
	ic = icup;
      }
      else if (direction == 0) {
	ic = icmiddle;
      }
      else {
	ic = icdown;
      }
      if (ic >= 0) {
	cp = lp->connections[ic];
	if (anchor_connection == NULL || acp != cp) {
	  if (direction == 0) {
	    astart = cp->angle - asin(1.0/2.0/radius);
	    aend = cp->angle + asin(1.0/2.0/radius);
	    bases[cp->start].x = xc + radius*cos(astart);
	    bases[cp->start].y = yc + radius*sin(astart);
	    bases[cp->end].x = xc + radius*cos(aend);
	    bases[cp->end].y = yc + radius*sin(aend);
	  }
	  else if (direction < 0) {
	    j = ic + 1;
	    if (j >= lp->nconnection) j = 0;
	    cp = lp->connections[ic];
	    cpnext = lp->connections[j];
	    cpx = cp->xrad;
	    cpy = cp->yrad;
	    ac = (cp->angle + cpnext->angle) / 2.0;
	    if (cp->angle > cpnext->angle) ac -= pi;
	    cnx = cos(ac);
	    cny = sin(ac);
	    lnx = cny;
	    lny = -cnx;
	    da = cpnext->angle - cp->angle;
	    if (da < 0.0) da += 2*pi;
	    if (cp->extruded) {
	      if (da <= pi/2) rl = 2.0;
	      else rl = 1.5;
	    }
	    else {
	      rl = 1.0;
	    }
	    bases[cp->end].x = bases[cpnext->start].x + rl*lnx;
	    bases[cp->end].y = bases[cpnext->start].y + rl*lny;
	    bases[cp->start].x = bases[cp->end].x + cpy;
	    bases[cp->start].y = bases[cp->end].y - cpx;
	  }
	  else {
	    j = ic - 1;
	    if (j < 0) j = lp->nconnection - 1;
	    cp = lp->connections[j];
	    cpnext = lp->connections[ic];
	    cpnextx = cpnext->xrad;
	    cpnexty = cpnext->yrad;
	    ac = (cp->angle + cpnext->angle) / 2.0;
	    if (cp->angle > cpnext->angle) ac -= pi;
	    cnx = cos(ac);
	    cny = sin(ac);
	    lnx = -cny;
	    lny = cnx;
	    da = cpnext->angle - cp->angle;
	    if (da < 0.0) da += 2*pi;
	    if (cp->extruded) {
	      if (da <= pi/2) rl = 2.0;
	      else rl = 1.5;
	    }
	    else {
	      rl = 1.0;
	    }
	    bases[cpnext->start].x = bases[cp->end].x + rl*lnx;
	    bases[cpnext->start].y = bases[cp->end].y + rl*lny;
	    bases[cpnext->end].x = bases[cpnext->start].x - cpnexty;
	    bases[cpnext->end].y = bases[cpnext->start].y + cpnextx;
	  }
	}
      }
      if (direction < 0) {
	if (icdown == icend) {
	  icdown = -1;
	}
	else if (icdown >= 0) {
	  if (++icdown >= lp->nconnection) {
	    icdown = 0;
	  }
	}
	direction = 1;
      }
      else {
	if (icup == icstart) icup = -1;
	else if (icup >= 0) {
	  if (--icup < 0) {
	    icup = lp->nconnection - 1;
	  }
	}
	direction = -1;
      }
      done = icup == -1 && icdown == -1;
    }
    icnext = icend + 1;
    if (icnext >= lp->nconnection) icnext = 0;
    if (icend != icstart && (! (icstart == icstart1 && icnext == icstart1))) {
      /*
       *	    Move the bases just constructed (or the radius) so
       *	    that the bisector of the end points is radius distance
       *	    away from the loop center.
       */
      cp = lp->connections[icstart];
      cpnext = lp->connections[icend];
      dx = bases[cpnext->end].x - bases[cp->start].x;
      dy = bases[cpnext->end].y - bases[cp->start].y;
      midx = bases[cp->start].x + dx/2.0;
      midy = bases[cp->start].y + dy/2.0;
      rr = sqrt(dx*dx + dy*dy);
      mx = dx / rr;
      my = dy / rr;
      vx = xc - midx;
      vy = yc - midy;
      rr = sqrt(dx*dx + dy*dy);
      vx /= rr;
      vy /= rr;
      dotmv = vx*mx + vy*my;
      nrx = dotmv*mx - vx;
      nry = dotmv*my - vy;
      rr = sqrt(nrx*nrx + nry*nry);
      nrx /= rr;
      nry /= rr;
      /*
       *	    Determine which side of the bisector the center should be.
       */
      dx = bases[cp->start].x - xc;
      dy = bases[cp->start].y - yc;
      ac = atan2(dy,dx);
      if (ac < 0.0) ac += 2*pi;
      dx = bases[cpnext->end].x - xc;
      dy = bases[cpnext->end].y - yc;
      acn = atan2(dy,dx);
      if (acn < 0.0) acn += 2*pi;
      if (acn < ac) acn += 2*pi;
      if (acn - ac > pi) sign = -1;
      else sign = 1;
      nmidx = xc + sign*radius*nrx;
      nmidy = yc + sign*radius*nry;
      if (rooted) {
	xc -= nmidx - midx;
	yc -= nmidy - midy;
      }
      else {
	for (ic=icstart; ; ++ic >= lp->nconnection ? (ic = 0) : 0) {
	  cp = lp->connections[ic];
	  i = cp->start;
	  bases[i].x += nmidx - midx;
	  bases[i].y += nmidy - midy;
	  i = cp->end;
	  bases[i].x += nmidx - midx;
	  bases[i].y += nmidy - midy;
	  if (ic == icend) break;
	}
      }
    }
    icstart = icnext;
    done_all_connections = icstart == icstart1;
  }
  for (ic=0; ic < lp->nconnection; ic++) {
    cp = lp->connections[ic];
    j = ic + 1;
    if (j >= lp->nconnection) j = 0;
    cpnext = lp->connections[j];
    dx = bases[cp->end].x - xc;
    dy = bases[cp->end].y - yc;
    rc = sqrt(dx*dx + dy*dy);
    ac = atan2(dy,dx);
    if (ac < 0.0) ac += 2*pi;
    dx = bases[cpnext->start].x - xc;
    dy = bases[cpnext->start].y - yc;
    rcn = sqrt(dx*dx + dy*dy);
    acn = atan2(dy,dx);
    if (acn < 0.0) acn += 2*pi;
    if (acn < ac) acn += 2*pi;
    dan = acn - ac;
    dcp = cpnext->angle - cp->angle;
    if (dcp <= 0.0) dcp += 2*pi;
    if (fabs(dan-dcp) > pi) {
      if (cp->extruded) {
	fprintf(stderr, "Warning from traverse_loop. Loop %d has crossed regions\n",
	       lp->number);
      }
      else if ((cpnext->start - cp->end) != 1) {
	cp->extruded = true;
	goto set_radius;	    /* Forever shamed */
      }
    }
    if (cp->extruded) {
      construct_extruded_segment(cp,cpnext);
    }
    else {
      n = cpnext->start - cp->end;
      if (n < 0) n += nbase + 1;
      angleinc = dan / n;
      for (j = 1;  j < n;  j++) {
	i = cp->end + j;
	if (i > nbase) i -= nbase + 1;
	a = ac + j*angleinc;
	rr = rc + (rcn-rc)*(a-ac)/dan;
	bases[i].x = xc + rr*cos(a);
	bases[i].y = yc + rr*sin(a);
      }
    }
  }
  for (ic=0; ic < lp->nconnection; ic++) {
    if (icroot != ic) {
      cp = lp->connections[ic];
      generate_region(cp);
      traverse_loop(cp->loop,cp);
    }
  }
  n = 0;
  sx = 0.0;
  sy = 0.0;
  for (ic = 0;  ic < lp->nconnection;  ic++) {
    j = ic + 1;
    if (j >= lp->nconnection) j = 0;
    cp = lp->connections[ic];
    cpnext = lp->connections[j];
    n += 2;
    sx += bases[cp->start].x + bases[cp->end].x;
    sy += bases[cp->start].y + bases[cp->end].y;
    if (!cp->extruded) {
      for (j = cp->end + 1;  j != cpnext->start;  j++) {
	if (j > nbase) j -= nbase + 1;
	n++;
	sx += bases[j].x;
	sy += bases[j].y;
      }
    }
  }
  lp->x = sx / n;
  lp->y = sy / n;

  /* free connections (ih) */
  for (ic = 0;  ic < lp->nconnection;  ic++)
    free(lp->connections[ic]);
  free(lp->connections);
}
Example #24
0
 inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
 {
     lp_lat = pj_authlat(asin( 2. * xy_y * this->m_par.k0 / this->m_proj_parm.qp), this->m_proj_parm.apa);
     lp_lon = xy_x / this->m_par.k0;
 }
Example #25
0
void RangeSteering::cycleDock(ros::Duration dt)
{
    ros::Time currentTime = ros::Time::now();
    double w0, w;
    geometry_msgs::Twist vel;
    vel.linear.y = 0.0;
    vel.linear.z = 0.0;
    vel.angular.x = 0.0;
    vel.angular.y = 0.0;

    //~ /* Check if close enough to start local docking */
    //~ double dockStartRange = sqrt(dockStart*dockStart + baseDist*baseDist/4);
    //~
    //~
    //~ if (fabs(leftRange - dockStartRange) < 0.1 && fabs(rightRange - dockStartRange) < 0.1)
    //~ {
    //~ dockStarted = true;
    //~ cout << "Starting Local Docking!" << endl;
    //~ }

    switch (dockState)
    {
    /* Set the turn angle to base, heading is calculated based on the followed radius */
    /* Approximated by 90 degrees (actually dependent on radius and both ranges) */
    case STATE_START_TURN_TO:
    {
        vel.linear.x = 0.0;
        /* Heading dependent on distance */
        double beta = -followDirection * (M_PI / 2 - asin(baseDist / (2 * leftRange)));
        homeHeading = odoHeading - beta;
        dockState = STATE_TURN;
        cout << "Start turning home!, beta: " << beta << ", home: " << homeHeading << ", odoHeading: " << odoHeading
             << endl;
        break;
    }
    case STATE_TURN:
    {

        vel.linear.x = 0.0;
        vel.angular.z = followDirection * turnSpeed;
        double diff = fabs(odoHeading - homeHeading);
        if (diff >= 2 * M_PI)
        {
            diff -= 2 * M_PI;
        }
        else if (diff <= -2 * M_PI)
        {
            diff += 2 * M_PI;
        }

        cout << "----Turning diff: " << diff * 180 / M_PI << "---" << endl;

        /* Stop within +-5 deg of final angle */
        if (diff < 5 * M_PI / 180)
        {
            vel.angular.z = 0;
            dockState = STATE_DOCK_STRAIGHT;
            dockStarted = true;
            cout << "----Turning done, start straight! " << endl;
        }

        cmdPub.publish(vel);
        break;
    }
    case STATE_DOCK_STRAIGHT:
    {
        if (dockStarted)
        {

            w = 0;

            //~ printf("cosA: %f, xL: %f, yL: %f, dx: %f, dy: %f, distToHome: %f, angleToHome: %f\n",  cosAlpha, xL, yL,
            //dx,dy, distToHome, angleToHome);

            /* Check if docking complete */
            if (distToHome < 0.1)
            {
                cout << "Docking Completed!" << endl;
                doDock = false;
                dockStarted = false;
                vel.linear.x = 0.0;
                vel.angular.z = 0.0;
                cmdPub.publish(vel);
            }
            if (doDock)
            {
                vel.linear.x = 0.2; // speed*(distToHome/dockStart);

                errDock = angleToHome;
                if (errOldDock != errDock)
                {
                    dErrDock = errDock - errOldDock;
                }
                errOldDock = errDock;
                w = kpDock * errDock + kdDock * dErrDock;
                vel.angular.z = w;
                cout << "e = " << errDock << ", dErrDock = " << dErrDock << ", w = " << w
                     << ", kpDock*err = " << kpDock* errDock << ",  kdDock*dErrDock = " << kdDock* dErrDock << endl;
                cmdPub.publish(vel);
            }
            break;
        }
    }
    }
}
Example #26
0
//---------------------------------------------------------------------------
float ofxKinect::getAccelRoll(){
	return ofRadToDeg(asin(getMksAccel().x/OFX_KINECT_GRAVITY));
}
Example #27
0
/*
 * Check the vertex list for adjacent pairs of
 * points which are too close together for the
 * subsequent dot- and cross-product calculations
 * of Girard's theorem.
 */
void RemoveDups() {
  int i, nvnew;
  Vec Vnew[16];
  Vec tmp;
  double lon, lat;

  double separation;

  if (DEBUG >= 4) {
    printf("RemoveDups() TOLERANCE = %13.6e [%13.6e arcsec]\n\n", TOLERANCE,
           TOLERANCE / DEG_TO_RADIANS * 3600.);

    for (i = 0; i < nv; ++i) {
      lon = atan2(V[i].y, V[i].x) / DEG_TO_RADIANS;
      lat = asin(V[i].z) / DEG_TO_RADIANS;

      printf("RemoveDups() orig: %3d [%13.6e,%13.6e,%13.6e] "
             "-> (%10.6f,%10.6f)\n",
             i, V[i].x, V[i].y, V[i].z, lon, lat);

      fflush(stdout);
    }

    printf("\n");
  }

  Vnew[0].x = V[0].x;
  Vnew[0].y = V[0].y;
  Vnew[0].z = V[0].z;

  nvnew = 0;

  for (i = 0; i < nv; ++i) {
    ++nvnew;

    Vnew[nvnew].x = V[(i + 1) % nv].x;
    Vnew[nvnew].y = V[(i + 1) % nv].y;
    Vnew[nvnew].z = V[(i + 1) % nv].z;

    Cross(&V[i], &V[(i + 1) % nv], &tmp);

    separation = Normalize(&tmp);

    if (DEBUG >= 4) {
      printf("RemoveDups(): %3d x %3d: distance = %13.6e "
             "[%13.6e arcsec] (would become %d)\n",
             (i + 1) % nv, i, separation, separation / DEG_TO_RADIANS * 3600.,
             nvnew);

      fflush(stdout);
    }

    if (separation < TOLERANCE) {
      --nvnew;

      if (DEBUG >= 4) {
        printf("RemoveDups(): %3d is a duplicate (nvnew -> %d)\n", i, nvnew);

        fflush(stdout);
      }
    }
  }

  if (DEBUG >= 4) {
    printf("\n");
    fflush(stdout);
  }

  if (nvnew < nv) {
    for (i = 0; i < nvnew; ++i) {
      V[i].x = Vnew[i].x;
      V[i].y = Vnew[i].y;
      V[i].z = Vnew[i].z;
    }

    nv = nvnew;
  }
}
Example #28
0
static void node_shader_exec_math(void *UNUSED(data), int UNUSED(thread), bNode *node, bNodeExecData *UNUSED(execdata), bNodeStack **in, bNodeStack **out) 
{
	switch (node->custom1) {
	
	case 0: /* Add */
		out[0]->vec[0] = in[0]->vec[0] + in[1]->vec[0];
		break; 
	case 1: /* Subtract */
		out[0]->vec[0] = in[0]->vec[0] - in[1]->vec[0];
		break; 
	case 2: /* Multiply */
		out[0]->vec[0] = in[0]->vec[0] * in[1]->vec[0];
		break; 
	case 3: /* Divide */
		{
			if (in[1]->vec[0]==0)	/* We don't want to divide by zero. */
				out[0]->vec[0] = 0.0;
			else
				out[0]->vec[0] = in[0]->vec[0] / in[1]->vec[0];
			}
		break;
	case 4: /* Sine */
		{
			if (in[0]->hasinput || !in[1]->hasinput)  /* This one only takes one input, so we've got to choose. */
				out[0]->vec[0] = sin(in[0]->vec[0]);
			else
				out[0]->vec[0] = sin(in[1]->vec[0]);
		}
		break;
	case 5: /* Cosine */
		{
			if (in[0]->hasinput || !in[1]->hasinput)  /* This one only takes one input, so we've got to choose. */
				out[0]->vec[0] = cos(in[0]->vec[0]);
			else
				out[0]->vec[0] = cos(in[1]->vec[0]);
		}
		break;
	case 6: /* Tangent */
		{
			if (in[0]->hasinput || !in[1]->hasinput)  /* This one only takes one input, so we've got to choose. */
				out[0]->vec[0] = tan(in[0]->vec[0]);
			else
				out[0]->vec[0] = tan(in[1]->vec[0]);
		}
		break;
	case 7: /* Arc-Sine */
		{
			if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */
				/* Can't do the impossible... */
				if (in[0]->vec[0] <= 1 && in[0]->vec[0] >= -1)
					out[0]->vec[0] = asin(in[0]->vec[0]);
				else
					out[0]->vec[0] = 0.0;
			}
			else {
				/* Can't do the impossible... */
				if (in[1]->vec[0] <= 1 && in[1]->vec[0] >= -1)
					out[0]->vec[0] = asin(in[1]->vec[0]);
				else
					out[0]->vec[0] = 0.0;
			}
		}
		break;
	case 8: /* Arc-Cosine */
		{
			if (in[0]->hasinput || !in[1]->hasinput) { /* This one only takes one input, so we've got to choose. */
				/* Can't do the impossible... */
				if (in[0]->vec[0] <= 1 && in[0]->vec[0] >= -1)
					out[0]->vec[0] = acos(in[0]->vec[0]);
				else
					out[0]->vec[0] = 0.0;
			}
			else {
				/* Can't do the impossible... */
				if (in[1]->vec[0] <= 1 && in[1]->vec[0] >= -1)
					out[0]->vec[0] = acos(in[1]->vec[0]);
				else
					out[0]->vec[0] = 0.0;
			}
		}
		break;
	case 9: /* Arc-Tangent */
		{
			if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
				out[0]->vec[0] = atan(in[0]->vec[0]);
			else
				out[0]->vec[0] = atan(in[1]->vec[0]);
		}
		break;
	case 10: /* Power */
		{
			/* Only raise negative numbers by full integers */
			if (in[0]->vec[0] >= 0) {
				out[0]->vec[0] = pow(in[0]->vec[0], in[1]->vec[0]);
			}
			else {
				float y_mod_1 = fabsf(fmodf(in[1]->vec[0], 1.0f));
				
				/* if input value is not nearly an integer, fall back to zero, nicer than straight rounding */
				if (y_mod_1 > 0.999f || y_mod_1 < 0.001f) {
					out[0]->vec[0] = powf(in[0]->vec[0], floorf(in[1]->vec[0] + 0.5f));
				}
				else {
					out[0]->vec[0] = 0.0f;
				}
			}

		}
		break;
	case 11: /* Logarithm */
		{
			/* Don't want any imaginary numbers... */
			if (in[0]->vec[0] > 0  && in[1]->vec[0] > 0)
				out[0]->vec[0] = log(in[0]->vec[0]) / log(in[1]->vec[0]);
			else
				out[0]->vec[0] = 0.0;
		}
		break;
	case 12: /* Minimum */
		{
			if (in[0]->vec[0] < in[1]->vec[0])
				out[0]->vec[0] = in[0]->vec[0];
			else
				out[0]->vec[0] = in[1]->vec[0];
		}
		break;
	case 13: /* Maximum */
		{
			if (in[0]->vec[0] > in[1]->vec[0])
				out[0]->vec[0] = in[0]->vec[0];
			else
				out[0]->vec[0] = in[1]->vec[0];
		}
		break;
	case 14: /* Round */
		{
			if (in[0]->hasinput || !in[1]->hasinput) /* This one only takes one input, so we've got to choose. */
				out[0]->vec[0] = (in[0]->vec[0] < 0) ? (int)(in[0]->vec[0] - 0.5f) : (int)(in[0]->vec[0] + 0.5f);
			else
				out[0]->vec[0] = (in[1]->vec[0] < 0) ? (int)(in[1]->vec[0] - 0.5f) : (int)(in[1]->vec[0] + 0.5f);
		}
		break;
	case 15: /* Less Than */
		{
			if (in[0]->vec[0] < in[1]->vec[0])
				out[0]->vec[0] = 1.0f;
			else
				out[0]->vec[0] = 0.0f;
		}
		break;
	case 16: /* Greater Than */
		{
			if (in[0]->vec[0] > in[1]->vec[0])
				out[0]->vec[0] = 1.0f;
			else
				out[0]->vec[0] = 0.0f;
		}
		break;
	case 17: /* Modulo */
		{
			if (in[1]->vec[0] == 0.0f)
				out[0]->vec[0] = 0.0f;
			else
				out[0]->vec[0] = fmod(in[0]->vec[0], in[1]->vec[0]);
		}
		break;
	}
}
void test_extra(T)
{
   T t = 1;
   t = abs(t);
   t = abs(t*t);

   t = fabs(t);
   t = fabs(t*t);

   t = sqrt(t);
   t = sqrt(t*t);

   t = floor(t);
   t = floor(t*t);

   t = ceil(t);
   t = ceil(t*t);

   t = trunc(t);
   t = trunc(t*t);

   t = round(t);
   t = round(t*t);

   t = exp(t);
   t = exp(t*t);

   t = log(t);
   t = log(t*t);

   t = log10(t);
   t = log10(t*t);

   t = cos(t);
   t = cos(t*t);

   t = sin(t);
   t = sin(t*t);

   t = tan(t);
   t = tan(t*t);

   t = asin(t);
   t = asin(t*t);

   t = atan(t);
   t = atan(t*t);

   t = acos(t);
   t = acos(t*t);

   t = cosh(t);
   t = cosh(t*t);

   t = sinh(t);
   t = sinh(t*t);

   t = tanh(t);
   t = tanh(t*t);

   double dval = 2;
   t = pow(t, t);
   t = pow(t, t*t);
   t = pow(t, dval);
   t = pow(t*t, t);
   t = pow(t*t, t*t);
   t = pow(t*t, dval);
   t = pow(dval, t);
   t = pow(dval, t*t);

   t = atan2(t, t);
   t = atan2(t, t*t);
   t = atan2(t, dval);
   t = atan2(t*t, t);
   t = atan2(t*t, t*t);
   t = atan2(t*t, dval);
   t = atan2(dval, t);
   t = atan2(dval, t*t);

   t = fmod(t, t);
   t = fmod(t, t*t);
   t = fmod(t, dval);
   t = fmod(t*t, t);
   t = fmod(t*t, t*t);
   t = fmod(t*t, dval);
   t = fmod(dval, t);
   t = fmod(dval, t*t);

   typedef typename T::backend_type backend_type;
   typedef typename backend_type::exponent_type exp_type;
   exp_type e = 0;
   int i = 0;

   t = ldexp(t, i);
   t = ldexp(t*t, i);
   t = ldexp(t, e);
   t = ldexp(t*t, e);

   t = frexp(t, &i);
   t = frexp(t*t, &i);
   t = frexp(t, &e);
   t = frexp(t*t, &e);

   t = scalbn(t, i);
   t = scalbn(t*t, i);
   t = scalbn(t, e);
   t = scalbn(t*t, e);

   t = logb(t);
   t = logb(t*t);
   e = ilogb(t);
   e = ilogb(t*t);
}
Example #30
0
void meCloseHoles::Update()
{
	vtkPolyDataNormals * normals = vtkPolyDataNormals::New();
	normals->SetInput(this->input);
	normals->SplittingOff();
	normals->SetFeatureAngle(180.0);
	normals->ComputePointNormalsOff();
	normals->ComputeCellNormalsOn();
	normals->AutoOrientNormalsOn();
	normals->ConsistencyOn();
	normals->Update();
	normals->GetOutput()->BuildLinks();

	vtkPointLocator * locator = vtkPointLocator::New();
	locator->SetDataSet(this->input);
	locator->BuildLocator();

	vtkFeatureEdges * edges = vtkFeatureEdges::New();
	edges->SetInput(normals->GetOutput());
	edges->BoundaryEdgesOn();
	edges->FeatureEdgesOff();
	edges->ManifoldEdgesOff();
	edges->NonManifoldEdgesOff();
	edges->Update();          
	
	//PolyDataConnectivityFilter: each hole is identified as a region
	vtkPolyDataConnectivityFilter *connec = vtkPolyDataConnectivityFilter::New();
	connec->SetInput(edges->GetOutput());
	connec->SetExtractionModeToSpecifiedRegions();
	connec->Update();
	edges->Delete();

	int numEdgesInRegion = 0,
	    numRegions = connec->GetNumberOfExtractedRegions(),
	    nUsedIds,
	    pointId,
	    pointId1,
	    pointId2;

	this->patchOutput.clear();
	patchOutput.resize(numRegions);

	vtkCleanPolyData * clean = vtkCleanPolyData::New();
	vector<vtkPolyData*> poly(numRegions);
	vtkPolyData * patch;
	vtkPoints * points;
	vtkCellArray * polys;
	vtkIdList * ptIds = vtkIdList::New(),
	          * ptIds1 = vtkIdList::New(),
	          * ptIds2 = vtkIdList::New(),
	          * ptIdsOrdered = vtkIdList::New(),
	          * cellIds = vtkIdList::New(),
	          * cellIdsOrdered = vtkIdList::New();

	vector<int> nptsInCorona,
	            nptsUntilCorona;

	vector< vector<bool> > onlyOneCorrespondingPoint;
	vector< vector<double> > normalsAtPoint;

	//For each hole (region) the points are stored and sorted
	for(int idRegion = 0; idRegion < numRegions;	idRegion++ )
	{
		connec->AddSpecifiedRegion(idRegion);
		connec->Update();   
		numEdgesInRegion = connec->GetOutput()->GetNumberOfLines();
		clean->SetInput(connec->GetOutput());
		clean->Update();    

		//For each hole, point and connectivity lists (not sorted) are created.
		int npts = clean->GetOutput()->GetNumberOfPoints();
		clean->GetOutput()->BuildLinks(); //in order to sort
 		clean->GetOutput()->BuildCells();
		clean->GetOutput()->Update();

		double center[3]={0,0,0},
		       point[3],
		       point1[3],
		       point2[3];

		points = vtkPoints::New();
		ptIdsOrdered->Reset();
		cellIdsOrdered->Reset();

		int idUsed = 0;
		for(int id=0; id< numEdgesInRegion; id++)
		{
			cellIdsOrdered->InsertNextId(idUsed);
			clean->GetOutput()->GetCellPoints(idUsed,ptIds);
			ptIdsOrdered->InsertNextId(ptIds->GetId(0));
			clean->GetOutput()->GetPointCells(ptIds->GetId(1),cellIds);
			idUsed = (cellIds->GetId(0) == idUsed ? cellIds->GetId(1) : idUsed=cellIds->GetId(0));
			clean->GetOutput()->GetPoint(ptIds->GetId(0),point);
			points->InsertNextPoint(point);
		}
		for(int id=0; id<npts; id++)
		{
			clean->GetOutput()->GetPoint(id,point);
			for(int i=0; i<3; i++) center[i]+=point[i];
		}
		for(int i=0; i<3; i++) center[i]/=npts;

		double normal[3]={0,0,0};
		double area=0,
		       theta,
		       R,
		       a,
		       l,
		       b,
		       mn;
		if(this->closeHoleAlgorithm == SMOOTH_TO_CENTER || this->closeHoleAlgorithm == SMOOTH_PATCH)
		{
			points->GetPoint(npts-1,point1);
			for(int i=0; i<3; i++) point1[i]-=center[i];
			for(int id=0; id< npts; id++)
			{
				points->GetPoint(id,point);
				for(int i=0; i<3; i++)
				{
					point[i]-=center[i];
					point2[i]=point[i]-point1[i];
				}
				for(int i=0; i<3; i++) normal[i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
				for(int i=0; i<3; i++) point1[i]=point2[i];
			}
			for(int i=0; i<3; i++) area+=pow(normal[i],2);
			area=sqrt(area);
			for(int i=0; i<3; i++) normal[i]/=area;
		}
		int numberOfCoronas=( this->closeHoleAlgorithm==SINGLE_CENTER_POINT ? 1 : npts/6+1 );
		if(this->closeHoleAlgorithm == SMOOTH_PATCH) numberOfCoronas*=5;
		nptsInCorona.clear();
		nptsInCorona.resize(numberOfCoronas);
		nptsUntilCorona.clear();
		nptsUntilCorona.resize(numberOfCoronas+1);
		nptsUntilCorona[0]=0;
		nptsInCorona[0]=npts;
		onlyOneCorrespondingPoint.clear();
		onlyOneCorrespondingPoint.resize(numberOfCoronas);
		onlyOneCorrespondingPoint[0].resize(npts);
		if(this->closeHoleAlgorithm == SMOOTH_PATCH)
		{
			normalsAtPoint.resize(npts);
			for(int id=0; id<npts; id++)
			{
				normalsAtPoint[id].clear();
				normalsAtPoint[id].resize(3);
				points->GetPoint(id,point);
				pointId=locator->FindClosestPoint(point);
				normals->GetOutput()->GetPointCells(pointId,cellIds);
//				for(int i=0; i<3; i++) normalsAtPoint[id][i]=0;
				a=cellIds->GetNumberOfIds();
				for(int j=0; j<a; j++)
				{
					normals->GetOutput()->GetCellPoints(cellIds->GetId(j),ptIds);
					int sameId;
					if(ptIds->GetId(0)==pointId) sameId=0;
					else if(ptIds->GetId(1)==pointId) sameId=1;
					else sameId=2;
					this->input->GetPoint(ptIds->GetId((sameId+1)%3),point1);
					this->input->GetPoint(ptIds->GetId((sameId+2)%3),point2);
					for(int i=0; i<3; i++)
					{
						point1[i]-=point[i];
						point2[i]-=point[i];
					}
					for(int i=0; i<3; i++)
					{
						normalsAtPoint[id][i]+=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
					}
					area=0;
					for(int i=0; i<3; i++) area+=pow(normalsAtPoint[id][i],2);
					area=sqrt(area);
					for(int i=0; i<3; i++) normalsAtPoint[id][i]/=area;
				}
			}
		}
		
		double typicalLengthSq=0,
		       lengthSq;
		clean->GetOutput()->GetPoint(0,point);
		for(int i=0; i<3; i++) typicalLengthSq+=pow(point[i]-center[i],2);
		typicalLengthSq/=numberOfCoronas*numberOfCoronas;
		bool signChange = true;

		for(int c=1; c<numberOfCoronas; c++)
		{
			nptsUntilCorona[c]=nptsUntilCorona[c-1]+nptsInCorona[c-1];
			nUsedIds=0;
			if(this->closeHoleAlgorithm == SMOOTH_PATCH)
			{
/*				double displacement=1e10;
				for(int id=0; id<nptsInCorona[c-1]; id++)
				{
					points->GetPoint(nptsUntilCorona[c-1]+id,point);
					a=0;
					b=0;
					mn=0;
					for(int i=0; i<3; i++)
					{
						point[i]-=center[i];
						a+=point[i]*normalsAtPoint[id][i];
						b+=point[i]*normal[i];
						mn+=normalsAtPoint[id][i]*normal[i];
					}
					l=(mn*a-b)/(1-pow(mn,2));
					if(firstDisplacement)
					{
						if(displacement>l) displacement=l;
						firstDisplacement = false;
					}
					else
					{
						if(displacement>l) displacement=l;
					} 
				}
				for(int i=0; i<3; i++)
				{
					center[i]+=displacement*normal[i];
				}*/
				for(int i=0; i<3; i++)
				{
					center[i]=0;
					for(int id=0; id<nptsInCorona[c-1]; id++) center[i]+=points->GetPoint(nptsUntilCorona[c-1]+id)[i];
					center[i]/=nptsInCorona[c-1];
				}
			}
			for(int id=0; id<nptsInCorona[c-1]; id++)
			{
				points->GetPoint(id+nptsUntilCorona[c-1],point1);
				points->GetPoint((id+1)%nptsInCorona[c-1]+nptsUntilCorona[c-1],point2);
				lengthSq=0;
				for(int i=0; i<3; i++) lengthSq+=pow(point1[i]-point2[i],2);
				onlyOneCorrespondingPoint[c-1][id]=
					((this->closeHoleAlgorithm==LINEAR_TO_CENTER || this->closeHoleAlgorithm==SMOOTH_TO_CENTER ) 
					&& lengthSq<typicalLengthSq && (id==0 || !onlyOneCorrespondingPoint[c-1][id-1]) 
					&& nptsInCorona[c-1]>3	// Added to correct bad behaviour in extrange contours.
					);
				if(!onlyOneCorrespondingPoint[c-1][id])
				{
					switch(this->closeHoleAlgorithm)
					{
						case RADIAL_TRIANGLES :
							for(int i=0; i<3; i++)
							{
								point[i]=(point1[i]*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1);
							}
						break;
						case LINEAR_TO_CENTER :
						case LINEAR_TO_CENTER_NONREDUCING_TRIANGLES :
							for(int i=0; i<3; i++)
							{
								point[i]=((point1[i]+point2[i])/2*(numberOfCoronas-c)+center[i])/(numberOfCoronas-c+1);
							}
						break;
						case SMOOTH_TO_CENTER :
							a=0;
							l=0;
							for(int i=0; i<3; i++)
							{
								point[i]=(point1[i]+point2[i])/2-center[i];
								a+=point[i]*normal[i];
								l+=pow(point[i],2);
							}
							R=l/a/2;
							theta=asin(a/sqrt(l));
							theta=2*theta*(1.-1./(numberOfCoronas-c+1));
							if(fabs(theta)<0.001)
							{
								l=pow(a,2)/l;
								double fractionAngle=1.-1./(numberOfCoronas-c+1);
								for(int i=0; i<3; i++)
								{
									point[i]=pow(fractionAngle,2)*a*normal[i]+center[i]
										+fractionAngle/sqrt(1-l)*(point[i]-a*normal[i]);
								}
							}
							else
							{
								for(int i=0; i<3; i++)
								{
									point[i]=-R*cos(theta)*normal[i]
										+R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normal[i])
										+center[i]+R*normal[i];
								}
							}
						break;
						case SMOOTH_PATCH :
							points->GetPoint(nptsUntilCorona[c-1]+id,point);
							a=0;
							b=0;
							mn=0;
							for(int i=0; i<3; i++)
							{
								point[i]-=center[i];
								a+=point[i]*normalsAtPoint[id][i];
								b+=point[i]*normal[i];
								mn+=normalsAtPoint[id][i]*normal[i];
							}
							l=(mn*a-b)/(1-pow(mn,2));
							signChange = (l>0);

							a=0;
							l=0;
							b=0;
							mn=0;
							for(int i=0; i<3; i++)
							{
								point[i]=-(point1[i]+point2[i])/2+center[i];
								a+=point[i]*normalsAtPoint[id][i];
								l+=pow(point[i],2);
							}
							R=l/a/2;
							theta=asin(a/sqrt(l));
							double fractionAngle=1./(numberOfCoronas-c+1);
							if(signChange) fractionAngle*=-1;
							theta=2*theta*fractionAngle;
							if(fabs(theta)<0.001)
							{
								l=pow(a,2)/l;
								for(int i=0; i<3; i++)
								{
									point[i]=pow(fractionAngle,2)*a*normalsAtPoint[id][i]
										+center[i]-point[i]
										+fractionAngle/sqrt(1-l)*(point[i]-a*normalsAtPoint[id][i]);
								}
							}
							else
							{
								for(int i=0; i<3; i++)
								{
									point[i]=-R*cos(theta)*normalsAtPoint[id][i]
										+R*sin(theta)/sqrt(l-pow(a,2))*(point[i]-a*normalsAtPoint[id][i])
										+center[i]-point[i]+R*normalsAtPoint[id][i];
								}
							}
							for(int i=0; i<3; i++)
							{
								point1[i]-=point[i];
								point2[i]-=point[i];
							}
							for(int i=0; i<3; i++)
							{
								normalsAtPoint[nUsedIds][i]=point1[(i+1)%3]*point2[(i+2)%3]-point1[(i+2)%3]*point2[(i+1)%3];
							}
							area=0;
							for(int i=0; i<3; i++) area+=pow(normalsAtPoint[nUsedIds][i],2);
							area=sqrt(area);
							for(int i=0; i<3; i++) normalsAtPoint[nUsedIds][i]/=area;
						break;
					}
					nUsedIds++;
					points->InsertNextPoint(point);
				}
			}
			nptsInCorona[c]=nUsedIds;
			onlyOneCorrespondingPoint[c].resize(nUsedIds);
		}
		nptsUntilCorona[numberOfCoronas]=nptsUntilCorona[numberOfCoronas-1]+nptsInCorona[numberOfCoronas-1];
		points->InsertNextPoint(center);

		polys = vtkCellArray::New(); //A polygon is created

		for(int c=0; c<numberOfCoronas-1; c++)
		{
			int id2=0;
			for(int id=0; id<nptsInCorona[c] ; id++)
			{
				if(onlyOneCorrespondingPoint[c][id])
				{
					ptIds1->Reset();
					ptIds1->InsertNextId(id+nptsUntilCorona[c]);
					ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
					ptIds1->InsertNextId(id2%nptsInCorona[c+1]+nptsUntilCorona[c+1]);
					polys->InsertNextCell(ptIds1);
				}
				else
				{
					ptIds1->Reset();
					ptIds1->InsertNextId(id+nptsUntilCorona[c]);
					ptIds1->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
					ptIds1->InsertNextId(id2+nptsUntilCorona[c+1]);
					ptIds2->Reset();
					ptIds2->InsertNextId((id+1)%nptsInCorona[c]+nptsUntilCorona[c]);
					ptIds2->InsertNextId(id2+nptsUntilCorona[c+1]);
					ptIds2->InsertNextId((id2+1)%nptsInCorona[c+1]+nptsUntilCorona[c+1]);
					polys->InsertNextCell(ptIds1);
					polys->InsertNextCell(ptIds2);
					id2++;
				}
			}
		}
		for(int id=0; id< nptsInCorona[numberOfCoronas-1]; id++)
		{
			ptIds1->Reset();
			ptIds1->InsertNextId(id+nptsUntilCorona[numberOfCoronas-1]);
			ptIds1->InsertNextId((id+1)%nptsInCorona[numberOfCoronas-1]+nptsUntilCorona[numberOfCoronas-1]);
			ptIds1->InsertNextId(nptsUntilCorona[numberOfCoronas]);
			polys->InsertNextCell(ptIds1);
		}
		patch = vtkPolyData::New();
		patch->SetPoints(points);
		patch->SetPolys(polys);

		//Each sorted hole is stored
		poly[idRegion] = patch;
		patchOutput.at(idRegion) = vtkPolyData::New();
		patchOutput.at(idRegion)->DeepCopy(patch);
		connec->DeleteSpecifiedRegion(idRegion);
	}

	locator->Delete();
	ptIds->Delete();
	ptIds1->Delete();
	ptIds2->Delete();
	ptIdsOrdered->Delete();
	cellIds->Delete();
	cellIdsOrdered->Delete();
	connec->Delete();

	vtkAppendPolyData *append2 = vtkAppendPolyData::New();

	append2->AddInput(this->input);
	if(numRegions != 0)
	{
		vtkAppendPolyData *append = vtkAppendPolyData::New();
		for(int idRegion = 0; idRegion < numRegions;	idRegion++ )
		{
			append->AddInput(poly[idRegion]);
			append->Update();
			poly[idRegion]->Delete();
		}
		append2->AddInput(append->GetOutput());
		append->Delete();
	}
	append2->Update();

	vtkCleanPolyData * clean2 = vtkCleanPolyData::New();
	clean2->SetInput(append2->GetOutput());
	clean2->PointMergingOn();
	clean2->SetTolerance(0.0);
	clean2->PieceInvariantOff();
	clean2->Update();
	append2->Delete();

	normals->SetInput(clean2->GetOutput());
	normals->SplittingOff();
	normals->SetFeatureAngle(360.0);
	normals->ComputePointNormalsOff();
	normals->ComputeCellNormalsOn();
	normals->FlipNormalsOn();
	normals->AutoOrientNormalsOn();
	normals->ConsistencyOn();
	normals->Update();

	this->output->DeepCopy(normals->GetOutput());

	normals->Delete();
	clean->Delete();
	clean2->Delete();
}