From 5488f1bea60d2bffaa2fc0ff9dc1d6d5589a628d Mon Sep 17 00:00:00 2001 From: JoseMartinez <44341347+JoseM98@users.noreply.github.com> Date: Thu, 8 Feb 2024 10:12:38 +0100 Subject: [PATCH 1/3] Adjusted BB when doors are oppened. --- .../Carla/Util/BoundingBoxCalculator.cpp | 50 +++++++++++++++++-- .../Source/Carla/Util/BoundingBoxCalculator.h | 3 ++ .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 11 ++++ .../Carla/Vehicle/CarlaWheeledVehicle.h | 4 ++ 4 files changed, 64 insertions(+), 4 deletions(-) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp index cdf29ca8cd9..ee6adc7024d 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 6d0282905ad..2a6ac329d20 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 @@ class CARLA_API UBoundingBoxCalculator : public UBlueprintFunctionLibrary 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 8ba705acd5e..c9f3aadd642 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 0f5b4fea81d..259728dd778 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 @@ class CARLA_API ACarlaWheeledVehicle : public AWheeledVehicle 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: From 914007b2ce3013078763319f3dfc19139221d687 Mon Sep 17 00:00:00 2001 From: JoseMartinez <44341347+JoseM98@users.noreply.github.com> Date: Thu, 8 Feb 2024 10:17:56 +0100 Subject: [PATCH 2/3] Updating changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a361dd37a73..5832680450e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,7 @@ ## Latest Changes * Prevent from segfault on failing SignalReference identification when loading OpenDrive files * Added vehicle doors to the recorder + * Adjusted vehicle BoundingBox when the vehicle opens the doors. ## CARLA 0.9.15 From 19227624d7e0e28529b175d8891865f0ed50b61e Mon Sep 17 00:00:00 2001 From: JoseMartinez <44341347+JoseM98@users.noreply.github.com> Date: Thu, 8 Feb 2024 13:14:05 +0100 Subject: [PATCH 3/3] Removed KistmetSystemLibrary include. --- .../Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp index ee6adc7024d..a118e59f8d1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -24,7 +24,6 @@ #include "Rendering/SkeletalMeshRenderData.h" #include "Engine/SkeletalMeshSocket.h" #include "Kismet/KismetMathLibrary.h" -#include "Kismet/KismetSystemLibrary.h" namespace crp = carla::rpc;