Tracking: Cleanup, split code of follow track constraint

Makes a set of more isolated individual functions, which should make
it more clear what's going on. Improved naming, added some comments.

Some extra cleanup is possible to get rid of generic vectors called
vec and disp.

1 files changed, 318 insertions, 158 deletions

diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index 3c68753cbf9..7fe438c8c45 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -4590,230 +4590,390 @@ static void followtrack_id_looper(bConstraint *con, ConstraintIDFunc func, void
   func(con, (ID **)&data->depth_ob, false, userdata);
 }
 
-static void followtrack_evaluate(bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
+static MovieClip *followtrack_tracking_clip_get(bConstraint *con, bConstraintOb *cob)
 {
-  Depsgraph *depsgraph = cob->depsgraph;
-  Scene *scene = cob->scene;
   bFollowTrackConstraint *data = con->data;
-  MovieClip *clip = data->clip;
+
+  if (data->flag & FOLLOWTRACK_ACTIVECLIP) {
+    Scene *scene = cob->scene;
+    return scene->clip;
+  }
+
+  return data->clip;
+}
+
+static MovieTrackingObject *followtrack_tracking_object_get(bConstraint *con, bConstraintOb *cob)
+{
+  MovieClip *clip = followtrack_tracking_clip_get(con, cob);
+  MovieTracking *tracking = &clip->tracking;
+  bFollowTrackConstraint *data = con->data;
+
+  if (data->object[0]) {
+    return BKE_tracking_object_get_named(tracking, data->object);
+  }
+  return BKE_tracking_object_get_camera(tracking);
+}
+
+static Object *followtrack_camera_object_get(bConstraint *con, bConstraintOb *cob)
+{
+  bFollowTrackConstraint *data = con->data;
+
+  if (data->camera == NULL) {
+    Scene *scene = cob->scene;
+    return scene->camera;
+  }
+
+  return data->camera;
+}
+
+typedef struct FollowTrackContext {
+  int flag;
+  int frame_method;
+
+  Depsgraph *depsgraph;
+  Scene *scene;
+
+  MovieClip *clip;
+  Object *camera_object;
+  Object *depth_object;
+
   MovieTracking *tracking;
-  MovieTrackingTrack *track;
   MovieTrackingObject *tracking_object;
-  Object *camob = data->camera ? data->camera : scene->camera;
+  MovieTrackingTrack *track;
 
-  float ctime = DEG_get_ctime(depsgraph);
-  float framenr;
+  float depsgraph_time;
+  float clip_frame;
+} FollowTrackContext;
 
-  if (data->flag & FOLLOWTRACK_ACTIVECLIP) {
-    clip = scene->clip;
-  }
+static bool followtrack_context_init(FollowTrackContext *context,
+                                     bConstraint *con,
+                                     bConstraintOb *cob)
+{
+  bFollowTrackConstraint *data = con->data;
 
-  if (!clip || !data->track[0] || !camob) {
-    return;
+  context->flag = data->flag;
+  context->frame_method = data->frame_method;
+
+  context->depsgraph = cob->depsgraph;
+  context->scene = cob->scene;
+
+  context->clip = followtrack_tracking_clip_get(con, cob);
+  context->camera_object = followtrack_camera_object_get(con, cob);
+  if (context->clip == NULL || context->camera_object == NULL) {
+    return false;
   }
+  context->depth_object = data->depth_ob;
 
-  tracking = &clip->tracking;
+  context->tracking = &context->clip->tracking;
+  context->tracking_object = followtrack_tracking_object_get(con, cob);
+  if (context->tracking_object == NULL) {
+    return false;
+  }
 
-  if (data->object[0]) {
-    tracking_object = BKE_tracking_object_get_named(tracking, data->object);
+  context->track = BKE_tracking_track_get_named(
+      context->tracking, context->tracking_object, data->track);
+  if (context->track == NULL) {
+    return false;
   }
-  else {
-    tracking_object = BKE_tracking_object_get_camera(tracking);
+
+  context->depsgraph_time = DEG_get_ctime(context->depsgraph);
+  context->clip_frame = BKE_movieclip_remap_scene_to_clip_frame(context->clip,
+                                                                context->depsgraph_time);
+
+  return true;
+}
+
+static void followtrack_evaluate_using_3d_position_object(FollowTrackContext *context,
+                                                          bConstraintOb *cob)
+{
+  Object *camera_object = context->camera_object;
+  MovieTracking *tracking = context->tracking;
+  MovieTrackingTrack *track = context->track;
+  MovieTrackingObject *tracking_object = context->tracking_object;
+
+  /* Matrix of the object which is being solved prior to this contraint. */
+  float obmat[4][4];
+  copy_m4_m4(obmat, cob->matrix);
+
+  /* Object matrix of the camera. */
+  float camera_obmat[4][4];
+  copy_m4_m4(camera_obmat, camera_object->obmat);
+
+  /* Calculate inverted matrix of the solved camera at the current time. */
+  float reconstructed_camera_mat[4][4];
+  BKE_tracking_camera_get_reconstructed_interpolate(
+      tracking, tracking_object, context->clip_frame, reconstructed_camera_mat);
+  float reconstructed_camera_mat_inv[4][4];
+  invert_m4_m4(reconstructed_camera_mat_inv, reconstructed_camera_mat);
+
+  mul_m4_series(cob->matrix, obmat, camera_obmat, reconstructed_camera_mat_inv);
+  translate_m4(cob->matrix, track->bundle_pos[0], track->bundle_pos[1], track->bundle_pos[2]);
+}
+
+static void followtrack_evaluate_using_3d_position_camera(FollowTrackContext *context,
+                                                          bConstraintOb *cob)
+{
+  Object *camera_object = context->camera_object;
+  MovieTrackingTrack *track = context->track;
+
+  /* Matrix of the object which is being solved prior to this contraint. */
+  float obmat[4][4];
+  copy_m4_m4(obmat, cob->matrix);
+
+  float reconstructed_camera_mat[4][4];
+  BKE_tracking_get_camera_object_matrix(camera_object, reconstructed_camera_mat);
+
+  mul_m4_m4m4(cob->matrix, obmat, reconstructed_camera_mat);
+  translate_m4(cob->matrix, track->bundle_pos[0], track->bundle_pos[1], track->bundle_pos[2]);
+}
+
+static void followtrack_evaluate_using_3d_position(FollowTrackContext *context, bConstraintOb *cob)
+{
+  MovieTrackingTrack *track = context->track;
+  if ((track->flag & TRACK_HAS_BUNDLE) == 0) {
+    return;
   }
 
-  if (!tracking_object) {
+  if ((context->tracking_object->flag & TRACKING_OBJECT_CAMERA) == 0) {
+    followtrack_evaluate_using_3d_position_object(context, cob);
     return;
   }
 
-  track = BKE_tracking_track_get_named(tracking, tracking_object, data->track);
+  followtrack_evaluate_using_3d_position_camera(context, cob);
+}
 
-  if (!track) {
+/* Apply undistortion if it is enabled in constraint settings. */
+static void followtrack_undistort_if_needed(FollowTrackContext *context,
+                                            const int clip_width,
+                                            const int clip_height,
+                                            float marker_position[2])
+{
+  if ((context->flag & FOLLOWTRACK_USE_UNDISTORTION) == 0) {
     return;
   }
 
-  framenr = BKE_movieclip_remap_scene_to_clip_frame(clip, ctime);
+  /* Undistortion need to happen in pixel space. */
+  marker_position[0] *= clip_width;
+  marker_position[1] *= clip_height;
 
-  if (data->flag & FOLLOWTRACK_USE_3D_POSITION) {
-    if (track->flag & TRACK_HAS_BUNDLE) {
-      float obmat[4][4], mat[4][4];
+  BKE_tracking_undistort_v2(
+      context->tracking, clip_width, clip_height, marker_position, marker_position);
 
-      copy_m4_m4(obmat, cob->matrix);
+  /* Normalize pixel coordinates back. */
+  marker_position[0] /= clip_width;
+  marker_position[1] /= clip_height;
+}
 
-      if ((tracking_object->flag & TRACKING_OBJECT_CAMERA) == 0) {
-        float imat[4][4];
+/* Modify the marker position matching the frame fitting method. */
+static void followtrack_fit_frame(FollowTrackContext *context,
+                                  const int clip_width,
+                                  const int clip_height,
+                                  float marker_position[2])
+{
+  if (context->frame_method == FOLLOWTRACK_FRAME_STRETCH) {
+    return;
+  }
 
-        copy_m4_m4(mat, camob->obmat);
+  Scene *scene = context->scene;
+  MovieClip *clip = context->clip;
 
-        BKE_tracking_camera_get_reconstructed_interpolate(
-            tracking, tracking_object, framenr, imat);
-        invert_m4(imat);
+  /* apply clip display aspect */
+  const float w_src = clip_width * clip->aspx;
+  const float h_src = clip_height * clip->aspy;
 
-        mul_m4_series(cob->matrix, obmat, mat, imat);
-        translate_m4(
-            cob->matrix, track->bundle_pos[0], track->bundle_pos[1], track->bundle_pos[2]);
-      }
-      else {
-        BKE_tracking_get_camera_object_matrix(camob, mat);
+  const float w_dst = scene->r.xsch * scene->r.xasp;
+  const float h_dst = scene->r.ysch * scene->r.yasp;
 
-        mul_m4_m4m4(cob->matrix, obmat, mat);
-        translate_m4(
-            cob->matrix, track->bundle_pos[0], track->bundle_pos[1], track->bundle_pos[2]);
-      }
-    }
+  const float asp_src = w_src / h_src;
+  const float asp_dst = w_dst / h_dst;
+
+  if (fabsf(asp_src - asp_dst) < FLT_EPSILON) {
+    return;
+  }
+
+  if ((asp_src > asp_dst) == (context->frame_method == FOLLOWTRACK_FRAME_CROP)) {
+    /* fit X */
+    float div = asp_src / asp_dst;
+    float cent = (float)clip_width / 2.0f;
+
+    marker_position[0] = (((marker_position[0] * clip_width - cent) * div) + cent) / clip_width;
   }
   else {
-    float vec[3], disp[3], axis[3], mat[4][4];
-    float aspect = (scene->r.xsch * scene->r.xasp) / (scene->r.ysch * scene->r.yasp);
-    float len, d;
+    /* fit Y */
+    float div = asp_dst / asp_src;
+    float cent = (float)clip_height / 2.0f;
 
-    BKE_object_where_is_calc_mat4(camob, mat);
+    marker_position[1] = (((marker_position[1] * clip_height - cent) * div) + cent) / clip_height;
+  }
+}
 
-    /* camera axis */
-    vec[0] = 0.0f;
-    vec[1] = 0.0f;
-    vec[2] = 1.0f;
-    mul_v3_m4v3(axis, mat, vec);
+/* Effectively this is a Z-depth of the object form the movie clip camera.
+ * The idea is to preserve this depth while moving the object in 2D. */
+static float followtrack_distance_from_viewplane_get(FollowTrackContext *context,
+                                                     bConstraintOb *cob)
+{
+  Object *camera_object = context->camera_object;
 
-    /* distance to projection plane */
-    copy_v3_v3(vec, cob->matrix[3]);
-    sub_v3_v3(vec, mat[3]);
-    project_v3_v3v3(disp, vec, axis);
+  float camera_matrix[4][4];
+  BKE_object_where_is_calc_mat4(camera_object, camera_matrix);
 
-    len = len_v3(disp);
+  const float z_axis[3] = {0.0f, 0.0f, 1.0f};
 
-    if (len > FLT_EPSILON) {
-      CameraParams params;
-      int width, height;
-      float pos[2], rmat[4][4];
+  /* Direction of camera's local Z axis in the world space. */
+  float camera_axis[3];
+  mul_v3_m4v3(camera_axis, camera_matrix, z_axis);
 
-      BKE_movieclip_get_size(clip, NULL, &width, &height);
-      BKE_tracking_marker_get_subframe_position(track, framenr, pos);
+  /* Distance to projection plane. */
+  float vec[3];
+  copy_v3_v3(vec, cob->matrix[3]);
+  sub_v3_v3(vec, camera_matrix[3]);
 
-      if (data->flag & FOLLOWTRACK_USE_UNDISTORTION) {
-        /* Undistortion need to happen in pixel space. */
-        pos[0] *= width;
-        pos[1] *= height;
+  float projection[3];
+  project_v3_v3v3(projection, vec, camera_axis);
 
-        BKE_tracking_undistort_v2(tracking, width, height, pos, pos);
+  return len_v3(projection);
+}
 
-        /* Normalize pixel coordinates back. */
-        pos[0] /= width;
-        pos[1] /= height;
-      }
+/* For the evaluated constraint object project it to the surface of the depth object. */
+static void followtrack_project_to_depth_object_if_needed(FollowTrackContext *context,
+                                                          bConstraintOb *cob)
+{
+  if (context->depth_object == NULL) {
+    return;
+  }
 
-      /* aspect correction */
-      if (data->frame_method != FOLLOWTRACK_FRAME_STRETCH) {
-        float w_src, h_src, w_dst, h_dst, asp_src, asp_dst;
+  Object *depth_object = context->depth_object;
+  Mesh *depth_mesh = BKE_object_get_evaluated_mesh(depth_object);
+  if (depth_mesh == NULL) {
+    return;
+  }
 
-        /* apply clip display aspect */
-        w_src = width * clip->aspx;
-        h_src = height * clip->aspy;
+  float depth_object_mat_inv[4][4];
+  invert_m4_m4(depth_object_mat_inv, depth_object->obmat);
 
-        w_dst = scene->r.xsch * scene->r.xasp;
-        h_dst = scene->r.ysch * scene->r.yasp;
+  float ray_start[3], ray_end[3];
+  mul_v3_m4v3(ray_start, depth_object_mat_inv, context->camera_object->obmat[3]);
+  mul_v3_m4v3(ray_end, depth_object_mat_inv, cob->matrix[3]);
 
-        asp_src = w_src / h_src;
-        asp_dst = w_dst / h_dst;
+  float ray_direction[3];
+  sub_v3_v3v3(ray_direction, ray_end, ray_start);
+  normalize_v3(ray_direction);
 
-        if (fabsf(asp_src - asp_dst) >= FLT_EPSILON) {
-          if ((asp_src > asp_dst) == (data->frame_method == FOLLOWTRACK_FRAME_CROP)) {
-            /* fit X */
-            float div = asp_src / asp_dst;
-            float cent = (float)width / 2.0f;
+  BVHTreeFromMesh tree_data = NULL_BVHTreeFromMesh;
+  BKE_bvhtree_from_mesh_get(&tree_data, depth_mesh, BVHTREE_FROM_LOOPTRI, 4);
 
-            pos[0] = (((pos[0] * width - cent) * div) + cent) / width;
-          }
-          else {
-            /* fit Y */
-            float div = asp_dst / asp_src;
-            float cent = (float)height / 2.0f;
+  BVHTreeRayHit hit;
+  hit.dist = BVH_RAYCAST_DIST_MAX;
+  hit.index = -1;
 
-            pos[1] = (((pos[1] * height - cent) * div) + cent) / height;
-          }
-        }
-      }
+  const int result = BLI_bvhtree_ray_cast(tree_data.tree,
+                                          ray_start,
+                                          ray_direction,
+                                          0.0f,
+                                          &hit,
+                                          tree_data.raycast_callback,
+                                          &tree_data);
 
-      BKE_camera_params_init(&params);
-      BKE_camera_params_from_object(&params, camob);
+  if (result != -1) {
+    mul_v3_m4v3(cob->matrix[3], depth_object->obmat, hit.co);
+  }
 
-      if (params.is_ortho) {
-        vec[0] = params.ortho_scale * (pos[0] - 0.5f + params.shiftx);
-        vec[1] = params.ortho_scale * (pos[1] - 0.5f + params.shifty);
-        vec[2] = -len;
+  free_bvhtree_from_mesh(&tree_data);
+}
 
-        if (aspect > 1.0f) {
-          vec[1] /= aspect;
-        }
-        else {
-          vec[0] *= aspect;
-        }
+static void followtrack_evaluate_using_2d_position(FollowTrackContext *context, bConstraintOb *cob)
+{
+  Scene *scene = context->scene;
+  MovieClip *clip = context->clip;
+  MovieTrackingTrack *track = context->track;
+  Object *camera_object = context->camera_object;
+  const float clip_frame = context->clip_frame;
+  const float aspect = (scene->r.xsch * scene->r.xasp) / (scene->r.ysch * scene->r.yasp);
 
-        mul_v3_m4v3(disp, camob->obmat, vec);
+  const float object_depth = followtrack_distance_from_viewplane_get(context, cob);
+  if (object_depth < FLT_EPSILON) {
+    return;
+  }
 
-        copy_m4_m4(rmat, camob->obmat);
-        zero_v3(rmat[3]);
-        mul_m4_m4m4(cob->matrix, cob->matrix, rmat);
+  int clip_width, clip_height;
+  BKE_movieclip_get_size(clip, NULL, &clip_width, &clip_height);
 
-        copy_v3_v3(cob->matrix[3], disp);
-      }
-      else {
-        d = (len * params.sensor_x) / (2.0f * params.lens);
+  float marker_position[2];
+  BKE_tracking_marker_get_subframe_position(track, clip_frame, marker_position);
 
-        vec[0] = d * (2.0f * (pos[0] + params.shiftx) - 1.0f);
-        vec[1] = d * (2.0f * (pos[1] + params.shifty) - 1.0f);
-        vec[2] = -len;
+  followtrack_undistort_if_needed(context, clip_width, clip_height, marker_position);
+  followtrack_fit_frame(context, clip_width, clip_height, marker_position);
 
-        if (aspect > 1.0f) {
-          vec[1] /= aspect;
-        }
-        else {
-          vec[0] *= aspect;
-        }
+  float rmat[4][4];
+  CameraParams params;
+  BKE_camera_params_init(&params);
+  BKE_camera_params_from_object(&params, camera_object);
 
-        mul_v3_m4v3(disp, camob->obmat, vec);
+  if (params.is_ortho) {
+    float vec[3];
+    vec[0] = params.ortho_scale * (marker_position[0] - 0.5f + params.shiftx);
+    vec[1] = params.ortho_scale * (marker_position[1] - 0.5f + params.shifty);
+    vec[2] = -object_depth;
 
-        /* apply camera rotation so Z-axis would be co-linear */
-        copy_m4_m4(rmat, camob->obmat);
-        zero_v3(rmat[3]);
-        mul_m4_m4m4(cob->matrix, cob->matrix, rmat);
+    if (aspect > 1.0f) {
+      vec[1] /= aspect;
+    }
+    else {
+      vec[0] *= aspect;
+    }
 
-        copy_v3_v3(cob->matrix[3], disp);
-      }
+    float disp[3];
+    mul_v3_m4v3(disp, camera_object->obmat, vec);
 
-      if (data->depth_ob) {
-        Object *depth_ob = data->depth_ob;
-        Mesh *target_eval = BKE_object_get_evaluated_mesh(depth_ob);
-        if (target_eval) {
-          BVHTreeFromMesh treeData = NULL_BVHTreeFromMesh;
-          BVHTreeRayHit hit;
-          float ray_start[3], ray_end[3], ray_nor[3], imat[4][4];
-          int result;
+    copy_m4_m4(rmat, camera_object->obmat);
+    zero_v3(rmat[3]);
+    mul_m4_m4m4(cob->matrix, cob->matrix, rmat);
 
-          invert_m4_m4(imat, depth_ob->obmat);
+    copy_v3_v3(cob->matrix[3], disp);
+  }
+  else {
+    const float d = (object_depth * params.sensor_x) / (2.0f * params.lens);
 
-          mul_v3_m4v3(ray_start, imat, camob->obmat[3]);
-          mul_v3_m4v3(ray_end, imat, cob->matrix[3]);
+    float vec[3];
+    vec[0] = d * (2.0f * (marker_position[0] + params.shiftx) - 1.0f);
+    vec[1] = d * (2.0f * (marker_position[1] + params.shifty) - 1.0f);
+    vec[2] = -object_depth;
+
+    if (aspect > 1.0f) {
+      vec[1] /= aspect;
+    }
+    else {
+      vec[0] *= aspect;
+    }
 
-          sub_v3_v3v3(ray_nor, ray_end, ray_start);
-          normalize_v3(ray_nor);
+    float disp[3];
+    mul_v3_m4v3(disp, camera_object->obmat, vec);
 
-          BKE_bvhtree_from_mesh_get(&treeData, target_eval, BVHTREE_FROM_LOOPTRI, 4);
+    /* apply camera rotation so Z-axis would be co-linear */
+    copy_m4_m4(rmat, camera_object->obmat);
+    zero_v3(rmat[3]);
+    mul_m4_m4m4(cob->matrix, cob->matrix, rmat);
 
-          hit.dist = BVH_RAYCAST_DIST_MAX;
-          hit.index = -1;
+    copy_v3_v3(cob->matrix[3], disp);
+  }
 
-          result = BLI_bvhtree_ray_cast(
-              treeData.tree, ray_start, ray_nor, 0.0f, &hit, treeData.raycast_callback, &treeData);
+  followtrack_project_to_depth_object_if_needed(context, cob);
+}
 
-          if (result != -1) {
-            mul_v3_m4v3(cob->matrix[3], depth_ob->obmat, hit.co);
-          }
+static void followtrack_evaluate(bConstraint *con, bConstraintOb *cob, ListBase *UNUSED(targets))
+{
+  FollowTrackContext context;
+  if (!followtrack_context_init(&context, con, cob)) {
+    return;
+  }
 
-          free_bvhtree_from_mesh(&treeData);
-        }
-      }
-    }
+  bFollowTrackConstraint *data = con->data;
+  if (data->flag & FOLLOWTRACK_USE_3D_POSITION) {
+    followtrack_evaluate_using_3d_position(&context, cob);
+    return;
   }
+
+  followtrack_evaluate_using_2d_position(&context, cob);
 }
 
 static bConstraintTypeInfo CTI_FOLLOWTRACK = {