Merge 1ec58b4cf5 into 1ef3f55c95

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.
  * 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

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<FBoundingBox> 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<UStaticMeshComponent>(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;
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h

@@ -110,4 +110,7 @@ public:
     const FBoundingBox& InBB,
     TArray<UStaticMeshComponent*>& 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);
 };

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)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h

@@ -433,6 +433,10 @@ public:
   static void SetPhysicsConstraintAngle(
       UPhysicsConstraintComponent*Component, const FRotator &NewAngle);
   
+  const TArray<UPhysicsConstraintComponent*>& GetConstraintsComponents() const { return ConstraintsComponents; }
+
+  const TMap<UPhysicsConstraintComponent*, UPrimitiveComponent*>& GetConstraintDoor() const {return ConstraintDoor; }
+ 
 private:
 
   UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere)