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

bool S013010C_Aaron_Smith_Steering::AccumulateForce(Vector2D& totalForce, Vector2D newForce)
{
	//calculate how much steering force the vehicle has used so far
	double MagnitudeSoFar = totalForce.Length();

	//calculate how much steering force remains to be used by this vehicle
	double MagnitudeRemaining = mTank->GetMaxSpeed() - MagnitudeSoFar;

	//return false if there is no more force left to use
	if (MagnitudeRemaining <= 0.0) return false;

	//calculate the magnitude of the force we want to add
	double MagnitudeToAdd = newForce.Length();

	//if the magnitude of the sum of ForceToAdd and the running total
	//does not exceed the maximum force available to this vehicle, just
	//add together. Otherwise add as much of the ForceToAdd vector is
	//possible without going over the max.
	if (MagnitudeToAdd < MagnitudeRemaining)
	{
		totalForce += newForce;
	}

	else
	{
		//add it to the steering force
		totalForce += (Vec2DNormalize(newForce) * MagnitudeRemaining);
	}

	return true;
}

// This function calculates how much of its max steering force the 
// vehicle has left to apply and then applies that amount of the
// force to add.
bool SteeringBehavior::AccumulateForce(Vector2D &running_total,
                                       Vector2D force_to_add) {

  // Calculate how much steering force the vehicle has used so far
  double magnitude_so_far = running_total.Length();

  // Calculate how much steering force remains to be used by this vehicle
  double magnitude_remaining = vehicle_->MaxForce() - magnitude_so_far;

  // Return false if there is no more force left to use
  if (magnitude_remaining <= 0.0) return false;

  // Calculate the magnitude of the force we want to add
  double magnitude_to_add = force_to_add.Length();

  // If the magnitude of the sum of force_to_add and the running total
  // does not exceed the maximum force available to this vehicle, just
  // add together. Otherwise add as much of the force_to_add vector is
  // possible without going over the max.
  if (magnitude_to_add < magnitude_remaining) {
    running_total += force_to_add;
  } else {
    // Add it to the steering force
    running_total += (Vec2DNormalize(force_to_add) * magnitude_remaining);
  }

  return true;
}

bool Unit::DetectUnitCollision()
{
    RenderableObject* closesestUnit = objects[0];

    for(std::vector<RenderableObject*>::iterator it = objects.begin(); it != objects.end(); ++it) {
            RenderableObject* currentObject = *it;
            Vector2D length = GetPosition() - currentObject->GetPosition();
//            std::cout << "\n current : " << length.Length();
//            std::cout << "\n current test : " << (GetPosition() - closesestUnit->GetPosition()).Length();
            if ((GetPosition() - closesestUnit->GetPosition()).Length() == 0 || closesestUnit->IsDead()){
                closesestUnit = currentObject;
            }
            if (length.Length() <= (GetPosition() - closesestUnit->GetPosition()).Length() && length.Length() > 0 && !closesestUnit->IsDead()){

                closesestUnit = currentObject;
            }
    }
    Vector2D colPosition = closesestUnit->GetPosition();

    Vector2D currentToPosition = GetPosition() - colPosition;
    //std::cout << "\n final : " << currentToPosition.Length();
    if(currentToPosition.Length() < 30){
        return true;
    }
    return false;
}

//--------------------- AccumulateForce ----------------------------------
//
//  This function calculates how much of its max steering force the 
//  vehicle has left to apply and then applies that amount of the
//  force to add.
//------------------------------------------------------------------------
bool SteeringBehavior::AccumulateForce(Vector2D &RunningTot,
	Vector2D ForceToAdd)
{

	//calculate how much steering force the vehicle has used so far
	double MagnitudeSoFar = RunningTot.Length();

	//calculate how much steering force remains to be used by this vehicle
	double MagnitudeRemaining = m_pVehicle->MaxForce() - MagnitudeSoFar;

	//return false if there is no more force left to use
	if (MagnitudeRemaining <= 0.0) return false;

	//calculate the magnitude of the force we want to add
	double MagnitudeToAdd = ForceToAdd.Length();

	//if the magnitude of the sum of ForceToAdd and the running total
	//does not exceed the maximum force available to this vehicle, just
	//add together. Otherwise add as much of the ForceToAdd vector is
	//possible without going over the max.
	if (MagnitudeToAdd < MagnitudeRemaining)
	{
		RunningTot += ForceToAdd;
	}

	else
	{
		//add it to the steering force
		RunningTot += (Vec2DNormalize(ForceToAdd) * MagnitudeRemaining);
	}

	return true;
}

