Make rigidbody simulation handle animated objects gracefully

The animated objects was not updated for each internal substep for the rigidbody sim.
This would lead to unstable simulations or very annoying clipping artifacts.

Updated the code to use explicit substeps and tie it to the scene frame rate.

Fix T47402: Properly updating the animated objects fixes the reported issue.

Reviewed By: Brecht, Jacques

Differential Revision: http://developer.blender.org/D8762

11 files changed, 199 insertions, 46 deletions

diff --git a/source/blender/blenkernel/CMakeLists.txt b/source/blender/blenkernel/CMakeLists.txt
index 6f32eb8a90f..bf3077b7743 100644
--- a/source/blender/blenkernel/CMakeLists.txt
+++ b/source/blender/blenkernel/CMakeLists.txt
@@ -482,9 +482,15 @@ if(WITH_BULLET)
   list(APPEND INC
     ../../../intern/rigidbody
   )
+
+  if(NOT WITH_SYSTEM_BULLET)
+    list(APPEND LIB
+      extern_bullet
+    )
+  endif()
+
   list(APPEND LIB
     bf_intern_rigidbody
-    extern_bullet
 
     ${BULLET_LIBRARIES}
   )
diff --git a/source/blender/blenkernel/intern/rigidbody.c b/source/blender/blenkernel/intern/rigidbody.c
index 95a8b3b3c15..00b993296d0 100644
--- a/source/blender/blenkernel/intern/rigidbody.c
+++ b/source/blender/blenkernel/intern/rigidbody.c
@@ -1180,7 +1180,10 @@ RigidBodyWorld *BKE_rigidbody_create_world(Scene *scene)
 
   rbw->time_scale = 1.0f;
 
-  rbw->steps_per_second = 60;      /* Bullet default (60 Hz) */
+  /* Most high quality Bullet example files has an internal framerate of 240hz.
+   * The blender default scene has a frame rate of 24, so take 10 substeps (24fps * 10).
+   */
+  rbw->substeps_per_frame = 10;
   rbw->num_solver_iterations = 10; /* 10 is bullet default */
 
   rbw->shared->pointcache = BKE_ptcache_add(&(rbw->shared->ptcaches));
