void Game::processCommands()
{
float mass[3] = { 1.5f, 2.5f, 3.5f };
// proccess any commands
if (m_commands.size()) {
for (unsigned int i=0; i<m_commands.size(); i++)
{
Command m = m_commands[i];
if (m.type == CmdType::CMD_NEWCIRCLE) {
AddCircle(m.x, m.y, mass[massId]);
m_totalObjects++;
massId = (massId+1) % 3;
}
else if (m.type == CmdType::CMD_TOGGLESPRING) {
toggleRope();
}
else if (m.type == CmdType::CMD_MANYCIRCLES) {
for (int i=0; i<8; i++) {
AddCircle(-18.0f + (i+1)*4.5f, 18.0f, mass[massId]);
m_totalObjects++;
massId = (massId+1)%3;
}
}
}
m_commands.clear();
}
}
void TerrainDestructionScene::FireGrapeShot(int shotNumber)
{
if (m_networkManager.GetNumPeers() > 0)
{
glm::vec2 centrePos(m_cannon->GetPos().x, m_cannon->GetPos().y);
// First ball.
AddCircleNetwork(centrePos.x, centrePos.y, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x, m_cannonAim.y)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber)); // ID
// Second.
AddCircleNetwork(centrePos.x, centrePos.y + HALF_UNIT_SIZE / 2.0f, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x, m_cannonAim.y * 0.9)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+1)); // ID
// Third.
AddCircleNetwork(centrePos.x + HALF_UNIT_SIZE / 2.0f, centrePos.y + HALF_UNIT_SIZE / 2.0f, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x * 0.9, m_cannonAim.y)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+2)); // ID
// Fourth
AddCircleNetwork(centrePos.x + HALF_UNIT_SIZE / 2.0f, centrePos.y, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x * 0.9, m_cannonAim.y * 0.9)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+3)); // ID
}
else
{
glm::vec2 centrePos(m_cannon->GetPos().x, m_cannon->GetPos().y);
// First ball.
AddCircle(centrePos.x, centrePos.y, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x, m_cannonAim.y)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber)); // ID
// Second.
AddCircle(centrePos.x, centrePos.y + HALF_UNIT_SIZE / 2.0f, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x, m_cannonAim.y * 0.9)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+1)); // ID
// Third.
AddCircle(centrePos.x + HALF_UNIT_SIZE / 2.0f, centrePos.y + HALF_UNIT_SIZE / 2.0f, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x * 0.9, m_cannonAim.y)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+2)); // ID
// Fourth
AddCircle(centrePos.x + HALF_UNIT_SIZE / 2.0f, centrePos.y, // StartPos
HALF_UNIT_SIZE / 4.0f, // Radius
glm::normalize(glm::vec2(m_cannonAim.x * 0.9, m_cannonAim.y * 0.9)) * m_cannonPower, // Vel.
1.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber+3)); // ID
}
}
KaeruKing::KaeruKing(int rXPx, int rYPx)
{
KAERUKSX = GI("KAERUKSX");
KAERUKSY = GI("KAERUKSY");
KTAIKI_TIME = GF("KTAIKI_TIME");
KJUMPSTART_TIME = GF("KJUMPSTART_TIME");
KJMPSPX = GF("KJMPSPX");
KJMPTAKASA = GF("KJMPTAKASA");
mX = rXPx;
mY = rYPx - sMapAtHanteiY[0][0] + SP->CHSZY;
mSizeX = KAERUKSX;
mSizeY = KAERUKSY;
mStatus = JUMP;
mTaikiTimer = 0.0f;
mCurAshiba = NULL;
// 当たり判定
AddFrame(0);
AddCircle(0, SP->GRID_BOGYO, 44, 44, 44);
AddIndexedRect(0, SP->GRID_BOUND, TBOUND_IDX, 10, 12, 73, 85);
SetCurFrame(0);
}
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;
}
Apple::Apple(int rXPx, int rYPx)
{
APPLESZX = GI("APPLESZX");
APPLESZY = GI("APPLESZY");
APPLE_BRTM = GF("APPLE_BRTM");
mSizeX = APPLESZX;
mSizeY = APPLESZY;
mX = rXPx;
mY = rYPx;
mZ = 1.0f;
mNo = 0;
mTimer = 0.0f;
mBaseStatus = NORMAL;
// あたり判定
AddFrame(FR_NORMAL);
AddCircle(FR_NORMAL, SP->GRID_BOGYO, 20, 20, 15);
AddFrame(FR_DISABLE);
SetCurFrame(FR_DISABLE);
}
KariudoKen::KariudoKen(int rXPx, int rYPx)
{
KARIKENSX = GI("KARIKENSX");
KARIKENSY = GI("KARIKENSY");
KARIKENSPX = GF("KARIKENSPX");
KARIKENKGHANI = GF("KARIKENKGHANI");
KARIKENWTM1 = GF("KARIKENWTM1");
KARIKENWTM2 = GF("KARIKENWTM2");
mX = rXPx;
mY = rYPx - sMapAtHanteiY[0][0] + SP->CHSZY;
mSizeX = KARIKENSX;
mSizeY = KARIKENSY;
mStatus = ARUKI;
mKgTimer = 0.0f;
mSeFl = false;
// 当たり判定
AddFrame(FR_KAMAE);
AddFrame(FR_ZANZOU);
AddFrame(FR_DOWN);
AddRect(FR_KAMAE, SP->GRID_BOGYO, 43, 33, 81, 119);
AddIndexedRect( FR_KAMAE, SP->GRID_BOUND, TBOUND_IDX, 38, 22, 83, 120);
AddRect(FR_ZANZOU, SP->GRID_BOGYO, 43, 33, 81, 119);
AddCircle(FR_ZANZOU, SP->GRID_KOUGEKI, 38, 43, 35);
AddCircle(FR_ZANZOU, SP->GRID_KOUGEKI, 33, 69, 35);
AddIndexedRect( FR_ZANZOU, SP->GRID_BOUND, TBOUND_IDX, 38, 22, 83, 120);
AddRect(FR_DOWN, SP->GRID_BOGYO, 43, 33, 81, 119);
AddCircle(FR_DOWN, SP->GRID_BOGYO, 20, 62, 11);
AddIndexedRect( FR_DOWN, SP->GRID_BOUND, TBOUND_IDX, 38, 22, 83, 120);
//AddCircle(0, GRID_BOGYO, 60, 60, 30);
SetAnim(0);
}
void rubbercircle(List thelist, int color)
{
int x1, x2, y1, y2;
int radius;
int x3, y3;
Circle_Type circledata;
while (!(mouse_b & 2)) {
if (mouse_b & 1) {
x3 = x1 = mouse_x;
y3 = y1 = mouse_y;
scare_mouse();
/* draw circle while waiting for mouse release */
while (mouse_b & 1) {
x2 = mouse_x;
y2 = mouse_y;
if (x2 != x3 || y2 != y3) {
rrestore(screen);//first time through ii=0
radius = calc_radius(x1, y1, x2, y2);
/* save/draw/wait/restore */
do_circle(screen, x1, y1, radius, color, rsave);
x3 = x2;
y3 = y2;
}
}
rrestore(screen);
x2 = mouse_x;
y2 = mouse_y;
radius = calc_radius(x1, y1, x2, y2);
circlebres(screen, x1, y1, radius, color);
circledata.p1.x = x1;
circledata.p1.y = y1;
circledata.radius = radius;
circledata.color = color;
circledata.filled = FALSE;
circledata.fill_color = 0;
circledata.width = 1;
AddCircle(thelist, circledata);
unscare_mouse();
}
} //end while
}
int main(int argv, char** argc)
{
std::string fPath;
fPath += argc[1];
int length = atoi(argc[2]);
int width = atoi(argc[3]);
int noiseLevel = atoi(argc[4]);
unsigned char **image = (unsigned char **)malloc(sizeof(unsigned char *)*length);
for(int i = 0; i<length; i++)
{
image[i] = (unsigned char *)malloc(sizeof(unsigned char)*width);
for(int j = 0; j<width; j++)
image[i][j] = 0;
}
float **imageF = (float **)malloc(sizeof(float *)*length);
for(int i = 0; i<length; i++)
{
imageF[i] = (float *)malloc(sizeof(float)*width);
for(int j = 0; j<width; j++)
imageF[i][j] = 0;
}
//add circles
AddCircle(imageF, length, width, X1, Y1, R1, I1);
AddCircle(imageF, length, width, X2, Y2, R2, I2);
AddCircle(imageF, length, width, X3, Y3, R3, I3);
//add noise
AddUniformNoise(imageF, length, width, noiseLevel);
//conver float to unsigned
NormalizeImage(image, imageF, length, width);
//save and view file
saveImage(fPath, image, length, width);
viewImage(fPath, length, width);
}
void PGTaskMgr::Initialize() {
std::for_each(m_Task.begin(), m_Task.end(), safe_delete());
m_Task.clear();
// build Mercator Reference Grid
// find center of Task
double minlat = 0.0, minlon = 0.0, maxlat = 0.0, maxlon = 0.0;
for (int curwp = 0; ValidTaskPoint(curwp); ++curwp) {
if (curwp == 0) {
maxlat = minlat = WayPointList[Task[curwp].Index].Latitude;
maxlon = minlon = WayPointList[Task[curwp].Index].Longitude;
} else {
minlat = std::min(minlat, WayPointList[Task[curwp].Index].Latitude);
maxlat = std::max(maxlat, WayPointList[Task[curwp].Index].Latitude);
minlon = std::min(minlon, WayPointList[Task[curwp].Index].Longitude);
maxlon = std::max(maxlon, WayPointList[Task[curwp].Index].Longitude);
}
}
m_Grid.lat0 = deg2rad(minlat + maxlat) * 1 / 2;
m_Grid.lon0 = deg2rad(minlon + maxlon) * 1 / 2;
m_Grid.k0 = 1;
m_Grid.false_e = 0.0; // ????
m_Grid.false_n = 0.0; // ????
// build task point list
for (int curwp = 0; ValidTaskPoint(curwp); ++curwp) {
int TpType = 0;
double Radius;
GetTaskSectorParameter(curwp, &TpType, &Radius);
switch (TpType) {
case CIRCLE:
AddCircle(curwp);
break;
case SECTOR:
case DAe:
AddSector(curwp);
break;
case LINE:
AddLine(curwp);
break;
case CONE:
AddCone(curwp);
break;
case ESS_CIRCLE:
AddEssCircle(curwp);
break;
}
}
}
void TerrainDestructionScene::FireHeavyRound(int shotNumber)
{
if (m_networkManager.GetNumPeers() > 0)
{
AddCircleNetwork(m_cannon->GetPos().x, m_cannon->GetPos().y, // StartPos
HALF_UNIT_SIZE / 2.0f, // Radius
glm::vec2(m_cannonAim.x, m_cannonAim.y) * m_cannonPower, // Vel.
4.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber)); // ID
}
else
{ // No peers are connected, so no need to signal that new stuff has been added.
AddCircle(m_cannon->GetPos().x, m_cannon->GetPos().y + 10.0f, // StartPos
HALF_UNIT_SIZE / 2.0f, // Radius
glm::vec2(m_cannonAim.x, m_cannonAim.y) * m_cannonPower, // Velocity
4.0f, 0.9f, // Mass, Elast
std::string("shot") + to_string(shotNumber));
}
}
void Game1::Update(unsigned int time)
{
KeyboardState cState = Keyboard::GetState();
MouseState mState = Mouse::GetState();
world->Step(time * 0.001f, 6, 2);
if (cState.IsKeyDown(SDLK_y) && !oldState.IsKeyDown(SDLK_y))
{
AddCube(mState.X, mState.Y, mState.RelX, mState.RelY);
}
if (cState.IsKeyDown(SDLK_x) && !oldState.IsKeyDown(SDLK_x))
{
AddCircle(mState.X, mState.Y, mState.RelX * 10, mState.RelY * 10);
}
if (cState.IsKeyDown(SDLK_c) && !oldState.IsKeyDown(SDLK_c))
{
AddTriangle(mState.X, mState.Y, mState.RelX * 10, mState.RelY * 10);
}
if (cState.IsKeyDown(SDLK_v) && !oldState.IsKeyDown(SDLK_v))
{
AddRope(mState.X, mState.Y);
}
oldState = cState;
}
void YARPComplexTrackerTool::apply (YARPImageOf<YarpPixelBGR>& src, YARPImageOf<YarpPixelBGR>& dest, const YVector& jnts)
{
/// check whether it's a new target first.
_lock.Wait ();
bool _isnew = _new_target;
_lock.Post ();
/// timing stuff.
const double now = YARPTime::GetTimeAsSeconds();
///
bool act_vector = false;
/// print timing issues.
_diff_total += now - _last_round;
_diff_count ++;
if (now - _last_reset > PRINT_TIME)
{
printf("Time between frames in ms is %g\n", 1000 * _diff_total / _diff_count);
_diff_count = 0;
_diff_total = 0;
_last_reset = now;
}
_last_round = now;
/// LATER: this might be the place to auto-reset the tracker in case that any timeout expired.
///
///
if (now - _last_movement > 30)
{
setNewTarget (ISIZE/2, ISIZE/2);
}
/// a bit of copying.
_mono.CastCopy(src);
dest.PeerCopy(src);
///
/// deals with the new target.
if (_isnew)
{
_prev.PeerCopy (_mono);
_lock.Wait ();
_px = _tx = _ex;
_py = _ty = _ey;
_new_target = false;
_lock.Post ();
printf("*** Got new target %d %d\n", _px, _py);
_last_update = now;
}
_tracker.SetBlockSize (BLOCK_SIZE, BLOCK_SIZE);
_tracker.SetSearchWindowSize (SEARCH_SIZE, SEARCH_SIZE);
_sub_tracker.SetBlockSize (BLOCK_SIZE, BLOCK_SIZE);
_sub_tracker.SetSearchWindowSize (SEARCH_SIZE, SEARCH_SIZE);
_gaze.update (jnts);
if (!_isnew)
{
/// not a new target set the estimated offset.
int predx = 0, predy = 0;
_gaze.intersectRay (YARPHeadKinematics::KIN_LEFT, _prevRay, predx, predy);
///predx += ISIZE/2;
///predy += ISIZE/2;
///
YarpPixelBGR green (0, 255, 0);
AddCircleOutline (dest, green, predx, predy, 5);
AddCircleOutline (dest, green, predx, predy, 4);
_dgx = predx - _prev_gaze_x;
_dgy = predy - _prev_gaze_y;
///printf ("est vel: %lf %lf\n", _dgx, _dgy);
_tracker.SetSearchWindowOffset ((int)(_dgx+0.5), (int)(_dgy+0.5));
_sub_tracker.SetSearchWindowOffset ((int)(_dgx+0.5), (int)(_dgy+0.5));
}
else
{
_tracker.SetSearchWindowOffset (0, 0);
_sub_tracker.SetSearchWindowOffset (0, 0);
}
_tx = _px;
_ty = _py;
/// checks borders.
if (_tx < BXDX) _tx = BXDX;
if (_tx > ISIZE-1-BXDX) _tx = ISIZE-1-BXDX;
if (_ty < BXDX) _ty = BXDX;
if (_ty > ISIZE-1-BXDX) _ty = ISIZE-1-BXDX;
/// actual tracking.
bool fell = false;
double new_tx = ISIZE/2, new_ty = ISIZE/2, new_tx2 = ISIZE/2, new_ty2 = ISIZE/2;
int sub_x = ISIZE/2, sub_y = ISIZE/2, sub_x2 = ISIZE/2, sub_y2 = ISIZE/2;
_tracker.Apply (_prev, _mono, _tx, _ty);
int x = _tx, y = _ty;
x = _tracker.GetX();
y = _tracker.GetY();
/// predicted.
double new_dx = x - (_tx + _dgx);
double new_dy = y - (_ty + _dgy);
/// direction of motion.
double new_mag = sqrt (new_dx * new_dx + new_dy * new_dy);
if (new_mag < 0.001) new_mag = 0.001;
new_dx /= new_mag;
new_dy /= new_mag;
const double nscale = NSCALE;
/// search along two directions.
/// heuristic for exploring certain neighborhood of the current position.
///
///
new_tx = _tx - new_dx * nscale;
new_ty = _ty - new_dy * nscale;
new_tx2 = _tx + new_dx * nscale;
new_ty2 = _ty + new_dy * nscale;
_sub_tracker.Apply (_prev, _mono, new_tx2, new_ty2);
sub_x2 = _sub_tracker.GetX();
sub_y2 = _sub_tracker.GetY();
_sub_tracker.Apply (_prev, _mono, new_tx, new_ty);
sub_x = _sub_tracker.GetX();
sub_y = _sub_tracker.GetY();
double sub_dx = sub_x - (new_tx + _dgx);
double sub_dy = sub_y - (new_ty + _dgy);
double sub_mag = sqrt (sub_dx * sub_dx + sub_dy * sub_dy);
double sub_dx2 = sub_x2 - (new_tx2 + _dgx);
double sub_dy2 = sub_y2 - (new_ty2 + _dgy);
double sub_mag2 = sqrt (sub_dx2 * sub_dx2 + sub_dy2 * sub_dy2);
if (new_mag > MAGDEFAULT)
{
act_vector = true;
if (sub_mag > MAGDEFAULT && sub_mag2 < MAGDEFAULT)
{
printf("Should fall inwards\n");
x = (int)sub_x;
y = (int)sub_y;
fell = true;
}
if (sub_mag2 > MAGDEFAULT && sub_mag < MAGDEFAULT)
{
printf("Should fall outwards\n");
x = (int)sub_x2;
y = (int)sub_y2;
fell = true;
}
}
_tx = x;
_ty = y;
float quality = _tracker.GetQuality();
bool low_quality = false;
if (quality < QTHRESHOLD)
{
///printf("low match quality (%g)\n", quality);
if (_low_q_ct < QTHR2)
{
_low_q_ct++;
}
/// things are not going well.
if (_low_q_ct > QTHR3)
{
low_quality = true;
x = _tx = _px + _dgx;
y = _ty = _py + _dgy;
}
}
else
{
/// ok recovering?
_low_q_ct -= 3;
if (_low_q_ct < 0) _low_q_ct = 0;
}
_movement = false;
/// to check for movement (of the target).
double dist = sqrt((double)((_px-_tx)*(_px-_tx)+(_py-_ty)*(_py-_ty)));
if (fell || (dist > 2) || (sqrt(_dgx * _dgx + _dgy * _dgy) > 2.0))
{
_prev.PeerCopy(_mono);
_px = _tx; _py = _ty;
}
/// target moved, all ok?
if (dist > 5)
{
_movement = true;
_last_movement = now;
}
///
///
/// computes the ray for the kin estimation.
#if defined(__QNXEurobot__)
_gaze.computeRay (YARPEurobotHeadKin::KIN_LEFT, _prevRay, x, y); ///-ISIZE/2, y-ISIZE/2);
#else // ----- #ifdef __QNXEurobot__ -----
_gaze.computeRay (YARPBabybotHeadKin::KIN_LEFT, _prevRay, x, y); ///-ISIZE/2, y-ISIZE/2);
#endif // ----- #ifdef __QNXEurobot__ -----
_prev_gaze_x = x;
_prev_gaze_y = y;
///printf ("target: %d %d ray: %f %f %f\n", x, y, _prevRay(1), _prevRay(2), _prevRay(3));
///
///
/// just a bit of display of results.
YarpPixelBGR pix(255,0,0);
YarpPixelBGR pixg(0,255,0);
YarpPixelBGR pixb(0,0,255);
YarpPixelBGR pixr(128,64,0);
YarpPixelBGR pixk(0,0,0);
YarpPixelBGR pixw(255,255,255);
AddCircleOutline (dest, pixw, (int)x, (int)y, 5);
AddCircleOutline (dest, pixk, (int)x, (int)y, 6);
AddCircle (dest, pixk, (int)x, (int)y, 4);
AddCrossHair (dest, pixw, (int)x+1, (int)y, 6);
AddCrossHair (dest, pixw, (int)x-1, (int)y, 6);
AddCrossHair (dest, pixw, (int)x, (int)y+1, 6);
AddCrossHair (dest, pixw, (int)x, (int)y-1, 6);
AddCrossHair (dest, pixr, (int)x, (int)y, 6);
AddCircle (dest, pixw, (int)x, (int)y, 2);
if (act_vector)
{
AddCircle (dest, pix, (int)(new_tx + _dgx), (int)(new_ty + _dgy), 3);
AddCircle (dest, pix, (int)(new_tx2 + _dgx), (int)(new_ty2 + _dgy), 3);
AddCircle (dest, pixb, (int)sub_x, (int)sub_y, 2);
AddCircle (dest, pixb, (int)sub_x2, (int)sub_y2, 2);
}
_lock.Wait();
_xx = x - ISIZE / 2;
_yy = y - ISIZE / 2;
_lock.Post();
}
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;
}
int main_alt()
{
in_head.Register("/egomap/i:head");
in_img.Register("/egomap/i:img");
out_img.Register("/egomap/o:img");
out_cmd.Register("/egomap/o:cmd");
in_voice.Register("/egomap/i:cmd");
while (1)
{
JointPos joints;
in_img.Read();
state_mutex.Wait();
joints = state_joint;
CogGaze gaze;
gaze.Apply(joints);
double roll = gaze.roll_right;
double theta = gaze.theta_right;
double phi = gaze.phi_right;
//printf("DIR %g %g %g\n", theta, phi, roll);
global_theta = theta;
global_phi = phi;
global_roll = roll;
state_mutex.Post();
double z_x = gaze.z_right[0];
double z_y = gaze.z_right[1];
YARPImageOf<YarpPixelBGR> img;
img.Refer(in_img.Content());
int width = img.GetWidth();
int height = img.GetHeight();
float s = 50;
for (int i=0; i<width; i++)
{
YarpPixelBGR pix0(0,255,0);
img(i,width/2) = pix0;
}
for (int i=0; i<width; i++)
{
float s2 = (i-width/2.0);
float x = cos(roll)*s2;
float y = -sin(roll)*s2;
YarpPixelBGR pix(255,0,0);
img.SafePixel((int)(0.5+x+(width+1)/2.0),(int)(0.5+y+(width+1)/2.0)) = pix;
}
int step = 500;
for (int i=0; i<step; i++)
{
float theta = i*M_PI*2.0/step;
YarpPixelBGR pix(255,0,0);
float x = cos(theta)*s;
float y = sin(theta)*s;
//printf("%g %g %g\n", theta, x, y);
img.SafePixel(x+width/2,y+width/2) = pix;
}
for (int i=0; i<MAX_TARGETS; i++)
{
if (target_manager.Exists(i))
{
TargetLocation& loc = target_manager.Get(i);
float target_theta = loc.theta;
float target_phi = loc.phi;
float z_y = loc.phi/(M_PI/2);
float z_x = loc.theta/(M_PI/2);
//printf("Drawing circle for %g %g\n", loc.theta, loc.phi);
float x = z_x*s;
float y = z_y*s;
// YarpPixelBGR pix0(0,128,255);
// AddCircle(img,pix0,(int)x+width/2,(int)y+height/2,4);
// We now try to map back
// onto approximate retinotopic coordinates.
// current x, y, z available in gaze::x_right,y_right,z_right
double x_vis, y_vis;
int visible;
visible = gaze.Intersect(target_theta,target_phi,x_vis,y_vis,
CAMERA_SOURCE_RIGHT_WIDE);
/*
float zt[3];
zt[0] = sin(target_theta);
zt[1] = -cos(target_phi)*cos(target_theta);
zt[2] = sin(target_phi)*cos(target_theta);
float delta_theta = zt[0]*gaze.x_right[0] +
zt[1]*gaze.x_right[1] + zt[2]*gaze.x_right[2];
float delta_phi = zt[0]*gaze.y_right[0] +
zt[1]*gaze.y_right[1] + zt[2]*gaze.y_right[2];
float sanity = zt[0]*gaze.z_right[0] +
zt[1]*gaze.z_right[1] + zt[2]*gaze.z_right[2];
//float delta_theta = zt[0]; //target_theta - global_theta;
//float delta_phi = zt[1]; //target_phi - global_phi;
float factor_theta = 67; // just guessed these numbers
float factor_phi = 67; // so far, not linear in reality
float nx = delta_theta;
float ny = delta_phi;
float r = global_roll;
float sinr = sin(r);
float cosr = cos(r);
float fx = factor_theta;
float fy = factor_phi;
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
delta_theta *= factor_theta;
delta_phi *= factor_phi;
delta_phi *= 4.0/3.0;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += img.GetWidth()/2;
delta_phi += img.GetHeight()/2;
*/
int sanity = visible;
double delta_theta = x_vis;
double delta_phi = y_vis;
delta_theta -= 64;
delta_phi -= 64;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += 64;
delta_phi += 64;
if (sanity>0)
{
YarpPixelBGR pix1((len<50)?255:0,128,0);
AddCircle(img,pix1,(int)(delta_theta+0.5),
(int)(delta_phi+0.5),4);
}
else
{
//printf("Object occluded\n");
}
}
}
z_y = phi/(M_PI/2);
z_x = theta/(M_PI/2);
if (0)
for (int i=0; i<5; i++)
{
float x = z_x*s;
float y = z_y*s;
YarpPixelBGR pix(255,0,0);
YarpPixelBGR pix2(0,0,255);
img.SafePixel(i+x+width/2,y+width/2) = pix;
img.SafePixel(-i+x+width/2,y+width/2) = pix;
img.SafePixel(i+x+width/2,y+width/2-1) = pix2;
img.SafePixel(-i+x+width/2,y+width/2+1) = pix2;
img.SafePixel(x+width/2,i+y+width/2) = pix;
img.SafePixel(x+width/2,-i+y+width/2) = pix;
img.SafePixel(x+width/2+1,i+y+width/2) = pix2;
img.SafePixel(x+width/2-1,-i+y+width/2) = pix2;
}
out_img.Content().PeerCopy(in_img.Content());
out_img.Write();
}
return 0;
}
void GSTrailMakingTest::ResetTest()
{
m_circles.clear();
m_correct = 0;
m_incorrect = 0;
m_currentCircle = -1;
m_isFinished = false;
static const float MAX_TIME = 120.0f; // TODO TEMP TEST
m_timer = MAX_TIME;
// Grid of allowable positions
static const int GRID_W = 8;
static const int GRID_H = 7;
float w = 2.0f / GRID_W;
float h = 1.6f / GRID_H; // top of screen has info/buttons
#ifdef RANDOM_DISTRIBUTION
Vec2f grid[GRID_W * GRID_H];
int g = 0;
for (int i = 0; i < GRID_H; i++)
{
for (int j = 0; j < GRID_W; j++)
{
grid[g++] = Vec2f(j * w - 0.9f, i * h - 0.9f);
}
}
std::random_shuffle(grid, grid + GRID_W * GRID_H);
// Create circles
g = 0;
for (int i = 0; i < 25; i++)
{
Vec2f pos = grid[g++] + Vec2f(Rnd(-0.05f, 0.05f), Rnd(-0.05f, 0.05f));
AddCircle(i, pos);
}
#endif // RANDOM_DISTRIBUTION
// Kind of Random walk
// Array of bools so we don't have 2 circles overlapping
OH_CRAP:
bool grid[GRID_W * GRID_H];
for (int i = 0; i < GRID_W * GRID_H; i++)
{
grid[i] = false;
}
// Start in the middle
Vec2i pos(GRID_W / 2, GRID_H / 2);
for (int i = 0; i < 25; i++)
{
Vec2i newPos = pos;
int count = 0;
int gridSq = newPos.x * GRID_W + newPos.y;
while (grid[gridSq])
{
// TODO Fix this
count++;
if (count > 50)
{
std::cout << "Oh crap, got boxed in, restarting random walk..\n";
count = 0;
goto OH_CRAP; // I R teh awsom programmer
}
// reach further if we can't find an empty square???
int reach = (count < 10) ? 3 : 5;
// Find a new position close to the old position
newPos = Vec2i(pos.x + rand() % reach - reach/2,
pos.y + rand() % reach - reach/2);
if (newPos.x < 0) newPos.x = 0;
if (newPos.x >= GRID_W) newPos.x = GRID_W - 1;
if (newPos.y < 0) newPos.y = 0;
if (newPos.y >= GRID_H) newPos.y = GRID_H - 1;
gridSq = newPos.x * GRID_W + newPos.y;
}
pos = newPos;
grid[gridSq] = true; // mark grid square as used
// Convert grid coord to screen coord, with a bit of randomness
float x = pos.x * w - 0.9f;
float y = pos.y * h - 0.7f;
Vec2f posf = Vec2f(x + Rnd(-0.05f, 0.05f), y + Rnd(0, 0.05f));
AddCircle(i, posf);
}
// Reset test scores
m_currentCircle = -1;
m_correct = 0;
}
//
// 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;
}
