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

/*---------------------------------------------------------------
						Navigate
  ---------------------------------------------------------------*/
void  CHolonomicND::navigate(
	const mrpt::math::TPoint2D &target,
	const std::vector<double>	&obstacles,
	double			maxRobotSpeed,
	double			&desiredDirection,
	double			&desiredSpeed,
	CHolonomicLogFileRecordPtr &logRecord,
	const double    max_obstacle_dist)
{
	TGapArray			gaps;
	TSituations			situation;
	unsigned int		selectedSector;
	double				riskEvaluation;
	CLogFileRecord_NDPtr log;
	double				evaluation;

	// Create a log record for returning data.
	if (!logRecord.present())
	{
		log = CLogFileRecord_ND::Create();
		logRecord = log;
	}


	// Search gaps:
	gaps.clear();
	gapsEstimator( obstacles, target, gaps);


	// Select best gap:
	searchBestGap(	obstacles,
					1.0,
					gaps,
					target,
					selectedSector,
					evaluation,
					situation,
					riskEvaluation,
					log);

	if (situation == SITUATION_NO_WAY_FOUND)
	{
		// No way found!
		desiredDirection = 0;
		desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));

		// Speed control: Reduction factors
		// ---------------------------------------------
		const double targetNearnessFactor = std::min( 1.0, target.norm()/(options.TARGET_SLOW_APPROACHING_DISTANCE));
		const double riskFactor = std::min(1.0, riskEvaluation / options.RISK_EVALUATION_DISTANCE );
		desiredSpeed = maxRobotSpeed * std::min(riskFactor,targetNearnessFactor);
	}

	m_last_selected_sector = selectedSector;

	// LOG --------------------------
	if (log)
	{
		// gaps:
		{
			int	i,n = gaps.size();
			log->gaps_ini.resize(n);
			log->gaps_end.resize(n);
			for (i=0;i<n;i++)
			{
				log->gaps_ini[i]  = gaps[i].ini;
				log->gaps_end[i]  = gaps[i].end;
			}
		}
		// Selection:
		log->selectedSector = selectedSector;
		log->evaluation = evaluation;
		log->situation = situation;
		log->riskEvaluation = riskEvaluation;
	}
}

//.........这里部分代码省略.........
						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] );
	}

}

//.........这里部分代码省略.........
	//  borrar el estrecho:
    for (i=0;i<gaps_temp.size();i++)
    {
        int     ini_i = gaps_temp[i].ini;
        int     fin_i = gaps_temp[i].end;
		int		ancho_i = fin_i - ini_i;

		if ( !borrar_gap[i] )
		{
			for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
			{
				if (i!=j)
				{
					int     ini_j = gaps_temp[j].ini;
					int     fin_j = gaps_temp[j].end;
					int		ancho_j = fin_j - ini_j;

					// j dentro de i y UN POCO mas grande nada mas:
					if (	!borrar_gap[j] &&
							ini_j>=ini_i &&
							fin_j<=fin_i &&
							ancho_i < (0.05f*n) &&
							ancho_j < (0.25f*n)
						)
						borrar_gap[i] = true;
				}
			}
		}
	}

	// Si dentro tiene mas de 1, borrarlo:
   for (i=0;i<gaps_temp.size();i++)
    {
        int     ini_i = gaps_temp[i].ini;
        int     fin_i = gaps_temp[i].end;
		int		nDentro = 0;

		if ( !borrar_gap[i] )
		{
			for (unsigned int j=0;j<gaps_temp.size();j++)
			{
				if (i!=j)
				{
					int     ini_j = gaps_temp[j].ini;
					int     fin_j = gaps_temp[j].end;

					// j dentro de i:
					if (    !borrar_gap[j] &&
							ini_j>=ini_i &&
							fin_j<=fin_i ) nDentro++;
				}
			}
			if (nDentro>1) borrar_gap[i] = true;
		}
	}


	// Uno dentro de otro y practicamente a la misma altura: Eliminarlo tambien:
   for (i=0;i<gaps_temp.size();i++)
    {
		if (!borrar_gap[i])
		{
            double	ent_i = gaps_temp[i].entranceDistance;
            int     ini_i = gaps_temp[i].ini;
            int     fin_i = gaps_temp[i].end;

            double MIN_GAPS_ENTR_DIST = (MaximoAbsoluto-MinimoAbsoluto)/10.0f;

            for (unsigned int j=0;j<gaps_temp.size() && !borrar_gap[i];j++)
				if (i!=j)
				{
                    double	ent_j = gaps_temp[j].entranceDistance;
                    int		ini_j = gaps_temp[j].ini;
                    int		fin_j = gaps_temp[j].end;

                    // j dentro de i y casi misma "altura":
                    if (    !borrar_gap[j] &&
							!borrar_gap[i] &&
                            ini_j>=ini_i &&
                            fin_j<=fin_i &&
                            fabs(ent_i-ent_j)< MIN_GAPS_ENTR_DIST )
                                    borrar_gap[i]=true;
				}
		}
    }

    // Copiar solo huecos no marcados para borrar:
    // ---------------------------------------------------
    gaps_out.clear();
	gaps_out.reserve(15);
    for (i=0;i<gaps_temp.size();i++)
            if ( !borrar_gap[i] )
			{
				// Calcular direccion representativa:
				calcRepresentativeSectorForGap( gaps_temp[i], target, obstacles);

				gaps_out.push_back( gaps_temp[i] );
			}

}

