bool Circle::calcTangentIntersectionPoints(const Vector2D startPoint, Vector2D &point1, Vector2D &point2){ if(isInside(startPoint)){ // Startpoint is inside circle -> there are no tangent interception points return(false); } //float d = posCenter.getLength()-startPoint.getLength(); float d = (posCenter-startPoint).getLength(); float r = radius; float alphaRad = asin(r/d); float p = sqrt(d*d-r*r); point1.setX(cos(alphaRad)*p); point1.setY(sin(alphaRad)*p); point2.setX(cos(-alphaRad)*p); point2.setY(sin(-alphaRad)*p); point1=point1.rotate((posCenter-startPoint).getDirection()); point2=point2.rotate((posCenter-startPoint).getDirection()); point1+=startPoint; point2+=startPoint; return(true); }
Vector2D ScenRectangle::calcShift(Point2D point,Vector2D lv,float radius) { Vector2D shift; if(point.x<=0) { shift.x=-(radius); shift.y=0; shift.rotate(rotAngle); } if(point.x>=width) { shift.x=radius; shift.y=0; shift.rotate(rotAngle); } if(point.y<=0) { shift.y=-radius; shift.x=0; shift.rotate(rotAngle); } if(point.y>=height) { shift.y=radius; shift.x=0; shift.rotate(rotAngle); } return shift; }//Fine calcShift
float ScenRectangle::intersect(Node *node) { //////////////// if(nodeOverlap(*node)) { Point2D pt(node->getPosition()); //Trasformo il punto nelle coordinate locali del rettangolo pt.translate(-this->p.x,-this->p.y); pt.rotate(-rotAngle); Vector2D lv; lv=node->getVelocity(); lv.rotate(-rotAngle); Vector2D shift=calcShift(pt,lv,node->getRadius()+0.01f); node->setPosition(shift); //return 0; } //////////////////////////// float radius=node->getRadius(); ScenRectangle boundRect(Point2D(p.x-radius,p.y-radius), width+radius*2,height+radius*2); //Creo il raggio e lo trasformo nelle coodinate locali del rettangolo Ray2D ray=transformRay(Ray2D(node->getPosition(),node->getVelocity())); //Lo devo traslare perchè calcolo //l'intersezione con un rettangolo allargato ray.o.x+=radius; ray.o.y+=radius; //Calcolo l'intersezione return boundRect.rayLocalIntersect(ray); }//Fine intersect
void Chassis::setWheelsSpeed() { for (iterator i = wheels.begin(); i != wheels.end(); i++) { Vector2D rot = (*i)->r; rot.rotate(M_PI / 2); rot.mul(angular_speed); (*i)->v = v; (*i)->v.add(rot); } }
Vector2D CameraController::worldToScreen( const Vector2D& worldPos ) { Vector2D screenPos ( (worldPos.x - m_position.x), (worldPos.y - m_position.y) ); screenPos.rotate( -m_rotation ); screenPos.x /= m_viewWidth; screenPos.y /= -m_viewHeight; screenPos.x += 0.5f; screenPos.y += 0.5f; return screenPos; }
bool SkillCircle::execute(RobotCommand &rc) { if(_rid==-1) return false; Vector2D vc; vc = _wm->ourRobot[_rid].pos.loc; vc.setLength(500); vc.rotate(10); rc.FinalPos.loc=vc; rc.TargetPos.loc=vc; rc.FinalPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian(); rc.TargetPos.dir=(_wm->ball.pos.loc - _wm->ourRobot[_rid].pos.loc).dir().radian(); rc.Speed=1; return true; }
void PositionManager::setPoint(PointOrient2D<float> &po, Vector2D<float> &v, float omega) { if (simu) { pos_cur = po; return; } std::cout << "Pas simu" << std::endl; // TODO add pid // convert in robot frame v.rotate(-offset_angle); pnm.sendSetPointSpeed(v, omega); }
void Cluster::predictRelativePosition(){ if(estimatedVelocity.x == 0.0f && estimatedVelocity.y == 0.0f) //virgin state estimatedVelocity = velocity; else{ // else estimate new velocity from acceleration... if(velocity.normalizedDot(estimatedVelocity) <= -20.7){ acceleration.reset(); } else{ // calculate new relative velocity float angle = 0; //calc rotation angle to align velocity vector to x-axis Vector2D xAxis(1,0); if(velocity.norm() > 0) angle = acos(velocity.normalizedDot(xAxis)); if(velocity.y > 0) angle = 2*PI - angle; Vector2D forward = estimatedVelocity.rotate(angle); Vector2D target = velocity.rotate(angle); float m = target.norm()-forward.norm(); //filter and set acceleration magnitude... float a = 0; if(target.norm() > 0) a = 1-target.normalizedDot(xAxis); if(target.y < 0) a = -a; //filter and set acceleration angle... } //estimate new position float angle = 0; Vector2D xAxis(1,0); if(velocity.norm() > 0) angle = acos(velocity.normalizedDot(xAxis)); if(velocity.y > 0) angle = 2*PI - angle; Vector2D forward = estimatedVelocity.rotate(angle); float m = forward.norm() + acceleration.m * delta; float a = acceleration.a * delta; Vector2D newVelocity; newVelocity.x = m * cos(abs(a)); int sgn = 0; if(a != 0.0f) sgn = a < 0 ? -1 : 1; newVelocity.y = m * sin(abs(a)) * sgn; newVelocity = newVelocity.rotate(-angle); // filter - synthetic velocity Vector2D measured; Vector2D predicted; if(measured.normalizedDot(predicted) < 0.5){ acceleration.reset(); estimatedVelocity = velocity; } } // compute new forward velocity... => jaer code: SyntheticVelocityPredictor }
/*! */ void Bhv_GoalieChaseBall::doGoToCatchPoint( PlayerAgent * agent, const Vector2D & target_point ) { const ServerParam & SP = ServerParam::i(); const WorldModel & wm = agent->world(); double dash_power = 0.0; Vector2D rel = target_point - wm.self().pos(); rel.rotate( - wm.self().body() ); AngleDeg rel_angle = rel.th(); const double angle_buf = std::fabs( AngleDeg::atan2_deg( SP.catchableArea() * 0.9, rel.r() ) ); dlog.addText( Logger::TEAM, __FILE__": GoToCatchPoint. (%.1f, %.1f). angle_diff=%.1f. angle_buf=%.1f", target_point.x, target_point.y, rel_angle.degree(), angle_buf ); agent->debugClient().setTarget( target_point ); // forward dash if ( rel_angle.abs() < angle_buf ) { dash_power = std::min( wm.self().stamina() + wm.self().playerType().extraStamina(), SP.maxDashPower() ); dlog.addText( Logger::TEAM, __FILE__": forward dash" ); agent->debugClient().addMessage( "GoToCatch:Forward" ); agent->doDash( dash_power ); } // back dash else if ( rel_angle.abs() > 180.0 - angle_buf ) { dash_power = SP.minDashPower(); double required_stamina = ( SP.minDashPower() < 0.0 ? SP.minDashPower() * -2.0 : SP.minDashPower() ); if ( wm.self().stamina() + wm.self().playerType().extraStamina() < required_stamina ) { dash_power = wm.self().stamina() + wm.self().playerType().extraStamina(); if ( SP.minDashPower() < 0.0 ) { dash_power *= -0.5; if ( dash_power < SP.minDashPower() ) { dash_power = SP.minDashPower(); } } } dlog.addText( Logger::TEAM, __FILE__": back dash. power=%.1f", dash_power ); agent->debugClient().addMessage( "GoToCatch:Back" ); agent->doDash( dash_power ); } // forward dash turn else if ( rel_angle.abs() < 90.0 ) { dlog.addText( Logger::TEAM, __FILE__": turn %.1f for forward dash", rel_angle.degree() ); agent->debugClient().addMessage( "GoToCatch:F-Turn" ); agent->doTurn( rel_angle ); } else { rel_angle -= 180.0; dlog.addText( Logger::TEAM, __FILE__": turn %.1f for back dash", rel_angle.degree() ); agent->debugClient().addMessage( "GoToCatch:B-Turn" ); agent->doTurn( rel_angle ); } agent->setNeckAction( new Neck_TurnToBall() ); }