bool isTaskInSolvedState()

in src/simulator/task_validation.cpp [344:400]
50 lines of code
16 McCabe index (conditional complexity)

bool isTaskInSolvedState(const ::task::Task& task,
                         const b2WorldWithData& world) {
  checkTaskValidity(task);
  const b2Body* body1 = nullptr;
  const b2Body* body2 = nullptr;

  for (const b2Body* box2dBody = world.GetBodyList(); box2dBody != nullptr;
       box2dBody = box2dBody->GetNext()) {
    if (box2dBody->GetUserData() == nullptr) {
      throw std::runtime_error("Found a Box2d body without userdata");
    }
    Box2dData* box2d_data = static_cast<Box2dData*>(box2dBody->GetUserData());
    if (box2d_data == nullptr) {
      throw std::runtime_error(
          "Found a Box2d body with userdata that is not Box2dData");
    }
    if (box2d_data->object_type == Box2dData::GENERAL) {
      if (box2d_data->object_id == task.bodyId1) {
        body1 = box2dBody;
      } else if (box2d_data->object_id == task.bodyId2) {
        body2 = box2dBody;
      }
    }
    // Found both bodies, quit loop
    if (body1 != nullptr && body2 != nullptr) {
      break;
    }
  }

  if (body1 == nullptr || body2 == nullptr) {
    throw std::runtime_error("Task body IDs not present in the scene");
  }

  const auto& thriftBody1 = task.scene.bodies[task.bodyId1];
  const auto& thriftBody2 = task.scene.bodies[task.bodyId2];
  // A custom check for a pair of touching balls to improve stability.
  if (isTwoBallTouchingCase(thriftBody1, thriftBody2, task.relationships)) {
    const auto r1 = body1->GetFixtureList()->GetShape()->m_radius;
    const auto r2 = body2->GetFixtureList()->GetShape()->m_radius;
    const float distance = sqrt(
        geometry::squareDistance(body1->GetPosition(), body2->GetPosition()));
    return distance < r1 + r2 + kBallTouchingThreshold;
  }

  ::scene::Shape scaledPhantomShape;
  if (task.__isset.phantomShape && task.phantomShape.__isset.polygon) {
    scaledPhantomShape = p2mShape(task.phantomShape);
  }

  for (auto r = task.relationships.begin(); r != task.relationships.end();
       ++r) {
    if (isValidRelationship(*body1, *body2, *r, scaledPhantomShape) == false) {
      return false;
    }
  }
  return true;
}