Complex logic that I'm certain can be simplified

This is code from a grappling hook implementation. This is specifically code that, after a block is detected between two way points, it tries to find a path around.

This particular piece of the function considers the blocks that are in the way and decides which corners are appropriate to wrap around.

The point will always be on the edge. If it's not the very first (anchor) point, it will always be on a corner. So if the problem block is our block we just choose the two closest corners. Otherwise we need to look at this diagram.

In the picture the block at 3 o'clock corresponds with case 1, at 1:30 is case 3, 12 o'clock is case 2, and 10:30 is case 4. 9 o'clock is a special case, when the point and the block share an orthogonal edge, it should use the far point instead of the near point.

Here's my code, problem is a Vec2I which is a std::array with several convenience functions. origin is a GrapplingHookPathPoint which is a struct defined as:

struct GrapplingHookPathPoint {
  GrapplingHookPathPoint() : anchorPoint(Vec2F::filled(nan())), anchorBroken(), endpoint() {}
  GrapplingHookPathPoint(Vec2I anchoredBlock, Vec2F anchorPoint, bool anchorBroken, bool endpoint)
      : anchoredBlock(anchoredBlock), anchorPoint(anchorPoint), anchorBroken(anchorBroken), endpoint(endpoint) {}
  Vec2I anchoredBlock;
  Vec2F anchorPoint;
  bool anchorBroken;
  bool endpoint;
  bool operator==(GrapplingHookPathPoint const& rhs) const;
};

Vec2F is std::array, RectF is a Rectangle class

```
if (dest.endpoint && RectF::withSize(Vec2F(problem), {1, 1}).contains(dest.anchorPoint))
continue;

if (origin.anchoredBlock == problem) {
Logger::debug("door 0\n");
if (origin.anchorPoint[0] == origin.anchoredBlock[0] &&
origin.anchorPoint[1] == origin.anchoredBlock[1]) { // bottom - left corner
Logger::debug("subdoor -3\n");
corners.append({problem, Vec2F(problem) + Vec2F{1, 0}, false, false});
corners.append({problem, Vec2F(problem) + Vec

I can't follow your code at all, mainly because I don't understand your terminology. What'a a GrapplingHookPathPoint? anchoredBlock? anchorPoint? problem?

Anyway, I'll describe how I would approach the problem instead.

Let's assume that all coordinates are expressed relative to the desired origin point. If not, make it so.

Then, for each square (which I'll call a Block), sort the four vertices according to their angle if they were expressed in polar coordinates. The first and last vertex after sorting are the ones you are looking for. One advantage of this solution over yours is that the squares do not have to be rotationally aligned with the x and y axes. The main benefit is that there are many fewer cases to be covered — the computer, rather than the programmer, does the hard work.

I'd like to avoid having to call atan2(), both because trigonometric functions are computationally intensive and because arctangents fail at 90°. Instead of sorting by the angle, I can just sort by the cosine of the angle, as long as all of the points are on the same side of the X-axis. (The cosine of the angle can be calculated using the rule A ⋅ B = |A| |B| cos θ .) If a square straddles the X-axis, I can try using the Y-axis instead. If a square straddles both the X- and the Y-axis, then it must contain the origin, which is an error.

Tested using clang++ -std=c++11 -g -o cr31597 cr31597.cpp and on ideone.

```
#include // for std::sort
#include
#include
#include // for sqrt

//////////////////////////////////////////////////////////////////////

class Vec2F {
public:
float x, y;
static const Vec2F ORIGIN, I, J;

Vec2F() : x(0), y(0) {}
Vec2F(float x, float y) : x(x), y(y) {}
Vec2F(const Vec2F &other) : x(other.x), y(other.y) {}
Vec2F &operator=(const Vec2F &other) {
if (this != &other) {
x = other.x;
y = other.y;
}
return *this;
}
bool operator==(const Vec2F &other) const {
return x == other.x && y == other.y;
}
const Vec2F operator+(const Vec2F &other) const {
return Vec2F(x + other.x, y + other.y);
}
Vec2F operator-() const {
return Vec2F(-x, -y);
}
Vec2F operator-(const Vec2F &other) const {
return *this + -other;
}
Vec2F operator*(float scale) const {
return Vec2F(scale x, scale y);
}
float cross(const Vec2F &other) const {
return x other.y - y other.x;
}
float dot(const Vec2F &other) const {
return x other.x + y other.y;
}
float magnitude() const {
return sqrt(x x + y y);
}
float sinOfAngleRelTo(const Vec2F &ref) const {
return ref.cross(*this) / ref.magnitude() / this->magnitude();
}
float cosOfAngleRelTo(const Vec2F &ref) const {
return ref.dot(*this) / ref.magnitude() / this->magnitude();
}
friend std::ostream &operator vertices;

// Block is specified by its southwest corner
Block(const Vec2F &sw) {
Vec2F v[] = {
sw + Vec2F::J, sw + Vec2F(1, 1),
sw, sw + Vec2F::I,
};
const_cast& >(vertices) =
std::vector(v, v + sizeof(v) / sizeof(Vec2F));
}
Block(const Block &other) : vertices(other.vertices) {}
friend std::ostream &operator';
}
private:
Block &operator=(const Block &other);
};

//////////////////////////////////////////////////////////////////////

// Comparator for Vec2F to be used to sort points by how much their angle
// deviates from a reference vector
class AxisComparator {
public:
static const AxisComparator HORIZ, VERT;
const Vec2F ref;

AxisComparator(const Vec2F &ref) : ref(ref) {}

// Returns true if a should be ordered before b
bool operator()(const Vec2F &a, const Vec2F &b) const {
float cosAngleA = a.cosOfAngleRelTo(ref);
float cosAngleB = b.cosOfAngleRelTo(ref);
if (cosAngleA == cosAngleB) {
// Same angle; the longer vector is considered more extreme.
return cosAngleA b.magnitude()
: a.magnitude() &vv) const {
int side = 0;
for (auto i = vv.begin(); i != vv.end(); ++i) {
float sinAngle = (*i).sinOfAngleRelTo(ref);
if (sinAngle == 0.0) { // aligned with ref
return true;
} else if (side == 0) { // 1st time through
side = (sinAngle 0.0) { // wrong side
return true;
}
}
return false;
}
};

const AxisComparator AxisComparator::HORIZ(Vec2F::I),
AxisComparator::VERT(Vec2F::J);

//////////////////////////////////////////////////////////////////////

class Grapple {
public:
const Block block;
private:
std::vector vertices;
const Vec2F v1, v2;

public:
Grapple(const Block &b) :

StackExchange Code Review Q#31597, answer score: 3