bool CHostageImprov::FaceTowards(const Vector &target, float deltaT)
{
	bool bError = false;
	Vector2D to = (target - GetFeet()).Make2D();

#ifndef PLAY_GAMEDLL
	to.NormalizeInPlace();
#else
	// TODO: fix test demo
	float_precision float_x = target.x - GetFeet().x;
	float_precision float_y = target.y - GetFeet().y;
	float_precision flLen = to.Length();

	if (flLen <= 0)
	{
		to.x = 1;
		to.y = 0;
	}
	else
	{
		to.x = float_x / flLen;
		to.y = float_y / flLen;
	}
#endif

	float moveAngle = GetMoveAngle();

	Vector2D lat(BotCOS(moveAngle), BotSIN(moveAngle));
	Vector2D dir(-lat.y, lat.x);

	float_precision dot = DotProduct(to, dir);

	if (DotProduct(to, lat) < 0.0f)
	{
		if (dot >= 0.0f)
			dot = 1.0f;
		else
			dot = -1.0f;

		bError = true;
	}

	const float maxTurnRate = 0.05f;

	if (bError || Q_fabs(dot) >= maxTurnRate)
	{
		const float tolerance = 300.0f;
		float moveRatio = dot * deltaT * tolerance + moveAngle;

		BotCOS(moveRatio);
		BotSIN(moveRatio);

		m_moveAngle = moveRatio;
		m_hostage->pev->angles.y = moveRatio;

		return false;
	}

	return true;
}

Vector2D Unit::AvoidUnitCollision()
{
    if (DetectUnitCollision()){
        std::cout << "\n" << " found";
        RenderableObject* closesestUnit = objects[0];
        for(std::vector<RenderableObject*>::iterator it = objects.begin(); it != objects.end(); ++it) {
            RenderableObject* currentObject = *it;
            Vector2D length = GetPosition() - currentObject->GetPosition();
//            std::cout << "\n current : " << length.Length();
//            std::cout << "\n current test : " << (GetPosition() - closesestUnit->GetPosition()).Length();
            if ((GetPosition() - closesestUnit->GetPosition()).Length() == 0){
                closesestUnit = currentObject;
            }
            if (length.Length() <= (GetPosition() - closesestUnit->GetPosition()).Length() && length.Length() > 0){

                closesestUnit = currentObject;
            }
        }
        Vector2D colPosition = closesestUnit->GetPosition();
        Vector2D currentToPosition = (GetPosition() + velocity) - colPosition;
        return currentToPosition.Normalized();
    }
    else{
        return Vector2D(0,0);
    }
}

void Shape::resolveCollision( Shape & shape )
{
    Vector2D delta = position - shape.position;
    float d = delta.Length();
    Vector2D mtd = delta * ( ( ( getRadius() + shape.getRadius() ) - d ) / d );

    float im1 = 1 / getMass();
    float im2 = 1 / shape.getMass();

    // speed
    Vector2D v = velocity - shape.velocity;
    float vn = v * (mtd.Normalize());

    // intersecting but moving away
    if ( vn > 0.0f ) return;

    // impulse
    float i = (-(1.0f + RESTITUTION) * vn ) / ( im1 + im2 );
    Vector2D impulse = mtd * i;

    // momentum
    velocity += impulse * im1;
    shape.velocity -= impulse * im1;

}

