C++ FVector::GetUnsafeNormal方法代码示例

void UFlareSpacecraftNavigationSystem::DockingAutopilot(AFlareSpacecraft* DockStation, int32 DockId, float DeltaSeconds)
{
	// The dockin has multiple phase
	// - Rendez-vous : go at the entrance of the docking corridor.
	//     Its a large sphere  in front of the dock with a speed limit. During
	//     the approch phase the ship can go as phase is as he want.
	// 
	// - Approch (500 m): The ship advance in the approch corridor and try to keep itself
	//    near the dock axis an align th ship.
	//    The approch corridor is a cone with speed limitation more and more strict
	//    approching the station. There is a prefered  speed and a limit speed. If
	//    the limit speed is reach, the ship must abord and return to the entrance
	//    of the docking corridor. The is also a angular limit.
	// 
	// - Final Approch (5 m) : The ship slowly advance and wait for the docking trying to keep
	//    the speed and alignement.
	// 
	// - Docking : As soon as the ship is in the docking limits, the ship is attached to the station

	if (!DockStation)
	{
		// TODO LOG
		return;
	}
	//FLOG("==============DockingAutopilot==============");

	float DockingDockToDockDistanceLimit = 20; // 20 cm of linear distance to dock
	float FinalApproachDockToDockDistanceLimit = 100; // 1 m of linear distance
	float ApproachDockToDockDistanceLimit = 40000; // 400 m approch distance

	float FinalApproachDockToDockLateralDistanceLimit = 100; // 50 cm of linear lateral distance

	float DockingAngleLimit = 2; // 1° of angle error to dock
	float FinalApproachAngleLimit = 10;// 10° of angle error to dock

	float DockingVelocityLimit = 200; // 1 m/s
	float FinalApproachVelocityLimit = 500; // 5 m/s

	float DockingLateralVelocityLimit = 20; // 10 cm/s
	float FinalApproachLateralVelocityLimit = 50; // 0.5 m/s
	float ApproachLateralVelocityLimit = 1000; // 10 m/s

	float DockingAngularVelocityLimit = 10; // 5 °/s
	float FinalApproachAngularVelocityLimit = 10; // 10 °/s


	FVector ShipDockAxis = Spacecraft->Airframe->GetComponentToWorld().GetRotation().RotateVector(FVector(1, 0, 0)); // Ship docking port are always at front
	FVector ShipDockLocation = GetDockLocation();
	FVector ShipDockOffset = ShipDockLocation - Spacecraft->GetActorLocation();
	FVector ShipAngularVelocity = Spacecraft->Airframe->GetPhysicsAngularVelocity();




	FFlareDockingInfo StationDockInfo = DockStation->GetDockingSystem()->GetDockInfo(DockId);
	FVector StationCOM = DockStation->Airframe->GetBodyInstance()->GetCOMPosition();
	FVector StationDockAxis = DockStation->Airframe->GetComponentToWorld().GetRotation().RotateVector(StationDockInfo.LocalAxis);
	FVector StationDockLocation =  DockStation->Airframe->GetComponentTransform().TransformPosition(StationDockInfo.LocalLocation);
	FVector StationDockOffset = StationDockLocation - DockStation->GetActorLocation();
	FVector StationAngularVelocity = DockStation->Airframe->GetPhysicsAngularVelocity();

	// Compute docking infos
	FVector DockToDockDeltaLocation = StationDockLocation - ShipDockLocation;
	float DockToDockDistance = DockToDockDeltaLocation.Size();

	// The linear velocity of the docking port induced by the station or ship rotation
	FVector ShipDockSelfRotationInductedLinearVelocity = (PI /  180.f) * FVector::CrossProduct(ShipAngularVelocity, ShipDockLocation-COM);
	FVector ShipDockLinearVelocity = ShipDockSelfRotationInductedLinearVelocity + Spacecraft->GetLinearVelocity() * 100;

	FVector StationDockSelfRotationInductedLinearVelocity = (PI /  180.f) * FVector::CrossProduct(StationAngularVelocity, StationDockLocation- StationCOM);
	FVector StationDockLinearVelocity = StationDockSelfRotationInductedLinearVelocity + DockStation->GetLinearVelocity() * 100;

	float InAxisDistance = FVector::DotProduct(DockToDockDeltaLocation, -StationDockAxis);
	FVector RotationInductedLinearVelocityAtShipDistance = (PI /  180.f) * FVector::CrossProduct(StationAngularVelocity, (StationDockLocation - StationCOM).GetUnsafeNormal() * (InAxisDistance + (StationDockLocation - StationCOM).Size()));
	FVector LinearVelocityAtShipDistance = RotationInductedLinearVelocityAtShipDistance + DockStation->GetLinearVelocity() * 100;


	AFlareSpacecraft* AnticollisionDockStation = DockStation;
	FVector RelativeDockToDockLinearVelocity = StationDockLinearVelocity - ShipDockLinearVelocity;
	float DockToDockAngle = FMath::RadiansToDegrees(FMath::Acos(FVector::DotProduct(-ShipDockAxis, StationDockAxis)));

	// Angular velocity must be the same
	FVector RelativeDockAngularVelocity = ShipAngularVelocity - StationAngularVelocity;

/*
	FLOGV("ShipLocation=%s", *(Spacecraft->GetActorLocation().ToString()));

	FLOGV("ShipDockAxis=%s", *ShipDockAxis.ToString());
	FLOGV("ShipDockOffset=%s", *ShipDockOffset.ToString());
	FLOGV("ShipDockLocation=%s", *ShipDockLocation.ToString());
	FLOGV("ShipAngularVelocity=%s", *ShipAngularVelocity.ToString());

	FLOGV("StationLocation=%s", *(DockStation->GetActorLocation().ToString()));
	FLOGV("StationCOM=%s", *StationCOM.ToString());
	FLOGV("LocalAxis=%s", *(StationDockInfo.LocalAxis.ToString()));
	FLOGV("StationDockAxis=%s", *StationDockAxis.ToString());
	FLOGV("StationDockOffset=%s", *StationDockOffset.ToString());
	FLOGV("StationDockLocation=%s", *StationDockLocation.ToString());
	FLOGV("StationAngularVelocity=%s", *StationAngularVelocity.ToString());

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