C++ Vector2D::rotate方法代码示例

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

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