Rainbow ellipses collision algorithm

ellipses.cpp

/*
 * Copyright (C) 2021-2023 Jean-Marie BARAN (jeanmarie.baran@gmail.com)
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the “Software”), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

/*
 * Algorithm for ellipse to ellipse collision detection. Full explanation at:
 * https://medium.com/@jeanmarie.baran/algorithm-for-ellipse-to-ellipse-collision-detection-6384b0486ef5
 *
 * Disclaimer 1: This algorithm works only for axis aligned ellipses.
 * Disclaimer 2: This algorithm cannot find the actual intersection point(s), it
 * can only tell whether two ellipses intersect - not where.
 * Disclaimer 3: In the case of one ellipse enclosing the other, this algorithm
 * reports a collision, although mathematically speaking the ellipses do not
 * intersect.
 */

#include <cmath>
#include <iostream>
#include <utility>

using std::abs;
using std::ceil;
using std::cout;
using std::endl;
using std::sqrt;
using std::swap;

double square(double value) { return value * value; }
double vectorMagnitude(double vX, double vY) {
  return sqrt(square(vX) + square(vY));
}

/* For a circle at coordinates (a, b) with radius `r`:
 * (x - a) ^ 2 + (y - b) ^ 2 = r ^ 2.
 * In this algorithm (x, y) are used instead of (a, b) to be more consistent
 * with the definition of the ellipse below. */
struct Circle {
  double x;  // X-axis coordinate of the center.
  double y;  // Y-axis coordinate of the center.
  double r;
};

/* For an ellipse centered at the origin with width 2a and height 2b, and
 * assuming a >= b:
 * (x ^ 2) / (a ^ 2) + (y ^ 2) / (b ^ 2) = 1.
 * c = sqrt(a ^ 2 - b ^ 2)
 * For any point on the ellipse, the foci (-c, 0) and (c, 0) form two segments
 * whose cumulated lengths is equal to 2a. Also very helpful:
 * https://en.wikipedia.org/wiki/Ellipse */
struct Ellipse {
  Ellipse(double a, double b, double x, double y)
      : a{a}, b{b}, c{sqrt(abs(square(a) - square(b)))}, x{x}, y{y} {};

  double a;  // Horizontal semi-axis (/!\ not necessarily the major semi-axis).
  double b;  // Idem but vertical.
  double c;
  double x;  // X-axis coordinate of the center.
  double y;  // Y-axis coordinate of the center.
};

constexpr double kResolution{1};

/* This algorithm is an approximation, but a rather good one. It consists in
 * transforming the problem of two ellipses to the problem of one circle and one
 * ellipse. Note that the ellipses are always axis-aligned, which simplifies the
 * algorithm. Points are selected on the circle, if at least one of them is
 * inside the ellipse, there is a collision.
 *
 * Summary: points are selected on the major axis of the ellipse. For each, let
 * a vector from the center of the circle and pointing toward that point, whose
 * length is equal to the radius of the circle. Let the point at the extremity
 * of the vector. If that point is in the ellipse, there is a collision.
 * Otherwise, continue with the other points on the major axis.
 *
 * Steps:
 * 1. translate everything so that the first ellipse is centered at the
 * origin
 * 2. deform the space to make a circle out of the second ellipse
 * 3. for simplicity if the ellipse major axis is vertical, swap the coordinates
 * so that it becomes horizontal
 * 4. decide on the number of points to check (this affects the accuracy of the
 * algorithm: more points means more accuracy)
 * 5. subdivide the major axis for getting the required number of points
 * 6. for each point, compute the vector described in the summary above
 * 7. for each vector, check whether the point pointed at by the vector is in
 * the ellipse; if yes return a collision, otherwise continue
 * 8. eliminate the case where the circle contains the ellipse. For performance
 * reasons, this is actually done between steps three and four.
 *
 * A bounding boxes test could be executed first, to speed up cases where
 * ellipses are far away. And because ellipses could actually be circles, a
 * circle to circle collision test could also be performed beforehand. */
