First version

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/CarSimManagerComponent.cpp

@@ -6,16 +6,22 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include "CarSimManagerComponent.h"
+#include "Carla/Game/CarlaEngine.h"
 #include "Carla/Vehicle/CarlaWheeledVehicle.h"
 #include "Carla/Util/EmptyActor.h"
 
+#ifdef WITH_CARSIM
+#include "CarSimMovementComponent.h"
+#include "VsVar.h"
+#endif
 
 void UCarSimManagerComponent::CreateCarsimComponent(
-      ACarlaWheeledVehicle* Vehicle, FString Simfile)
+      ACarlaWheeledVehicle* Vehicle, FString Simfile, int ForceFrames)
 {
   #ifdef WITH_CARSIM
   UCarSimManagerComponent* CarSimManagerComponent = NewObject<UCarSimManagerComponent>(Vehicle);
   CarSimManagerComponent->SimfilePath = Simfile;
+  CarSimManagerComponent->FramesApplyingForce = ForceFrames;
   CarSimManagerComponent->RegisterComponent();
   Vehicle->SetCarlaMovementComponent(CarSimManagerComponent);
   #else
@@ -68,6 +74,7 @@ void UCarSimManagerComponent::BeginPlay()
   // set callbacks to react to collisions
   CarlaVehicle->OnActorHit.AddDynamic(this, &UCarSimManagerComponent::OnCarSimHit);
   CarlaVehicle->GetMesh()->OnComponentBeginOverlap.AddDynamic(this, &UCarSimManagerComponent::OnCarSimOverlap);
+  CarlaVehicle->GetMesh()->OnComponentEndOverlap.AddDynamic(this, &UCarSimManagerComponent::OnCarSimEndOverlap);
   CarlaVehicle->GetMesh()->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Overlap);
 
   // workaround to prevent carsim from interacting with its own car
@@ -81,6 +88,35 @@ void UCarSimManagerComponent::BeginPlay()
   #endif
 }
 
+void UCarSimManagerComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
+{
+  #ifdef WITH_CARSIM
+
+  // get current vehicle position
+  const auto Trans = CarlaVehicle->GetActorTransform().GetTranslation();
+  // get current velocity
+  auto Root = Cast<UPrimitiveComponent>(CarlaVehicle->GetRootComponent());
+  if (Root == nullptr) return;
+  FVector Vel = Root->GetPhysicsLinearVelocity("None");
+  FVector Dir = Trans + (Vel.GetSafeNormal() * 200.0f);
+  DrawDebugLine(GetWorld(), Trans, Dir, FColor(255, 255, 255), true, 1);
+
+  if (ResetForces != 0 && ResetForces <= FCarlaEngine::FrameCounter)
+  {
+    #undef GetObject
+    TScriptInterface<IVsVar> FX, FY, FZ;
+    FX = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FX_RP_1", "", EVsVarDirection::Input);
+    FY = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FY_RP_1", "", EVsVarDirection::Input);
+    FZ = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FZ_RP_1", "", EVsVarDirection::Input);
+    IVsVar::Execute_SetFloatValue(FX.GetObject(), 0.0f);
+    IVsVar::Execute_SetFloatValue(FY.GetObject(), 0.0f);
+    IVsVar::Execute_SetFloatValue(FZ.GetObject(), 0.0f);
+    ResetForces = 0;
+    UE_LOG(LogCarla, Warning, TEXT("Carsim: Reset forces") );
+  }
+  #endif
+}
+
 void UCarSimManagerComponent::ProcessControl(FVehicleControl &Control)
 {
   #ifdef WITH_CARSIM
@@ -95,6 +131,11 @@ void UCarSimManagerComponent::ProcessControl(FVehicleControl &Control)
   #endif
 }
 
