void AddNewShape(const std::vector<std::string> & inputParts, std::vector<std::shared_ptr<CShape>> & figures, std::vector<std::shared_ptr<sf::Shape>> & shapes)
{
if (inputParts.size() == 4 && inputParts[0].compare(std::string("point")) == 0)
{
AddPoint(inputParts, figures, shapes);
}
else if (inputParts.size() == 6 && inputParts[0].compare(std::string("line")) == 0)
{
AddLine(inputParts, figures, shapes);
}
else if (inputParts.size() == 9 && inputParts[0].compare(std::string("triangle")) == 0)
{
AddTriangle(inputParts, figures, shapes);
}
else if (inputParts.size() == 7 && inputParts[0].compare(std::string("rectangle")) == 0)
{
AddRectangle(inputParts, figures, shapes);
}
else if (inputParts.size() == 6 && inputParts[0].compare(std::string("circle")) == 0 && std::stod(inputParts[3]) >= 0)
{
AddCircle(inputParts, figures, shapes);
}
else
{
throw std::invalid_argument("Incorrect command");
}
}
//
// only find object_no.
//
int YARPObjectContainer::Segment (int object_no, YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& xx, int& yy)
{
if (!m_active)
{
printf ("YARPObjectContainer: need to update stats first\n");
out.PeerCopy(scan);
xx = yy = 0;
return -1;
}
double x, y, quality;
m_locator[object_no].BackProject (scan, m_backp[object_no]);
double ex, ey;
m_locator[object_no].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
bool valid = false;
if (m_locator[object_no].Find (ex, ey, x, y, quality) >= 0)
{
double mean = 0, stddev = 0;
const double THR = 4.0; // it was 2.0
m_locator[object_no].GetExpectancy (mean, stddev);
valid = (fabs(quality - mean) < stddev * THR) ? true : false;
#ifdef _DEBUG
printf ("object: %d location: %lf %lf q: %lf\n", object_no, x, y, quality);
#endif
}
if (valid)
{
YarpPixelBGR red;
red.r = 255;
red.g = red.b = 0;
double betterx = x;
double bettery = y;
AddRectangle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));
//AddCircleOutline (m_backp[object_no], red, int(max_x+.5), int(max_y+.5), 10);
AddCircle (m_backp[object_no], red, int(betterx+.5), int(bettery+.5), 5);
// return processed image.
out.PeerCopy (m_backp[object_no]);
xx = int (betterx + .5);
yy = int (bettery + .5);
}
else
{
xx = yy = 0;
}
return (valid) ? 0 : -1;
}
void ai09::sharifcup_play_2nd ( void )
{
ERRTSetObstacles(cmf, 0, 0, 0, 0, 0, 0);
AddRectangle(700, -1050, 210, 210);
OwnRobot[cmf].target.Angle = 90;
ERRTNavigate2Point(cmf, Vec2(200, -1500), 0, 100, &VELOCITY_PROFILE_AROOM);
}
//AddRectangles-------------------------------------------------------------
Status GraphicsPath::AddRectangles( const RectF* rects, INT count )
{
if ( !rects )
return SetStatus( InvalidParameter );
Status status = Ok;
for ( int i = 0; i < count; i++ )
status = AddRectangle( rects[ i ] );
return status;
}
void wxGraphicsPathData::AddRoundedRectangle( wxDouble x, wxDouble y, wxDouble w, wxDouble h, wxDouble radius)
{
if ( radius == 0 )
AddRectangle(x,y,w,h);
else
{
MoveToPoint( x + w, y + h / 2);
AddArcToPoint(x + w, y + h, x + w / 2, y + h, radius);
AddArcToPoint(x, y + h, x, y + h / 2, radius);
AddArcToPoint(x, y , x + w / 2, y, radius);
AddArcToPoint(x + w, y, x + w, y + h / 2, radius);
CloseSubpath();
}
}
//
// only find object and orientation.
//
int YARPObjectContainer::FindSimple (YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out)
{
if (!m_active)
{
printf ("YARPObjectContainer: need to update stats first\n");
out.PeerCopy(scan);
m_last_known_object = -1;
m_orientation = 0;
m_displacement = 0;
return -1;
}
YarpPixelBGR red;
red.r = 255;
red.g = red.b = 0;
double x, y, quality;
double max_quality = 0;
int max_obj = -1;
double max_x = 0, max_y = 0;
const int NOBJ = m_canonical_stats->m_goodneurons;
for (int obj = 0; obj < NOBJ; obj++)
{
m_locator[obj].BackProject (scan, m_backp[obj]);
double ex, ey;
m_locator[obj].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
if (m_locator[obj].Find (ex, ey, x, y, quality) >= 0)
{
double mean = 0, stddev = 0;
const double THR = 2.0;
m_locator[obj].GetExpectancy (mean, stddev);
bool thr_cond = (fabs(quality - mean) < stddev * THR) ? true : false;
#ifdef _DEBUG
printf ("object: %d location: %lf %lf q: %lf\n", obj, x, y, quality);
#endif
if (quality > max_quality && thr_cond)
{
max_quality = quality;
max_obj = obj;
max_x = x;
max_y = y;
}
}
}
m_last_known_object = max_obj;
if (max_obj == -1)
{
printf ("I (COG) don't know about this object, if there's one at all!\n");
out.PeerCopy(scan);
m_orientation = 0;
m_displacement = 0;
return -1;
}
// if max_quality is not good enough then skip the following search.
// if pointiness is not good enough then skip the following search.
#ifdef _DEBUG
printf ("object is %d, improving position\n", max_obj);
#endif
double ex, ey;
m_locator[max_obj].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
// try to improve the position estimation.
double betterx = 0, bettery = 0;
m_locator[max_obj].ImprovedSearch (scan, ex, ey, max_x, max_y, betterx, bettery);
#ifdef _DEBUG
printf ("object is %d, looking for orientation\n", max_obj);
#endif
double angle = -1;
double angle2 = -1;
double ave = 0;
double std = 0;
m_locator[max_obj].GetNumberOfPoints (ave, std);
// need to add a threshold on pointiness!
YARPSearchRotation sr (50, 0.0);
sr.Search (int(m_canonical_stats->m_neuron_numbers(max_obj+1)), *m_canonical, scan, betterx, bettery, ave, std, angle, angle2);
// store for future use.
m_orientation = angle;
m_displacement = 0;
#ifdef _DEBUG
printf ("orientation: %lf\n", angle * radToDeg);
#endif
int x1 = betterx + cos(angle) * 40;
int y1 = bettery + sin(angle) * 40;
int x2 = betterx - cos(angle) * 40;
int y2 = bettery - sin(angle) * 40;
AddSegment (m_backp[max_obj], red, x1, y1, x2, y2);
AddRectangle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));
AddCircleOutline (m_backp[max_obj], red, int(max_x+.5), int(max_y+.5), 10);
AddCircle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), 5);
// return processed image.
out.PeerCopy (m_backp[max_obj]);
return 0;
}
int YARPObjectContainer::Find (YARPImageOf<YarpPixelBGR>& scan, YARPImageOf<YarpPixelBGR>& out, int& bestaction)
{
if (!m_active)
{
printf ("YARPObjectContainer: need to update stats first\n");
bestaction = -1;
out.PeerCopy(scan);
m_last_known_object = -1;
m_orientation = 0;
m_displacement = 0;
return -1;
}
YarpPixelBGR red;
red.r = 255;
red.g = red.b = 0;
double x, y, quality;
double max_quality = 0;
int max_obj = -1;
double max_x = 0, max_y = 0;
const int NOBJ = m_canonical_stats->m_goodneurons;
for (int obj = 0; obj < NOBJ; obj++)
{
m_locator[obj].BackProject (scan, m_backp[obj]);
double ex, ey;
m_locator[obj].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
if (m_locator[obj].Find (ex, ey, x, y, quality) >= 0)
{
double mean = 0, stddev = 0;
const double THR = 2.0;
m_locator[obj].GetExpectancy (mean, stddev);
bool thr_cond = (fabs(quality - mean) < stddev * THR) ? true : false;
printf ("object: %d location: %lf %lf q: %lf\n", obj, x, y, quality);
if (quality > max_quality && thr_cond)
{
max_quality = quality;
max_obj = obj;
max_x = x;
max_y = y;
}
}
}
m_last_known_object = max_obj;
if (max_obj == -1)
{
printf ("I (COG) don't know about this object, if there's one at all!\n");
bestaction = -1;
out.PeerCopy(scan);
m_orientation = 0;
m_displacement = 0;
return -1;
}
// if max_quality is not good enough then skip the following search.
// if pointiness is not good enough then skip the following search.
printf ("object is %d, improving position\n", max_obj);
double ex, ey;
m_locator[max_obj].GetExtent (ex, ey);
ex *= SCALE;
ey *= SCALE;
// try to improve the position estimation.
double betterx = 0, bettery = 0;
m_locator[max_obj].ImprovedSearch (scan, ex, ey, max_x, max_y, betterx, bettery);
printf ("object is %d, looking for orientation\n", max_obj);
double angle = -1;
double angle2 = -1;
double ave = 0;
double std = 0;
m_locator[max_obj].GetNumberOfPoints (ave, std);
// need to add a threshold on pointiness!
YARPSearchRotation sr (50, 0.0);
sr.Search (int(m_canonical_stats->m_neuron_numbers(max_obj+1)), *m_canonical, scan, betterx, bettery, ave, std, angle, angle2);
// store for future use.
m_orientation = angle;
m_displacement = 0;
printf ("orientation: %lf\n", angle * radToDeg);
int x1 = betterx + cos(angle) * 40;
int y1 = bettery + sin(angle) * 40;
int x2 = betterx - cos(angle) * 40;
int y2 = bettery - sin(angle) * 40;
AddSegment (m_backp[max_obj], red, x1, y1, x2, y2);
AddRectangle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), int (ex/2+.5), int (ey/2+.5));
AddCircleOutline (m_backp[max_obj], red, int(max_x+.5), int(max_y+.5), 10);
AddCircle (m_backp[max_obj], red, int(betterx+.5), int(bettery+.5), 5);
// return processed image.
out.PeerCopy (m_backp[max_obj]);
// angle is the current orientation.
// search for the best affordance.
const double affordance = m_canonical_stats->GetPrincipalAffordance (max_obj);
double o1 = angle+affordance;
double o2 = angle-affordance;
printf ("---- object: %d, affordance: %lf, test1: %lf, test2: %lf\n", max_obj, affordance*radToDeg, o1*radToDeg, o2*radToDeg);
if (o1 > pi/2) o1 -= pi;
else
if (o1 < -pi/2) o1 += pi;
if (o2 > pi/2) o2 -= pi;
else
if (o2 < -pi/2) o2 += pi;
double score1 = 0;
double score2 = 0;
int bestaction1 = m_canonical_stats->GetBestActionGeneric (o1, score1);
int bestaction2 = m_canonical_stats->GetBestActionGeneric (o2, score2);
printf ("---- score(s) %lf %lf\n", score1, score2);
if (score1 < score2)
bestaction = bestaction1;
else
bestaction = bestaction2;
printf ("the best action for this object in this position is %d\n", bestaction);
return 0;
}
void UIModel::SetToNumber(int n)
{
m_vertices.clear();
m_indices.clear();
XMFLOAT3 standardNormal = { 0.0f, 1.0f, 0.0f };
XMFLOAT3 pos[4];
pos[0] = { -m_width / 2 ,0.0f, +m_height / 2, };
pos[1] = { +m_width / 2 ,0.0f, +m_height / 2, };
pos[2] = { +m_width / 2 ,0.0f, -m_height / 2, };
pos[3] = { -m_width / 2 ,0.0f, -m_height / 2, };
XMFLOAT3 normal = { 0.0f, 1.0f, 0.0f };
XMFLOAT4 rgba;
switch (n)
{
case 0:
//red
rgba = { 1.0f, 0.0f, 0.0f, 1.0f };
break;
case 1:
//orange
rgba = { 1.0f, 165.0f/255.0f, 0.0f, 1.0f };
break;
case 2:
//yellow
rgba = { 1.0f, 1.0f, 0.0f, 1.0f };
break;
case 3:
//green
rgba = { 0.0f, 1.0f, 0.0f, 1.0f };
break;
case 4:
//skyblue
rgba = { 0.0f, 1.0f, 1.0f, 1.0f };
break;
case 5:
//blue
rgba = { 0.0f, 0.0f, 1.0f, 1.0f };
break;
case 6:
//dark blue
rgba = { 0.0f, 0.0f, 0.6f, 1.0f };
break;
case 7:
//purple
rgba = { 0.3f, 0.1f, 0.6f, 1.0f };
break;
case 8:
//black
rgba = { 0.0f, 0.0f, 0.0f, 1.0f };
break;
case 9:
//white
rgba = { 1.0f, 1.0f, 1.0f, 1.0f };
break;
default:
break;
}
MyVertex v1 = { pos[0], rgba, normal,{ 0.0f, 0.0f } };
MyVertex v2 = { pos[1], rgba, normal,{ 1.0f, 0.0f } };
MyVertex v3 = { pos[2], rgba, normal,{ 1.0f, 1.0f } };
MyVertex v4 = { pos[3], rgba, normal,{ 0.0f, 1.0f } };
AddRectangle(v1, v2, v3, v4);
}
BOOL CRectangleBestPacker::AddRectangle(CSubImage* pcSubImage)
{
return AddRectangle(&pcSubImage->mcImageRect, pcSubImage);
}
BOOL CRectangleBestPacker::AddRectangle(CImageCel* pcImageCel)
{
return AddRectangle(&pcImageCel->GetSubImage()->mcImageRect, pcImageCel);
}