xref: /aosp_15_r20/external/libchrome-gestures/src/split_correcting_filter_interpreter.cc (revision aed3e5085e770be5b69ce25295ecf6ddf906af95)

// Copyright 2012 The ChromiumOS Authors
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "include/split_correcting_filter_interpreter.h"

#include <math.h>

#include "include/tracer.h"
#include "include/util.h"

namespace gestures {

// Takes ownership of |next|:
SplitCorrectingFilterInterpreter::SplitCorrectingFilterInterpreter(
    PropRegistry* prop_reg, Interpreter* next, Tracer* tracer)
    : FilterInterpreter(nullptr, next, tracer, false),
      enabled_(prop_reg, "Split Corrector Enabled", false),
      merge_max_separation_(prop_reg, "Split Merge Max Separation", 17.0),
      merge_max_movement_(prop_reg, "Split Merge Max Movement", 3.0),
      merge_max_ratio_(prop_reg, "Merge Max Ratio", sinf(DegToRad(19.0))) {
  InitName();
}

void SplitCorrectingFilterInterpreter::SyncInterpretImpl(
    HardwareState& hwstate,
    stime_t* timeout) {
  const char name[] = "SplitCorrectingFilterInterpreter::SyncInterpretImpl";
  LogHardwareStatePre(name, hwstate);

  // Update internal state
  if (enabled_.val_) {
    RemoveMissingUnmergedContacts(hwstate);
    MergeFingers(hwstate);
    UnmergeFingers(hwstate);
    UpdateUnmergedLocations(hwstate);
    SetLastTrackingIds(hwstate);

    // Use internal state to update hwstate
    UpdateHwState(hwstate);
  }
  LogHardwareStatePost(name, hwstate);
  next_->SyncInterpret(hwstate, timeout);
}

void SplitCorrectingFilterInterpreter::RemoveMissingUnmergedContacts(
    const HardwareState& hwstate) {
  for (UnmergedContact* it = unmerged_;
       it < &unmerged_[arraysize(unmerged_)] &&
           it->Valid();) {
    if (!hwstate.GetFingerState(it->input_id)) {
      // Erase this element
      std::copy(it + 1, &unmerged_[arraysize(unmerged_)], it);
      unmerged_[arraysize(unmerged_) - 1].Invalidate();
    } else {
      ++it;
    }
  }
}

void SplitCorrectingFilterInterpreter::MergeFingers(
    const HardwareState& hwstate) {
  std::set<const FingerState*> unused;
  for (size_t i = 0; i < hwstate.finger_cnt; i++) {
    if (!SetContainsValue(last_tracking_ids_, hwstate.fingers[i].tracking_id))
      unused.insert(&hwstate.fingers[i]);
  }
  if (unused.empty())
    return;
  for (UnmergedContact* it = unmerged_; it->Valid();) {
    // Current state of the unmerged finger
    const FingerState* existing_contact = hwstate.GetFingerState(it->input_id);
    if (!existing_contact) {
      Err("How is existing_contact null?");
      return;
    }
    // try all fingers for possible merging
    float min_error = INFINITY;
    std::set<const FingerState*>::iterator min_error_it = unused.end();
    for (std::set<const FingerState*>::iterator unused_it =
             unused.begin(), e = unused.end(); unused_it != e; ++unused_it) {
      const FingerState* new_contact = *unused_it;
      if (new_contact == existing_contact)
        continue;
      float error = AreMergePair(*existing_contact, *new_contact, *it);
      if (error < 0)
        continue;
      if (error < min_error) {
        min_error = error;
        min_error_it = unused_it;
      }
    }
    if (min_error_it != unused.end()) {
      // we have a merge!
      AppendMergedContact(*existing_contact, *(*min_error_it), it->output_id);
      unused.erase(min_error_it);
      // Delete this UnmergedContact
      std::copy(it + 1, &unmerged_[arraysize(unmerged_)], it);
      unmerged_[arraysize(unmerged_) - 1].Invalidate();
      continue;
    }
    it++;
  }
  if (unused.empty())
    return;
  // Put the unused new fingers into the unmerged fingers
  // Find next slot
  UnmergedContact* it = unmerged_;
  for (; it->Valid() && it != &unmerged_[kMaxFingers]; ++it) {}
  for (std::set<const FingerState*>::iterator unused_it =
           unused.begin(), e = unused.end(); unused_it != e; ++unused_it) {
    if (it == &unmerged_[kMaxFingers]) {
      Err("How is there no space?");
      return;
    }
    const FingerState& fs = *(*unused_it);
    it->input_id = it->output_id = fs.tracking_id;
    it->position_x = fs.position_x;
    it->position_y = fs.position_y;
    it++;
  }
}

void SplitCorrectingFilterInterpreter::AppendMergedContact(
    const FingerState& input_a,
    const FingerState& input_b,
    short output_id) {
  for (size_t i = 0; i < arraysize(merged_); i++) {
    if (merged_[i].Valid())
      continue;
    merged_[i].input_fingers[0] = input_a;
    merged_[i].input_fingers[1] = input_b;
    merged_[i].output_id = output_id;
    return;
  }
  Err("No free merged contact?");
  return;
}

void SplitCorrectingFilterInterpreter::AppendUnmergedContact(
    const FingerState& fs, short output_id) {
  for (size_t i = 0; i < arraysize(unmerged_); i++) {
    if (unmerged_[i].Valid())
      continue;
    unmerged_[i].input_id = fs.tracking_id;
    unmerged_[i].output_id = output_id;
    unmerged_[i].position_x = fs.position_x;
    unmerged_[i].position_y = fs.position_y;
    return;
  }
  Err("No free unmerged contact?");
}

float SplitCorrectingFilterInterpreter::AreMergePair(
    const FingerState& existing_contact,
    const FingerState& new_contact,
    const UnmergedContact& merge_recipient) const {
  // Is it close enough to the old contact?
  const float kMaxSepSq =
      merge_max_separation_.val_ * merge_max_separation_.val_;
  float sep_sq = DistSq(new_contact, existing_contact);
  if (sep_sq > kMaxSepSq) {
    return -1;
  }
  // Does this new contact help?
  float existing_move_sq = DistSq(merge_recipient, existing_contact);
  float mid_x = (new_contact.position_x + existing_contact.position_x) * 0.5;
  float mid_y = (new_contact.position_y + existing_contact.position_y) * 0.5;
  float old_to_mid_dist_sq = DistSqXY(merge_recipient, mid_x, mid_y);
  if (old_to_mid_dist_sq < existing_move_sq)
    return old_to_mid_dist_sq;  // Return new distance; definite improvement

  // Check if the merge recipient is too far from new_contact
  float current_dist_sq = DistSq(existing_contact, new_contact);
  float new_to_reicpient_sq = DistSq(merge_recipient, new_contact);
  if (current_dist_sq < new_to_reicpient_sq)
    return -1;

  // Check if the new contact is, more or less, "along the line" from
  // existing contact through merge_recipient, and beyond.

  // Distance_sq from new_contact to the line that goes through merge_recipient
  // and existing_contact.
  const float orthogonal_dist_sq =
      DistSqFromPointToLine(merge_recipient.position_x,
                            merge_recipient.position_y,
                            existing_contact.position_x,
                            existing_contact.position_y,
                            new_contact.position_x,
                            new_contact.position_y);

  // Imagine a right-triangle like so:
  //                         /|(new point)
  //                      /   | <== orthogonal_dist
  // (existing_contact)/__m___|(right angle)
  //                    ^^^^^ line between existing_contact and merge_recipient
  // m = merge_recipient point.
  // We compute the maximum ratio of orthogonal_dist / hypotenuse length

  if (orthogonal_dist_sq <
      merge_max_ratio_.val_ * merge_max_ratio_.val_ * current_dist_sq)
    return old_to_mid_dist_sq;  // merge!

  return -1;  // no merge
}

// static
float SplitCorrectingFilterInterpreter::DistSqFromPointToLine(float line_x_0,
                                                              float line_y_0,
                                                              float line_x_1,
                                                              float line_y_1,
                                                              float point_x,
                                                              float point_y) {
  // Find general form (A*x + B*y + C = 0) of a line given two points.
  float line_a = line_y_0 - line_y_1;
  float line_b = line_x_1 - line_x_0;
  float line_c = line_x_0 * line_y_1 - line_y_0 * line_x_1;
  // Compute min distance from line to point_(x,y)
  float num = line_a * point_x + line_b * point_y + line_c;
  float den_sq = line_a * line_a + line_b * line_b;
  if (den_sq == 0.0)
    return 0.0;  // don't crash
  return num * num / den_sq;
}

void SplitCorrectingFilterInterpreter::UnmergeFingers(
    const HardwareState& hwstate) {
  const float kMaxSepSq =
      merge_max_separation_.val_ * merge_max_separation_.val_;
  const float kMaxMoveSq =
      merge_max_movement_.val_ * merge_max_movement_.val_;
  for (size_t i = 0; i < arraysize(merged_);) {
    MergedContact* mc = &merged_[i];
    if (!mc->Valid())
      break;
    const FingerState* first =
        hwstate.GetFingerState(mc->input_fingers[0].tracking_id);
    const FingerState* second =
        hwstate.GetFingerState(mc->input_fingers[1].tracking_id);
    if (first && second && DistSq(*first, *second) <= kMaxSepSq &&
        DistSq(*first, mc->input_fingers[0]) < kMaxMoveSq &&
        DistSq(*second, mc->input_fingers[1]) < kMaxMoveSq) {
      i++;
      continue;
    }
    if (first)
      AppendUnmergedContact(*first, mc->output_id);
    if (second)
      // For no good reason, if we have both first and second, we give
      // first the output id, thus it takes over for the merged finger
      AppendUnmergedContact(*second,
                            first ? second->tracking_id : mc->output_id);
    // Delete this element
    std::copy(&merged_[i + 1], &merged_[arraysize(merged_)], &merged_[i]);
    merged_[arraysize(merged_) - 1].Invalidate();
  }
}

void SplitCorrectingFilterInterpreter::UpdateHwState(
    HardwareState& hwstate) const {
  for (size_t i = 0; i < hwstate.finger_cnt; i++) {
    FingerState* fs = &hwstate.fingers[i];
    const UnmergedContact* unmerged = FindUnmerged(fs->tracking_id);
    if (unmerged && unmerged->Valid()) {
      // Easier case. Just update tracking id
      fs->tracking_id = unmerged->output_id;
      continue;
    }
    const MergedContact* merged = FindMerged(fs->tracking_id);
    if (merged && merged->Valid()) {
      short other_id = merged->input_fingers[0].tracking_id != fs->tracking_id ?
          merged->input_fingers[0].tracking_id :
          merged->input_fingers[1].tracking_id;
      FingerState* other_fs = hwstate.GetFingerState(other_id);
      if (!other_fs) {
        Err("Missing other finger state?");
        return;
      }
      JoinFingerState(fs, *other_fs);
      fs->tracking_id = merged->output_id;
      RemoveFingerStateFromHardwareState(hwstate, other_fs);
      continue;
    }
    Err("Neither unmerged nor merged?");
    return;
  }
  hwstate.touch_cnt = hwstate.finger_cnt;
}

const UnmergedContact* SplitCorrectingFilterInterpreter::FindUnmerged(
    short input_id) const {
  for (size_t i = 0; i < arraysize(unmerged_) && unmerged_[i].Valid(); i++)
    if (unmerged_[i].input_id == input_id)
      return &unmerged_[i];
  return nullptr;
}

const MergedContact* SplitCorrectingFilterInterpreter::FindMerged(
    short input_id) const {
  for (size_t i = 0; i < arraysize(merged_) && merged_[i].Valid(); i++)
    if (merged_[i].input_fingers[0].tracking_id == input_id ||
        merged_[i].input_fingers[1].tracking_id == input_id)
      return &merged_[i];
  return nullptr;
}

// static
void SplitCorrectingFilterInterpreter::JoinFingerState(
    FingerState* in_out, const FingerState& newfinger) {
  float FingerState::*fields[] = { &FingerState::touch_major,
                                   &FingerState::touch_minor,
                                   &FingerState::width_major,
                                   &FingerState::width_minor,
                                   &FingerState::pressure,
                                   &FingerState::orientation,
                                   &FingerState::position_x,
                                   &FingerState::position_y };
  for (size_t f_idx = 0; f_idx < arraysize(fields); f_idx++) {
    float FingerState::*field = fields[f_idx];
    in_out->*field = (in_out->*field + newfinger.*field) * 0.5;
  }
  in_out->flags |= newfinger.flags |
      GESTURES_FINGER_WARP_X |
      GESTURES_FINGER_WARP_Y;
}

// static
void SplitCorrectingFilterInterpreter::RemoveFingerStateFromHardwareState(
    HardwareState& hwstate,
    FingerState* fs) {
  std::copy(fs + 1, &hwstate.fingers[hwstate.finger_cnt], fs);
  hwstate.finger_cnt--;
}

void SplitCorrectingFilterInterpreter::SetLastTrackingIds(
    const HardwareState& hwstate) {
  last_tracking_ids_.clear();
  for (size_t i = 0; i < hwstate.finger_cnt; i++)
    last_tracking_ids_.insert(hwstate.fingers[i].tracking_id);
}

void SplitCorrectingFilterInterpreter::UpdateUnmergedLocations(
    const HardwareState& hwstate) {
  for (size_t i = 0; i < arraysize(unmerged_) && unmerged_[i].Valid(); i++) {
    const FingerState* fs = hwstate.GetFingerState(unmerged_[i].input_id);
    if (!fs) {
      Err("Missing finger state?");
      continue;
    }
    unmerged_[i].position_x = fs->position_x;
    unmerged_[i].position_y = fs->position_y;
  }
}

void SplitCorrectingFilterInterpreter::Dump(
    const HardwareState& hwstate) const {
  Log("Last Tracking IDs:");
  for (std::set<short>::const_iterator it = last_tracking_ids_.begin(),
           e = last_tracking_ids_.end(); it != e; ++it)
    Log("  %d", *it);
  Log("Unmerged:");
  for (size_t i = 0; i < arraysize(unmerged_); i++)
    Log("  %sin: %d out: %d x: %f y: %f",
        unmerged_[i].Valid() ? "" : "INV ",
        unmerged_[i].input_id,
        unmerged_[i].output_id,
        unmerged_[i].position_x,
        unmerged_[i].position_y);
  Log("Merged:");
  for (size_t i = 0; i < arraysize(merged_); i++)
    Log("  %sin: %d in: %d out: %d",
        merged_[i].Valid() ? "" : "INV ",
        merged_[i].input_fingers[0].tracking_id,
        merged_[i].input_fingers[1].tracking_id,
        merged_[i].output_id);
  Log("HW state IDs:");
  for (size_t i = 0; i < hwstate.finger_cnt; i++)
    Log("  %d", hwstate.fingers[i].tracking_id);
}

};  // namespace gestures