diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp index cdf29ca8c..ee6adc702 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -23,6 +23,8 @@ #include "Rendering/SkeletalMeshRenderData.h" #include "Engine/SkeletalMeshSocket.h" +#include "Kismet/KismetMathLibrary.h" +#include "Kismet/KismetSystemLibrary.h" namespace crp = carla::rpc; @@ -30,10 +32,11 @@ static FBoundingBox ApplyTransformToBB( FBoundingBox InBB, const FTransform& Transform) { - auto Scale = Transform.GetScale3D(); + const auto Scale = Transform.GetScale3D(); + const auto TransformRotation = Transform.GetRotation().Rotator(); InBB.Origin *= Scale; - InBB.Rotation = Transform.GetRotation().Rotator(); - InBB.Origin = InBB.Rotation.RotateVector(InBB.Origin) + Transform.GetLocation(); + InBB.Rotation = UKismetMathLibrary::ComposeRotators(TransformRotation, InBB.Rotation); + InBB.Origin = TransformRotation.RotateVector(InBB.Origin) + Transform.GetLocation(); InBB.Extent *= Scale; return InBB; } @@ -183,6 +186,14 @@ FBoundingBox UBoundingBoxCalculator::GetVehicleBoundingBox( } } + // Calculate bounding boxes of the doors. + FBoundingBox DoorsBB = GetVehicleDoorsBoundingBox(Vehicle); + if(DoorsBB.Extent != FVector::ZeroVector) + { + // Combine doors Bounding Box with the vehicle BB. + BB = CombineBBs({DoorsBB, BB}); + } + // Component-to-world transform for this component auto& CompToWorldTransform = ParentComp->GetComponentTransform(); BB = ApplyTransformToBB(BB, CompToWorldTransform); @@ -445,7 +456,7 @@ TArray UBoundingBoxCalculator::GetBBsOfActor( { Result.Add(BoundingBox); } - return Result;; + return Result; } // Pedestrians, we just use the capsule component at the moment. @@ -645,3 +656,34 @@ void UBoundingBoxCalculator::GetMeshCompsFromActorBoundingBox( } } } + +FBoundingBox UBoundingBoxCalculator::GetVehicleDoorsBoundingBox(const ACarlaWheeledVehicle* Vehicle) +{ + FBoundingBox DoorsBB; + if(Vehicle && Vehicle->GetConstraintsComponents().Num() > 0) + { + FBox DoorsBox(ForceInit); + const FTransform& ActorToWorld = Vehicle->GetActorTransform(); + const FTransform WorldToActor = ActorToWorld.Inverse(); + + // Iterates over all doors of the vehicle + for(const UPhysicsConstraintComponent* ConstraintComp : Vehicle->GetConstraintsComponents()) + { + const UPrimitiveComponent* DoorComponent = Vehicle->GetConstraintDoor()[ConstraintComp]; + if(const UStaticMeshComponent* StaticMeshComp = Cast(DoorComponent)) + { + const FTransform ComponentToActor = StaticMeshComp->GetComponentTransform() * WorldToActor; + DoorsBox += StaticMeshComp->CalcBounds(ComponentToActor).GetBox(); + } + } + + if(DoorsBox.IsValid) + { + // DoorBB is aligned with the Vehicle orientation, for this reason it is not necessary to assign rotation. + DoorsBB.Origin = DoorsBox.GetCenter(); + DoorsBB.Extent = DoorsBox.GetExtent(); + } + } + + return DoorsBB; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h index 6d0282905..2a6ac329d 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h @@ -110,4 +110,7 @@ public: const FBoundingBox& InBB, TArray& OutStaticMeshComps); + // Return the combined BB of all doors of the vehicle, with the same orientation. + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static FBoundingBox GetVehicleDoorsBoundingBox(const ACarlaWheeledVehicle* Vehicle); }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index 8ba705acd..c9f3aadd6 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp @@ -1021,6 +1021,15 @@ void ACarlaWheeledVehicle::OpenDoorPhys(const EVehicleDoor DoorIdx) } RecordDoorChange(DoorIdx, true); + + // Wait until door is max opened to recalculate its bounds. + float TimeNeededToHaveItOpened = (AngleLimit + Constraint->ConstraintInstance.AngularRotationOffset.Yaw) / (DoorOpenStrength > 0.f ? DoorOpenStrength : 1.f); + TimeNeededToHaveItOpened = TimeNeededToHaveItOpened < 0.f ? TimeNeededToHaveItOpened * -1.f : TimeNeededToHaveItOpened; + FTimerHandle DoorMaxOpenRangeTimerHandle; + GetWorldTimerManager().SetTimer(DoorMaxOpenRangeTimerHandle, [this]() + { + AdjustVehicleBounds(); + }, TimeNeededToHaveItOpened, false); } void ACarlaWheeledVehicle::CloseDoorPhys(const EVehicleDoor DoorIdx) @@ -1035,6 +1044,8 @@ void ACarlaWheeledVehicle::CloseDoorPhys(const EVehicleDoor DoorIdx) DoorComponent->AttachToComponent( GetMesh(), FAttachmentTransformRules(EAttachmentRule::KeepWorld, true)); RecordDoorChange(DoorIdx, false); + + AdjustVehicleBounds(); } void ACarlaWheeledVehicle::RecordDoorChange(const EVehicleDoor DoorIdx, bool bIsOpen) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index 0f5b4fea8..259728dd7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h @@ -428,6 +428,10 @@ public: UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable) static void SetPhysicsConstraintAngle( UPhysicsConstraintComponent*Component, const FRotator &NewAngle); + + const TArray& GetConstraintsComponents() const { return ConstraintsComponents; } + + const TMap& GetConstraintDoor() const {return ConstraintDoor; } private: