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; }