void Flying::handleMovement(Vector2D velocity) { Vector2D newPos = m_position; newPos.y(newPos.y() + velocity.y()); // Check collisions in y axis if (newPos.y() + getCollider().y <= 0 || newPos.y() + getCollider().y + getCollider().h >= TheGame::Instance().getMapHeight() || checkCollideTile(newPos) || checkCollideObject(newPos)) { m_bYCollision = !m_bYCollision; } else { m_position.y(newPos.y()); } // Update new position newPos = m_position; newPos.x(m_position.x() + velocity.x()); // Check collisions in x axis if (newPos.x() + getCollider().x <= 0 || newPos.x() + getCollider().x + getCollider().w >= TheGame::Instance().getMapWidth() || checkCollideTile(newPos) || checkCollideObject(newPos)) { // There have been a collision, so change direction m_bXCollision = !m_bXCollision; } else { // Move to new x position m_position.x(newPos.x()); } }
void MarkerMapObject::paint(QPainter &painter, QRect view, int scale) { // Save the painter state for this paint session // so that it can be restored after without influencing other objects painter.save(); { // Translate, scale, and rotate... QTransform t; t.scale(1.0/scale, 1.0/scale); t.translate(x - view.x()*scale, view.height()*scale - y + view.y()*scale); painter.setTransform(t); for(int index = 0; index < transformationCount; index++) { long arrowLength = 1000*scale; long ballSize = 20*scale; Vector2D a = transformation[index].trans(); Vector2D b = (transformation[index] * Trafo2D::trans(0, arrowLength)).trans(); painter.setPen(transformationColors[index]); QPen pen = painter.pen(); pen.setWidth(2*scale); painter.setPen(pen); painter.drawLine(a.x(), -a.y(), b.x(), -b.y()); painter.setBrush(QBrush(transformationColors[index])); painter.drawEllipse(a.x()-ballSize/2, -(a.y()+ballSize/2), ballSize, ballSize); } } painter.restore(); }
void BounceSystem::update() { for (size_t i = 0; i < entityList.size(); i++) { BaseEntity* current = entityList[i]; DimensionComponent *dim = (DimensionComponent*) current->getComponent("dimension"); SDL_Rect *rect = dim->getRect(); Vector2D *position = dim->getPosition(); int centerx = position->x() + (rect->w / 2); int centery = position->y() + (rect->h / 2); if (centerx >= mWidth) { position->x() = 1.f; } if (centerx < 0) { position->x() = mWidth - rect->w - 1; } if (centery >= mHeight) { position->y() = 1.f; } if (centery < 0) { position->y() = mHeight - 1.f; } } }
// Returns the rough angle in degrees the robot needs to turn in order to // be properly aligned with the spline... double SplineNavigation::getAngleForSplineAlignment() { // Get two points, a and b, where a is the origin and b is very close to the origin node. // The slope of the two will give us the angle Vector2D a = Vector2D(navSplineX->getValue(0, 0.0), navSplineY->getValue(0, 0.0)); Vector2D b = Vector2D(navSplineX->getValue(0, 0.1), navSplineY->getValue(0, 0.1)); double angle = -Deg(std::atan2(b.x() - a.x(), b.y() - a.y())); return angle; }
void VectorTest::draw(){ Vector2D u = t * Vector2D(0,0); Vector2D v = t * Vector2D(1,0); Vector2D w = t * Vector2D(0,1); graph->draw(u.x(), u.y(), v.x(), v.y(), w.x(), w.y(), sliderScale->value()); }
void RTreeLeaf::computeBoundingBox() { float lc_x = point.x(); float lc_y = point.y(); float hc_x = point.x(); float hc_y = point.y(); bounding_box = Rectangle(Vector2D(lc_x, lc_y), Vector2D(hc_x, hc_y)); bounding_box.ensureMinSideLength(MIN_BB_SIDE_LENGTH); }
bool StructureMap::isPointOutside(Vector2D p){ // If the StructureMap isn't finish, no Point can be calculated if (!finish) return false; // Init variables double x1, x2; int crossings = 0; double eps = 0.000001; // Check each line for(int i = 0; i < collisions.count(); i++){ // Make sure that it doesn't depend on the direction left to right or right to left. if ( collisions.at(i).x() < collisions.at( (i+1) % collisions.count() ).x() ){ x1 = collisions.at(i).x(); x2 = collisions.at( (i+1) % collisions.count() ).x(); } else { x1 = collisions.at( (i+1) % collisions.count() ).x(); x2 = collisions.at(i).x(); } // Check if a line is in the range of the checking Point if (p.x() > x1 && p.x() <= x2 && ( p.y() < collisions.at(i).y() || p.y() < collisions.at( (i+1)%collisions.count() ).y() )){ double dx = collisions.at( (i+1)%collisions.count() ).x() - collisions.at(i).x(); double dy = collisions.at( (i+1)%collisions.count() ).y() - collisions.at(i).y(); double k = 0; if ( std::abs(dx) < eps ){ k = INT_MAX; } else { k = dy/dx; } double m = collisions.at(i).y() - (k * collisions.at(i).x()); double y2 = k * p.x() + m; if (p.y() <= y2){ crossings++; } } } if (crossings %2 == 1){ // Point p is inside the polygon return false; } return true; }
Polyangle Polyangle::shrink(const double l) const { int length = lesPoints.size(); QVector<Vector2D> newPoints; newPoints.resize(length); for(int cpt = 0; cpt <length ; cpt++) { Vector2D a = lesPoints.at((cpt-1+length)%length); Vector2D b = lesPoints.at((cpt)); Vector2D c = lesPoints.at((cpt+1)%length); Vector2D ab = b-a; Vector2D bc = c-b; Vector2D ca = c-a; //calcul des vecteurs orthogonaux Vector2D orthoAB = Vector2D(-ab.y(), ab.x()); Vector2D orthoBC = Vector2D(-bc.y(), bc.x()); //on l'inverse si il est dans le mauvais sens if(orthoAB*bc < 0) { orthoAB = -orthoAB; } if(orthoBC*ca > 0) { orthoBC = -orthoBC; } orthoAB.normalize(); orthoBC.normalize(); //création des droites afin de récupérer le point d'intersection Vector2D aPrime = a + orthoAB * l; Vector2D bPrime = b + orthoAB * l; Vector2D cPrime; Droite aPrimeBPrime(aPrime, bPrime - aPrime); bPrime = b + orthoBC * l; cPrime = c + orthoBC * l; Droite bPrimeCPrime(bPrime, cPrime - bPrime); aPrimeBPrime.getIntersection(bPrimeCPrime, aPrime); newPoints[cpt] = aPrime; } return Polyangle(newPoints); }
void Flatland::Body::x( double value ) { Vector2D tmp = position(); tmp.x( value ); return cpBodySetPos( _body, tmp ); }
void JoystickNavigationTask::process() { if(!navigation) return; Vector2D wheelSpeed = navigation->getWheelSpeed(0,0); create->controller->setWheelSpeed((short)wheelSpeed.x(), (short)wheelSpeed.y()); }
Vector3D Terrain::norm(const Vector2D &p) const { Vector2D px(p.x()+epsilon,p.y()); Vector2D pxminus(p.x()-epsilon, p.y()); Vector2D py(p.x(),p.y()+epsilon); Vector2D pyminus(p.x(),p.y()-epsilon); double hpx=getHauteur(px); double hpxminus = getHauteur(pxminus); double hpy=getHauteur(py); double hpyminus = getHauteur(pyminus); Vector3D resu(-(hpx-hpxminus),2*epsilon,-(hpy-hpyminus)); resu.normalize(); return resu; }
QImage Camera::printScreen(Terrain &t,const Vector2D& a, const Vector2D& b, const Vector3D& s, int l, int h) { QImage im(l,h,QImage::Format_ARGB32); double min=t.getHauteurMin(a,b); double max=t.getHauteurMax(a,b); Vector3D aBox(a.x(),min,a.y()); Vector3D bBox(b.x(),max*1.5,b.y()); for(int i=0; i<l; ++i) { for(int j=0; j<h; ++j) { im.setPixel(i,h-1-j,ptScreen(t,aBox,bBox,s,i,j,l,h)); } } return im; }
void SplineNavigation::processAddNavPoint(Vector2D point) { Navigation::processAddNavPoint(point); // Append points to our spline datastructures navSplineX->addNode(point.x()); navSplineY->addNode(point.y()); // Calculate points for wheel splines... calculateLeftRightWheelSpline(); }
Vector3D Terrain::norm(const Vector2D &p) { /* Vector2D px(p.x()+epsilon,p.y()); Vector2D py(p.x(),p.y()+epsilon); double hp=getHauteur(p); double hpx=getHauteur(px); double hpy=getHauteur(py); Vector3D vp(p.x(),p.y(),(double)hp); Vector3D vpx(px.x(),px.y(),(double)hpx); Vector3D vpy(py.x(),py.y(),(double)hpy); Vector3D u=vpx-vp; Vector3D v=vpy-vp; Vector3D n(-u.z()/u.x(),1.0f,-v.z()/v.y()); n.normalize(); return n;*/ //std::cout<<p.x()<<" "<<p.x()+epsilon<<std::endl; Vector2D px(p.x()+epsilon,p.y()); Vector2D pxminus(p.x()-epsilon, p.y()); Vector2D py(p.x(),p.y()+epsilon); Vector2D pyminus(p.x(),p.y()-epsilon); double hpx=getHauteur(px); double hpxminus = getHauteur(pxminus); double hpy=getHauteur(py); double hpyminus = getHauteur(pyminus); //std::cout<<hpx<<" "<<hpxminus<<std::endl; Vector3D resu(-(hpx-hpxminus),2*epsilon,-(hpy-hpyminus)); resu.normalize(); return resu; }
void Rectangle::ensureMinSideLength(float min_side_length) { float lc_x = low_corner.x(); float lc_y = low_corner.y(); float hc_x = high_corner.x(); float hc_y = high_corner.y(); float diff_x = hc_x - lc_x; if (diff_x < min_side_length) { hc_x += 0.5f * (min_side_length - diff_x); lc_x -= 0.5f * (min_side_length - diff_x); } float diff_y = hc_y - lc_y; if (diff_y < min_side_length) { hc_y += 0.5f * (min_side_length - diff_y); lc_y -= 0.5f * (min_side_length - diff_y); } low_corner = Vector2D(lc_x, lc_y); high_corner = Vector2D(hc_x, hc_y); }
ProbeGrid::ProbeGrid(const Rectangle2D &bbox, const Vector2D &divisions) : bbox(bbox), divisions(divisions), cellSize(bbox.getWidth() / divisions.x(), bbox.getLength() / divisions.y()) { // Resize vectors resize(divisions.y() + 1, Column_t(divisions.x() + 1)); // Fill in coords double y = bbox.getMin().y(); for (ProbeGrid::row_iterator row = begin(); row != end(); row++) { double x = bbox.getMin().x(); for (ProbeGrid::col_iterator col = row->begin(); col != row->end(); col++) { col->x() = x; col->y() = y; x += cellSize.x(); } y += cellSize.y(); } }
void FingerprintNavigationTask::process(){ // Follow the wall Vector2D wheelSpeed = navigation->getWheelSpeed(0,0); core->controller->setWheelSpeed((short)wheelSpeed.x(), (short)wheelSpeed.y()); // Is it time for a fingerprint? if(QTime::currentTime() > lastFingerprint.addMSecs(fingerprintInterval + fingerprintWaitTime)) { // Create and send message with emulated data if(core->remoteInterface) { // Send a fingerprint message... RemoteInterfaceMessage *message = new RemoteInterfaceMessage("fingerprint"); // Local position message->properties->insert("local-position-x", QString("%1").arg(core->tracker->getX())); message->properties->insert("local-position-y", QString("%1").arg(core->tracker->getY())); // Get GPS coordinates based on an offset added with the degrees based on x,y meters at that offset GPSPosition gps = Util::gpsPositionFromString(core->stringSetting("Map_GPSOffset")); gps.latitude += gps.latitudesPerMeters(core->tracker->getX() / 1000.0); // convert mm to m gps.longitude += gps.longitudesPerMeters(core->tracker->getY() / 1000.0); // convert mm to m message->properties->insert("global-position-latitude", QString("%1").arg(gps.latitude)); message->properties->insert("global-position-longitude", QString("%1").arg(gps.longitude)); // Add additional map information message->properties->insert("floor", core->stringSetting("Map_Floor")); message->properties->insert("map-description", core->stringSetting("Map_Description")); message->properties->insert("map-file", core->stringSetting("Map_PhysicalMap_Filename")); message->properties->insert("wait-time", QString("%1").arg(fingerprintWaitTime)); // Send off the message as a broadcast to all connected core->remoteInterface->broadcastMessage(message); } else { Debug::warning("[FingerprintNavigationTask] no RemoteInterface available for sending fingerprint message!"); } // Stop the robot and wait core->controller->setWheelSpeed(0, 0); lastFingerprint = QTime::currentTime(); SleeperThread::msleep(fingerprintWaitTime); } // Is Task finished if (core->structureMap->isFinish() == true){ this->status = Finished; Debug::print("[FingerprintTask] FingerprintTask Finished"); } }
void SplineNavigation::calculateLeftRightWheelSpline() { double wheelNodeDistanceBetweenNode = targetSpeed * ((double) interval / 1000.0); // Init and drop nodes wheelLeftSplineX->clearNodes(); wheelLeftSplineY->clearNodes(); wheelRightSplineX->clearNodes(); wheelRightSplineY->clearNodes(); // Calculate new nodes for wheel splines for (int navNode = 0; navNode < navSplineX->getNodeCount() - 1; navNode++) { // Get the distance between the two nav nodes double distanceBetweenNodes = getDistanceBetweenNavNodes(navNode, navNode+1); // Create the wheel nodes at equal distance between the two nodes for (int wheelNode = 0; wheelNode < (distanceBetweenNodes / wheelNodeDistanceBetweenNode); wheelNode++) { // Get the t value for the nav node, which is discretisized based on the wheel node distance between nodes double t = (double) wheelNode / (double) (distanceBetweenNodes / wheelNodeDistanceBetweenNode); // Get the node positions for both the left and right wheel splines Vector2D c(navSplineX->getValue(navNode, t), navSplineY->getValue(navNode, t)); Vector2D c1(navSplineX->getFirstDerivative(navNode, t), navSplineY->getFirstDerivative(navNode, t)); Vector2D v = c1 / norm(c1); Vector2D n = Vector2D(-v.y(), v.x()); Vector2D l = c + (wheelOffset * n); Vector2D r = c - (wheelOffset * n); wheelLeftSplineX->addNode(l.x()); wheelLeftSplineY->addNode(l.y()); wheelRightSplineX->addNode(r.x()); wheelRightSplineY->addNode(r.y()); //Debug::print("n=%3 nlx=%1 wlx=%2", l.x(), wheelLeftSplineX->getValue(wheelNode, 0.0), wheelNode); } } }
std::pair<LocalPoint,LocalVector> secondOrderAccurate( const GlobalPoint& startingPos, const GlobalVector& startingDir, double rho, const Plane& plane) { typedef Basic2DVector<float> Vector2D; // translate problem to local frame of the plane LocalPoint lPos = plane.toLocal(startingPos); LocalVector lDir = plane.toLocal(startingDir); LocalVector yPrime = plane.toLocal( GlobalVector(0,0,1.f)); float sinPhi=0, cosPhi=0; Vector2D pos; Vector2D dir; sinPhi = yPrime.y(); cosPhi = yPrime.x(); pos = Vector2D( lPos.x()*cosPhi + lPos.y()*sinPhi, -lPos.x()*sinPhi + lPos.y()*cosPhi); dir = Vector2D( lDir.x()*cosPhi + lDir.y()*sinPhi, -lDir.x()*sinPhi + lDir.y()*cosPhi); double d = -lPos.z(); double x = pos.x() + dir.x()/lDir.z()*d - 0.5*rho*d*d; double y = pos.y() + dir.y()/lDir.z()*d; LocalPoint thePos( x*cosPhi - y*sinPhi, x*sinPhi + y*cosPhi, 0); float px = dir.x()+rho*d; LocalVector theDir( px*cosPhi - dir.y()*sinPhi, px*sinPhi + dir.y()*cosPhi, lDir.z()); return std::pair<LocalPoint,LocalVector>(thePos,theDir); }
void Chaser::update() { if (m_playerPos != nullptr) { Vector2D diff = *m_playerPos - m_position; // Check if player is inside Chaser field of vision if (diff.length() <= m_viewDistance) { if (abs(diff.x()) < m_treshold) { // Player caught in x axis, stop m_velocity.x(0); } else { if (diff.x() < 0) { // x speed can be negative so we have to take abs value m_velocity.x(-abs(m_xSpeed)); } else if (diff.x() > 0) { //m_velocity.x(m_xSpeed); m_velocity.x(abs(m_xSpeed)); } } } else { // Player is outside Chaser's vision camp m_velocity.x(0); } m_velocity.y(m_ySpeed); handleMovement(m_velocity); } handleAnimation(); }
double smooth_noiseBilinear(Vector2D p,int seed){ double x; double y; double fractional_x; double fractional_y; int integer_x; int integer_y; if(p.x()>=0){ x=p.x(); integer_x = (int)x; fractional_x = x - integer_x; } else{ integer_x=((int)p.x())-1; fractional_x=p.x()-integer_x; } if(p.y()>=0){ y=p.y(); integer_y = (int)y; fractional_y = y - integer_y; } else{ integer_y=((int)p.y())-1; fractional_y=p.y()-integer_y; } Vector2D pf(fractional_x,fractional_y); double xy = noise(integer_x,integer_y,seed); double x1y = noise(integer_x+1,integer_y,seed); double xy1 = noise(integer_x,integer_y+1,seed); double x1y1 = noise(integer_x+1,integer_y+1,seed); return bilinearInterpolate(xy,x1y,xy1,x1y1,pf); }
/** * @Inherit */ inline virtual void draw(SimViewer::SimViewer& viewer, const Vector2D& posRoot, scalar angleRoot, const Vector2D& posLeaf, scalar angleLeaf, scalar value) { Joint::draw(viewer, posRoot, angleRoot, posLeaf, angleLeaf, value); Vector2D pos = posRoot + Vector2D::rotate( Joint::getPosRoot(), angleRoot); viewer.drawJoint(pos.x(), pos.y(), (Joint::getAngleRoot()+angleRoot)*180.0/M_PI, (value)*180.0/M_PI); }
double smooth_noise(Vector2D p,int seed){ double x=p.x(); double y=p.y(); int integer_x = (int)x; double fractional_x = x - integer_x; int integer_y = (int)y; double fractional_y = y - integer_y; double t0 = smooth_noise_firstdim(integer_x, integer_y - 1, fractional_x,seed); double t1 = smooth_noise_firstdim(integer_x, integer_y, fractional_x,seed); double t2 = smooth_noise_firstdim(integer_x, integer_y + 1, fractional_x,seed); double t3 = smooth_noise_firstdim(integer_x, integer_y + 2, fractional_x,seed); double resu= cubic_interpolate(t0, t1, t2, t3, fractional_y); return MathUtils::fonctionQuadratique(-0.2,1.2,resu); }
vector<ProbePoint *> ProbeGrid::find(const Vector2D &p) { double xA = (p.x() - bbox.getMin().x()) / cellSize.x(); double yA = (p.y() - bbox.getMin().y()) / cellSize.y(); int x = floor(xA); int y = floor(yA); if (y < 0 || (int)size() <= y || x < 0 || (int)(*this)[y].size() <= x) THROWS("Point " << p << " not in grid (" << x << ", " << y << ')' << " (" << (*this)[y].size() << ", " << size() << ')'); if (y == yA && y == (int)size() - 1) y--; if (x == xA && x == (int)(*this)[y].size() - 1) x--; vector<ProbePoint *> points; points.push_back(&(*this)[y][x]); points.push_back(&(*this)[y][x + 1]); points.push_back(&(*this)[y + 1][x]); points.push_back(&(*this)[y + 1][x + 1]); return points; }
/** * @Inherit */ inline virtual void draw(SimViewer::SimViewer& viewer, const Vector2D& posRoot, scalar angleRoot, const Vector2D& posLeaf, scalar angleLeaf, scalar value) { // Joint::draw(viewer, posRoot, angleRoot, // posLeaf, angleLeaf, value); Vector2D pos_j = posRoot + Vector2D::rotate( Joint::getPosRoot(), angleRoot); viewer.drawJoint(pos_j.x(), pos_j.y(), (Joint::getAngleRoot()+angleRoot)*180.0/M_PI, (value)*180.0/M_PI); Vector2D pos = posLeaf + Vector2D::rotate( Joint::getPosLeaf(), angleLeaf); viewer.drawSegmentByEnd(posRoot.x(),posRoot.y(), pos_j.x(),pos_j.y() ,0.05,sf::Color(0,200,0,100)); // viewer.drawJoint(pos.x(), pos.y(), // (Joint::getAngleRoot()+angleRoot)*180.0/M_PI, // (value)*180.0/M_PI); double ori=(Joint::getAngleLeaf()+angleLeaf); Vector2D rot; //Draw the cam Vector2D pos_r=pos; Vector2D pos_l=pos; double x=0.0; double y=0.0; Leph::Symbolic::Bounder bounder; SymbolPtr xs = Symbol::create("x"); for(int i=0;i<100;i++) { x+=.3/100.0; xs->reset(); bounder.setValue(xs,x); // y=F(x); y=_F(xs)->evaluate(bounder); rot=Vector2D::rotate(Vector2D(x,y),ori+M_PI); viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100)); pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y()); xs->reset(); bounder.setValue(xs,-x); y=_F(xs)->evaluate(bounder); // y=F(-x); rot=Vector2D::rotate(Vector2D(-x,y),ori+M_PI); viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100)); pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y()); } viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,(Joint::getAngleLeaf()+angleLeaf-M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100)); viewer.drawSegment(pos_j.x(),pos_j.y(),_sH,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi)*180.0/M_PI,0.01,sf::Color(255,0,255,100)); viewer.drawCircle(pos_j.x()+_sH*cos((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),pos_j.y()+_sH*sin((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),0.03,sf::Color(255,0,255,100)); viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255)); //Draw spring Vector2D pos1 = posRoot + Vector2D::rotate( Joint::getPosRoot(), angleRoot); Vector2D pos2 = posLeaf + Vector2D::rotate( Joint::getPosLeaf(), angleLeaf); double zval=Vector2D::dist(pos1,pos2); scalar tmp=zval/10.0; scalar sig=1.0; Vector2D tmppos=pos1; scalar angle=atan2(pos2.y()-pos1.y(),pos2.x()-pos1.x()); Vector2D tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI); viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100)); tmppos=tmpnew; sig*=-1.0; tmp=zval/5.0; for(int i=1;i<5;i++) { tmpnew=tmppos+Vector2D::rotate(Vector2D(0.2*sig,tmp), angle+M_PI/2.0+M_PI); viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100)); tmppos=tmpnew; sig*=-1.0; } tmp=zval/10.0; tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI); viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100)); }
/** * @Inherit */ inline virtual void draw(SimViewer::SimViewer& viewer, const Vector2D& posRoot, scalar angleRoot, const Vector2D& posLeaf, scalar angleLeaf, scalar value) { Joint::draw(viewer, posRoot, angleRoot, posLeaf, angleLeaf, value); // Vector2D pos = posRoot + Vector2D::rotate( // Joint::getPosRoot(), angleRoot); Vector2D pos = posLeaf + Vector2D::rotate( Joint::getPosLeaf(), angleLeaf); viewer.drawJoint(pos.x(), pos.y(), (Joint::getAngleRoot()+angleRoot)*180.0/M_PI, (value)*180.0/M_PI); pos = posRoot + Vector2D::rotate( Joint::getPosRoot(), angleRoot); double ori=(Joint::getAngleRoot()+angleRoot); Vector2D rot; //Draw the cam Vector2D pos_r=pos; Vector2D pos_l=pos; double x=0.0; double y=0.0; Leph::Symbolic::Bounder bounder; SymbolPtr xs = Symbol::create("x"); for(int i=0;i<100;i++) { x+=.3/100.0; xs->reset(); bounder.setValue(xs,x); // y=F(x); y=_F(xs)->evaluate(bounder); // std::cout<<"x: "<<x<<" "<<xs->evaluate(bounder)<<" y: "<<y<<std::endl; rot=Vector2D::rotate(Vector2D(x,y),ori); viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100)); pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y()); xs->reset(); bounder.setValue(xs,-x); y=_F(xs)->evaluate(bounder); // y=F(-x); rot=Vector2D::rotate(Vector2D(-x,y),ori); viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100)); pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y()); } viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100)); //Draw the lever viewer.drawSegment(posLeaf.x(),posLeaf.y(),_sH,(angleLeaf-M_PI/2.0)*180.0/M_PI,0.01,sf::Color(255,0,255,100)); viewer.drawCircle(posLeaf.x()+_sH*cos(angleLeaf-M_PI/2.0),posLeaf.y()+_sH*sin(angleLeaf-M_PI/2.0),0.03,sf::Color(255,0,255,100)); viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255)); }
Vector2D Camera::radianAngle( const Vector2D& angle ) { return Vector2D( angle.x() * M_PI / 180.0, angle.y() * M_PI / 180.0 ); }
Vector3D::Vector3D(const Vector2D &vector, double zpos):xp(vector.x()), yp(vector.y()), zp(zpos){}
Vector3D::Vector3D(const Vector2D &vector):xp(vector.x()), yp(vector.y()), zp(0.0){}
void DiscoveryTask::process() { // Check navigation module if( core->navigation->name == "System Of Weights" && !running){ ((SystemOfWeightsNavigation*)core->navigation)->removeAllWeights(); ((SystemOfWeightsNavigation*)core->navigation)->addWeight(new FullSpeedWeight(core)); ((SystemOfWeightsNavigation*)core->navigation)->addWeight(new OrientationWeight(core)); ((SystemOfWeightsNavigation*)core->navigation)->addWeight(new CollisionAvoidanceWeight(core)); ((SystemOfWeightsNavigation*)core->navigation)->addWeight(new AccelerationFilterWeight(core)); ((SystemOfWeightsNavigation*)core->navigation)->addWeight(new ControllerSpeedWeight(core)); running = true; } else if (core->navigation->name == "Spline"){ // using SplineNavigation } else if (!running){ Debug::warning("[DiscoveryTask] Navigation module is not of type Spline!"); Task::status = Task::Finished; return; } // Check if the whole map is explored if (core->navigation->isAtLastNavPoint()) { if (isExplored() == true) { core->controller->setWheelSpeed(0, 0); Task::status = Task::Finished; Debug::print("[DiscoveryTask] DiscoveryTask finished"); return; } else { Debug::print("[DiscoveryTask]Add new TerrainCutPoints"); core->navigation->clearNavPoints(); addNewTerrainCutPoints(); tick = 0; return; } } // Look ahead if it is good to move there using a sweep across a start and end angle... // double heat = 0.0; // for (int i = 0; i < core->longSetting("Task_DiscoveryTask_Range_mm"); i += 10) { // for (int angle = -(core->longSetting("Task_DiscoveryTask_SweepAngle") / 2); angle <= (core->longSetting("Task_DiscoveryTask_SweepAngle") / 2); angle++) { // Trafo2D checkpoint = core->movementTracker->transformation * Trafo2D::rot(-Rad(angle)) * Trafo2D::trans(0, i); // heat += core->heatMap->getChannelValue(HeatMap::CollisionAreaChannel, (long) ((checkpoint.trans().x()) / core->scale), (long) ((checkpoint.trans().y()) / core->scale)); // if (i < core->longSetting("Task_DiscoveryTask_CriticalAreaDistance_mm") && heat > 0) { // collisionInCriticalArea = true; // } // } // } // // // Safe to move? // if (heat > 0.0 && lastProcessWasCollision == false) { // // for (int angle = -(core->longSetting("Task_DiscoveryTask_SweepAngle") / 4); angle <= (core->longSetting("Task_DiscoveryTask_SweepAngle") / 4); angle++) { // Trafo2D checkpoint = core->movementTracker->transformation * Trafo2D::rot(-Rad(angle)) * Trafo2D::trans(0, 2000); // core->heatMap->registerHeat(HeatMap::CollisionAreaChannel, (long) ((checkpoint.trans().x()) / core->scale), (long) ((checkpoint.trans().y()) / core->scale)); // } // // // is the collision in the critical area? // if (collisionInCriticalArea == false) { // //Debug::print("\t[Start] Collision"); //// if (core->navigation->currentNavPoint < core->navigation->getNavPoints()->count()-2){ //// core->navigation->currentNavPoint++; //// //if(core->navigation->isAtLastNavPoint()) return; //// } //// else { // core->navigation->clearNavPoints(); // addNewTerrainCutPoints(); // tick = 0; // lastProcessWasCollision = true; //// } // // //Debug::print("\t[Start] No Collision"); // } else { // //Debug::print("\t[Start] Turn away from Collision"); // // turn away from the collision // ((EmssController*) core->controller)->turn(90,(int) core->doubleSetting("Navigation_SplineNavigation_TargetSpeed_mmps")); // core->navigation->clearNavPoints(); // addNewTerrainCutPoints(); // tick = 0; // lastProcessWasCollision = true; // collisionInCriticalArea = false; // //Debug::print("\t[End] Turn away from Collision"); // } // //Debug::print("\t[End] Collision in Area?"); // } else { // lastProcessWasCollision = false; // } // Align the robot and then crawl the spline... Vector2D wheelSpeed = core->navigation->getWheelSpeed(tick, interval); core->controller->setWheelSpeed((short) wheelSpeed.x(), (short) wheelSpeed.y()); tick++; }