+// UCarSimMovementComponent *UCarSimManagerComponent::GetCarsimMovementComponent()
+// {
+//   return CarSimMovementComponent;
+// }
+
 void UCarSimManagerComponent::OnCarSimHit(AActor *Actor,
     AActor *OtherActor,
     FVector NormalImpulse,
@@ -118,11 +159,69 @@ void UCarSimManagerComponent::OnCarSimOverlap(UPrimitiveComponent* OverlappedCom
       ECollisionResponse::ECR_Block)
   {
     #ifdef WITH_CARSIM
-    // handle collision forces here
+    // get current vehicle position
+    const auto Trans = CarlaVehicle->GetActorTransform().GetTranslation();
+    // get current velocity
+    auto Root = Cast<UPrimitiveComponent>(CarlaVehicle->GetRootComponent());
+    if (Root == nullptr)
+      return;
+    FVector Vel = Root->GetPhysicsLinearVelocity("None");
+    // reflect the velocity vector with the normal vector
+    FVector NewVel = Vel - (2 * (FVector::DotProduct(Vel, SweepResult.ImpactNormal)) * SweepResult.ImpactNormal);
+    // get the mass of the vehicle
+    FVector Impulse = NewVel * Root->GetMass(); // / FramesApplyingForce;
+    // apply delta time
+    Impulse *= GetWorld()->GetDeltaSeconds();
+
+    // FVector Dir = Trans + (Impulse.GetSafeNormal() * 200.0f);
+    FVector Dir = SweepResult.ImpactPoint + (SweepResult.ImpactNormal * 200.0f);
+    DrawDebugLine(GetWorld(), SweepResult.ImpactPoint, Dir, FColor(255, 0, 0), true, 5, 255, 10);
+
+    // send the force to Carsim
+    #undef GetObject
+    TScriptInterface<IVsVar> FX, FY, FZ;
+    FX = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FX_RP_1", "", EVsVarDirection::Input);
+    FY = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FY_RP_1", "", EVsVarDirection::Input);
+    FZ = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FZ_RP_1", "", EVsVarDirection::Input);
+    IVsVar::Execute_SetFloatValue(FX.GetObject(), Impulse.X);
+    IVsVar::Execute_SetFloatValue(FY.GetObject(), Impulse.Y);
+    IVsVar::Execute_SetFloatValue(FZ.GetObject(), 0.0f);
+    // mark to reset this force next tick
+    ResetForces = FCarlaEngine::FrameCounter + FramesApplyingForce;
+    UE_LOG(LogCarla, Warning, TEXT("Carsim: Applied force: %f (%f, %f, %f) %f"), Root->GetMass(), Impulse.X, Impulse.Y, Impulse.Z, GetWorld()->GetDeltaSeconds());
+    UE_LOG(LogCarla, Warning, TEXT("Carsim:              : (%f, %f, %f)"), SweepResult.Normal.X, SweepResult.Normal.Y, SweepResult.Normal.Z);
+    UE_LOG(LogCarla, Warning, TEXT("Carsim:              : (%f, %f, %f)"), SweepResult.ImpactNormal.X, SweepResult.ImpactNormal.Y, SweepResult.ImpactNormal.Z);
     #endif
   }
 }
 
+void UCarSimManagerComponent::OnCarSimEndOverlap(UPrimitiveComponent* OverlappedComponent,
+    AActor* OtherActor,
+    UPrimitiveComponent* OtherComp,
+    int32 OtherBodyIndex)
+{
+  // if (OtherComp->GetCollisionResponseToChannel(
+  //     ECollisionChannel::ECC_WorldDynamic) ==
+  //     ECollisionResponse::ECR_Block)
+  // {
+  //   #ifdef WITH_CARSIM
+  //   // if (ResetForces != 0 && ResetForces <= FCarlaEngine::FrameCounter)
+  //   // {
+  //   #undef GetObject
+  //   TScriptInterface<IVsVar> FX, FY, FZ;
+  //   FX = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FX_RP_1", "", EVsVarDirection::Input);
+  //   FY = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FY_RP_1", "", EVsVarDirection::Input);
+  //   FZ = IVsConnectObject::Execute_GetVsVar(CarSimMovementComponent, "IMP_FZ_RP_1", "", EVsVarDirection::Input);
+  //   IVsVar::Execute_SetFloatValue(FX.GetObject(), 0.0f);
+  //   IVsVar::Execute_SetFloatValue(FY.GetObject(), 0.0f);
+  //   IVsVar::Execute_SetFloatValue(FZ.GetObject(), 0.0f);
+  //   // ResetForces = 0;
+  //   UE_LOG(LogCarla, Warning, TEXT("Carsim: Reset forces") );
+  //   // }
+  //   #endif
+  // }  
+}
+
 void UCarSimManagerComponent::UseCarSimRoad(bool bEnabled)
 {
   #ifdef WITH_CARSIM

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/CarSimManagerComponent.h

@@ -32,12 +32,14 @@ class CARLA_API UCarSimManagerComponent : public UBaseCarlaMovementComponent
 public:
 
   static void CreateCarsimComponent(
-      ACarlaWheeledVehicle* Vehicle, FString Simfile);
+      ACarlaWheeledVehicle* Vehicle, FString Simfile, int ForceFrames = 1);
 
   FString SimfilePath = "";
 
   virtual void BeginPlay() override;
 
+  virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
+
   void ProcessControl(FVehicleControl &Control) override;
 
   FVector GetVelocity() const override;
@@ -50,8 +52,13 @@ public:
 
   float GetVehicleForwardSpeed() const override;
 
+  // UCarSimMovementComponent *GetCarsimMovementComponent();
+
 private:
 
+  uint64_t ResetForces { 0 };
+  int FramesApplyingForce { 1 };
+
   // On car mesh hit, only works when carsim is enabled
   UFUNCTION()
   void OnCarSimHit(AActor *Actor,
@@ -69,4 +76,11 @@ private:
       bool bFromSweep,
       const FHitResult & SweepResult);
 
+  // On car mesh overlap end, only works when carsim is enabled
+  // (this event triggers when overlapping with static environment)
+  UFUNCTION()
+  void OnCarSimEndOverlap(UPrimitiveComponent* OverlappedComponent,
+      AActor* OtherActor,
+      UPrimitiveComponent* OtherComp,
+      int32 OtherBodyIndex);
 };