C++ Vector2f函数代码示例

 Vector2f operator-() const
 {
   return Vector2f(-x, -y);
 }

//InitMenuSettings
bool Menu::InitMenuSettings ()
{
    Err fErr (m_Err, L"InitMenuSettings");

    wstring sMenuSettings, sFullscreen, sVerticalSync, sFramelimit, sOk,
            sLanguage, sResolution, sColorDepth, sMouseSensitivity,
            sAntialiasing, sAccept, sHelp;
    bool bFullscreen, bVerticalSync, bHelp;
    int nFramelimit, nMouseSensitivity, nAntialiasing;
    if (!m_pLanguage->Get (L"SettingsTitle", &sMenuSettings) ||
        !m_pLanguage->Get (L"SettingsResolution", &sResolution) ||
        !m_pLanguage->Get (L"SettingsColorDepth", &sColorDepth) ||
        !m_pLanguage->Get (L"SettingsFullscreen", &sFullscreen) ||
        !m_pLanguage->Get (L"SettingsAntialiasing", &sAntialiasing) ||
        !m_pLanguage->Get (L"SettingsVerticalSync", &sVerticalSync) ||
        !m_pLanguage->Get (L"SettingsFrameLimit", &sFramelimit) ||
        !m_pLanguage->Get (L"SettingsLanguage", &sLanguage) ||
        !m_pLanguage->Get (L"SettingsHelp", &sHelp) ||
        !m_pLanguage->Get (L"SettingsMouseSensitivity", &sMouseSensitivity) ||
        !m_pLanguage->Get (L"SettingsOk", &sOk) ||
        !m_pLanguage->Get (L"SettingsAccept", &sAccept) ||
        !m_pConfig->GetBool (sCONF_FULLSCREEN, &bFullscreen) ||
        !m_pConfig->GetBool (s_sVerticalSync, &bVerticalSync) ||
        !m_pConfig->GetBool (sCONF_HELP, &bHelp) ||
        !m_pConfig->GetNum (s_sFramelimit, &nFramelimit) ||
        !m_pConfig->GetNum (s_sMouseSensitivity, &nMouseSensitivity) ||
        !m_pConfig->GetNum (s_sAntialiasing, &nAntialiasing))
    {
        return false;
    }

    std::vector<wstring> lsLanguages;
    if (!ListFiles (m_sLanguageDir, &lsLanguages))
    {
        return fErr.Set (L"Could not list the language files");
    }
    unsigned int nLanguageId = 0;
    for (; nLanguageId != lsLanguages.size (); nLanguageId++)
    {
        if (lsLanguages.at (nLanguageId) == m_pLanguage->GetFileName ())
        {
            break;
        }
    }

    std::vector<wstring> lsResolutions, lsDepths;
    unsigned int nOldColorDepth = 0, nCurResId = 0,
                 nCurDepthId = 0, nCurDepth = 0;
    bool bNextDepth = false;
    wstring sCurResolution;
    if (!m_pConfig->Get (s_sResolution, &sCurResolution) ||
        !m_pConfig->GetNum (s_sColorDepth, &nCurDepth))
    {
        return false;
    }
    for (unsigned int i = 0; i != sf::VideoMode::GetModesCount (); i++)
    {
        sf::VideoMode Mode = sf::VideoMode::GetMode (i);

        if (Mode.BitsPerPixel != nOldColorDepth)
        {
            if (nOldColorDepth != 0)
            {
                bNextDepth = true;
            }
            nOldColorDepth = Mode.BitsPerPixel;
            if (nCurDepth == nOldColorDepth)
            {
                nCurDepthId = lsDepths.size ();
            }
            lsDepths.push_back (NumToStr (nOldColorDepth));
        }
        //Because all resolutions are mutliply saved for every color depth
        if (bNextDepth)
        {
            continue;
        }

        wstring sEntry (NumToStr (Mode.Width) + s_sResDelim +
                        NumToStr (Mode.Height));
        if (sEntry == sCurResolution)
        {
            nCurResId = lsResolutions.size ();
        }
        lsResolutions.push_back (sEntry);
    }

    if (!m_ScreenSettings.Init (&m_Err, m_pProfiles, m_pWindow,
                                sMenuSettings) ||
        !m_ScreenSettings.AddList (sResolution, Vector2f (50, 150),
                                   lsResolutions, nCurResId,
                                   eSettingsListResolution) ||
        !m_ScreenSettings.AddList (sColorDepth, Vector2f (500, 150), lsDepths,
                                   nCurDepthId, eSettingsListColorDepth) ||
        !m_ScreenSettings.AddCheckbox (sFullscreen, Vector2f (50, 250),
                                       eSettingsCheckFullscreen, bFullscreen) ||
        !m_ScreenSettings.AddEditbox (sAntialiasing, Vector2f (500, 250), 50,
                                      eSettingsEditAntialiasing,
                                      NumToStr (nAntialiasing)) ||
//.........这里部分代码省略.........

void KrecikApp::createCar()
{
    Vector2f ltPos(320.0f, B2toSF(l.getB2Vertexes()[3].y) - 250.0f); // pozycja left-top autka

    // koła
    float whRad = 10.0f;
    float whFr = cc.wheelFriction;
    b2CircleShape wh;
    wh.m_radius = SFtoB2(whRad);

    wh.m_p = SFtoB2(ltPos + Vector2f(20.0f, 60.0f));
    Body* lwheel = this->addDynamicBody("left wheel", wh, &whTex);
    lwheel->b->GetFixtureList()->SetFriction(whFr);

    wh.m_p = SFtoB2(ltPos + Vector2f(90.0f, 60.0f));
    Body* rwheel = this->addDynamicBody("right wheel", wh, &whTex);
    rwheel->b->GetFixtureList()->SetFriction(whFr);

    // karoseria
    b2PolygonShape sh1; // budka kierownicza
    b2PolygonShape sh2; // reszta autka
    const unsigned vxNum = 4;
    b2Vec2 vx[vxNum];
    vx[0] = SFtoB2(ltPos + Vector2f(80.0f, 20.0f));
    vx[1] = SFtoB2(ltPos + Vector2f(60.0f, 0.0f));
    vx[2] = SFtoB2(ltPos + Vector2f(30.0f, 0.0f));
    vx[3] = SFtoB2(ltPos + Vector2f(30.0f, 20.0f));
    sh1.Set(vx, vxNum);
    vx[0] = SFtoB2(ltPos + Vector2f(0.0f, 20.0f));
    vx[1] = SFtoB2(ltPos + Vector2f(0.0f, 40.0f));
    vx[2] = SFtoB2(ltPos + Vector2f(100.0f, 40.0f));
    vx[3] = SFtoB2(ltPos + Vector2f(100.0f, 20.0f));
    sh2.Set(vx, vxNum);
    Body* car = this->addDynamicBody("car", sh1, &bodyTex, false);
    car->addFixture(sh2, 1.0f, &chassTex, Vector2f(50.0f, 30.0f));

    // amortyzatory
    b2WheelJointDef spr;
    spr.collideConnected = false;
    spr.enableMotor = true;
    spr.motorSpeed = 0.0f;
    spr.dampingRatio = cc.springDamp;
    spr.frequencyHz = cc.springHz;
    spr.maxMotorTorque = 1000.0f;

    spr.Initialize(car->b, lwheel->b, SFtoB2(ltPos + Vector2f(20.0f, 60.0f)), b2Vec2(0.0f, 0.4f));
    this->world.CreateJoint(&spr);

    spr.Initialize(car->b, rwheel->b, SFtoB2(ltPos + Vector2f(90.0f, 60.0f)), b2Vec2(0.0f, 0.4f));
    this->world.CreateJoint(&spr);
}

	ParticleEmitterSnow::ParticleEmitterSnow()
	{
		m_mode= eParticleModeGravity;

		// Gravity Mode: gravity
		m_modeA.gravity = Vector2f(0,0);

		// Gravity Mode: speed of particles
		m_modeA.speed = 10;
		m_modeA.speedVar = 1;

		// Gravity Mode: radial
		m_modeA.radialAccel = 0;
		m_modeA.radialAccelVar = 1;

		// Gravity mode: tagential
		m_modeA.tangentialAccel = 0;
		m_modeA.tangentialAccelVar = 1;

		// emitter position
		//CCSize winSize = CCDirector::sharedDirector()->getWinSize();
		//this->setPosition(ccp(winSize.width/2, winSize.height + 10));
		//m_tPosVar = ccp( winSize.width/2, 0 );

		// angle
		m_angle = 90;
		m_angleVar = 5;

		// life of particles
		m_life = 60;
		m_lifeVar = 10;
		m_startFrame = 0;
		m_endFrame = 50;
		m_sourcePos.x = 480;
		m_sourcePos.y = 320;
		m_sourcePosVar.x = 480;

		// size, in pixels
		m_startSize = 0.8f;
		m_startSizeVar = 0.2f;
		//m_fEndSize = kCCParticleStartSizeEqualToEndSize;

		// emits per second
		m_emissionRate = 30;

		// color of particles
		m_startColor.r = 1.0f;
		m_startColor.g = 1.0f;
		m_startColor.b = 1.0f;
		m_startColor.a = 1.0f;
		m_startColorVar.r = 0.0f;
		m_startColorVar.g = 0.0f;
		m_startColorVar.b = 0.0f;
		m_startColorVar.a = 0.0f;
		m_endColor.r = 0.5f;
		m_endColor.g = 0.5f;
		m_endColor.b = 0.5f;
		m_endColor.a = 1.0f;
		m_endColorVar.r = 0.0f;
		m_endColorVar.g = 0.0f;
		m_endColorVar.b = 0.0f;
		m_endColorVar.a = 0.0f;

		setTexture("X:\\particle\\snow.png");

	}

void Player::update(Uint32 ticks) {
	
	int rangeY =worldHeight-frameHeight;
    int rangeX =worldWidth-frameWidth;
    
   advanceFrame(ticks);
   
   
   ShootingSprite::update(ticks);
	
	
	
	if (!flagStop){
	  
	  if (up && count>0)
	  
	 { velocityY(velocityY()-95);
		 --count;
		 physics = true;

		  }
	if (physics) {
		float deltaT =  0.05;
		Vector2f accel = Vector2f(0,60);
		Vector2f vnew = getVelocity() + accel* deltaT;
         setPosition(getPosition() + vnew * deltaT);
	     setVelocity(vnew);
	}
			if (forward) {
		 X(X()+8.0); flag=true;
	}
	       if (back) {
		 X(X()-8.0); flag=false;
	}
		
	}
	else{
		score = 0;
		
		}

	
	
   
   // Collision Check with edge
	  
	if (  Y() < 0) {
    velocityY( 0);
    Y(0);
    
     }

 
     if ( Y() > rangeY) {
     velocityY(0);
     velocityX(0);
     Y(rangeY);
     count=2;
     //std::cout<<"hit the ground"<<std::endl;
     //std::cout<<velocityY()<<std::endl;
     physics = false;
  }
  //else std::cout << "height = " << Y() << std::endl;
  
  if (  X() < 0) {
     velocityX( 0 );
     X(0);	
  }
  if (  X() > rangeX) {
     velocityX( 0 );
	 X( rangeX);
}

	forward = back = up = down = false;
	
}

void Player::Update(float elapsed)
{
    //Add friction into the acceleration.

    //If the player isn't accelerating, slow down his velocity completely.
    if (Acceleration.x == 0.0f && Acceleration.y == 0.0f)
    {
        if (Velocity.x != 0.0f || Velocity.y != 0.0f)
        {
            Acceleration = -Velocity * LevelConstants::Instance.PlayerFriction;
        }
    }
    else
    {
        //Split the acceleration and velocity into "forward" and "side" components.
        Vector2f forward = LookDir.XY().Normalized(),
                 side = Vector2f(forward.y, forward.x),
                 forwardA = forward * forward.Dot(Acceleration),
                 forwardV = forward * forward.Dot(Velocity),
                 sideA = side * side.Dot(Acceleration),
                 sideV = side * side.Dot(Velocity);

        //Add friction to any velocity components that go against the acceleration.
        if (forwardA.Dot(forwardV) < 0.0f && (abs(forwardV.x) > 0.00001f || abs(forwardV.y) > 0.00001f))
        {
            Acceleration -= forwardV.Normalized() * LevelConstants::Instance.PlayerFriction;
        }
        if (sideA.Dot(sideV) < 0.0f && (abs(sideV.x) > 0.00001f || abs(sideV.y) > 0.00001f))
        {
            Acceleration -= sideV.Normalized() * LevelConstants::Instance.PlayerFriction;
        }
    }


    //Update velocity and acceleration.
    Velocity += Acceleration * elapsed;
    Acceleration = Vector2f();

    //Constrain the velocity.
    float speedSqr = Velocity.LengthSquared();
    if (speedSqr > LevelConstants::Instance.PlayerMaxSpeed * LevelConstants::Instance.PlayerMaxSpeed)
    {
        Velocity = (Velocity / sqrtf(speedSqr)) * LevelConstants::Instance.PlayerMaxSpeed;
    }

    //Update position.
    TryMove(elapsed);


    //Handle weapons.
    if (Fire)
    {
        if (!firedLastFrame)
        {
            firedLastFrame = true;
            GetCurrentWeapon()->StartFire();
        }
        Fire = false;
    }
    else if (firedLastFrame)
    {
        GetCurrentWeapon()->StopFire();
        firedLastFrame = false;
    }
    GetCurrentWeapon()->Update(elapsed);
}

// Called on a decorator to generate any required per-element data for a newly decorated element.
DecoratorDataHandle DecoratorTiledHorizontal::GenerateElementData(Element* element)
{
	// Initialise the tiles for this element.
	for (int i = 0; i < 3; i++)
		tiles[i].CalculateDimensions(element, *(GetTexture(tiles[i].texture_index)));

	DecoratorTiledHorizontalData* data = new DecoratorTiledHorizontalData(element);

	Vector2f padded_size = element->GetBox().GetSize(Box::PADDING);

	Vector2f left_dimensions = tiles[LEFT].GetDimensions(element);
	Vector2f right_dimensions = tiles[RIGHT].GetDimensions(element);
	Vector2f centre_dimensions = tiles[CENTRE].GetDimensions(element);

	// Scale the tile sizes by the height scale.
	ScaleTileDimensions(left_dimensions, padded_size.y, 1);
	ScaleTileDimensions(right_dimensions, padded_size.y, 1);
	ScaleTileDimensions(centre_dimensions, padded_size.y, 1);

	// Shrink the x-sizes on the left and right tiles if necessary.
	if (padded_size.x < left_dimensions.x + right_dimensions.x)
	{
		float minimum_width = left_dimensions.x + right_dimensions.x;
		left_dimensions.x = padded_size.x * (left_dimensions.x / minimum_width);
		right_dimensions.x = padded_size.x * (right_dimensions.x / minimum_width);
	}

    int num_indices;

	// Generate the geometry for the left tile.
	num_indices = tiles[LEFT].GenerateGeometry(data->geometry[tiles[LEFT].texture_index]->GetVertices(), element, Vector2f(0, 0), left_dimensions, left_dimensions, color_multiplier);
	data->geometry[tiles[LEFT].texture_index]->IncreaseNumIndices(num_indices);

    // Generate the geometry for the centre tiles.
	num_indices = tiles[CENTRE].GenerateGeometry(data->geometry[tiles[CENTRE].texture_index]->GetVertices(), element, Vector2f(left_dimensions.x, 0), Vector2f(padded_size.x - (left_dimensions.x + right_dimensions.x), centre_dimensions.y), centre_dimensions, color_multiplier);
    data->geometry[tiles[CENTRE].texture_index]->IncreaseNumIndices(num_indices);
	
    // Generate the geometry for the right tile.
	num_indices = tiles[RIGHT].GenerateGeometry(data->geometry[tiles[RIGHT].texture_index]->GetVertices(), element, Vector2f(padded_size.x - right_dimensions.x, 0), right_dimensions, right_dimensions, color_multiplier);
    data->geometry[tiles[RIGHT].texture_index]->IncreaseNumIndices(num_indices);

	// Set the textures on the geometry.
	const Texture* texture = NULL;
	int texture_index = 0;
	while ((texture = GetTexture(texture_index)) != NULL)
		data->geometry[texture_index++]->SetTexture(texture);

	return reinterpret_cast<DecoratorDataHandle>(data);
}

void Model::MakeRaft(GCodeState &state, float &z)
{
  vector<InFillHit> HitsBuffer;

  uint LayerNr = 0;
  float size = settings.Raft.Size;

  Vector2f raftMin =  Vector2f(Min.x - size + printOffset.x, Min.y - size + printOffset.y);
  Vector2f raftMax =  Vector2f(Max.x + size + printOffset.x, Max.y + size + printOffset.y);

  Vector2f Center = (Vector2f(Max.x + size, Max.y + size)-Vector2f(Min.x + size, Min.y + size))/2+Vector2f(printOffset.x, printOffset.y);

  float Length = sqrtf(2)*(   ((raftMax.x)>(raftMax.y)? (raftMax.x):(raftMax.y))  -  ((raftMin.x)<(raftMin.y)? (raftMin.x):(raftMin.y))  )/2.0f;	// bbox of object

  float E = 0.0f;
  float rot;

  while(LayerNr < settings.Raft.Phase[0].LayerCount + settings.Raft.Phase[1].LayerCount)
    {
      Settings::RaftSettings::PhasePropertiesType *props;
      props = LayerNr < settings.Raft.Phase[0].LayerCount ?
	&settings.Raft.Phase[0] : &settings.Raft.Phase[1];
      rot = (props->Rotation+(float)LayerNr * props->RotationPrLayer)/180.0f*M_PI;
      Vector2f InfillDirX(cosf(rot), sinf(rot));
      Vector2f InfillDirY(-InfillDirX.y, InfillDirX.x);

      Vector3f LastPosition;
      bool reverseLines = false;

      Vector2f P1, P2;
      for(float x = -Length ; x < Length ; x+=props->Distance)
	{
	  P1 = (InfillDirX *  Length)+(InfillDirY*x) + Center;
	  P2 = (InfillDirX * -Length)+(InfillDirY*x) + Center;

	  if(reverseLines)
	    {
	      Vector2f tmp = P1;
	      P1 = P2;
	      P2 = tmp;
			}

	  //			glBegin(GL_LINES);
	  //			glVertex2fv(&P1.x);
	  //			glVertex2fv(&P2.x);

	  // Crop lines to bbox*size
	  Vector3f point;
	  InFillHit hit;
	  HitsBuffer.clear();
	  Vector2f P3(raftMin.x, raftMin.y);
	  Vector2f P4(raftMin.x, raftMax.y);
	  //			glVertex2fv(&P3.x);
	  //			glVertex2fv(&P4.x);
	  if(IntersectXY(P1,P2,P3,P4,hit))	//Intersect edges of bbox
	    HitsBuffer.push_back(hit);
	  P3 = Vector2f(raftMax.x,raftMax.y);
	  //			glVertex2fv(&P3.x);
	  //			glVertex2fv(&P4.x);
	  if(IntersectXY(P1,P2,P3,P4,hit))
	    HitsBuffer.push_back(hit);
	  P4 = Vector2f(raftMax.x,raftMin.y);
	  //			glVertex2fv(&P3.x);
	  //			glVertex2fv(&P4.x);
	  if(IntersectXY(P1,P2,P3,P4,hit))
	    HitsBuffer.push_back(hit);
	  P3 = Vector2f(raftMin.x,raftMin.y);
	  //			glVertex2fv(&P3.x);
	  //			glVertex2fv(&P4.x);
	  if(IntersectXY(P1,P2,P3,P4,hit))
	    HitsBuffer.push_back(hit);
	  //			glEnd();

	  if(HitsBuffer.size() == 0)	// it can only be 2 or zero
	    continue;
	  if(HitsBuffer.size() != 2)
	    continue;

	  std::sort(HitsBuffer.begin(), HitsBuffer.end(), InFillHitCompareFunc);

	  P1 = HitsBuffer[0].p;
	  P2 = HitsBuffer[1].p;

	  state.MakeAcceleratedGCodeLine (Vector3f(P1.x,P1.y,z), Vector3f(P2.x,P2.y,z),
					  props->MaterialDistanceRatio,
					  E, z, settings.Slicing, settings.Hardware);
	  reverseLines = !reverseLines;
	}
      // Set startspeed for Z-move
      Command g;
      g.Code = SETSPEED;
      g.where = Vector3f(P2.x, P2.y, z);
      g.f=settings.Hardware.MinPrintSpeedZ;
      g.comment = "Move Z";
      g.e = E;
      gcode.commands.push_back(g);
      z += props->Thickness * settings.Hardware.LayerThickness;

      // Move Z
      g.Code = ZMOVE;
//.........这里部分代码省略.........

Vector2f Vector2f::operator-(const Vector2f& other) const {
	return Vector2f(v[0] - other.v[0], v[1] - other.v[1]);
}

Vector2f Transform::TransformPoint(float x, float y) const
{
    return Vector2f(myMatrix[0] * x + myMatrix[4] * y + myMatrix[12],
                    myMatrix[1] * x + myMatrix[5] * y + myMatrix[13]);
}

/*
  return the distance in meters in North/East plane as a N/E vector
  from loc1 to loc2
 */
Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
{
    return Vector2f((loc2.lat - loc1.lat) * LOCATION_SCALING_FACTOR,
                    (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
}

// update L1 control for waypoint navigation
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP, float dist_min)
{

    struct Location _current_loc;
    float Nu;
    float xtrackVel;
    float ltrackVel;

    uint32_t now = AP_HAL::micros();
    float dt = (now - _last_update_waypoint_us) * 1.0e-6f;
    if (dt > 0.1) {
        dt = 0.1;
        _L1_xtrack_i = 0.0f;
    }
    _last_update_waypoint_us = now;

    // Calculate L1 gain required for specified damping
    float K_L1 = 4.0f * _L1_damping * _L1_damping;

    // Get current position and velocity
    if (_ahrs.get_position(_current_loc) == false) {
        // if no GPS loc available, maintain last nav/target_bearing
        _data_is_stale = true;
        return;
    }

    Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

    // update _target_bearing_cd
    _target_bearing_cd = get_bearing_cd(_current_loc, next_WP);

    //Calculate groundspeed
    float groundSpeed = _groundspeed_vector.length();
    if (groundSpeed < 0.1f) {
        // use a small ground speed vector in the right direction,
        // allowing us to use the compass heading at zero GPS velocity
        groundSpeed = 0.1f;
        _groundspeed_vector = Vector2f(cosf(get_yaw()), sinf(get_yaw())) * groundSpeed;
    }

    // Calculate time varying control parameters
    // Calculate the L1 length required for specified period
    // 0.3183099 = 1/1/pipi
    _L1_dist = MAX(0.3183099f * _L1_damping * _L1_period * groundSpeed, dist_min);

    // Calculate the NE position of WP B relative to WP A
    Vector2f AB = location_diff(prev_WP, next_WP);
    float AB_length = AB.length();

    // Check for AB zero length and track directly to the destination
    // if too small
    if (AB.length() < 1.0e-6f) {
        AB = location_diff(_current_loc, next_WP);
        if (AB.length() < 1.0e-6f) {
            AB = Vector2f(cosf(get_yaw()), sinf(get_yaw()));
        }
    }
    AB.normalize();

    // Calculate the NE position of the aircraft relative to WP A
    Vector2f A_air = location_diff(prev_WP, _current_loc);

    // calculate distance to target track, for reporting
    _crosstrack_error = A_air % AB;

    //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
    //and further than L1 distance from WP A. Then use WP A as the L1 reference point
    //Otherwise do normal L1 guidance
    float WP_A_dist = A_air.length();
    float alongTrackDist = A_air * AB;
    if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f)
    {
        //Calc Nu to fly To WP A
        Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
        xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
        ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
        Nu = atan2f(xtrackVel,ltrackVel);
        _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
    } else if (alongTrackDist > AB_length + groundSpeed*3) {
        // we have passed point B by 3 seconds. Head towards B
        // Calc Nu to fly To WP B
        Vector2f B_air = location_diff(next_WP, _current_loc);
        Vector2f B_air_unit = (B_air).normalized(); // Unit vector from WP B to aircraft
        xtrackVel = _groundspeed_vector % (-B_air_unit); // Velocity across line
        ltrackVel = _groundspeed_vector * (-B_air_unit); // Velocity along line
        Nu = atan2f(xtrackVel,ltrackVel);
        _nav_bearing = atan2f(-B_air_unit.y , -B_air_unit.x); // bearing (radians) from AC to L1 point
    } else { //Calc Nu to fly along AB line

        //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
        xtrackVel = _groundspeed_vector % AB; // Velocity cross track
        ltrackVel = _groundspeed_vector * AB; // Velocity along track
        float Nu2 = atan2f(xtrackVel,ltrackVel);
        //Calculate Nu1 angle (Angle to L1 reference point)
        float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f);
        //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
        sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
        float Nu1 = asinf(sine_Nu1);

//.........这里部分代码省略.........

void RendererHUDClassic::Compute()
{
	{
		FastVertex thrustBuffer[6 * 32];
		memset( thrustBuffer, 0, sizeof( thrustBuffer ) );
		for ( uint32_t i = 0; i < 32; i++ ) {
			float height = std::exp( i * 0.05f );
			int ofsy = height * 10.0f;
			float x = 50 + i * ( 5 + 1 );
			float y = 720 - 75 - ofsy;
			
			thrustBuffer[i*6 + 0].x = x;
			thrustBuffer[i*6 + 0].y = y;
			thrustBuffer[i*6 + 1].x = x+5;
			thrustBuffer[i*6 + 1].y = y+ofsy;
			thrustBuffer[i*6 + 2].x = x+5;
			thrustBuffer[i*6 + 2].y = y;
			thrustBuffer[i*6 + 3].x = x;
			thrustBuffer[i*6 + 3].y = y;
			thrustBuffer[i*6 + 4].x = x;
			thrustBuffer[i*6 + 4].y = y+ofsy;
			thrustBuffer[i*6 + 5].x = x+5;
			thrustBuffer[i*6 + 5].y = y+ofsy;

			for ( int j = 0; j < 6; j++ ) {
				Vector2f vec = VR_Distort( Vector2f( thrustBuffer[i*6 + j].x, thrustBuffer[i*6 + j].y ) );
				thrustBuffer[i*6 + j].x = vec.x;
				thrustBuffer[i*6 + j].y = vec.y;
			}
		}
		glGenBuffers( 1, &mThrustVBO );
		glBindBuffer( GL_ARRAY_BUFFER, mThrustVBO );
		glBufferData( GL_ARRAY_BUFFER, sizeof(FastVertex) * 6 * 32, thrustBuffer, GL_STATIC_DRAW );
	}

	{
		FastVertexColor linkBuffer[6 * 16];
		memset( linkBuffer, 0, sizeof( linkBuffer ) );
		for ( uint32_t i = 0; i < 16; i++ ) {
			float height = std::exp( i * 0.1f );
			int ofsy = height * 12.0f;
			float x = 1280.0f * 0.03f + i * ( 13 + 1 );
			float y = 145.0f;

			linkBuffer[i*6 + 0].x = x;
			linkBuffer[i*6 + 0].y = y-ofsy;
			linkBuffer[i*6 + 1].x = x+13;
			linkBuffer[i*6 + 1].y = y;
			linkBuffer[i*6 + 2].x = x+13;
			linkBuffer[i*6 + 2].y = y-ofsy;
			linkBuffer[i*6 + 3].x = x;
			linkBuffer[i*6 + 3].y = y-ofsy;
			linkBuffer[i*6 + 4].x = x;
			linkBuffer[i*6 + 4].y = y;
			linkBuffer[i*6 + 5].x = x+13;
			linkBuffer[i*6 + 5].y = y;
			Vector4f fcolor = Vector4f( 0.5f + 0.5f * 0.0625f * ( 15 - i ), 0.5f + 0.5f * 0.0625f * i, 0.5f, 1.0f );
			uint32_t color = 0xFF7F0000 | ( (uint32_t)( fcolor.y * 255.0f ) << 8 ) | ( (uint32_t)( fcolor.x * 255.0f ) );
			linkBuffer[i*6 + 0].color = linkBuffer[i*6 + 1].color = linkBuffer[i*6 + 2].color = linkBuffer[i*6 + 3].color = linkBuffer[i*6 + 4].color = linkBuffer[i*6 + 5].color = color;

			for ( int j = 0; j < 6; j++ ) {
				Vector2f vec = VR_Distort( Vector2f( linkBuffer[i*6 + j].x, linkBuffer[i*6 + j].y ) );
				linkBuffer[i*6 + j].x = vec.x;
				linkBuffer[i*6 + j].y = vec.y;
			}
		}
		glGenBuffers( 1, &mLinkVBO );
		glBindBuffer( GL_ARRAY_BUFFER, mLinkVBO );
		glBufferData( GL_ARRAY_BUFFER, sizeof(FastVertexColor) * 6 * 16, linkBuffer, GL_STATIC_DRAW );
	}

	{
		glGenBuffers( 1, &mBatteryVBO );
		glBindBuffer( GL_ARRAY_BUFFER, mBatteryVBO );
		glBufferData( GL_ARRAY_BUFFER, sizeof(FastVertex) * 6, nullptr, GL_DYNAMIC_DRAW );
	}

	{
		glGenBuffers( 1, &mLineVBO );
		glBindBuffer( GL_ARRAY_BUFFER, mLineVBO );
		glBufferData( GL_ARRAY_BUFFER, sizeof(FastVertexColor) * 2 * 128, nullptr, GL_DYNAMIC_DRAW );
	}
}

// update L1 control for loitering
void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction)
{
    struct Location _current_loc;

    // scale loiter radius with square of EAS2TAS to allow us to stay
    // stable at high altitude
    radius = loiter_radius(radius);

    // Calculate guidance gains used by PD loop (used during circle tracking)
    float omega = (6.2832f / _L1_period);
    float Kx = omega * omega;
    float Kv = 2.0f * _L1_damping * omega;

    // Calculate L1 gain required for specified damping (used during waypoint capture)
    float K_L1 = 4.0f * _L1_damping * _L1_damping;

    //Get current position and velocity
    if (_ahrs.get_position(_current_loc) == false) {
        // if no GPS loc available, maintain last nav/target_bearing
        _data_is_stale = true;
        return;
    }

    Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

    //Calculate groundspeed
    float groundSpeed = MAX(_groundspeed_vector.length() , 1.0f);


    // update _target_bearing_cd
    _target_bearing_cd = get_bearing_cd(_current_loc, center_WP);


    // Calculate time varying control parameters
    // Calculate the L1 length required for specified period
    // 0.3183099 = 1/pi
    _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;

    //Calculate the NE position of the aircraft relative to WP A
    Vector2f A_air = location_diff(center_WP, _current_loc);

    // Calculate the unit vector from WP A to aircraft
    // protect against being on the waypoint and having zero velocity
    // if too close to the waypoint, use the velocity vector
    // if the velocity vector is too small, use the heading vector
    Vector2f A_air_unit;
    if (A_air.length() > 0.1f) {
        A_air_unit = A_air.normalized();
    } else {
        if (_groundspeed_vector.length() < 0.1f) {
            A_air_unit = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw));
        } else {
            A_air_unit = _groundspeed_vector.normalized();
        }
    }

    //Calculate Nu to capture center_WP
    float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
    float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
    float Nu = atan2f(xtrackVelCap,ltrackVelCap);

    _prevent_indecision(Nu);
    _last_Nu = Nu;

    Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2

    //Calculate lat accln demand to capture center_WP (use L1 guidance law)
    float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);

    //Calculate radial position and velocity errors
    float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
    float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle

    // keep crosstrack error for reporting
    _crosstrack_error = xtrackErrCirc;

    //Calculate PD control correction to circle waypoint_ahrs.roll
    float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);

    //Calculate tangential velocity
    float velTangent = xtrackVelCap * float(loiter_direction);

    //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
    if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
        latAccDemCircPD =  MAX(latAccDemCircPD, 0.0f);
    }

    // Calculate centripetal acceleration demand
    float latAccDemCircCtr = velTangent * velTangent / MAX((0.5f * radius), (radius + xtrackErrCirc));

    //Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
    float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);

    // Perform switchover between 'capture' and 'circle' modes at the
    // point where the commands cross over to achieve a seamless transfer
    // Only fly 'capture' mode if outside the circle
    if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
        _latAccDem = latAccDemCap;
        _WPcircle = false;
//.........这里部分代码省略.........

void Ball::Start()
{
	mTrail.clear();
	mVelocity = Vector2f(0, (float)INITIAL_SPEED);
}