C++ TGapArray::push_back方法代码示例

//.........这里部分代码省略.........
						gaps_temp.resize( gaps_temp.size() + 1 );
						TGap	& newGap = *gaps_temp.rbegin();

						newGap.ini				= sec_ini;
						newGap.end				= sec_end;
						newGap.minDistance		= min( obstacles[sec_ini], obstacles[sec_end] );
						newGap.maxDistance		= maxDist;
					}
				}

				if (is_inside)
					maxDist = std::max( maxDist, obstacles[i] );
			}
	}

	//Start to filter the gap list
	//--------------------------------------------------------------

	const size_t nTempGaps = gaps_temp.size();

	std::vector<bool> delete_gaps;
	delete_gaps.assign( nTempGaps, false);

	// First, remove redundant gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i] == 1)
			continue;

		for (size_t j=i+1;j<nTempGaps;j++)
		{
			if (gaps_temp[i].ini == gaps_temp[j].ini || gaps_temp[i].end == gaps_temp[j].end)
				delete_gaps[j] = 1;
		}
	}

	// Remove gaps with a big depth
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i] == 1)
			continue;

		if ((gaps_temp[i].maxDistance - gaps_temp[i].minDistance) > max_depth*GAPS_MAX_RELATIVE_DEPTH)
			delete_gaps[i] = 1;
	}

	//Delete gaps which contain more than one other gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i])
			continue;

		unsigned int inner_gap_count = 0;

		for (unsigned int j=0;j<nTempGaps;j++)
		{
			if (i==j || delete_gaps[j])
				continue;

			// j is inside of i?
			if (gaps_temp[j].ini >= gaps_temp[i].ini && gaps_temp[j].end <= gaps_temp[i].end )
				if (++inner_gap_count>1)
				{
					delete_gaps[i] = 1;
					break;
				}
		}
	}

	//Delete gaps included in other gaps
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i])
			continue;

		for (unsigned int j=0;j<nTempGaps;j++)
		{
			if (i==j || delete_gaps[j])
				continue;
			if (gaps_temp[i].ini <= gaps_temp[j].ini && gaps_temp[i].end >= gaps_temp[j].end)
				delete_gaps[j] = 1;
		}
	}


	// Copy as result only those gaps not marked for deletion:
	// --------------------------------------------------------
	gaps_out.clear();
	gaps_out.reserve( nTempGaps/2 );
	for (size_t i=0;i<nTempGaps;i++)
	{
		if (delete_gaps[i]) continue;

		// Compute the representative direction ("sector") for this gap:
		calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);

		gaps_out.push_back( gaps_temp[i] );
	}

}

/*---------------------------------------------------------------
						Find gaps in the obtacles.
  ---------------------------------------------------------------*/
void  CHolonomicND::gapsEstimator(
		vector_double		&obstacles,
		poses::CPoint2D		&target,
		TGapArray			&gaps_out )
{
	unsigned int	i,n;
	int				nMaximos=0;
    double			MaximoAbsoluto = -100;
	double			MinimoAbsoluto = 100;
    vector_int		MaximoIdx;
    vector_double	MaximoValor;

    // Hacer una lista con los maximos de las distancias a obs:
    // ----------------------------------------------------------
	MaximoIdx.resize(obstacles.size());
	MaximoValor.resize(obstacles.size());
	n = obstacles.size();

    for (i=1;i<(n-1);i++)
    {
		// Actualizar max. y min. absolutos:
		MaximoAbsoluto= max( MaximoAbsoluto, obstacles[i] );
		MinimoAbsoluto= min( MinimoAbsoluto, obstacles[i] );

		// Buscar maximos locales:
		if ( ( obstacles[i] >= obstacles[i+1] &&
			  obstacles[i] > obstacles[i-1] ) ||
			  ( obstacles[i] > obstacles[i+1] &&
			  obstacles[i] >= obstacles[i-1] ) )
		{
				MaximoIdx[nMaximos] = i;
				MaximoValor[nMaximos++] = obstacles[i];
		}
    }

    //  Crear GAPS:
    // --------------------------------------------------------
	TGapArray    gaps_temp;
   	gaps_temp.reserve( 150 );

	for (double factorUmbral = 0.975f;factorUmbral>=0.04f;factorUmbral-=0.05f)
	{
            double   umbral = factorUmbral* MaximoAbsoluto + (1.0f-factorUmbral)*MinimoAbsoluto;
			bool	dentro = false;
			int		sec_ini=0, sec_end;
			double	maxDist=0;

			for (i=0;i<n;i++)
			{
				if ( !dentro && (!i || obstacles[i]>=umbral) )
				{
					sec_ini = i;
					maxDist = obstacles[i];
					dentro = true;
				}
				else if (dentro && (i==(n-1) || obstacles[i]<umbral ))
				{
					sec_end = i;
					dentro = false;

					if ( (sec_end-sec_ini) > 2 )
					{
						// Add new gap:
						TGap	newGap;
						newGap.ini				= sec_ini;
						newGap.end				= sec_end;
						newGap.entranceDistance = min( obstacles[sec_ini], obstacles[sec_end] );
						newGap.maxDistance		= maxDist;

						gaps_temp.push_back(newGap);
					}
				}

				if (dentro) maxDist = max( maxDist, obstacles[i] );
			}
	}

    // Proceso de eliminacion de huecos redundantes:
    // -------------------------------------------------------------
	std::vector<bool>	borrar_gap;
	borrar_gap.resize( gaps_temp.size() );
        for (i=0;i<gaps_temp.size();i++)
			borrar_gap[i] = false;


    // Eliminar huecos con muy poca profundidad si estan dentro de otros:
	double	maxProfundidad = 0;
	for (i=0;i<gaps_temp.size();i++)
    {
		double profundidad =
				gaps_temp[i].maxDistance -
				gaps_temp[i].entranceDistance;
		maxProfundidad = max(maxProfundidad, profundidad);
	}

	for (i=0;i<gaps_temp.size();i++)
    {
//.........这里部分代码省略.........