added option to turn on off ground lock

Kjell-K · Jul 28, 2017 · d4b379c · d4b379c
1 parent edcde6f
commit d4b379c
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 6 deletions.
diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp
@@ -15,7 +15,8 @@ namespace msr { namespace airlib {
 
 class FastPhysicsEngine : public PhysicsEngineBase {
 public:
-    FastPhysicsEngine()
+    FastPhysicsEngine(bool enable_ground_lock = true)
+        : enable_ground_lock_(enable_ground_lock)
     { 
         FastPhysicsEngine::reset();
     }
@@ -79,7 +80,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         CollisonResponseInfo& collison_response_info = body.getCollisionResponseInfo();
         //if collision was already responsed then do not respond to it until we get updated information
         if (collison_info.has_collided && collison_response_info.collision_time_stamp != collison_info.time_stamp) {
-            bool is_collison_response = getNextKinematicsOnCollison(dt, collison_info, body, current, next, next_wrench);
+            bool is_collison_response = getNextKinematicsOnCollison(dt, collison_info, body, 
+                current, next, next_wrench, enable_ground_lock_);
             updateCollisonResponseInfo(collison_info, next, is_collison_response, collison_response_info);
         }
 
@@ -101,7 +103,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
     //return value indicates if collison response was generated
     static bool getNextKinematicsOnCollison(TTimeDelta dt, const CollisionInfo& collison_info, const PhysicsBody& body, 
-        const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench)
+        const Kinematics::State& current, Kinematics::State& next, Wrench& next_wrench, bool enable_ground_lock)
     {
         /************************* Collison response ************************/
         const real_T dt_real = static_cast<real_T>(dt);
@@ -179,7 +181,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         next.accelerations.angular = Vector3r::Zero();
 
         next.pose = current.pose;
-        if (ground_lock) {
+        if (enable_ground_lock && ground_lock) {
             float pitch, roll, yaw;
             VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
             pitch = roll = 0;
@@ -380,6 +382,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
     std::stringstream debug_string_;
     int grounded_;
+    bool enable_ground_lock_;
+
 };
 
 }} //namespace

diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp
@@ -39,8 +39,20 @@ void ASimModeWorldBase::createWorld()
 {
     if (physics_engine_name == "" || usage_scenario == kUsageScenarioComputerVision)
         physics_engine_.release(); //no physics engine
-    else if (physics_engine_name == "FastPhysicsEngine")
-        physics_engine_.reset(new msr::airlib::FastPhysicsEngine());
+    else if (physics_engine_name == "FastPhysicsEngine") {
+        msr::airlib::Settings fast_phys_settings;
+        if (msr::airlib::Settings::singleton().getChild("FastPhysicsEngine", fast_phys_settings)) {
+            physics_engine_.reset(
+                new msr::airlib::FastPhysicsEngine(fast_phys_settings.getBool("EnableGroundLock", true))
+            );
+        }
+        else {
+            physics_engine_.reset(
+                new msr::airlib::FastPhysicsEngine()
+            );
+        }
+
+    }
     else {
         physics_engine_.release();
         UAirBlueprintLib::LogMessageString("Unrecognized physics engine name: ",  physics_engine_name, LogDebugLevel::Failure);