bool collide(Ellipse const& ellipse1, Ellipse const& ellipse2) {
  // Step 1: Translation to center the first ellipse at the origin.
  double tX{-ellipse1.x};
  double tY{-ellipse1.y};

  // Step 2: Deformation to create a circle from the second ellipse.
  double dX{ellipse2.b};
  double dY{ellipse2.a};

  // Second ellipse to circle.
  Circle circle{};
  circle.x = (ellipse2.x + tX) * dX;
  circle.y = (ellipse2.y + tY) * dY;
  circle.r = ellipse2.a * dX;

  // First ellipse to origin.
  Ellipse ellipse{ellipse1.a * dX, ellipse1.b * dY, 0, 0};

  // Step 3: Swap axes so that the major axis of the ellipse is horizontal.
  if (ellipse.a < ellipse.b) {
    // Swap axes of the ellipse.
    swap(ellipse.a, ellipse.b);
    // Swap coordinates of the circle.
    swap(circle.x, circle.y);
  }

  /* Now the ellipse is centered at the origin, with horizontal radius `a` and
   * vertical radius `b` and the distance to the foci `c`. The major axis
   * coincides with the horizontal axis. */

  /* Step 8: This is a special case when the circle contains the ellipse. In
   * this case, the rest of the algorithm would not report a collision. Just
   * check whether the center of the ellipse is in the circle. It eliminates
   * that case, and also a handful of others where they intersect (which would
   * have been detected later anyway). */
  if (vectorMagnitude(0 - circle.x, 0 - circle.y) < circle.r) {
    return true;
  }

  /* Step 4: Decide on the number of points to check on the major axis of the
   * ellipse. */
  int64_t num_points{static_cast<int64_t>(ceil(2 * ellipse.a * kResolution))};
  double step{2 * ellipse.a / static_cast<double>(num_points)};
  for (int64_t i{0}; i < num_points; i++) {
    // Step 5: Subdivide the axis (get the i-nth point).
    double x{-ellipse.a + static_cast<double>(i) * step};
    double y{0};

    /* Step 6: Construct the vector. Firstly build the point of intersection of
     * the circle with the ray cast from the center of the circle and passing by
     * (x, y). */
    double vX{x - circle.x};
    double vY{y - circle.y};
    double magnitude{vectorMagnitude(vX, vY)};

    // Secondly transform the vector so that its magnitude equals the radius.
    vX *= circle.r / magnitude;
    vY *= circle.r / magnitude;

    // Finally get the point on the circle (reuse the variables x, y).
    x = circle.x + vX;
    y = circle.y + vY;

    // Step 7: Check whether the point (x, y) is in the ellipse.
    double f1X{-ellipse.c};
    double f1Y{0};
    double f2X{ellipse.c};
    double f2Y{0};
    if (vectorMagnitude(x - f1X, y - f1Y) + vectorMagnitude(x - f2X, y - f2Y) <=
        2 * ellipse.a) {
      return true;
    }
  }

  // If no point was found to be in the ellipse, there is no collision.
  return false;
}

void test(bool expected, bool result) {
  cout << "Expected " << (expected ? "true" : "false") << ": "
       << (result ? "true" : "false") << endl;
}

int main(int argc, char* argv[]) {
  test(true, collide(Ellipse{1, 1, 0, 0}, Ellipse{1, 1, 1, 0}));
  test(true, collide(Ellipse{11.2, 5, -5, 0}, Ellipse{11.2, 10, 10, -5}));
  test(true, collide(Ellipse{7.1, 5, -5, -15}, Ellipse{11.2, 10, 10, -10}));
  test(true, collide(Ellipse{5, 35.4, -10, 0}, Ellipse{40.3, 20, 30, -30}));
  test(true, collide(Ellipse{10, 18, -15, 5}, Ellipse{21.2, 15, 15, 10}));
  test(true, collide(Ellipse{5, 11.2, 10, 10}, Ellipse{21.2, 15, 15, 10}));
  test(true, collide(Ellipse{5, 11.2, 10, 5}, Ellipse{21.2, 15, 15, 10}));
  test(true, collide(Ellipse{1, 60, -40, 0}, Ellipse{60, 1, 0, 10}));
  test(true, collide(Ellipse{6.27, 8.68, -8, -2}, Ellipse{10, 11.66, 8, 0}));
  cout << "---" << endl;
  test(false, collide(Ellipse{1, 1, 0, 0}, Ellipse{1, 1, 3, 0}));
  test(false, collide(Ellipse{11.2, 5, -15, 10}, Ellipse{11.2, 10, 10, -5}));
  test(false, collide(Ellipse{7.1, 5, -5, -15}, Ellipse{11.2, 10, 10, -5}));
  test(false, collide(Ellipse{5, 35.4, -10, 0}, Ellipse{36.4, 10, 30, -30}));
  test(false, collide(Ellipse{10, 14.1, -15, 0}, Ellipse{21.2, 15, 15, 10}));
  test(false, collide(Ellipse{1, 60, -62, -52}, Ellipse{60, 1, 0, 10}));
  test(false, collide(Ellipse{20, 63.3, -40, -20}, Ellipse{40, 72.1, 20, 0}));
  test(false, collide(Ellipse{60, 84.9, -80, -20}, Ellipse{100, 116.6, 80, 0}));
  test(false, collide(Ellipse{40.3, 35, 10, 20}, Ellipse{3.4, 2.2, 42.5, -10}));

  return 0;
}