void ACinemotusPlayerController::AbsoluteTick(float DeltaTime)
{

	
	TotalYawAbs += addYaw;
	UPrimitiveComponent* prim = GetPawn()->GetMovementComponent()->UpdatedComponent;
	//USceneComponent* sComponent = GetPawn()->GetRootComponent();
	//sComponent->SetRelativeLocation;

	bool SetPrimDirectly = true;
	FQuat finalQuat;

	if (((currentCaptureState&ECinemotusCaptureState::EAbsolute) == ECinemotusCaptureState::EAbsolute) && 
		((currentCaptureState&ECinemotusCaptureState::EAbsoluteOff) == 0))
	{
		finalQuat = FRotator(0, TotalYawAbs, 0).Quaternion()*(HydraLatestData->controllers[CAM_HAND].quat);
	}
	else
	{
		finalQuat =  FRotator(0, addYaw, 0).Quaternion()*prim->GetComponentQuat();
	}
	SetControlRotation(finalQuat.Rotator());
	if (SetPrimDirectly && prim)
	{
		prim->SetWorldLocationAndRotation(prim->GetComponentLocation(), finalQuat);// not sure need
	}


	HandleMovementAbs(DeltaTime, ((currentCaptureState&ECinemotusCaptureState::EAbsolute) == ECinemotusCaptureState::EAbsolute));
}
Example #2
0
void ALeapMotionHandActor::UpdateBones(float DeltaSeconds)
{
	if (BoneActors.Num() == 0) { return; }

	float CombinedScale = GetCombinedScale();

	FLeapMotionDevice* Device = FLeapMotionControllerPlugin::GetLeapDeviceSafe();
	if (Device && Device->IsConnected())
	{
		int BoneArrayIndex = 0;
		for (ELeapBone LeapBone = bShowArm ? ELeapBone::Forearm : ELeapBone::Palm; LeapBone <= ELeapBone::Finger4Tip; ((int8&)LeapBone)++)
		{
			FVector TargetPosition;
			FRotator TargetOrientation;

			bool Success = Device->GetBonePostionAndOrientation(HandId, LeapBone, TargetPosition, TargetOrientation);

			if (Success)
			{
				// Offset target position & rotation by the SpawnReference actor's transform
				FQuat RefQuat = GetRootComponent()->GetComponentRotation().Quaternion();
				TargetPosition = RefQuat * TargetPosition * CombinedScale + GetRootComponent()->GetComponentLocation();
				TargetOrientation = (RefQuat * TargetOrientation.Quaternion()).Rotator();

				// Get current position & rotation
				ALeapMotionBoneActor* BoneActor = BoneActors[BoneArrayIndex++];

				UPrimitiveComponent* PrimitiveComponent = Cast<UPrimitiveComponent>(BoneActor->GetRootComponent());
				if (PrimitiveComponent && PrimitiveComponent->IsSimulatingPhysics())
				{
					FVector CurrentPositon = PrimitiveComponent->GetComponentLocation();
					FRotator CurrentRotation = PrimitiveComponent->GetComponentRotation();

					// Compute linear velocity
					FVector LinearVelocity = (TargetPosition - CurrentPositon) / DeltaSeconds;

					// Compute angular velocity
					FVector Axis;
					float Angle;
					ConvertDeltaRotationsToAxisAngle(CurrentRotation, TargetOrientation, Axis, Angle);
					if (Angle > PI) { Angle -= 2 * PI; }
					FVector AngularVelcity = Axis * (Angle / DeltaSeconds);

					// Apply velocities
					PrimitiveComponent->SetPhysicsLinearVelocity(LinearVelocity);
					PrimitiveComponent->SetAllPhysicsAngularVelocity(AngularVelcity * 180.0f / PI);
				}
			}
		}
	}
}
Example #3
0
void ACullDistanceVolume::GetPrimitiveMaxDrawDistances(TMap<UPrimitiveComponent*,float>& OutCullDistances)
{
	// Nothing to do if there is no brush component or no cull distances are set
	if (GetBrushComponent() && CullDistances.Num() > 0 && bEnabled)
	{
		for (auto It(OutCullDistances.CreateIterator()); It; ++It)
		{
			UPrimitiveComponent* PrimitiveComponent = It.Key();

			// Check whether primitive can be affected by cull distance volumes.
			if( ACullDistanceVolume::CanBeAffectedByVolumes( PrimitiveComponent ) )
			{
				// Check whether primitive supports cull distance volumes and its center point is being encompassed by this volume.
				if( EncompassesPoint( PrimitiveComponent->GetComponentLocation() ) )
				{		
					// Find best match in CullDistances array.
					float PrimitiveSize			= PrimitiveComponent->Bounds.SphereRadius * 2;
					float CurrentError			= FLT_MAX;
					float CurrentCullDistance	= 0;
					for( int32 CullDistanceIndex=0; CullDistanceIndex<CullDistances.Num(); CullDistanceIndex++ )
					{
						const FCullDistanceSizePair& CullDistancePair = CullDistances[CullDistanceIndex];
						if( FMath::Abs( PrimitiveSize - CullDistancePair.Size ) < CurrentError )
						{
							CurrentError		= FMath::Abs( PrimitiveSize - CullDistancePair.Size );
							CurrentCullDistance = CullDistancePair.CullDistance;
						}
					}

					float& CullDistance = It.Value();

					// LD or other volume specified cull distance, use minimum of current and one used for this volume.
					if (CullDistance > 0)
					{
						CullDistance = FMath::Min(CullDistance, CurrentCullDistance);
					}
					// LD didn't specify cull distance, use current setting directly.
					else
					{
						CullDistance = CurrentCullDistance;
					}
				}
			}
		}
	}
}
Example #4
0
bool ACharacter::RestoreReplicatedMove(const FSimulatedRootMotionReplicatedMove & RootMotionRepMove)
{
	UPrimitiveComponent * ServerBase = RootMotionRepMove.RootMotion.MovementBase;

	// Relative Position
	if( RootMotionRepMove.RootMotion.bRelativePosition )
	{
		bool bSuccess = false;
		if( MovementBaseUtility::UseRelativePosition(ServerBase) )
		{
			const FVector ServerLocation = ServerBase->GetComponentLocation() + RootMotionRepMove.RootMotion.Location;
			const FRotator ServerRotation = (FRotationMatrix(RootMotionRepMove.RootMotion.Rotation) * FRotationMatrix(ServerBase->GetComponentRotation())).Rotator();
			UpdateSimulatedPosition(ServerLocation, ServerRotation);
			bSuccess = true;
		}
		// If we received local space position, but can't resolve parent, then move can't be used. :(
		if( !bSuccess )
		{
			return false;
		}
	}
	// Absolute position
	else
	{
		UpdateSimulatedPosition(RootMotionRepMove.RootMotion.Location, RootMotionRepMove.RootMotion.Rotation);
	}

	// Set base
	if( MovementBase != ServerBase )
	{
		SetBase( ServerBase );
	}

	// fixme laurent is this needed?
	if( CharacterMovement )
	{
		CharacterMovement->bJustTeleported = false;
	}
	return true;
}
void ACinemotusPlayerController::RelativeTick(float DeltaTime)
{

	UPrimitiveComponent* prim = GetPawn()->GetMovementComponent()->UpdatedComponent;
	bool SetPrimDirectly = true;
	FQuat finalQuat;
	if ((currentCaptureState & ECinemotusCaptureState::ERelativeRotation) == ECinemotusCaptureState::ERelativeRotation)
	{
		FRotator rot = HydraLatestData->controllers[CAM_HAND].angular_velocity;
		const FQuat OldRotation = prim->GetComponentQuat();//GetControlRotation().Quaternion(); //TODO: hold onto a quaternion potentially
		const FRotator OldRotationRotator = OldRotation.Rotator();
		FRotator worldRotator = FRotator(0, DeadZone(rot.Yaw*DeltaTime, 0.0) + addYaw, 0);
		FRotator worldRotator1 = FRotator(DeadZone(rot.Pitch*DeltaTime, 0.0), 0, 0);
		FRotator localRotator = FRotator(0, 0, DeadZone(rot.Roll*DeltaTime, 0.0));
		const FQuat WorldRot = worldRotator.Quaternion();
		const FQuat pitchRot = worldRotator1.Quaternion();
		const FQuat LocalRot = localRotator.Quaternion();

		////This one does roll around local forward, pitch around world right flattened and yaw around world up
		////			FQuat finalQuat = pitchRot*WorldRot*((OldRotation*LocalRot));

		finalQuat = WorldRot*((OldRotation*LocalRot)*pitchRot);
	}
	else
	{
		finalQuat = FRotator(0, addYaw, 0).Quaternion()*prim->GetComponentQuat();
	}

	SetControlRotation(finalQuat.Rotator());
	if (SetPrimDirectly && prim)
	{
		prim->SetWorldLocationAndRotation(prim->GetComponentLocation(), finalQuat);// not sure need
	}



	HandleMovementAbs(DeltaTime, (currentCaptureState & ECinemotusCaptureState::ERelativeTranslation) == ECinemotusCaptureState::ERelativeTranslation);

}
void UBuoyancyForceComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction)
{
	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);

	// If disabled or we are not attached to a parent component, return.
	if (!bIsActive || !GetAttachParent()) return;

	if (!OceanManager) return;

	UPrimitiveComponent* BasePrimComp = Cast<UPrimitiveComponent>(GetAttachParent());
	if (!BasePrimComp) return;

	if (!BasePrimComp->IsSimulatingPhysics())
	{
		if (!SnapToSurfaceIfNoPhysics) return;

		UE_LOG(LogTemp, Warning, TEXT("Running in no physics mode.."));

		float waveHeight = OceanManager->GetWaveHeightValue(BasePrimComp->GetComponentLocation(), World, true, TwoGerstnerIterations).Z;
		BasePrimComp->SetWorldLocation(FVector(BasePrimComp->GetComponentLocation().X, BasePrimComp->GetComponentLocation().Y, waveHeight));
		return;
	}

	//Get gravity
	float Gravity = BasePrimComp->GetPhysicsVolume()->GetGravityZ();

	//--------------- If Skeletal ---------------
	USkeletalMeshComponent* SkeletalComp = Cast<USkeletalMeshComponent>(GetAttachParent());
	if (SkeletalComp && ApplyForceToBones)
	{
		TArray<FName> BoneNames;
		SkeletalComp->GetBoneNames(BoneNames);

		for (int32 Itr = 0; Itr < BoneNames.Num(); Itr++)
		{
			FBodyInstance* BI = SkeletalComp->GetBodyInstance(BoneNames[Itr], false);
			if (BI && BI->IsValidBodyInstance()
				&& BI->bEnableGravity) //Buoyancy doesn't exist without gravity
			{
				bool isUnderwater = false;
				//FVector worldBoneLoc = SkeletalComp->GetBoneLocation(BoneNames[Itr]);
				FVector worldBoneLoc = BI->GetCOMPosition(); //Use center of mass of the bone's physics body instead of bone's location
				FVector waveHeight = OceanManager->GetWaveHeightValue(worldBoneLoc, World, true, TwoGerstnerIterations);

				float BoneDensity = MeshDensity;
				float BoneTestRadius = FMath::Abs(TestPointRadius);
				float SignedBoneRadius = FMath::Sign(Gravity) * TestPointRadius; //Direction of radius (test radius is actually a Z offset, should probably rename it!). Just in case we need an upside down world.

				//Get density & radius from the override array, if available.
				for (int pointIndex = 0; pointIndex < BoneOverride.Num(); pointIndex++)
				{
					FStructBoneOverride Override = BoneOverride[pointIndex];

					if (Override.BoneName.IsEqual(BoneNames[Itr]))
					{
						BoneDensity = Override.Density;
						BoneTestRadius = FMath::Abs(Override.TestRadius);
						SignedBoneRadius = FMath::Sign(Gravity) * BoneTestRadius;
					}
				}

				//If test point radius is below water surface, add buoyancy force.
				if (waveHeight.Z > (worldBoneLoc.Z + SignedBoneRadius))
				{
					isUnderwater = true;

					float DepthMultiplier = (waveHeight.Z - (worldBoneLoc.Z + SignedBoneRadius)) / (BoneTestRadius * 2);
					DepthMultiplier = FMath::Clamp(DepthMultiplier, 0.f, 1.f);

					float Mass = SkeletalComp->CalculateMass(BoneNames[Itr]); //Mass of this specific bone's physics body

					 /**
					* --------
					* Buoyancy force formula: (Volume(Mass / Density) * Fluid Density * -Gravity) / Total Points * Depth Multiplier
					* --------
					*/
					float BuoyancyForceZ = Mass / BoneDensity * FluidDensity * -Gravity * DepthMultiplier;

					//Velocity damping.
					FVector DampingForce = -BI->GetUnrealWorldVelocity() * VelocityDamper * Mass * DepthMultiplier;

					//Experimental xy wave force
					if (EnableWaveForces)
					{
						float waveVelocity = FMath::Clamp(BI->GetUnrealWorldVelocity().Z, -20.f, 150.f) * (1 - DepthMultiplier);
						DampingForce += FVector(OceanManager->GlobalWaveDirection.X, OceanManager->GlobalWaveDirection.Y, 0) * Mass * waveVelocity * WaveForceMultiplier;
					}

					//Add force to this bone
					BI->AddForce(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ));
					//BasePrimComp->AddForceAtLocation(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ), worldBoneLoc, BoneNames[Itr]);
				}

				//Apply fluid damping & clamp velocity
				if (isUnderwater)
				{
					BI->SetLinearVelocity(-BI->GetUnrealWorldVelocity() * (FluidLinearDamping / 10), true);
					BI->SetAngularVelocity(-BI->GetUnrealWorldAngularVelocity() * (FluidAngularDamping / 10), true);

					//Clamp the velocity to MaxUnderwaterVelocity
					if (ClampMaxVelocity && BI->GetUnrealWorldVelocity().Size() > MaxUnderwaterVelocity)
					{
						FVector	Velocity = BI->GetUnrealWorldVelocity().GetSafeNormal() * MaxUnderwaterVelocity;
						BI->SetLinearVelocity(Velocity, false);
					}
				}

				if (DrawDebugPoints)
				{
					FColor DebugColor = FLinearColor(0.8, 0.7, 0.2, 0.8).ToRGBE();
					if (isUnderwater) { DebugColor = FLinearColor(0, 0.2, 0.7, 0.8).ToRGBE(); } //Blue color underwater, yellow out of watter
					DrawDebugSphere(World, worldBoneLoc, BoneTestRadius, 8, DebugColor);
				}
			}
		}
		return;
	}
	//--------------------------------------------------------

	float TotalPoints = TestPoints.Num();
	if (TotalPoints < 1) return;

	int PointsUnderWater = 0;
	for (int pointIndex = 0; pointIndex < TotalPoints; pointIndex++)
	{
		if (!TestPoints.IsValidIndex(pointIndex)) return; //Array size changed during runtime

		bool isUnderwater = false;
		FVector testPoint = TestPoints[pointIndex];
		FVector worldTestPoint = BasePrimComp->GetComponentTransform().TransformPosition(testPoint);
		FVector waveHeight = OceanManager->GetWaveHeightValue(worldTestPoint, World, !EnableWaveForces, TwoGerstnerIterations);

		//Direction of radius (test radius is actually a Z offset, should probably rename it!). Just in case we need an upside down world.
		float SignedRadius = FMath::Sign(BasePrimComp->GetPhysicsVolume()->GetGravityZ()) * TestPointRadius;

		//If test point radius is below water surface, add buoyancy force.
		if (waveHeight.Z > (worldTestPoint.Z + SignedRadius)
			&& BasePrimComp->IsGravityEnabled()) //Buoyancy doesn't exist without gravity
		{
			PointsUnderWater++;
			isUnderwater = true;

			float DepthMultiplier = (waveHeight.Z - (worldTestPoint.Z + SignedRadius)) / (TestPointRadius * 2);
			DepthMultiplier = FMath::Clamp(DepthMultiplier, 0.f, 1.f);

			//If we have a point density override, use the overridden value instead of MeshDensity
			float PointDensity = PointDensityOverride.IsValidIndex(pointIndex) ? PointDensityOverride[pointIndex] : MeshDensity;

			/**
			* --------
			* Buoyancy force formula: (Volume(Mass / Density) * Fluid Density * -Gravity) / Total Points * Depth Multiplier
			* --------
			*/
			float BuoyancyForceZ = BasePrimComp->GetMass() / PointDensity * FluidDensity * -Gravity / TotalPoints * DepthMultiplier;

			//Experimental velocity damping using VelocityAtPoint.
			FVector DampingForce = -GetUnrealVelocityAtPoint(BasePrimComp, worldTestPoint) * VelocityDamper * BasePrimComp->GetMass() * DepthMultiplier;

			//Experimental xy wave force
			if (EnableWaveForces)
			{
				DampingForce += BasePrimComp->GetMass() * FVector2D(waveHeight.X, waveHeight.Y).Size() * FVector(OceanManager->GlobalWaveDirection.X, OceanManager->GlobalWaveDirection.Y, 0) * WaveForceMultiplier / TotalPoints;
				//float waveVelocity = FMath::Clamp(GetUnrealVelocityAtPoint(BasePrimComp, worldTestPoint).Z, -20.f, 150.f) * (1 - DepthMultiplier);
				//DampingForce += OceanManager->GlobalWaveDirection * BasePrimComp->GetMass() * waveVelocity * WaveForceMultiplier / TotalPoints;
			}

			//Add force for this test point
			BasePrimComp->AddForceAtLocation(FVector(DampingForce.X, DampingForce.Y, DampingForce.Z + BuoyancyForceZ), worldTestPoint);
		}

		if (DrawDebugPoints)
		{
			FColor DebugColor = FLinearColor(0.8, 0.7, 0.2, 0.8).ToRGBE();
			if (isUnderwater) { DebugColor = FLinearColor(0, 0.2, 0.7, 0.8).ToRGBE(); } //Blue color underwater, yellow out of watter
			DrawDebugSphere(World, worldTestPoint, TestPointRadius, 8, DebugColor);
		}
	}

	//Clamp the velocity to MaxUnderwaterVelocity if there is any point underwater
	if (ClampMaxVelocity && PointsUnderWater > 0
		&& BasePrimComp->GetPhysicsLinearVelocity().Size() > MaxUnderwaterVelocity)
	{
		FVector	Velocity = BasePrimComp->GetPhysicsLinearVelocity().GetSafeNormal() * MaxUnderwaterVelocity;
		BasePrimComp->SetPhysicsLinearVelocity(Velocity);
	}

	//Update damping based on number of underwater test points
	BasePrimComp->SetLinearDamping(_baseLinearDamping + FluidLinearDamping / TotalPoints * PointsUnderWater);
	BasePrimComp->SetAngularDamping(_baseAngularDamping + FluidAngularDamping / TotalPoints * PointsUnderWater);
}
void UGripMotionControllerComponent::TickGrip()
{
	if (GrippedActors.Num())
	{
		FTransform WorldTransform;
		FTransform InverseTransform = this->GetComponentTransform().Inverse();

		for (int i = GrippedActors.Num() - 1; i >= 0; --i)
		{
			if (GrippedActors[i].Actor || GrippedActors[i].Component)
			{
				// GetRelativeTransformReverse had some serious f*****g floating point errors associated with it that was f*****g everything up
				// Not sure whats wrong with the function but I might want to push a patch out eventually
				WorldTransform = GrippedActors[i].RelativeTransform.GetRelativeTransform(InverseTransform);

				UPrimitiveComponent *root = GrippedActors[i].Component;
				AActor *actor = GrippedActors[i].Actor;

				if (!root)
					root = Cast<UPrimitiveComponent>(GrippedActors[i].Actor->GetRootComponent());

				if (!root)
					continue;

				if (!actor)
					actor = root->GetOwner();

				if (!actor)
					continue;

				if (bIsServer)
				{
					// Handle collision intersection detection, this is used to intelligently manage some of the networking and late update features.
					switch (GrippedActors[i].GripCollisionType.GetValue())
					{
					case EGripCollisionType::InteractiveCollisionWithPhysics:
					case EGripCollisionType::InteractiveHybridCollisionWithSweep:
					{
						TArray<FOverlapResult> Hits;
						FComponentQueryParams Params(NAME_None, this->GetOwner());
						Params.bTraceAsyncScene = root->bCheckAsyncSceneOnMove;
						Params.AddIgnoredActor(actor);
						Params.AddIgnoredActors(root->MoveIgnoreActors);

						if(GetWorld()->ComponentOverlapMulti(Hits, root, root->GetComponentLocation(), root->GetComponentQuat(), Params))
						{
							GrippedActors[i].bColliding = true;
						}
						else
						{
							GrippedActors[i].bColliding = false;
						}
					}

					// Skip collision intersects with these types, they dont need it
					case EGripCollisionType::PhysicsOnly:
					case EGripCollisionType::SweepWithPhysics:
					case EGripCollisionType::InteractiveCollisionWithSweep:
					default:break;
					}
				}

				// Need to figure out best default behavior
				/*if (GrippedActors[i].bHasSecondaryAttachment && GrippedActors[i].SecondaryAttachment)
				{
				WorldTransform.SetRotation((WorldTransform.GetLocation() - GrippedActors[i].SecondaryAttachment->GetComponentLocation()).ToOrientationRotator().Quaternion());
				}*/

				if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveCollisionWithPhysics)
				{
					UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveCollisionWithSweep)
				{
					if (bIsServer)
					{
						FHitResult OutHit;

						// Need to use without teleport so that the physics velocity is updated for when the actor is released to throw
						root->SetWorldTransform(WorldTransform, true, &OutHit);

						if (OutHit.bBlockingHit)
						{
							GrippedActors[i].bColliding = true;

							if (!actor->bReplicateMovement)
								actor->SetReplicateMovement(true);
						}
						else
						{
							GrippedActors[i].bColliding = false;

							if (actor->bReplicateMovement)
								actor->SetReplicateMovement(false);
						}
					}
					else
					{
						if (!GrippedActors[i].bColliding)
						{
							root->SetWorldTransform(WorldTransform, true);
						}
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::InteractiveHybridCollisionWithSweep)
				{
					// Need to use without teleport so that the physics velocity is updated for when the actor is released to throw
					//if (bIsServer)
					//{
					FBPActorPhysicsHandleInformation * GripHandle = GetPhysicsGrip(GrippedActors[i]);
					if (!GrippedActors[i].bColliding)
					{
						// Make sure that there is no collision on course before turning off collision and snapping to controller
						if (bIsServer && actor && actor->bReplicateMovement)
						{
							FCollisionQueryParams QueryParams(NAME_None, false, this->GetOwner());
							FCollisionResponseParams ResponseParam;
							root->InitSweepCollisionParams(QueryParams, ResponseParam);
							QueryParams.AddIgnoredActor(actor);
							QueryParams.AddIgnoredActors(root->MoveIgnoreActors);

							if (GetWorld()->SweepTestByChannel(root->GetComponentLocation(), WorldTransform.GetLocation(), root->GetComponentQuat(), ECollisionChannel::ECC_Visibility, root->GetCollisionShape(), QueryParams, ResponseParam))
							{
								GrippedActors[i].bColliding = true;
							}
							else
							{
								GrippedActors[i].bColliding = false;
							}
						}
						
						// If still not colliding
						if (!GrippedActors[i].bColliding)
						{
							if (bIsServer && actor->bReplicateMovement) // So we don't call on on change event over and over locally
								actor->SetReplicateMovement(false);

							if (bIsServer && GripHandle)
							{
								DestroyPhysicsHandle(GrippedActors[i]);

								if(GrippedActors[i].Actor)
									GrippedActors[i].Actor->DisableComponentsSimulatePhysics();
								else
									root->SetSimulatePhysics(false);
							}

							root->SetWorldTransform(WorldTransform, false);
						}
						else
						{
							if (bIsServer && GrippedActors[i].bColliding && GripHandle)
								UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
						}

					}
					else if (GrippedActors[i].bColliding && !GripHandle)
					{
						if (bIsServer && !actor->bReplicateMovement)
							actor->SetReplicateMovement(true);

						root->SetSimulatePhysics(true);

						if (bIsServer)
							SetUpPhysicsHandle(GrippedActors[i]);
					}
					else
					{
						if (bIsServer && GrippedActors[i].bColliding && GripHandle)
							UpdatePhysicsHandleTransform(GrippedActors[i], WorldTransform);
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::SweepWithPhysics)
				{
					if (bIsServer)
					{
						
						FVector OriginalPosition(root->GetComponentLocation());
						FRotator OriginalOrientation(root->GetComponentRotation());

						FVector NewPosition(WorldTransform.GetTranslation());
						FRotator NewOrientation(WorldTransform.GetRotation());

						// Now sweep collision separately so we can get hits but not have the location altered
						if (bUseWithoutTracking || NewPosition != OriginalPosition || NewOrientation != OriginalOrientation)
						{
							FVector move = NewPosition - OriginalPosition;

							// ComponentSweepMulti does nothing if moving < KINDA_SMALL_NUMBER in distance, so it's important to not try to sweep distances smaller than that. 
							const float MinMovementDistSq = (FMath::Square(4.f*KINDA_SMALL_NUMBER));

							if (bUseWithoutTracking || move.SizeSquared() > MinMovementDistSq || NewOrientation != OriginalOrientation)
							{
								if (CheckComponentWithSweep(root, move, NewOrientation, false))
								{

								}
							}
						}

						// Move the actor, we are not offsetting by the hit result anyway
						root->SetWorldTransform(WorldTransform, false);
					}
					else
					{
						// Move the actor, we are not offsetting by the hit result anyway
						root->SetWorldTransform(WorldTransform, false);
					}
				}
				else if (GrippedActors[i].GripCollisionType == EGripCollisionType::PhysicsOnly)
				{
					// Move the actor, we are not offsetting by the hit result anyway
					root->SetWorldTransform(WorldTransform, false);
				}
			}
			else
			{
				if (bIsServer)
				{
					DestroyPhysicsHandle(GrippedActors[i]);

					GrippedActors.RemoveAt(i); // If it got garbage collected then just remove the pointer, won't happen with new uproperty use, but keeping it here anyway
				}
			}

		}
	}

}
bool UGripMotionControllerComponent::CheckComponentWithSweep(UPrimitiveComponent * ComponentToCheck, FVector Move, FRotator newOrientation, bool bSkipSimulatingComponents/*,  bool &bHadBlockingHitOut*/)
{
	TArray<FHitResult> Hits;
	// WARNING: HitResult is only partially initialized in some paths. All data is valid only if bFilledHitResult is true.
	FHitResult BlockingHit(NoInit);
	BlockingHit.bBlockingHit = false;
	BlockingHit.Time = 1.f;
	bool bFilledHitResult = false;
	bool bMoved = false;
	bool bIncludesOverlapsAtEnd = false;
	bool bRotationOnly = false;

	UPrimitiveComponent *root = ComponentToCheck;

	if (!root || !root->IsQueryCollisionEnabled())
		return false;

	FVector start(root->GetComponentLocation());

	const bool bCollisionEnabled = root->IsQueryCollisionEnabled();

	if (bCollisionEnabled)
	{
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
		if (!root->IsRegistered())
		{
			UE_LOG(LogTemp, Warning, TEXT("MovedComponent %s not initialized in grip motion controller"), *root->GetFullName());
		}
#endif

		UWorld* const MyWorld = GetWorld();
		FComponentQueryParams Params(TEXT("sweep_params"), root->GetOwner());

		FCollisionResponseParams ResponseParam;
		root->InitSweepCollisionParams(Params, ResponseParam);

		bool const bHadBlockingHit = MyWorld->ComponentSweepMulti(Hits, root, start, start + Move, newOrientation, Params);

		if (bHadBlockingHit)
		{
			int32 BlockingHitIndex = INDEX_NONE;
			float BlockingHitNormalDotDelta = BIG_NUMBER;
			for (int32 HitIdx = 0; HitIdx < Hits.Num(); HitIdx++)
			{
				const FHitResult& TestHit = Hits[HitIdx];

				// Ignore the owning actor to the motion controller
				if (TestHit.Actor == this->GetOwner() || (bSkipSimulatingComponents && TestHit.Component->IsSimulatingPhysics()))
				{
					if (Hits.Num() == 1)
					{
						//bHadBlockingHitOut = false;
						return false;
					}
					else
						continue;
				}

				if (TestHit.bBlockingHit && TestHit.IsValidBlockingHit())
				{
					if (TestHit.Time == 0.f)
					{
						// We may have multiple initial hits, and want to choose the one with the normal most opposed to our movement.
						const float NormalDotDelta = (TestHit.ImpactNormal | Move);
						if (NormalDotDelta < BlockingHitNormalDotDelta)
						{
							BlockingHitNormalDotDelta = NormalDotDelta;
							BlockingHitIndex = HitIdx;
						}
					}
					else if (BlockingHitIndex == INDEX_NONE)
					{
						// First non-overlapping blocking hit should be used, if an overlapping hit was not.
						// This should be the only non-overlapping blocking hit, and last in the results.
						BlockingHitIndex = HitIdx;
						break;
					}
					//}
				}
			}

			// Update blocking hit, if there was a valid one.
			if (BlockingHitIndex >= 0)
			{
				BlockingHit = Hits[BlockingHitIndex];
				bFilledHitResult = true;
			}
		}
	}

	// Handle blocking hit notifications. Avoid if pending kill (which could happen after overlaps).
	if (BlockingHit.bBlockingHit && !root->IsPendingKill())
	{
		check(bFilledHitResult);
		if (root->IsDeferringMovementUpdates())
		{
			FScopedMovementUpdate* ScopedUpdate = root->GetCurrentScopedMovement();
			ScopedUpdate->AppendBlockingHitAfterMove(BlockingHit);
		}
		else
		{
			if(root->GetOwner())
				root->DispatchBlockingHit(*root->GetOwner(), BlockingHit);
		}

		return true;
	}

	return false;
}
bool UGripMotionControllerComponent::SetUpPhysicsHandle(const FBPActorGripInformation &NewGrip)
{
	UPrimitiveComponent *root = NewGrip.Component;
	if(!root)
		root = Cast<UPrimitiveComponent>(NewGrip.Actor->GetRootComponent());
	
	if (!root)
		return false;

	// Needs to be simulating in order to run physics
	root->SetSimulatePhysics(true);
	root->SetEnableGravity(false);

	FBPActorPhysicsHandleInformation * HandleInfo = CreatePhysicsGrip(NewGrip);

#if WITH_PHYSX
	// Get the PxRigidDynamic that we want to grab.
	FBodyInstance* BodyInstance = root->GetBodyInstance(NAME_None/*InBoneName*/);
	if (!BodyInstance)
	{
		return false;
	}

	ExecuteOnPxRigidDynamicReadWrite(BodyInstance, [&](PxRigidDynamic* Actor)
	{
		PxScene* Scene = Actor->getScene();
	
		// Get transform of actor we are grabbing

		FTransform WorldTransform;
		FTransform InverseTransform = this->GetComponentTransform().Inverse();
		WorldTransform = NewGrip.RelativeTransform.GetRelativeTransform(InverseTransform);

		PxVec3 KinLocation = U2PVector(WorldTransform.GetLocation() - (WorldTransform.GetLocation() - root->GetComponentLocation()));
		PxTransform GrabbedActorPose = Actor->getGlobalPose();
		PxTransform KinPose(KinLocation, GrabbedActorPose.q);

		// set target and current, so we don't need another "Tick" call to have it right
		//TargetTransform = CurrentTransform = P2UTransform(KinPose);

		// If we don't already have a handle - make one now.
		if (!HandleInfo->HandleData)
		{
			// Create kinematic actor we are going to create joint with. This will be moved around with calls to SetLocation/SetRotation.
			PxRigidDynamic* KinActor = Scene->getPhysics().createRigidDynamic(KinPose);
			KinActor->setRigidDynamicFlag(PxRigidDynamicFlag::eKINEMATIC, true);
			KinActor->setMass(0.0f); // 1.0f;
			KinActor->setMassSpaceInertiaTensor(PxVec3(0.0f, 0.0f, 0.0f));// PxVec3(1.0f, 1.0f, 1.0f));
			KinActor->setMaxDepenetrationVelocity(PX_MAX_F32);

			// No bodyinstance
			KinActor->userData = NULL;
			
			// Add to Scene
			Scene->addActor(*KinActor);

			// Save reference to the kinematic actor.
			HandleInfo->KinActorData = KinActor;

			// Create the joint
			PxVec3 LocalHandlePos = GrabbedActorPose.transformInv(KinLocation);
			PxD6Joint* NewJoint = PxD6JointCreate(Scene->getPhysics(), KinActor, PxTransform::createIdentity(), Actor, PxTransform(LocalHandlePos));
			
			if (!NewJoint)
			{
				HandleInfo->HandleData = 0;
			}
			else
			{
				// No constraint instance
				NewJoint->userData = NULL;
				HandleInfo->HandleData = NewJoint;

				// Remember the scene index that the handle joint/actor are in.
				FPhysScene* RBScene = FPhysxUserData::Get<FPhysScene>(Scene->userData);
				const uint32 SceneType = root->BodyInstance.UseAsyncScene(RBScene) ? PST_Async : PST_Sync;
				HandleInfo->SceneIndex = RBScene->PhysXSceneIndex[SceneType];
				
				// Setting up the joint
				NewJoint->setMotion(PxD6Axis::eX, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eY, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eZ, PxD6Motion::eFREE);
				NewJoint->setDrivePosition(PxTransform(PxVec3(0, 0, 0)));

				NewJoint->setMotion(PxD6Axis::eTWIST, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING1, PxD6Motion::eFREE);
				NewJoint->setMotion(PxD6Axis::eSWING2, PxD6Motion::eFREE);

				//UpdateDriveSettings();
				if (HandleInfo->HandleData != nullptr)
				{
					HandleInfo->HandleData->setDrive(PxD6Drive::eX, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eY, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
					HandleInfo->HandleData->setDrive(PxD6Drive::eZ, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

					HandleInfo->HandleData->setDrive(PxD6Drive::eSLERP, PxD6JointDrive(NewGrip.Stiffness, NewGrip.Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));

						//HandleData->setDrive(PxD6Drive::eTWIST, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
						//HandleData->setDrive(PxD6Drive::eSWING, PxD6JointDrive(Stiffness, Damping, PX_MAX_F32, PxD6JointDriveFlag::eACCELERATION));
				}
			}
		}
	});
#else
	return false;
#endif // WITH_PHYSX

	return true;
}