diff --git a/CHANGELOG.md b/CHANGELOG.md index 138f015a1..23804ad31 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,7 @@ * Fixed a bug that caused navigation information not to be loaded when switching maps * 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. * Added functions to get actor' components transform * Added posibility to Digital Twins to work with local files (osm and xodr) * Enable proper material merging for Building in Digital Twins diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp index cdf29ca8c..a118e59f8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -23,6 +23,7 @@ #include "Rendering/SkeletalMeshRenderData.h" #include "Engine/SkeletalMeshSocket.h" +#include "Kismet/KismetMathLibrary.h" namespace crp = carla::rpc; @@ -30,10 +31,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 +185,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 +455,7 @@ TArray UBoundingBoxCalculator::GetBBsOfActor( { Result.Add(BoundingBox); } - return Result;; + return Result; } // Pedestrians, we just use the capsule component at the moment. @@ -645,3 +655,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 dbcc64449..98c9f1e5b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp @@ -1085,6 +1085,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) @@ -1099,6 +1108,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 ae10be15f..aca82599f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h @@ -432,6 +432,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: