void Poly::draw(int gl_type, double z, bool randomized) const { Vector2d v; uint count = vertices.size(); if (!closed && gl_type == GL_LINE_LOOP) { gl_type = GL_LINES; count--; } glBegin(gl_type); for (uint i=0; i < count; i++){ v = getVertexCircular(i); if (randomized) v = random_displaced(v); glVertex3f(v.x(),v.y(),z); if ( gl_type == GL_LINES ) { Vector2d vn = getVertexCircular(i+1); if (randomized) vn = random_displaced(vn); glVertex3f(vn.x(),vn.y(),z); } } glEnd(); // if (hole) { // glBegin(GL_LINES); // for (uint i=0; i < count; i++){ // glVertex3d(center.x(),center.y(),z); // Vector2d vn = vertices[i]; // if (randomized) vn = random_displaced(vn); // glVertex3d(vn.x(),vn.y(),z); // } // glEnd(); // } }
int clipSegmentLine(Vector2d& p1, Vector2d& p2, double a, double b, double c ){ bool p1inside = true; bool p2inside = true; if (a*p1.x()+b*p1.y()+c < 0){ p1inside=false; } if (a*p2.x()+b*p2.y()+c < 0){ p2inside=false; } if (p1inside && p2inside) return 2; if (!p1inside && !p2inside) return -1; Vector2d dp=p2-p1; double den=a*dp.x()+b*dp.y(); if (den==0) return -1; double num = c + a*p1.x()+b*p1.y(); double t = - num/den; if (p1inside){ p2=p1+dp*t; return 1; } p1=p1+dp*t; return 0; }
// directed (one end is wider than the other) vector<Poly> dir_thick_line(const Vector2d &from, const Vector2d &to, double fr_width, double to_width) { vector<Poly> p; if (fr_width < 0.001 || to_width < 0.001) return p; if (to.squared_distance(from) < 0.001) return p; if ((fr_width < 0) != (to_width < 0)) return p; Poly poly; Vector2d fdir = (to-from); fdir.normalize(); Vector2d tdir = fdir; fdir *= fr_width/4.; tdir *= to_width/4.; Vector2d fr_dirp(-fdir.y(), fdir.x()); Vector2d to_dirp(-tdir.y(), tdir.x()); poly.addVertex(from-fdir-fr_dirp); poly.addVertex(from-fdir+fr_dirp); poly.addVertex(to+tdir+to_dirp); poly.addVertex(to+tdir-to_dirp); p.push_back(poly); return p; //return Clipping::getOffset(poly, distance/4, jmiter, 0); // slow: // poly.addVertex(from); // poly.addVertex(to); // return Clipping::getOffset(poly, distance/2, jround, distance/2.); }
void glDrawCairoSurface(const Cairo::RefPtr<Cairo::ImageSurface> surface, const Vector2d &min, const Vector2d &max, const double z) { if (surface==0) return; int w = surface->get_width(); int h = surface->get_height(); unsigned char * data = surface->get_data(); GLuint texture; glGenTextures( 1, &texture ); glBindTexture( GL_TEXTURE_2D, texture ); // http://www.nullterminator.net/gltexture.html // select modulate to mix texture with color for shading glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE ); // when texture area is small, bilinear filter the closest mipmap glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_NEAREST ); // when texture area is large, bilinear filter the original glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR ); // build our texture mipmaps gluBuild2DMipmaps( GL_TEXTURE_2D, GL_ALPHA, w, h, GL_ALPHA, GL_UNSIGNED_BYTE, data ); glEnable(GL_TEXTURE_2D); glBegin(GL_QUADS); glTexCoord2d(0.0,0.0); glVertex3d(min.x(),min.y(),z); glTexCoord2d(1.0,0.0); glVertex3d(max.x(),min.y(),z); glTexCoord2d(1.0,1.0); glVertex3d(max.x(),max.y(),z); glTexCoord2d(0.0,1.0); glVertex3d(min.x(),max.y(),z); glEnd(); glDisable(GL_TEXTURE_2D); glDeleteTextures( 1, &texture ); }
bool Poly::vertexInside2(const Vector2d &point, double maxoffset) const { // Shoot a ray along +X and count the number of intersections. // If n_intersections is even, return false, else return true Vector2d EndP(point.x()+10000, point.y()); int intersectcount = 1; // we want to test if uneven double maxoffsetSq = maxoffset*maxoffset; Vector2d dummy; for(size_t i=0; i<vertices.size();i++) { const Vector2d P1 = getVertexCircular(i-1); const Vector2d P2 = vertices[i]; if (point_segment_distance_Sq(point, P1, P2, dummy) <= maxoffsetSq) return true; // Skip horizontal lines, we can't intersect with them, // because the test line is horizontal if(P1.y() == P2.y()) continue; Intersection hit; if(IntersectXY(point,EndP,P1,P2,hit,maxoffset)) intersectcount++; } return (intersectcount%2==0); }
/* * Iterate the vertices, and calculate whether this * shape is a hole or enclosed, based on whether the * segment normals point outward (a hole) or inward * (enclosing) */ void Poly::calcHole() const // hole is mutable { if(vertices.size() < 3) return; // hole is undefined Vector2d p(-INFTY, -INFTY); int v=0; center = Vector2d(0,0); Vector2d q; for(size_t vert=0;vert<vertices.size();vert++) { q = vertices[vert]; center += q; if(q.x() > p.x()) { p = q; v=vert; } else if(q.x() == p.x() && q.y() > p.y()) { p.y() = q.y(); v=vert; } } center /= vertices.size(); // we have the x-most vertex (with the highest y if there was a contest), v Vector2d V1 = getVertexCircular(v-1); Vector2d V2 = getVertexCircular(v); Vector2d V3 = getVertexCircular(v+1); // Vector2d Va=V2-V1; // Vector2d Vb=V3-V2; hole = isleftof(V2, V3, V1); //cross(Vb,Va) > 0; holecalculated = true; }
void center_perpendicular(const Vector2d &from, const Vector2d &to, Vector2d &p1, Vector2d &p2) { Vector2d center = (from+to)/2.; Vector2d dir = Vector2d(from.y()-to.y(), to.x()-from.x()); p1 = center; p2 = center + dir; }
void rotate(Vector2d &p, const Vector2d ¢er, double angle, bool ccw) { if (p==center) return ; if (!ccw) angle = -angle; double cosa = cos(angle); double sina = sin(angle); Vector2d r(p - center); p.x() = center.x() + r.x() * cosa - r.y() * sina; p.y() = center.y() + r.x() * sina + r.y() * cosa; }
bool isFeetDirection(const Vector2d& v, int x, int y, const Vector2d& dir){ bool res = false; int dx = x - v.x(); int dy = y - v.y(); if (dir.x()) res = abs(dy) <= kMinDist && dx * dir.x() >= 0; else res = abs(dx) <= kMinDist && dy * dir.y() >= 0; return res; }
int getCairoSurfaceDatapoint(const Cairo::RefPtr<Cairo::ImageSurface> surface, const Vector2d &min, const Vector2d &max, const Vector2d &p) { if (surface==0) return 0; const int w = surface->get_stride(); const int h = surface->get_height(); const unsigned char * data = surface->get_data(); const Vector2d diag = max - min; const Vector2d relp = p - min; const int ipx = (int)(relp.x() / diag.x() * (double)w); const int ipy = (int)(relp.y() / diag.y() * (double)h); int value = data[ipy*w + ipx]; return value; }
void MapOverview::ShowSamples(const QList<Robot::DetectedSample> &samples) { static const QFont font("Times", 3, QFont::Bold); qDeleteAll(mSampleDetections->childItems()); for(auto& sample : samples) { Vector2d loc = sample.location; auto circle = new QGraphicsEllipseItem(loc.x()-0.2, loc.y()-0.2, 0.4, 0.4, mSampleDetections); //circle->setPos(circle->pos() + mRobotInstance->pos()); circle->setPen(QPen(Qt::red, 0)); //mSampleDetections->addToGroup(circle); //std::cout << "Sample at " << loc.transpose() << "\n"; /* auto text = new QGraphicsTextItem(sample.name); text->setPos(loc.x(), loc.y()); text->setFont(font); text->setDefaultTextColor(Qt::red); mTagDetections->addToGroup(text); */ } }
bool Layer::setMinMax(const vector<Poly> &polys) { Vector2d NewMin = Vector2d( INFTY, INFTY); Vector2d NewMax = Vector2d(-INFTY, -INFTY); for (uint i = 0; i< polys.size(); i++) { vector<Vector2d> minmax = polys[i].getMinMax(); NewMin.x() = min(NewMin.x(), minmax[0].x()); NewMin.y() = min(NewMin.y(), minmax[0].y()); NewMax.x() = max(NewMax.x(), minmax[1].x()); NewMax.y() = max(NewMax.y(), minmax[1].y()); } if (NewMin==Min && NewMax==Max) return false; Min = NewMin; Max = NewMax; return true; }
bool rasterpolys(const vector<Poly> &polys, const Vector2d &min, const Vector2d &max, double resolution, Cairo::RefPtr<Cairo::ImageSurface> &surface, Cairo::RefPtr<Cairo::Context> &context) { Vector2d diag = max - min; int width = (int)ceil(diag.x()/resolution); int height = (int)ceil(diag.y()/resolution); if (height <= 0 || width <= 0) return false; surface = Cairo::ImageSurface::create(Cairo::FORMAT_A8, width, height); //surface->set_device_offset(-min.x()/resolution, -min.y()/resolution); context = Cairo::Context::create (surface); context->set_fill_rule(Cairo::FILL_RULE_WINDING); context->set_antialias(Cairo::ANTIALIAS_DEFAULT); context->scale(1/resolution, 1/resolution); context->translate(-min.x(), -min.y()); context->set_source_rgb (0,0,0); //cerr << min << endl <<getMatrix(context) << endl; // draw boundary // context->begin_new_path(); // context->set_line_width(0.5); // context->move_to(min.x(),min.y()); // context->line_to(max.x(),min.y()); // context->line_to(max.x(),max.y()); // context->line_to(min.x(),max.y()); // // context->move_to(0,0); // // context->line_to(diag.x(),0); // // context->line_to(diag.x(),diag.y()); // // context->line_to(0,diag.y()); // context->close_path(); // context->stroke(); context->begin_new_path(); context->set_line_width(0); for (uint i=0; i<polys.size(); i++) { Vector2d v = polys[i][0]; context->move_to(v.x(),v.y()); for (uint j=0; j<polys[i].size(); j++) { Vector2d v = polys[i][j]; context->line_to(v.x(),v.y()); } } context->fill(); return true; }
void getDistance(size_t x, size_t y, double &distance) { // std::cout << "Distance for " << x << " " << y << " CenterX " << xCenter << " " << yCenter << " " << ((int)(x - xCenter)) * scaleX << " "<< scaleX << " " << 4*0.1 << std::endl; Vector2d posAligned = inverseOrientation * (Vector2d(x * scaleX, y * scaleY) - pos); distance = boxLut->getDistanceToBox(posAligned.x(), posAligned.y()); }
vector<Vector2d> Poly::getMinMax() const{ double minx=6000,miny=6000; double maxx=-6000,maxy=-6000; vector<Vector2d> range; range.resize(2); Vector2d v; for (uint i=0; i < vertices.size();i++){ v = vertices[i]; if (v.x()<minx) minx=v.x(); if (v.x()>maxx) maxx=v.x(); if (v.y()<miny) miny=v.y(); if (v.y()>maxy) maxy=v.y(); } range[0] = Vector2d(minx,miny); range[1] = Vector2d(maxx,maxy); return range; }
bool EdgePointXY::write(std::ostream& os) const { Vector2d p = measurement(); os << p.x() << " " << p.y(); for (int i = 0; i < 2; ++i) for (int j = i; j < 2; ++j) os << " " << information()(i, j); return os.good(); }
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){ /////////////////////////////////// // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one CovarianceEstimator ce(_graph); ce.setVertices(vset); ce.setGauge(_lastVertex); ce.compute(); assert(!_lastVertex->fixed() && "last Vertex is fixed"); assert(_firstRobotPose->fixed() && "first Vertex is not fixed"); OptimizableGraph::VertexSet tmpvset = vset; for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){ VertexSE2 *vertex = (VertexSE2*) *it; MatrixXd Pv = ce.getCovariance(vertex); Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1); SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate(); Vector2d hxy (delta.translation().x(), delta.translation().y()); double perceptionRange =1; if (hxy.x()-perceptionRange>0) hxy.x() -= perceptionRange; else if (hxy.x()+perceptionRange<0) hxy.x() += perceptionRange; else hxy.x() = 0; if (hxy.y()-perceptionRange>0) hxy.y() -= perceptionRange; else if (hxy.y()+perceptionRange<0) hxy.y() += perceptionRange; else hxy.y() = 0; double d2 = hxy.transpose() * Pxy.inverse() * hxy; if (d2 > 5.99) vset.erase(*it); } }
void Game::fragmentExplosion(Vector2d<float> pos) { pos.setXY(pos.x()+ 298 - plyer->rect().x(),pos.y() + 208 - plyer->rect().y()); for (int i = 0; i < 30; i++) { int ang = System::rnd(360); double ttl = 300 + System::rnd(150); double velocity = 1.4 + System::rnd(1000)/1000.0; creaturesView()->particlesManager().addParticle(pos,ttl,ang,velocity,0.05, Game::darkGray); } }
int QuadtreeNode<Item>::Quadrant(const Vector2d &pos) const { if (!rect_.IsPositionIn(pos)) { return -1; } if (pos.x() < center_.x()) { if (pos.y() < center_.y()) { return 0; // Top Left } else { return 2; // Bottom Left } } else { if (pos.y() < center_.y()) { return 1; // Top Right } else { return 3; // Bottom Right } } }
void GridManager::AddBox(Vector2d p, Vector2d size, GridBox* box) { if(box == nullptr) { std::cout << "[GridManager::AddBox] Cannot add null box." << std::endl; return; } CellConfig def_config; def_config.pressure = GetPressure(); def_config.T = GetTemperature(); def_config.boundary_T = GetTemperature(); Vector2d& cell_size = Config::vCellSize; box->p = Vector2i(p.x() / cell_size.x(), p.y() / cell_size.y()); box->size = Vector2i(size.x() / cell_size.x(), size.y() / cell_size.y()); box->def_config = def_config; boxes_stack_.push_back(box); }
//------------------------------------------------------------------------------ void Monster::updateAi() { assert(mpEngine); //on les fait courrir après le joeur Point2d playerPos = mpEngine->getPlayer().getPosition(); Point2d pos = getPosition(); Vector2d dir = playerPos - pos; if( dir.x() < 0 ) moveLeft(); else moveRight(); if( dir.y() > 0 ) jump(); }
//------------------------------------------------------------------------------ //void GameEntity::setAcceleration( const Vector2d& iV ) //{ mAcceleration = iV; } //------------------------------------------------------------------------------ void GameEntity::setBoundingBox( const Rectangle& iV ) { mBoundingBox = iV; mBoundingCircle.setRadius( sqrt( pow(iV.width()/2.0, 2) + pow( iV.height() / 2.0, 2 ) ) ); const Stage& s = mpEngine->getStage(); const Vector2d bbSize = mBoundingBox.size(); int kernel = max( bbSize.x(), bbSize.y() ) / max( s.getCellSize().x(), s.getCellSize().y() ) * 3; kernel = max(kernel, 3); //le kernel doit au minimum etre de 3. if( kernel % 2 == 0 ) ++kernel; //kernel ne doit pas etre pair mCollisionSearchGrid.set( kernel, kernel ); }
// The following functions // (inSegment, intersect2D_Segments and dist3D_Segment_to_Segment) // are licensed as: // // Copyright 2001, softSurfer (www.softsurfer.com) // This code may be freely used and modified for any purpose // providing that this copyright notice is included with it. // SoftSurfer makes no warranty for this code, and cannot be held // liable for any real or imagined damage resulting from its use. // Users of this code must verify correctness for their application. // // inSegment(): determine if a point is inside a segment // Input: a point P, and a collinear segment p1--p2 // Return: true = P is inside p1--p2 // fasle = P is not inside p1--p2 bool inSegment( const Vector2d &P, const Vector2d &p1, const Vector2d &p2) { if (p1.x() != p2.x()) { // S is not vertical if (p1.x() <= P.x() && P.x() <= p2.x()) return true; if (p1.x() >= P.x() && P.x() >= p2.x()) return true; } else { // S is vertical, so test y coordinate if (p1.y() <= P.y() && P.y() <= p2.y()) return true; if (p1.y() >= P.y() && P.y() >= p2.y()) return true; } return false; }
bool fit_arc(const int m_dat, const arc_data_struct data, const int n_par, double *par, double sq_error, Vector2d &result_center, double &result_radiussq) { lm_status_struct status; lm_control_struct control = lm_control_double; control.printflags = 0; // 3 = monitor status (+1) and parameters (+2) control.maxcall = 200; control.ftol = sq_error; // max square error sum // printf( "Fitting:\n" ); lmmin( n_par, par, m_dat, (const void*) &data, evaluate_arcfit, &control, &status, lm_printout_std ); result_center.x() = par[0]; result_center.y() = par[1]; result_radiussq = par[2]; double fvec[m_dat]; int info; evaluate_arcfit(par, m_dat, (const void*) &data, fvec, &info); double totres = 0; for (int i=0; i<m_dat; ++i ) totres+=fvec[i]; return (totres < sq_error); #if 0 printf( "\nResults:\n" ); printf( "status after %d function evaluations:\n %s\n", status.nfev, lm_infmsg[status.info] ); printf("obtained parameters:\n"); int i; for ( i=0; i<n_par; ++i ) printf(" par[%i] = %12g\n", i, par[i]); printf("obtained norm:\n %12g\n", status.fnorm ); #endif // printf("fitting data as follows:\n"); // double ff; // for ( i=0; i<m_dat; ++i ){ // ff = f(tx[i], tz[i], par); // printf( " t[%2d]=%12g,%12g y=%12g fit=%12g residue=%12g\n", // i, tx[i], tz[i], y[i], ff, y[i] - ff ); // } // free(data.px); // free(data.py); }
vector<Poly> thick_line(const Vector2d &from, const Vector2d &to, double width) { vector<Poly> p; if (width < 0.001) return p; if (to.squared_distance(from) < 0.001) return p; Poly poly; Vector2d dir = (to-from); dir.normalize(); dir *= width/4.; Vector2d dirp(-dir.y(),dir.x()); poly.addVertex(from-dir-dirp); poly.addVertex(from-dir+dirp); poly.addVertex(to+dir+dirp); poly.addVertex(to+dir-dirp); p.push_back(poly); return Clipping::getOffset(poly, width/4, jmiter, 0); // slow: // poly.addVertex(from); // poly.addVertex(to); // return Clipping::getOffset(poly, distance/2, jround, distance/2.); }
// find center for best fit of arcpoints bool fit_arc(const vector<Vector2d> &points, double sq_error, Vector2d &result_center, double &result_radiussq) { if (points.size() < 3) return false; const int n_par = 3; // center x,y and arc radius_sq // start values: const Vector2d P = points[0]; const Vector2d Q = points.back(); const Vector2d startxy = (P+Q)/2.; double par[3] = { startxy.x(), startxy.y(), P.squared_distance(Q) }; int m_dat = points.size(); arc_data_struct data; data.px = new double[m_dat]; data.py = new double[m_dat]; for (int i = 0; i < m_dat; i++) { data.px[i] = points[i].x(); data.py[i] = points[i].y(); } return fit_arc(m_dat, data, n_par, par, sq_error, result_center, result_radiussq); }
vector<Segment> Shape::getCutlines(const Matrix4d &T, double z, vector<Vector2d> &vertices, double &max_gradient, vector<Triangle> &support_triangles, double supportangle, double thickness) const { Vector2d lineStart; Vector2d lineEnd; vector<Segment> lines; // we know our own tranform: Matrix4d transform = T * transform3D.transform ; int count = (int)triangles.size(); // #ifdef _OPENMP // #pragma omp parallel for schedule(dynamic) // #endif for (int i = 0; i < count; i++) { Segment line(-1,-1); int num_cutpoints = triangles[i].CutWithPlane(z, transform, lineStart, lineEnd); if (num_cutpoints == 0) { if (supportangle >= 0 && thickness > 0) { if (triangles[i].isInZrange(z-thickness, z, transform)) { const double slope = -triangles[i].slopeAngle(transform); if (slope >= supportangle) { support_triangles.push_back(triangles[i].transformed(transform)); } } } continue; } if (num_cutpoints > 0) { int havev = find_vertex(vertices, lineStart); if (havev >= 0) line.start = havev; else { line.start = vertices.size(); vertices.push_back(lineStart); } if (abs(triangles[i].Normal.z()) > max_gradient) max_gradient = abs(triangles[i].Normal.z()); if (supportangle >= 0) { const double slope = -triangles[i].slopeAngle(transform); if (slope >= supportangle) support_triangles.push_back(triangles[i].transformed(transform)); } } if (num_cutpoints > 1) { int havev = find_vertex(vertices, lineEnd); if (havev >= 0) line.end = havev; else { line.end = vertices.size(); vertices.push_back(lineEnd); } } // Check segment normal against triangle normal. Flip segment, as needed. if (line.start != -1 && line.end != -1 && line.end != line.start) { // if we found a intersecting triangle Vector3d Norm = triangles[i].transformed(transform).Normal; Vector2d triangleNormal = Vector2d(Norm.x(), Norm.y()); Vector2d segment = (lineEnd - lineStart); Vector2d segmentNormal(-segment.y(),segment.x()); triangleNormal.normalize(); segmentNormal.normalize(); if( (triangleNormal-segmentNormal).squared_length() > 0.2){ // if normals do not align, flip the segment int iswap=line.start;line.start=line.end;line.end=iswap; } // cerr << "line "<<line.start << "-"<<line.end << endl; lines.push_back(line); } } return lines; }
/** * @brief idle */ void idle() { double deltaT = (double)TIMER_MS; optotrak.updateMarkers(deltaT); markers = optotrak.getAllMarkers(); // Coordinates picker allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p); allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p); allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p); allVisibleFingers = allVisibleThumb && allVisibleIndex; allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p); allVisibleHead = allVisiblePatch && isVisible(markers.at(17).p) && isVisible(markers.at(18).p); //if ( allVisiblePatch ) headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p,deltaT); // update thumb coordinates thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p,deltaT); // update index coordinates indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p,deltaT); // Compute the coordinates of visual thumb thumb = thumbCoords.getP1().p; index = indexCoords.getP1().p; eyeLeft = headEyeCoords.getLeftEye().p; eyeRight = headEyeCoords.getRightEye().p; rigidCurrentThumb.setRigidBody(markers.at(11).p,markers.at(12).p,markers.at(13).p); rigidCurrentIndex.setRigidBody(markers.at(7).p,markers.at(8).p,markers.at(9).p); rigidStartThumb.computeTransformation(rigidCurrentThumb); rigidStartIndex.computeTransformation(rigidCurrentIndex); if (headCalibrationDone==3 && fingerCalibrationDone==4) { if ( !allVisibleIndex || !allVisibleThumb || !isVisible(markers.at(6).p)) boost::thread invisibleBeep( beepInvisible); Vector2d thumbProjection = cam.computeProjected(thumb); thumbProjectedInside = ( thumbProjection.x() >= 0 && thumbProjection.x() <= SCREEN_WIDTH ) && ( thumbProjection.y() >= 0 && thumbProjection.y() <= SCREEN_HEIGHT ); Vector2d indexProjection = cam.computeProjected(index); indexProjectedInside = ( indexProjection.x() >= 0 && indexProjection.x() <= SCREEN_WIDTH ) && ( indexProjection.y() >= 0 && indexProjection.y() <= SCREEN_HEIGHT ); trialFrame++; // Work on fingers coordinates double wThumb=0.5, wIndex=0.5; Vector3d m = (wThumb*thumb+wIndex*index)/(wThumb+wIndex); Vector3d d = thumb-index; visualThumb = thumb; visualIndex = index; double clamp= 2*factors.getCurrent().at("StimulusRadius"); double gain = 1.0;//in quello di Bob era invece: factors.getCurrent().at("Gain"); if ( d.norm() > clamp ) { visualThumb = m + 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1)); visualIndex = m - 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1)); } // Check when to advance trial double stimRadius = factors.getCurrent().at("StimulusRadius"); double radiusTolerance = util::str2num<double>(parameters.find("RadiusTolerance")); fingNow = fingersInSphericalVolume(thumb,index, visualStimCenter, stimRadius-radiusTolerance, stimRadius+radiusTolerance, selectedFinger ); // SubjectName\tFingerDist\tTrialNumber\tTrialFrame\tTotTime\tVisualStimX\tVisualStimY\tVisualStimZ\tfStimulusRadius\tfDistances\tfGain\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw\tVisualThumbXraw\tVisualThumbYraw\tVisualThumbZraw\tVisualIndexXraw\tVisualIndexYraw\tVisualIndexZraw\tIsDrawing\tIsThumbProjected\tIsIndexProjected if (!isSaving) return; RowVector3d junk(9999,9999,9999); markersFile << fixed << setprecision(3) << parameters.find("SubjectName") << "\t" << fingerDistance << "\t" << totalTrialNumber << "\t" << trialFrame << "\t" << globalTimer.getElapsedTimeInMilliSec() << "\t" << visualStimCenter.x() << "\t" << visualStimCenter.y() << "\t" << visualStimCenter.z() << "\t" << factors.getCurrent().at("StimulusRadius") << "\t" << factors.getCurrent().at("Distances") << "\t" << factors.getCurrent().at("Gain") << "\t" << factors.getCurrent().at("DisappearRadius") << "\t" << ( isVisible(eyeLeft) ? eyeLeft.transpose() : junk ) << "\t" << ( isVisible(eyeRight) ? eyeRight.transpose() : junk ) << "\t" << ( isVisible(markers.at(6).p) ? markers.at(6).p.transpose() : junk ) << "\t" << ( isVisible(thumb) ? thumb.transpose() : junk ) << "\t" << ( isVisible(index) ? index.transpose() : junk ) << "\t" << ( isVisible(visualThumb) ? visualThumb.transpose() : junk ) << "\t" << ( isVisible(visualIndex) ? visualIndex.transpose() : junk ) << "\t" << isDrawing << "\t" << thumbProjectedInside << "\t" << indexProjectedInside << "\t" << fingNow << endl; // Switch to this function if you want cylindrical stimulus //fingersInCylindricalVolume( thumb,index, Vector3d(0,0,factors.getCurrent().at("Distances")), util::str2num<double>(parameters.find("StimulusHeight")), stimRadius,stimRadius+20, selectedFinger ); if (trialMode == STIMULUSMODE ) { if ( !fingNow ) // always starts counting the time if the finger is outside { fingersTimer.start(); } // only if the finger is inside the volume the counter has counted more that 1 second if ( fingersTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("LeaningTime")) ) { plato_write(PLATO_LEFT_RIGHT_CLOSED); advanceTrial(); plato_write(PLATO_LEFT_RIGHT_OPEN); } trialTimer.start(); // continue keeping it starting during stimulus drawingTrialFrame++; invisibleIndexFrames+=!allVisibleIndex; invisibleThumbFrames+=!allVisibleThumb; invisibleWristFrames+=!isVisible(markers.at(6).p); } else { if ( trialTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("IntervalTime")) ) { boost::thread trialBeep( beepTrial ); trialMode=STIMULUSMODE; } } isDrawing = (trialMode == STIMULUSMODE); } }
bool operator <(const sortable_point &p) const { return (v.x() < p.v.x()) || ((v.x() == p.v.x()) && (v.y() < p.v.y())); }
// 2D cross product of OA and OB vectors, i.e. z-component of their 3D cross product. // Returns a positive value, if OAB makes a counter-clockwise turn, // negative for clockwise turn, and zero if the points are collinear. double cross_2(const Vector2d &O, const Vector2d &A, const Vector2d &B) { return (A.x() - O.x()) * (B.y() - O.y()) - (A.y() - O.y()) * (B.x() - O.x()); }