C++ vec3::GetDistanceSquared方法代码示例

void VehicleAi::OnTick() {
	if (!game_->GetVehicle() || !game_->GetVehicle()->IsLoaded() ||
		!game_->GetLevel() || !game_->GetLevel()->IsLoaded() ||
		game_->GetFlybyMode() != Game::kFlybyInactive) {
		return;
	}

	const cure::TimeManager* _time = GetManager()->GetGameManager()->GetTimeManager();
	const int mode_run_delta_frame_count = _time->GetCurrentPhysicsFrameDelta(mode_start_frame_);
	const float mode_run_time = _time->ConvertPhysicsFramesToSeconds(mode_run_delta_frame_count);
	const float aim_distance = AIM_DISTANCE;

	float strength = 1.0f;
	const vec3 _position = game_->GetVehicle()->GetPosition();
	const vec3 _velocity = game_->GetVehicle()->GetVelocity();
	switch (mode_) {
		case kModeFindBestPath:
		case kModeFindPathOffElevator: {
			float start_time = 0.5f;
			if (active_path_ != -1) {
				// Synchronize all paths.
				Spline* _path = game_->GetLevel()->QueryPath()->GetPath(active_path_);
				start_time = _path->GetCurrentInterpolationTime();
				active_path_ = -1;
			}
			log_.Headlinef("Trying to find new path... starting iterating from  %.2f.", start_time);
			vec3 elevator_direction;
			if (mode_ == kModeFindPathOffElevator) {
				game_->GetVehicle()->SetEnginePower(0, 0);
				game_->GetVehicle()->SetEnginePower(2, -strength);	// Negative = use full brakes, not only hand brake.
				const cure::Elevator* _nearest_elevator;
				const vec3 elevator_position = GetClosestElevatorPosition(_position, _nearest_elevator);
				if (elevator_position.GetDistanceSquared(_position) > ELEVATOR_TOO_CLOSE_DISTANCE*ELEVATOR_TOO_CLOSE_DISTANCE) {
					log_.AHeadline("Fell off elevator while looking for get-off route. Looking for somewhere else to go.");
					SetMode(kModeFindBestPath);
					return;
				}
				elevator_direction = _nearest_elevator->GetVelocity().GetNormalized(0.5f);
			}
			float best_path_distance = 1000000;
			std::vector<PathIndexLikeliness> relevant_paths;
			bool lifting_towards_goal = false;
			const int path_count = game_->GetLevel()->QueryPath()->GetPathCount();
			for (int x = 0; x < path_count; ++x) {
				bool current_lifting_towards_goal = false;
				Spline* _path = game_->GetLevel()->QueryPath()->GetPath(x);
				_path->GotoAbsoluteTime(start_time);
				float _likeliness = 1;
				const float nearest_distance = GetClosestPathDistance(_position, x, &_likeliness)/SCALE_FACTOR/2;
				log_.Infof(" - Path %2i is %2.2f units away.", x, nearest_distance);
				if (mode_ == kModeFindPathOffElevator) {
					if (_path->GetCurrentInterpolationTime() > 0.7f) {
						// This path is probably the one I used to get ON the elevator (or one
						// just like it from another direction), we're not using that!
						log_.AInfo("   (Not relevant, too close to path end.)");
						continue;
					} else {
						const float towards_distance = GetClosestPathDistance(_position+elevator_direction, x)/SCALE_FACTOR/2;
						if (towards_distance < nearest_distance) {
							current_lifting_towards_goal = true;
							if (!lifting_towards_goal) {
								lifting_towards_goal = true;
								best_path_distance = 1000000;
							}
						}
					}
				}
				if (!current_lifting_towards_goal && lifting_towards_goal) {
					// This elevator isn't heading in the right direction, but at least one other is.
					continue;
				}
				PathIndexLikeliness pl;
				pl.path_index_ = x;
				pl.likeliness_ = _likeliness;
				pl.distance_ = nearest_distance;
				relevant_paths.push_back(pl);
				if (nearest_distance < best_path_distance) {
					best_path_distance = nearest_distance;
				}
			}
			// Sort out those that are too far away.
			float total_likeliness = 0;
			std::vector<PathIndexLikeliness>::iterator x;
			for (x = relevant_paths.begin(); x != relevant_paths.end();) {
				if (x->distance_ < best_path_distance+2.0f) {
					total_likeliness += x->likeliness_;
					++x;
				} else {
					x = relevant_paths.erase(x);
				}
			}
			if (mode_ == kModeFindPathOffElevator) {
				if (best_path_distance > 5 || relevant_paths.size() != 1) {
					if (relevant_paths.size() == 1) {
						// Point wheels in the right direction for us to get off safely.
						Spline* _path = game_->GetLevel()->QueryPath()->GetPath(relevant_paths[0].path_index_);
						const vec3 _direction = game_->GetVehicle()->GetOrientation() * vec3(0,1,0);
						const vec3 wanted_direction = _path->GetValue() - _position;
						const float angle = LEPRA_XY_ANGLE(wanted_direction, _direction);
						game_->GetVehicle()->SetEnginePower(1, angle*0.5f);
//.........这里部分代码省略.........

void Vehicle::OnTick() {
	Parent::OnTick();

	// If killed: check if we should expel wheels! :)
	if (health_) {
		return;
	}
	if (wheel_expel_tick_count_ <= 0) {
		return;
	}
	--kill_joints_tick_count_;
	--wheel_expel_tick_count_;
	if (kill_joints_tick_count_ <= 0) {
		kill_joints_tick_count_ = 0x7FFFFFFF;
		// Remove the joints, but don't allow collisions with body yet.
		const vec3 position = GetPosition();
		const int bone_count = GetPhysics()->GetBoneCount();
		for (int x = 0; x < bone_count; ++x) {
			tbc::ChunkyBoneGeometry* wheel = GetPhysics()->GetBoneGeometry(x);
			if (wheel->GetJointType() != tbc::ChunkyBoneGeometry::kJointExclude) {
				GetManager()->GetGameManager()->GetPhysicsManager()->DeleteJoint(wheel->GetJointId());
				wheel->ResetJointId();
				// Push the wheel away somewhat, not too much.
				const int push_factor = 200;
				const vec3 wheel_position = GetManager()->GetGameManager()->GetPhysicsManager()->GetBodyPosition(wheel->GetBodyId());
				const vec3 force = (wheel_position-position).GetNormalized()*push_factor*wheel->GetMass();
				GetManager()->GetGameManager()->GetPhysicsManager()->AddForce(wheel->GetBodyId(), force);
			}
		}
	}
	if (wheel_expel_tick_count_ > 0) {
		return;
	}
	// Allow collisions with body.
	const vec3 position = GetPosition();
	const int bone_count = GetPhysics()->GetBoneCount();
	for (int x = 0; x < bone_count; ++x) {
		tbc::ChunkyBoneGeometry* wheel = GetPhysics()->GetBoneGeometry(x);
		if (wheel->GetJointType() == tbc::ChunkyBoneGeometry::kJointExclude ||
			!GetManager()->GetGameManager()->GetPhysicsManager()->GetForceFeedbackListenerId(wheel->GetBodyId())) {
			continue;
		}
		vec3 wheel_position = GetManager()->GetGameManager()->GetPhysicsManager()->GetBodyPosition(wheel->GetBodyId());
		bool far_away = (position.GetDistanceSquared(wheel_position) >= 5*5);
		if (!far_away) {
			const quat car_inverse = GetOrientation().GetInverse();
			wheel_position = car_inverse * (wheel_position - position);
			const float min_distance = wheel->GetShapeSize().x * 0.5f;
			if (::fabs(wheel_position.x) > ::fabs(GetPhysics()->GetOriginalBoneTransformation(x).GetPosition().x) + min_distance) {
				far_away = true;
			}
		}
		if (far_away) {
			GetManager()->RemovePhysicsBody(wheel->GetBodyId());
			GetManager()->GetGameManager()->GetPhysicsManager()->SetForceFeedbackListener(wheel->GetBodyId(), 0);
		} else {
			// Come back later for this one.
			wheel_expel_tick_count_ = 1;
		}
	}
}