C++ Vector::mag方法代码示例

void Steering::update()
{
	if (mSteeringEnabled) {
		if (mUpdateNeeded) {
			updatePath();
		}
		auto entity = mAvatar.getEntity();
		if (!mPath.empty()) {
			const auto& finalDestination = mPath.back();
			auto entity3dPosition = entity->getViewPosition();
			const WFMath::Point<2> entityPosition(entity3dPosition.x(), entity3dPosition.z());
			//First check if we've arrived at our actual destination.
			if (WFMath::Distance(WFMath::Point<2>(finalDestination.x(), finalDestination.z()), entityPosition) < 0.1f) {
				//We've arrived at our destination. If we're moving we should stop.
				if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
					moveInDirection(WFMath::Vector<2>::ZERO());
				}
				stopSteering();
			} else {
				//We should send a move op if we're either not moving, or we've reached a waypoint, or we need to divert a lot.

				WFMath::Point<2> nextWaypoint(mPath.front().x(), mPath.front().z());
				if (WFMath::Distance(nextWaypoint, entityPosition) < 0.1f) {
					mPath.pop_front();
					nextWaypoint = WFMath::Point<2>(mPath.front().x(), mPath.front().z());
				}

				WFMath::Vector<2> velocity = nextWaypoint - entityPosition;
				WFMath::Point<2> destination;
				velocity.normalize();

				if (mPath.size() == 1) {
					//if the next waypoint is the destination we should send a "move to position" update to the server, to make sure that we stop when we've arrived.
					//otherwise, if there's too much lag, we might end up overshooting our destination and will have to double back
					destination = nextWaypoint;
				}

				//Check if we need to divert in order to avoid colliding.
				WFMath::Vector<2> newVelocity;
				bool avoiding = mAwareness.avoidObstacles(entityPosition, velocity * mSpeed, newVelocity);
				if (avoiding) {
					auto newMag = newVelocity.mag();
					auto relativeMag = mSpeed / newMag;

					velocity = newVelocity;
					velocity.normalize();
					velocity *= relativeMag;
					mUpdateNeeded = true;
				}

				bool shouldSend = false;
				if (velocity.isValid()) {
					if (mLastSentVelocity.isValid()) {
						//If the entity has stopped, and we're not waiting for confirmation to a movement request we've made, we need to start moving.
						if (!entity->isMoving() && !mExpectingServerMovement) {
							shouldSend = true;
						} else {
							auto currentTheta = std::atan2(mLastSentVelocity.y(), mLastSentVelocity.x());
							auto newTheta = std::atan2(velocity.y(), velocity.x());

							//If we divert too much from where we need to go we must adjust.
							if (std::abs(currentTheta - newTheta) > WFMath::numeric_constants<double>::pi() / 20) {
								shouldSend = true;
							}
						}
					} else {
						//If we've never sent a movement update before we should do that now.
						shouldSend = true;
					}
				}
				if (shouldSend) {
					//If we're moving to a certain destination and aren't avoiding anything we should tell the server to move to the destination.
					if (destination.isValid() && !avoiding) {
						moveToPoint(WFMath::Point<3>(destination.x(), entity3dPosition.y(), destination.y()));
					} else {
						moveInDirection(velocity);
					}
				}
			}
		} else {
			//We are steering, but the path is empty, which means we can't find any path. If we're moving we should stop movement.
			//But we won't stop steering; perhaps we'll find a path later.
			if (mLastSentVelocity.isValid() && mLastSentVelocity != WFMath::Vector<2>::ZERO()) {
				moveInDirection(WFMath::Vector<2>::ZERO());
			}
		}
	}

}