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); */ } }
float ComponentAIBomber::distanceFromThePath() { Vector2d start = path->getStart(); Vector2d end = path->getEnd(); Vector2d normalPoint = Vector2d::getNormalPoint(parent->position, start, end); //COmprobamos que el punto esta entra el principio y el fin, si no es cogemos el final if (normalPoint.x < Math::min_(start.x,end.x) || normalPoint.x > Math::max_(start.x,end.x) ) { if(start.getDistanceFrom(normalPoint) < end.getDistanceFrom(normalPoint)) { normalPoint = start; }else { normalPoint = end; } } //Dibujamos linea /*GraphicsEngine* graphicsEngine = GameManager::getInstance()->getGraphicsEngine(); if(graphicsEngine != NULL) { if(GameManager::getInstance()->getDebugTools()->isShowingDebug()) { // graphicsEngine->drawDebugLine(start.asVector3d(), end.asVector3d()); graphicsEngine->drawDebugLine(normalPoint.asVector3d(), parent->position.asVector3d()); } }*/ float distance = parent->position.getDistanceFrom(normalPoint); return distance; }
// Ellipse Collisions bool PhysicsProcessor::collidedRectangleEllipse(Vector2d<float> pos1, Vector2d<float> dims,Vector2d<float> pos2, float r1, float r2) { // If the bounding boxes don't collide then the circle can't collide. /* lastCollisionA.ticks = timer->getTicks(); lastCollisionA.hasCollided = false; lastCollisionB.ticks = lastCollisionA.ticks; lastCollisionB.hasCollided = false;*/ // 1st we create bounding boxes for the circle and do a Rectangle test against the Rectangle. if(collidedRectangleRectangle(pos1, dims,pos2, Vector2d<float>(2*r1,2*r2))) { // Check the tallest extremites if(collidedLineRectangle(pos2 - Vector2d<float>(0.0f,r2),pos2 + Vector2d<float>(0.0f,r2),pos1,dims ) || collidedLineRectangle(pos2 - Vector2d<float>(r1,0.0f),pos2 + Vector2d<float>(r1,0.0f),pos1,dims) ) return true; /// NOTE we are cheating the result for now. float meanR = r1 + r2 * 0.5f; // If the boxes overlap do a radial distance check Vector2d<float> diff = pos2 - pos1; float meanDim = dims.x + dims.y * 0.5f; if(diff.lengthSquared() <= (meanDim*meanDim) + (r2*r2)) { //lastCollision.hasCollided = true; return true; } //return true; } return false; }
// Obtiene el objeto en una posicion indicada. Solo objetos registrados con collider GameObject* CollisionManager::getGameObjectIn( Vector2d position ) { for(std::size_t i = 0; i < colliderAlliesList.size(); i++) { Vector2d direction = position - colliderAlliesList[i]->getGameObject()->position; float distance = direction.getSqrLength(); if(distance <= colliderAlliesList[i]->getSqrCollisionRadius()) { return colliderAlliesList[i]->getGameObject(); } } for(std::size_t i = 0; i < colliderEnemyList.size(); i++) { Vector2d direction = position - colliderEnemyList[i]->getGameObject()->position; float distance = direction.getSqrLength(); if(distance <= colliderEnemyList[i]->getSqrCollisionRadius()) { return colliderEnemyList[i]->getGameObject(); } } return NULL; }
double Polyline::project(const Vector2d &point) const { double bestS = 0.; double minDistSq = (point - _pts[0]).squaredNorm(); for(int i = 0; i < _pts.endIdx(1); ++i) { double len = (_lengths[i + 1] - _lengths[i]); double invLen = 1. / len; Vector2d der = (_pts[i + 1] - _pts[i]) * invLen; double dot = der.dot(point - _pts[i]); if(dot < 0.) dot = 0.; if(dot > len) dot = len; Vector2d ptOnLine = _pts[i] + (_pts[i + 1] - _pts[i]) * (dot * invLen); double distSq = (ptOnLine - point).squaredNorm(); if(distSq < minDistSq) { minDistSq = distSq; bestS = _lengths[i] + dot; } } return bestS; }
//---------------------------------------------------------------------------- int BspTree2::CoPointLocation (const BspPolygon2& polygon, const Vector2d& vertex) const { // For numerical round-off error handling. const double epsilon = 0.00001; const int numEdges = (int)mCoincident.size(); for (int i = 0; i < numEdges; ++i) { Vector2d end0 = polygon.mVArray[mCoincident[i].I0]; Vector2d end1 = polygon.mVArray[mCoincident[i].I1]; Vector2d dir = end1 - end0; Vector2d diff = vertex - end0; double tmax = dir.Dot(dir); double t = dir.Dot(diff); if (-epsilon <= t && t <= tmax + epsilon) { return 0; } } // Does not matter which subtree you use. if (mPosChild) { return mPosChild->PointLocation(polygon, vertex); } if (mNegChild) { return mNegChild->PointLocation(polygon, vertex); } return 0; }
// Comprueba colision entre Vision y Collider. Un objeto entra en la vision de otro void CollisionManager::checkVisionBetween (ComponentVision* visionCollider, ComponentCollider* targetCollider) { //Optimizaciones de memoria GameObject *targetGameObject = targetCollider->getGameObject(); if(targetGameObject->isDead()) { return; } // Distancia entre los objetos Vector2d direction = targetGameObject->position - visionCollider->getGameObject()->position; float distance = direction.getSqrLength(); float sqrVisionRadius = visionCollider->getSqrVisionRadius(); float sqrTargetRadius = targetCollider->getSqrCollisionRadius(); // Dentro de vision if(distance <= sqrVisionRadius + sqrTargetRadius) { Collision collision; collision.collider = targetGameObject; collision.direction = direction; visionCollider->onVision(collision); } }
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 ); }
Vector2d s_func( double &xi, double &eta, const Vector4d &x, const Vector4d &y) { double xip = 1 + xi; double xim = 1 - xi; double etap = 1 + eta; double etam = 1 - eta; Vector4d shp; shp(0) = xim * etam / 4.0; shp(1) = xip * etam / 4.0; shp(2) = xim * etap / 4.0; shp(3) = xip * etap / 4.0; Vector2d map; map.setZero(); for (size_t i=0; i<4; i++) { map(0) += shp(i) * x(i); map(1) += shp(i) * y(i); } return map; }
bool contains(const Vector& x) const { // Deal with the null vector. if (x.squaredNorm() < _eps*_eps) return (_type & Blunt) != 0; // Otherwise decompose x w.r.t. to the basis. Vector2d theta; theta = _basis.fullPivLu().solve(x); double relError = (_basis*theta - x).squaredNorm() / x.squaredNorm(); if (relError > _eps) return false; // Deal with the degenerate cases (Pointed cone). if (_type & Pointed) { if (_type & PositiveCosine && _type & Blunt) return theta.minCoeff() > -_eps; return (_type & Blunt) != 0; } // Generic case. double min_coeff = theta.minCoeff(); if (_type & Convex) return min_coeff > _eps; return min_coeff > -_eps; }
void model::rotate(const Matrix2x2& M) { Vector2d tmp; //rotate vertices for(uint i=0;i<v_size;i++){ tmp.set(vertices[i].bk_p[0],vertices[i].bk_p[1]); tmp=M*tmp; vertices[i].p.set(tmp[0],tmp[1]); } //rotate edges for(uint i=0;i<e_size;i++){ for(int j=0;j<2;j++){ tmp.set(edges[i].bk_in_n[j][0],edges[i].bk_in_n[j][1]); tmp=M*tmp; edges[i].in_n[j].set(tmp[0],tmp[1]); } tmp.set(edges[i].v[0],edges[i].v[1]); tmp=M*tmp; edges[i].v.set(tmp[0],tmp[1]); } //rotate facets for(uint i=0;i<t_size;i++){ tmp.set(tris[i].n[0],tris[i].n[1]); tmp=M*tmp; tris[i].n.set(tmp[0],tmp[1]); } }
//------------------------------------------------------------------------------ void Projectile::updateState() { Vector2d v = getVelocity(); if( isEqual( v.x(), 0.0 ) ) setState( sVertical ); else setState( sHorizontal ); switch( getType() ) { case ptBasic: if( getRemainingLife() <= 0 ) { setState( sExploding ); } break; case ptBullet: { if( hasIntersections() || getRemainingLife() <= 0) { setState( sExploding ); } } break; case ptGrenade: { if( getRemainingLife() <= 0 ) { setState( sExploding ); Physics& p = mpEngine->getPhysics(); p.explode( getPosition(), 4 * 32, getExplosionDamage(), *mpEngine ); } }break; default: break; } }
bool operator()(const Edge& lhs, const Edge& rhs) const throw() { Vector2d lhv = m_center - Vector2d(lhs.start.x, lhs.start.y); Vector2d rhv = m_center - Vector2d(rhs.start.x, rhs.start.y); return lhv.PolarAngle() < rhv.PolarAngle(); }
double Vector2d::angle(Vector2d& other) { double value = m_x * other.getX() + m_y * other.getY(); double radians = acos(value); double degrees = radians * (180 / 3.14159265); return degrees; }
void addPoint() { Vector2d v; string m1,m2; string s = a[2]; double x,y; int pos; pos=s.find(',',0); if(pos!=std::string::npos) { m1 = s.substr(0,pos); m2 = s.substr(pos+1,s.size()-pos); x = stringToNum<double>(m1); y = stringToNum<double>(m2); } else { cout<<"direction error please write like this"<<endl; cout<<"p2 1 5,6"<<endl; exit(0); } v<<x,y; point[pcnt].setPoint(v); point[pcnt].setName(a[0]); pcnt++; cout<<"add a dot :"<<a[0]<<endl; cout<<"location is:"<<v.transpose()<<endl; }
double signedDistanceInsideConvexHull( const Ref<const Matrix<double, 2, Dynamic>> &pts, const Ref<const Vector2d> &q) { std::vector<Point> hull_pts = convexHull(eigenToPoints(pts)); double d_star = std::numeric_limits<double>::infinity(); Matrix2d R; R << 0, 1, -1, 0; for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) { Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x, hull_pts[i + 1].y - hull_pts[i].y); double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y); double n = ai.norm(); if (std::isnormal(n)) { ai = ai.array() / n; b = b / n; double d = b - ai.transpose() * q; // std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y << // std::endl; // std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y << // std::endl; // std::cout << "ai: " << ai.transpose() << std::endl; // std::cout << "b: " << b << std::endl; // std::cout << "d: " << d << std::endl; if (d < d_star) { d_star = d; } } } return d_star; }
static int AngleToBezier(Point2d* pts, float radius) { const Vector2d vec1 (pts[1] - pts[0]); // 第一条边 const Vector2d vec2 (pts[2] - pts[1]); // 第二条边 const float dHalfAngle = 0.5f * fabs(vec1.angleTo2(vec2)); // 夹角的一半 if (dHalfAngle < 1e-4f || fabs(dHalfAngle - _M_PI_2) < 1e-4f) // 两条边平行 return 0; const float dDist1 = 0.5f * vec1.length(); const float dDist2 = 0.5f * vec2.length(); float dArc = radius / tan(dHalfAngle); // 圆弧在边上的投影长度 if (dArc > dDist1 || dArc > dDist2) { float dArcOld = dArc; dArc = mgMin(dDist1, dDist2); if (dArc < dArcOld * 0.5f) return 3; } int count = 0; Point2d ptCenter, ptStart, ptEnd; float startAngle, sweepAngle; ptStart = pts[1].rulerPoint(pts[0], dArc, 0); ptEnd = pts[1].rulerPoint(pts[2], dArc, 0); if (mgArcTan(ptStart, ptEnd, pts[1] - ptStart, ptCenter, radius, &startAngle, &sweepAngle)) { count = mgAngleArcToBezier( pts, ptCenter, radius, radius, startAngle, sweepAngle); } return count; }
/* * 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 StringElement::buildParticlesAndSprings() { Vector2d xAxis = getEndPosition() - getStartPosition(); double length = xAxis.norm(); int requiredMasses = static_cast<int>(ceil(length / stdMaxMassDistance) + 1.0); double massDistance = length / requiredMasses; xAxis.normalize(); for (int i = 0; i < requiredMasses; i++) { Vector2d massPosition = getStartPosition() + (xAxis * (i * massDistance)); particles.push_back(createParticle(stdMassValue, stdMassRadius, massPosition)); } for (int i = 0; i < requiredMasses - 1; i++) { springs.push_back(new Spring(*particles[i], *particles[i + 1], stdK)); } if (getFromConnectionPoint().isConnected()) { ConnectionPoint::particle_list connectionParticles = getFromConnectionPoint().getConnectionParticles(); for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) { springs.push_back(new Spring(*particles[0], **it, stdK)); } } if (getToConnectionPoint().isConnected()) { ConnectionPoint::particle_list connectionParticles = getToConnectionPoint().getConnectionParticles(); for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) { springs.push_back(new Spring(*particles[particles.size() - 1], **it, stdK)); } } }
// 求本矢量投影到矢量xAxis上的垂直距离 // 在xAxis的逆时针方向时返回正值,顺时针则返回负值 float Vector2d::distanceToVector(const Vector2d& xAxis) const { float len = xAxis.length(); if (len < _MGZERO) return length(); return xAxis.crossProduct(*this) / len; }
double distance(Point2f aPoint, Vector2d aVector, Point2i startPoint) { Vector2d v = aVector; Vector2d w(aPoint.x - startPoint.x, aPoint.y - startPoint.y); double c1 = v.dot(w); double c2 = v.dot(v); if (c1 <= 0){ Point2d tmp; tmp.x = startPoint.x; tmp.y = startPoint.y; return distance(aPoint, tmp); } if (c2 <= 0){ Point2d endPoint; endPoint.x = startPoint.x + (float) aVector[0]; endPoint.y = startPoint.y + (float) aVector[1]; return distance(aPoint, endPoint); } double b = c1 / c2; Point2d pb(startPoint.x + b * aVector[0], startPoint.y + b * aVector[1]); return distance(aPoint, pb); }
void Point::optimize(const size_t n_iter) { Vector3d old_point = pos_; double chi2 = 0.0; Matrix3d A; Vector3d b; for(size_t i=0; i<n_iter; i++) { A.setZero(); b.setZero(); double new_chi2 = 0.0; // compute residuals for(auto it=obs_.begin(); it!=obs_.end(); ++it) { Matrix23d J; const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_); Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotationMatrix(), J); const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f)); new_chi2 += e.squaredNorm(); A.noalias() += J.transpose() * J; b.noalias() -= J.transpose() * e; } // solve linear system const Vector3d dp(A.ldlt().solve(b)); // check if error increased if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0])) { #ifdef POINT_OPTIMIZER_DEBUG cout << "it " << i << "\t FAILURE \t new_chi2 = " << new_chi2 << endl; #endif pos_ = old_point; // roll-back break; } // update the model Vector3d new_point = pos_ + dp; old_point = pos_; pos_ = new_point; chi2 = new_chi2; #ifdef POINT_OPTIMIZER_DEBUG cout << "it " << i << "\t Success \t new_chi2 = " << new_chi2 << "\t norm(b) = " << vk::norm_max(b) << endl; #endif // stop when converged if(vk::norm_max(dp) <= EPS) break; } #ifdef POINT_OPTIMIZER_DEBUG cout << endl; #endif }
void LidarLine2::set(const Line2& line) { Vector2d normal = line.getNormal(); l = abs(line.getC() / normal.getLength()); normal *= -line.getC(); alpha = normal.getAngle2D(); }
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; }
float Line2d::getDistanceTo(const Vector2d &point, Vector2d *intersection) const { float dist = fabsf(_a * point.getX() + _b * point.getY() + _c) / sqrt(_a * _a + _b * _b); if (intersection) { intersectsLine(getPerpendicular(point), intersection); } return dist; }
inline Vector2d Vector2d::normalized() const { Vector2d toReturn = *this; toReturn.normalize(); return toReturn; }
bool operator==(const Vector2d& a, const Vector2d& b) { //Implement Here. if (a.get_x() == b.get_x() && a.get_y() == b.get_y()) return true; else return false; }
double TP_trendofdelivery_tp_sentido::getAngle(const Vector2d &u, const Vector2d &v, const double &t) { Vector2d w = v - u; Vector2d k = w.normal(); k = k.normalize() * t; k = k + v; return k.angle(w); }
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()); }
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(); }