/*---------------------------------------------------------------
						Navigate
  ---------------------------------------------------------------*/
void  CHolonomicND::navigate(
				poses::CPoint2D	&target,
				vector_double	&obstacles,
				double			maxRobotSpeed,
				double			&desiredDirection,
				double			&desiredSpeed,
				CHolonomicLogFileRecordPtr &logRecord)
{
	TGapArray			gaps;
	TSituations			situation;
	int					selectedSector;
	double				riskEvaluation;
	CLogFileRecord_NDPtr log;
	double				evaluation;

	// Create a log record for returning data.
	if (!logRecord.present())
	{
		log = CLogFileRecord_ND::Create();
		logRecord = log;
	}


	// Search gaps:
    gaps.clear();
	gapsEstimator(	obstacles,
					target,
					gaps );


	// Select best gap:
	searchBestGap(	obstacles,
					1.0f,
					gaps,
					target,
					selectedSector,
					evaluation,
					situation,
					riskEvaluation,
					log);

	if (situation == SITUATION_NO_WAY_FOUND)
	{
		// No way found!
		desiredDirection = 0;
		desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		desiredDirection = (double)(M_PI*(-1 + 2*(0.5f+selectedSector)/((double)obstacles.size())));

		// Speed control: Reduction factors
		// ---------------------------------------------
		double		targetNearnessFactor = max(0.20, min(1.0, 1.0-exp(-(target.norm()+0.01)/TARGET_SLOW_APPROACHING_DISTANCE)));
		//printf(" TARGET NEARNESS = %f\n",targetNearnessFactor);
		double		riskFactor = min(1.0, riskEvaluation / RISK_EVALUATION_DISTANCE );

		desiredSpeed = maxRobotSpeed * min(riskFactor,targetNearnessFactor);
	}

	last_selected_sector = selectedSector;

	// LOG --------------------------
	if (log)
	{
		// gaps:
		if (situation != SITUATION_TARGET_DIRECTLY )
		{
			int	i,n = gaps.size();
			log->gaps_ini.resize(n);
			log->gaps_end.resize(n);
			for (i=0;i<n;i++)
			{
				log->gaps_ini[i]  = gaps[i].ini;
				log->gaps_end[i]  = gaps[i].end;
			}
		}
        // Selection:
        log->selectedSector = selectedSector;
        log->evaluation = evaluation;
        log->situation = situation;
        log->riskEvaluation = riskEvaluation;
	}
}