bool QCamFindShift_barycentre::findBarycentre(Vector2D & center) {
const unsigned char * img=cam().yuvFrame().Y();
double imgSum=0, barySum=0;
int max=0;
center.set(0,0);
int seuil=(maximum_+(int)average_)>>1;
for(int j=cam().size().height()-1;
j>=0;--j) {
for (int i=cam().size().width()-1;
i>=0;--i) {
unsigned char val=img[i+j*cam().size().width()];
imgSum += val;
if (val>max) max=val;
if (val>=seuil) {
barySum+=val;
center+=Vector2D(i,j)*val;
}
}
}
average_=imgSum/(cam().size().height()*cam().size().width());
maximum_=max;
if (barySum>0) {
center/=barySum;
return true;
} else {
return false;
}
}
void
GoalieLine::call()
{
Vector2D puntoReferencia; //Coordenadas de la bandera del centro de la porteria
double yTarget;
Vector2D target;
puntoReferencia.x =-52.5; // coordenadas de la bandera del centro de la porteria
puntoReferencia.y = 0.0;
//
if( (m_world->time() - m_world->bitacoraBalon.begin()->ciclo) < 3 ) // si esta reciente los datos del balon
{
if( m_world->bitacoraBalon.begin()->pos.x != puntoReferencia.x)
yTarget = ( (m_world->bitacoraBalon.begin()->pos.y - puntoReferencia.y) /
(m_world->bitacoraBalon.begin()->pos.x - puntoReferencia.x)
) * (m_x_target - puntoReferencia.x);
else
if(m_world->bitacoraBalon.begin()->pos.y <0.0)
yTarget = -7.0;
else
yTarget = 7.0;
if(yTarget >= 7.0 )
yTarget = 7.0;
if(yTarget <= -7.0)
yTarget = -7.0;
if( (m_world->me().pos.y > (yTarget - 0.5)) && (m_world->me().pos.y < (yTarget + 0.5)) )
m_center_ball->call();
else
{
target.set( m_x_target, yTarget );
m_go_to_point2->setRadius( 0.5 );
m_go_to_point2->setTarget( target );
m_go_to_point2->call();
}
}
else // No tengo datos recientes del balón, entonces lo busco
{
m_search_ball->call();
}
}
