From b5d6eb264b44f78d0e6610b8911d1f3c703cd1cd Mon Sep 17 00:00:00 2001
From: Axel <axellopez92@outlook.com>
Date: Wed, 29 Sep 2021 17:09:35 +0200
Subject: [PATCH] Fixed constraints breaking on spawn.

---
 .../Carla/Vehicle/CarlaWheeledVehicle.cpp     | 39 ++++++++++++++++---
 .../Carla/Vehicle/CarlaWheeledVehicle.h       | 10 ++++-
 2 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
index 26f12eb7a..5d6f152ae 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp
@@ -84,6 +84,15 @@ void ACarlaWheeledVehicle::BeginPlay()
   UDefaultMovementComponent::CreateDefaultMovementComponent(this);
 
   // Get constraint components and their initial transforms
+  ConstraintsComponents.Empty();
+  for (FName& ComponentName : ConstraintComponentNames)
+  {
+    UPhysicsConstraintComponent* ConstraintComponent = Cast<UPhysicsConstraintComponent>(GetDefaultSubobjectByName(ComponentName));
+    if (ConstraintComponent)
+    {
+      ConstraintsComponents.Add(ConstraintComponent);
+    }
+  }
   DoorComponentsTransform.Empty();
   FTransform ActorInverseTransform = GetActorTransform().Inverse();
   for (UPhysicsConstraintComponent * Constraint: ConstraintsComponents)
@@ -101,6 +110,7 @@ void ACarlaWheeledVehicle::BeginPlay()
       UE_LOG(LogCarla, Error, TEXT("Missing component for constraint: %s"), *(Constraint->GetName()));
     }
   }
+  ResetConstraints();
 
   float FrictionScale = 3.5f;
 
@@ -472,6 +482,8 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr
     Vehicle4W->PVehicle->mWheelsSimData.setTireData(i, PTireData);
   }
 
+  ResetConstraints();
+
   auto * Recorder = UCarlaStatics::GetRecorder(GetWorld());
   if (Recorder && Recorder->IsEnabled())
   {
@@ -616,10 +628,15 @@ void ACarlaWheeledVehicle::SetSimulatePhysics(bool enabled) {
       FTransform ComponentWorldTransform = ComponentTransform.Value * ActorTransform;
       Component->SetWorldTransform(ComponentWorldTransform);
     }
-    for (UPhysicsConstraintComponent* Constraint : ConstraintsComponents)
-    {
-      Constraint->InitComponentConstraint();
-    }
+    ResetConstraints();
+  }
+}
+
+void ACarlaWheeledVehicle::ResetConstraints()
+{
+  for (UPhysicsConstraintComponent* Constraint : ConstraintsComponents)
+  {
+    Constraint->InitComponentConstraint();
   }
 }
 
@@ -671,14 +688,24 @@ void ACarlaWheeledVehicle::OpenDoorPhys(const EVehicleDoor DoorIdx)
 {
   UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
   float AngleLimit = Constraint->ConstraintInstance.GetAngularSwing1Limit();
-  Constraint->SetAngularOrientationTarget(FRotator(0, 0, AngleLimit));
+  FRotator AngularRotationOffset = Constraint->ConstraintInstance.AngularRotationOffset;
+  if (Constraint->ConstraintInstance.AngularRotationOffset.Yaw < 0.0f)
+  {
+    AngleLimit = -AngleLimit;
+  }
+  Constraint->SetAngularOrientationTarget(FRotator(0, AngleLimit, 0));
   Constraint->SetAngularDriveParams(DoorOpenStrength, 1.0, 0.0);
 }
 
 void ACarlaWheeledVehicle::CloseDoorPhys(const EVehicleDoor DoorIdx)
 {
   UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
-  Constraint->SetAngularOrientationTarget(FRotator(0, 0, 0));
+  float AngleLimit = Constraint->ConstraintInstance.GetAngularSwing1Limit();
+  if (Constraint->ConstraintInstance.AngularRotationOffset.Yaw < 0.0f)
+  {
+    AngleLimit = -AngleLimit;
+  }
+  Constraint->SetAngularOrientationTarget(FRotator(0, -AngleLimit, 0));
   Constraint->SetAngularDriveParams(DoorCloseStrength, 1.0, 0.0);
 }
 
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h
index e36e8a14b..fa17b5cc2 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h
@@ -273,13 +273,16 @@ protected:
   TArray<float> DoorAnimAlpha;
 
   UPROPERTY(Category="Door Animation", EditAnywhere, BlueprintReadWrite)
-  TArray<UPhysicsConstraintComponent*> ConstraintsComponents;
+  TArray<FName> ConstraintComponentNames;
 
   UPROPERTY(Category="Door Animation", EditAnywhere, BlueprintReadWrite)
   float DoorOpenStrength = 100.0f;
 
   UPROPERTY(Category="Door Animation", EditAnywhere, BlueprintReadWrite)
-  float DoorCloseStrength = 10000.0f;
+  float DoorCloseStrength = 1000.0f;
+
+  UFUNCTION(BlueprintCallable, CallInEditor)
+  void ResetConstraints();
 
 private:
 
@@ -349,6 +352,9 @@ private:
   UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere, BlueprintReadOnly, meta = (AllowPrivateAccess = "true"))
   UBaseCarlaMovementComponent * BaseMovementComponent = nullptr;
 
+  UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere, BlueprintReadOnly, meta = (AllowPrivateAccess = "true"))
+  TArray<UPhysicsConstraintComponent*> ConstraintsComponents;
+
   UPROPERTY(Category="CARLA Wheeled Vehicle", VisibleAnywhere, BlueprintReadOnly, meta = (AllowPrivateAccess = "true"))
   TMap<UPrimitiveComponent*, FTransform> DoorComponentsTransform;