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