void Boid::SimulateUpdate(float deltaTime, Vector2D& position, Vector2D& speed, int& angle, Vector2D targetPosition)
{
	Vector2D steeringForce = SteeringUtils::DoSteeringSeek(targetPosition, position, speed, K_MAX_SPEED, K_MAX_STEER_FORCE);

	// Modify according to mass to get acceleration
	Vector2D acceleration = steeringForce / mass;

	// Update Speed with acceleration
	speed += acceleration * deltaTime;

	// Move entity

	// Add speed to speed counter and get number of pixels to move this frame (integer)
	traceSpeedCounter += (speed * deltaTime);
	Vector2D realSpeed = {};
	realSpeed.x = traceSpeedCounter.x > 0.0f ? floor(traceSpeedCounter.x) : ceil(traceSpeedCounter.x);
	realSpeed.y = traceSpeedCounter.y > 0.0f ? floor(traceSpeedCounter.y) : ceil(traceSpeedCounter.y);

	// Remainder for next frame
	traceSpeedCounter -= realSpeed;

	// Move position
	position += realSpeed;

	// Update Orientation if speed is higher than a safety threshold
	if (speed.Length() > 20.0f)
	{
		angle = FloatUtils::CalculateOrientation(speed);
	}
}

//--------------------------- Arrive -------------------------------------
//
//  This behavior is similar to seek but it attempts to arrive at the
//  target with a zero velocity
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Arrive(Vector2D     TargetPos,
	Deceleration deceleration)
{
	Vector2D ToTarget = TargetPos - m_pVehicle->Pos();

	//calculate the distance to the target
	double dist = ToTarget.Length();

	if (dist > 0)
	{
		//because Deceleration is enumerated as an int, this value is required
		//to provide fine tweaking of the deceleration..
		const double DecelerationTweaker = 0.3;

		//calculate the speed required to reach the target given the desired
		//deceleration
		double speed = dist / ((double)deceleration * DecelerationTweaker);

		//make sure the velocity does not exceed the max
		speed = min(speed, m_pVehicle->MaxSpeed());

		//from here proceed just like Seek except we don't need to normalize 
		//the ToTarget vector because we have already gone to the trouble
		//of calculating its length: dist. 
		Vector2D DesiredVelocity = ToTarget * speed / dist;

		return (DesiredVelocity - m_pVehicle->Velocity());
	}

	return Vector2D(0, 0);
}

//------------------------------ Pursuit ---------------------------------
//
//  this behavior creates a force that steers the agent towards the 
//  evader
//------------------------------------------------------------------------
Vector2D SteeringBehavior::Pursuit(const Vehicle* evader)
{
	//if the evader is ahead and facing the agent then we can just seek
	//for the evader's current position.
	Vector2D ToEvader = evader->Pos() - m_pVehicle->Pos();

	double RelativeHeading = m_pVehicle->Heading().Dot(evader->Heading());

	if ((ToEvader.Dot(m_pVehicle->Heading()) > 0) &&
		(RelativeHeading < -0.95))  //acos(0.95)=18 degs
	{
		return Seek(evader->Pos());
	}

	//Not considered ahead so we predict where the evader will be.

	//the lookahead time is propotional to the distance between the evader
	//and the pursuer; and is inversely proportional to the sum of the
	//agent's velocities
	double LookAheadTime = ToEvader.Length() /
		(m_pVehicle->MaxSpeed() + evader->Speed());

	//now seek to the predicted future position of the evader
	return Seek(evader->Pos() + evader->Velocity() * LookAheadTime);
}