@@ -1651,10 +1654,6 @@ static void rigidbody_update_sim_world(Scene *scene, RigidBodyWorld *rbw)
 static void rigidbody_update_sim_ob(
     Depsgraph *depsgraph, Scene *scene, RigidBodyWorld *rbw, Object *ob, RigidBodyOb *rbo)
 {
-  float loc[3];
-  float rot[4];
-  float scale[3];
-
   /* only update if rigid body exists */
   if (rbo->shared->physics_object == NULL) {
     return;
@@ -1680,14 +1679,21 @@ static void rigidbody_update_sim_ob(
     }
   }
 
-  mat4_decompose(loc, rot, scale, ob->obmat);
+  if (!(rbo->flag & RBO_FLAG_KINEMATIC)) {
+    /* update scale for all non kinematic objects */
+    float new_scale[3], old_scale[3];
+    mat4_to_size(new_scale, ob->obmat);
+    RB_body_get_scale(rbo->shared->physics_object, old_scale);
 
-  /* update scale for all objects */
-  RB_body_set_scale(rbo->shared->physics_object, scale);
-  /* compensate for embedded convex hull collision margin */
-  if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && rbo->shape == RB_SHAPE_CONVEXH) {
-    RB_shape_set_margin(rbo->shared->physics_shape,
-                        RBO_GET_MARGIN(rbo) * MIN3(scale[0], scale[1], scale[2]));
+    /* Avoid updating collision shape AABBs if scale didn't change. */
+    if (!compare_size_v3v3(old_scale, new_scale, 0.001f)) {
+      RB_body_set_scale(rbo->shared->physics_object, new_scale);
+      /* compensate for embedded convex hull collision margin */
+      if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && rbo->shape == RB_SHAPE_CONVEXH) {
+        RB_shape_set_margin(rbo->shared->physics_shape,
+                            RBO_GET_MARGIN(rbo) * MIN3(new_scale[0], new_scale[1], new_scale[2]));
+      }
+    }
   }
 
   /* Make transformed objects temporarily kinmatic
@@ -1697,11 +1703,6 @@ static void rigidbody_update_sim_ob(
     RB_body_set_mass(rbo->shared->physics_object, 0.0f);
   }
 
-  /* update rigid body location and rotation for kinematic bodies */
-  if (rbo->flag & RBO_FLAG_KINEMATIC || (is_selected && (G.moving & G_TRANSFORM_OBJ))) {
-    RB_body_activate(rbo->shared->physics_object);
-    RB_body_set_loc_rot(rbo->shared->physics_object, loc, rot);
-  }
   /* update influence of effectors - but don't do it on an effector */
   /* only dynamic bodies need effector update */
   else if (rbo->type == RBO_TYPE_ACTIVE &&
@@ -1765,8 +1766,6 @@ static void rigidbody_update_simulation(Depsgraph *depsgraph,
                                         RigidBodyWorld *rbw,
                                         bool rebuild)
 {
-  float ctime = DEG_get_ctime(depsgraph);
-
   /* update world */
   /* Note physics_world can get NULL when undoing the deletion of the last object in it (see
    * T70667). */
@@ -1799,9 +1798,6 @@ static void rigidbody_update_simulation(Depsgraph *depsgraph,
     if (ob->type == OB_MESH) {
       /* validate that we've got valid object set up here... */
       RigidBodyOb *rbo = ob->rigidbody_object;
-      /* Update transformation matrix of the object
-       * so we don't get a frame of lag for simple animations. */
-      BKE_object_where_is_calc_time(depsgraph, scene, ob, ctime);
 
       /* TODO remove this whole block once we are sure we never get NULL rbo here anymore. */
       /* This cannot be done in CoW evaluation context anymore... */
@@ -1859,9 +1855,6 @@ static void rigidbody_update_simulation(Depsgraph *depsgraph,
   FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN (rbw->constraints, ob) {
     /* validate that we've got valid object set up here... */
     RigidBodyCon *rbc = ob->rigidbody_constraint;
-    /* Update transformation matrix of the object
-     * so we don't get a frame of lag for simple animations. */
-    BKE_object_where_is_calc_time(depsgraph, scene, ob, ctime);
 
     /* TODO remove this whole block once we are sure we never get NULL rbo here anymore. */
     /* This cannot be done in CoW evaluation context anymore... */
@@ -1891,6 +1884,104 @@ static void rigidbody_update_simulation(Depsgraph *depsgraph,
   FOREACH_COLLECTION_OBJECT_RECURSIVE_END;
 }
 
+typedef struct KinematicSubstepData {
+  RigidBodyOb *rbo;
+  float old_pos[3];
+  float new_pos[3];
+  float old_rot[4];
+  float new_rot[4];
+  bool scale_changed;
+  float old_scale[3];
+  float new_scale[3];
+} KinematicSubstepData;
+
+static ListBase rigidbody_create_substep_data(RigidBodyWorld *rbw)
+{
+  /* Objects that we want to update substep location/rotation for. */
+  ListBase substep_targets = {NULL, NULL};
+
+  FOREACH_COLLECTION_OBJECT_RECURSIVE_BEGIN (rbw->group, ob) {
+    RigidBodyOb *rbo = ob->rigidbody_object;
+    /* only update if rigid body exists */
+    if (!rbo || rbo->shared->physics_object == NULL) {
+      continue;
+    }
+
+    if (rbo->flag & RBO_FLAG_KINEMATIC) {
+      float loc[3], rot[4], scale[3];
+
+      KinematicSubstepData *data = MEM_callocN(sizeof(KinematicSubstepData),
+                                               "RigidBody Substep data");
+
+      data->rbo = rbo;
+
+      RB_body_get_position(rbo->shared->physics_object, loc);
+      RB_body_get_orientation(rbo->shared->physics_object, rot);
+      RB_body_get_scale(rbo->shared->physics_object, scale);
+
+      copy_v3_v3(data->old_pos, loc);
+      copy_v4_v4(data->old_rot, rot);
+      copy_v3_v3(data->old_scale, scale);
+
+      mat4_decompose(loc, rot, scale, ob->obmat);
+
+      copy_v3_v3(data->new_pos, loc);
+      copy_v4_v4(data->new_rot, rot);
+      copy_v3_v3(data->new_scale, scale);
+
+      data->scale_changed = !compare_size_v3v3(data->old_scale, data->new_scale, 0.001f);
+
+      LinkData *ob_link = BLI_genericNodeN(data);
+      BLI_addtail(&substep_targets, ob_link);
+    }
+  }
+  FOREACH_COLLECTION_OBJECT_RECURSIVE_END;
+
+  return substep_targets;
+}
+
+static void rigidbody_update_kinematic_obj_substep(ListBase *substep_targets, float interp_fac)
+{
+  LISTBASE_FOREACH (LinkData *, link, substep_targets) {
+    KinematicSubstepData *data = link->data;
+    RigidBodyOb *rbo = data->rbo;
+
+    float loc[3], rot[4];
+
+    interp_v3_v3v3(loc, data->old_pos, data->new_pos, interp_fac);
+    interp_qt_qtqt(rot, data->old_rot, data->new_rot, interp_fac);
+
+    RB_body_activate(rbo->shared->physics_object);
+    RB_body_set_loc_rot(rbo->shared->physics_object, loc, rot);
+
+    if (!data->scale_changed) {
+      /* Avoid having to rebuild the collision shape AABBs if scale didn't change. */
+      continue;
+    }
+
+    float scale[3];
+
+    interp_v3_v3v3(scale, data->old_scale, data->new_scale, interp_fac);
+
+    RB_body_set_scale(rbo->shared->physics_object, scale);
+
+    /* compensate for embedded convex hull collision margin */
+    if (!(rbo->flag & RBO_FLAG_USE_MARGIN) && rbo->shape == RB_SHAPE_CONVEXH) {
+      RB_shape_set_margin(rbo->shared->physics_shape,
+                          RBO_GET_MARGIN(rbo) * MIN3(scale[0], scale[1], scale[2]));
+    }
+  }
+}
+
+static void rigidbody_free_substep_data(ListBase *substep_targets)
+{
+  LISTBASE_FOREACH (LinkData *, link, substep_targets) {
+    KinematicSubstepData *data = link->data;
+    MEM_freeN(data);
+  }
+
+  BLI_freelistN(substep_targets);
+}
 static void rigidbody_update_simulation_post_step(Depsgraph *depsgraph, RigidBodyWorld *rbw)
 {
   ViewLayer *view_layer = DEG_get_input_view_layer(depsgraph);
@@ -2072,7 +2163,6 @@ void BKE_rigidbody_rebuild_world(Depsgraph *depsgraph, Scene *scene, float ctime
 /* Run RigidBody simulation for the specified physics world */
 void BKE_rigidbody_do_simulation(Depsgraph *depsgraph, Scene *scene, float ctime)
 {
-  float timestep;
   RigidBodyWorld *rbw = scene->rigidbody_world;
   PointCache *cache;
   PTCacheID pid;
@@ -2125,14 +2215,23 @@ void BKE_rigidbody_do_simulation(Depsgraph *depsgraph, Scene *scene, float ctime
     /* update and validate simulation */
     rigidbody_update_simulation(depsgraph, scene, rbw, false);
 
+    const float frame_diff = ctime - rbw->ltime;
     /* calculate how much time elapsed since last step in seconds */
-    timestep = 1.0f / (float)FPS * (ctime - rbw->ltime) * rbw->time_scale;
-    /* Step simulation by the requested timestep,
-     * steps per second are adjusted to take time scale into account. */
-    RB_dworld_step_simulation(rbw->shared->physics_world,
-                              timestep,
-                              INT_MAX,
-                              1.0f / (float)rbw->steps_per_second * min_ff(rbw->time_scale, 1.0f));
+    const float timestep = 1.0f / (float)FPS * frame_diff * rbw->time_scale;
+
+    const float substep = timestep / rbw->substeps_per_frame;
+
+    ListBase substep_targets = rigidbody_create_substep_data(rbw);
+
+    const float interp_step = 1.0f / rbw->substeps_per_frame;
+    float cur_interp_val = interp_step;
+
+    for (int i = 0; i < rbw->substeps_per_frame; i++) {
+      rigidbody_update_kinematic_obj_substep(&substep_targets, cur_interp_val);
+      RB_dworld_step_simulation(rbw->shared->physics_world, substep, 0, substep);
+      cur_interp_val += interp_step;
+    }
+    rigidbody_free_substep_data(&substep_targets);
 
     rigidbody_update_simulation_post_step(depsgraph, rbw);
 
diff --git a/source/blender/blenlib/BLI_math_vector.h b/source/blender/blenlib/BLI_math_vector.h
index 3399287dd46..1425e7da1bc 100644
--- a/source/blender/blenlib/BLI_math_vector.h
+++ b/source/blender/blenlib/BLI_math_vector.h
@@ -367,6 +367,10 @@ MINLINE bool compare_len_v3v3(const float a[3],
                               const float b[3],
                               const float limit) ATTR_WARN_UNUSED_RESULT;
 
+MINLINE bool compare_size_v3v3(const float a[3],
+                               const float b[3],
+                               const float limit) ATTR_WARN_UNUSED_RESULT;
+
 MINLINE float line_point_side_v2(const float l1[2],
                                  const float l2[2],
                                  const float pt[2]) ATTR_WARN_UNUSED_RESULT;
diff --git a/source/blender/blenlib/intern/math_vector_inline.c b/source/blender/blenlib/intern/math_vector_inline.c
index 13a87b9ec6a..d3c975e3249 100644
--- a/source/blender/blenlib/intern/math_vector_inline.c
+++ b/source/blender/blenlib/intern/math_vector_inline.c
@@ -1372,6 +1372,24 @@ MINLINE bool compare_len_v3v3(const float v1[3], const float v2[3], const float
   return (dot_v3v3(d, d) <= (limit * limit));
 }
 
+MINLINE bool compare_size_v3v3(const float v1[3], const float v2[3], const float limit)
+{
+  for (int i = 0; i < 3; i++) {
+    if (v2[i] == 0.0f) {
+      /* Catch division by zero. */
+      if (v1[i] != v2[i]) {
+        return false;
+      }
+    }
+    else {
+      if (fabsf(v1[i] / v2[i] - 1.0f) > limit) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
 /** \name Vector Clamping
  * \{ */
 
diff --git a/source/blender/blenloader/intern/versioning_290.c b/source/blender/blenloader/intern/versioning_290.c
index 3c326565557..269c2083ef4 100644
--- a/source/blender/blenloader/intern/versioning_290.c
+++ b/source/blender/blenloader/intern/versioning_290.c
@@ -33,6 +33,7 @@
 #include "DNA_gpencil_types.h"
 #include "DNA_modifier_types.h"
 #include "DNA_object_types.h"
+#include "DNA_rigidbody_types.h"
 #include "DNA_screen_types.h"
 #include "DNA_shader_fx_types.h"
 
@@ -518,6 +519,23 @@ void blo_do_versions_290(FileData *fd, Library *UNUSED(lib), Main *bmain)
     }
   }
 
+  for (Scene *scene = bmain->scenes.first; scene; scene = scene->id.next) {
+    RigidBodyWorld *rbw = scene->rigidbody_world;
+
+    if (rbw == NULL) {
+      continue;
+    }
+
+    /* The substep method changed from "per second" to "per frame".
+     * To get the new value simply divide the old bullet sim fps with the scene fps.
+     */
+    rbw->substeps_per_frame /= FPS;
+
+    if (rbw->substeps_per_frame <= 0) {
+      rbw->substeps_per_frame = 1;
+    }
+  }
+
   /**
    * Versioning code until next subversion bump goes here.
    *
diff --git a/source/blender/bmesh/CMakeLists.txt b/source/blender/bmesh/CMakeLists.txt
index d2b747aa68f..92c52f5d8d0 100644
--- a/source/blender/bmesh/CMakeLists.txt
+++ b/source/blender/bmesh/CMakeLists.txt
@@ -189,10 +189,15 @@ endif()
 if(WITH_BULLET)
   list(APPEND INC_SYS
     ${BULLET_INCLUDE_DIRS}
+    "../../../intern/rigidbody/"
   )
+  if(NOT WITH_SYSTEM_BULLET)
   list(APPEND LIB
     extern_bullet
+    )
+  endif()
 
+  list(APPEND LIB
     ${BULLET_LIBRARIES}
   )
   add_definitions(-DWITH_BULLET)
diff --git a/source/blender/bmesh/operators/bmo_hull.c b/source/blender/bmesh/operators/bmo_hull.c
index 98d4431803a..84938084aec 100644
--- a/source/blender/bmesh/operators/bmo_hull.c
+++ b/source/blender/bmesh/operators/bmo_hull.c
@@ -28,7 +28,7 @@
 #  include "BLI_listbase.h"
 #  include "BLI_math.h"
 
-#  include "Bullet-C-Api.h"
+#  include "RBI_hull_api.h"
 
 /* XXX: using 128 for totelem and pchunk of mempool, no idea what good
  * values would be though */
diff --git a/source/blender/makesdna/DNA_rigidbody_types.h b/source/blender/makesdna/DNA_rigidbody_types.h
index 02a4a158d8c..cd11c18578a 100644
--- a/source/blender/makesdna/DNA_rigidbody_types.h
+++ b/source/blender/makesdna/DNA_rigidbody_types.h
@@ -75,8 +75,8 @@ typedef struct RigidBodyWorld {
   /** Number of objects in rigid body group. */
   int numbodies;
 
-  /** Number of simulation steps thaken per second. */
-  short steps_per_second;
+  /** Number of simulation substeps steps taken per frame. */
+  short substeps_per_frame;
   /** Number of constraint solver iterations made per simulation step. */
   short num_solver_iterations;
 
diff --git a/source/blender/makesdna/intern/dna_rename_defs.h b/source/blender/makesdna/intern/dna_rename_defs.h
index a73fc747f84..a0e983e7d3b 100644
--- a/source/blender/makesdna/intern/dna_rename_defs.h
+++ b/source/blender/makesdna/intern/dna_rename_defs.h
@@ -125,3 +125,4 @@ DNA_STRUCT_RENAME_ELEM(bTheme, tstatusbar, space_statusbar)
 DNA_STRUCT_RENAME_ELEM(bTheme, ttopbar, space_topbar)
 DNA_STRUCT_RENAME_ELEM(bTheme, tuserpref, space_preferences)
 DNA_STRUCT_RENAME_ELEM(bTheme, tv3d, space_view3d)
+DNA_STRUCT_RENAME_ELEM(RigidBodyWorld, steps_per_second, substeps_per_frame)
diff --git a/source/blender/makesrna/intern/rna_rigidbody.c b/source/blender/makesrna/intern/rna_rigidbody.c
index fb550b38b31..6c8b8e6859d 100644
--- a/source/blender/makesrna/intern/rna_rigidbody.c
+++ b/source/blender/makesrna/intern/rna_rigidbody.c
@@ -906,15 +906,15 @@ static void rna_def_rigidbody_world(BlenderRNA *brna)
   RNA_def_property_update(prop, NC_SCENE, "rna_RigidBodyWorld_reset");
 
   /* timestep */
-  prop = RNA_def_property(srna, "steps_per_second", PROP_INT, PROP_NONE);
-  RNA_def_property_int_sdna(prop, NULL, "steps_per_second");
+  prop = RNA_def_property(srna, "substeps_per_frame", PROP_INT, PROP_NONE);
+  RNA_def_property_int_sdna(prop, NULL, "substeps_per_frame");
   RNA_def_property_range(prop, 1, SHRT_MAX);
-  RNA_def_property_ui_range(prop, 60, 1000, 1, -1);
-  RNA_def_property_int_default(prop, 60);
+  RNA_def_property_ui_range(prop, 1, 1000, 1, -1);
+  RNA_def_property_int_default(prop, 10);
   RNA_def_property_ui_text(
       prop,
-      "Steps Per Second",
-      "Number of simulation steps taken per second (higher values are more accurate "
+      "Substeps Per Frame",
+      "Number of simulation steps taken per frame (higher values are more accurate "
       "but slower)");
   RNA_def_property_update(prop, NC_SCENE, "rna_RigidBodyWorld_reset");
 
diff --git a/source/blender/modifiers/CMakeLists.txt b/source/blender/modifiers/CMakeLists.txt
index d2ac9dc85de..01770abeca0 100644
--- a/source/blender/modifiers/CMakeLists.txt
+++ b/source/blender/modifiers/CMakeLists.txt
@@ -156,9 +156,11 @@ if(WITH_OPENSUBDIV)
 endif()
 
 if(WITH_BULLET)
-  list(APPEND LIB
-    extern_bullet
-  )
+  if(NOT WITH_SYSTEM_BULLET)
+    list(APPEND LIB
+      extern_bullet
+    )
+  endif()
   add_definitions(-DWITH_BULLET)
 endif()