void S013010C_Aaron_Smith_Tank::CreateFeelers(vector<Vector2D> &feelers, double angle, int distance,Vector2D startPos)
{
	feelers.clear();

	Vector2D ahead = Vec2DNormalize(mVelocity);
	if (ahead.Length() == 0)
		ahead = mHeading;

	double dynamicLength = mVelocity.Length() / GetMaxSpeed();
	if (dynamicLength < 0.6) dynamicLength = 0.6;
	cout << dynamicLength << endl;

	int dist2Amount = distance;
	int dist1Amount = dist2Amount / 2;
	Vector2D feelerVect1;
	Vector2D feelerVect2;
	if (angle != 0)
	{
		Vector2D fannedAhead;
		fannedAhead.x = ahead.x * cos(DegsToRads(angle)) - ahead.y * sin(DegsToRads(angle));
		fannedAhead.y = ahead.x * sin(DegsToRads(angle)) + ahead.y * cos(DegsToRads(angle));

		feelerVect2 = (startPos)+(fannedAhead *  dynamicLength * dist2Amount);
		feelerVect1 = (startPos)+(fannedAhead *  dynamicLength * dist1Amount);
	}
	else
	{
		feelerVect1 = GetCentrePosition() + (ahead * dynamicLength * dist1Amount);
		feelerVect2 = GetCentrePosition() + (ahead * dynamicLength * (dist2Amount + 50));
	}
	feelers.push_back(feelerVect1);
	feelers.push_back(feelerVect2);

}

//--------------------------- WallAvoidance --------------------------------
//
//  This returns a steering force that will keep the agent away from any
//  walls it may encounter
//------------------------------------------------------------------------
Vector2D SteeringBehavior::WallAvoidance(const std::vector<Wall2D>& walls)
{
	//the feelers are contained in a std::vector, m_Feelers
	CreateFeelers();

	double DistToThisIP = 0.0;
	double DistToClosestIP = MaxDouble;

	//this will hold an index into the vector of walls
	int ClosestWall = -1;

	Vector2D SteeringForce,
		point,         //used for storing temporary info
		ClosestPoint;  //holds the closest intersection point

	//examine each feeler in turn
	for (unsigned int flr = 0; flr < m_Feelers.size(); ++flr)
	{
		//run through each wall checking for any intersection points
		for (unsigned int w = 0; w < walls.size(); ++w)
		{
			if (LineIntersection2D(m_pVehicle->Pos(),
				m_Feelers[flr],
				walls[w].From(),
				walls[w].To(),
				DistToThisIP,
				point))
			{
				//is this the closest found so far? If so keep a record
				if (DistToThisIP < DistToClosestIP)
				{
					DistToClosestIP = DistToThisIP;

					ClosestWall = w;

					ClosestPoint = point;
				}
			}
		}//next wall


		//if an intersection point has been detected, calculate a force  
		//that will direct the agent away
		if (ClosestWall >= 0)
		{
			//calculate by what distance the projected position of the agent
			//will overshoot the wall
			Vector2D OverShoot = m_Feelers[flr] - ClosestPoint;

			//create a force in the direction of the wall normal, with a 
			//magnitude of the overshoot
			SteeringForce = walls[ClosestWall].Normal() * OverShoot.Length();
		}

	}//next feeler

	return SteeringForce;
}

void Vector2D::ProjToLine (const Vector2D &rclPt, const Vector2D &rclLine)
{
  double l  = rclLine.Length();
  double t1 = (rclPt * rclLine) / l;
  Vector2D clNormal = rclLine;
  clNormal.Normalize();
  clNormal.Scale(t1);  
  *this = clNormal;
}

vec_t Vector2DNormalize(Vector2D& v)
{
	vec_t l = v.Length();
	if (l != 0.0f) {
		v /= l;
	}
	else {
		v.x = v.y = 0.0f;
	}
	return l;
}

Vector2D B020612E_Steering::Pursuit(Vector2D target, Vector2D velocity)
{
	Vector2D targetPos = _pTank->mTargetPosition;
	double relative = _pTank->GetHeading().Dot(_pTank->mEnHeading);

	if ((targetPos.Dot(_pTank->GetHeading()) > 0) && (relative < -0.95))
		return Seek(targetPos);
	else
	{
		double lookAheadTime = targetPos.Length() / (_pTank->GetMaxSpeed() + _pTank->mEnSpeed);
		return Seek(targetPos + _pTank->mEnVel * lookAheadTime);
	}
}