Pinball/src/vector2D.h

#ifndef VECTOR2D_H
#define VECTOR2D_H

#include <num/num.h>
#include <stdint.h>

libnum::num32 sqrt_num32(libnum::num32 v) {
  uint32_t t, q, b, r;
  r = v.v;
  b = 0x40000000;
  q = 0;
  while (b > 0x40) {
    t = q + b;
    if (r >= t) {
      r -= t;
      q = t + b;
    }
    r <<= 1;
    b >>= 1;
  }
  q >>= 8;
  libnum::num32 ret;
  ret.v = q;
  return ret;
}


class Vector2D {

public:
  Vector2D() {
    this->x = libnum::num32(0);
    this->y = libnum::num32(0);
  }

  Vector2D(float x, float y) {
    this->x = libnum::num32(x);
    this->y = libnum::num32(y);
  }

  Vector2D(libnum::num32 x, libnum::num32 y) {
    this->x = x;
    this->y = y;
  }

  Vector2D(const Vector2D &v) {
    this->x = v.x;
    this->y = v.y;
  }

  ~Vector2D() {}

  void Set( Vector2D v )
  {
    this->x = v.x;
    this->y = v.y;
  }

  void Normalise( void )
  {
    libnum::num32 len = this->Length();
    this->x /= len;
    this->y /= len;
  }

  Vector2D Clone(void) {
    Vector2D NewVector(this->x, this->y);
    return NewVector;
  }

  Vector2D MakeVector(Vector2D A, Vector2D B) {
    Vector2D NewVector(B.x - A.x, B.y - A.y);
    return NewVector;
  }

  void AddVectors(Vector2D a, Vector2D b) {
    this->x = a.x + b.x;
    this->y = a.y + b.y;
  }

  void Add(Vector2D v, libnum::num32 scale) {
    this->x += v.x * scale;
    this->y += v.y * scale;
  }

  void SubtractVectors(Vector2D a, Vector2D b) {
    this->x = a.x - b.x;
    this->y = a.y - b.y;
  }

  void Subtract(Vector2D v, libnum::num32 scale) {
    this->x -= v.x * scale;
    this->y -= v.y * scale;
  }

  libnum::num32 Length(void) {
    return sqrt_num32(this->x * this->x + this->y * this->y);
  }

  void Scale(libnum::num32 scale) {
    this->x *= scale;
    this->y *= scale;
  }

  libnum::num32 Dot(Vector2D v) { return (this->x * v.x + this->y * v.y); }

  libnum::num32 Det(Vector2D v) { return (this->x * v.y - this->y * v.x); }

  Vector2D PerpCW(void) {
    Vector2D temp(-this->y, this->x);
    return temp;
  }

  Vector2D PerpCCW(void) {
    Vector2D temp(this->y, -this->x);
    return temp;
  }

  /* overloading of most interesting operators */
  libnum::num32 operator[](uint8_t pos) { return pos == 0 ? x : y; }

  Vector2D &operator=(const Vector2D &v) {
    this->x = v.x;
    this->y = v.y;
    return *this;
  }

  Vector2D operator+(const Vector2D &v) const {
    return Vector2D(x + v.x, y + v.y);
  }

  Vector2D operator-(const Vector2D &v) const {
    return Vector2D(x - v.x, y - v.y);
  }

  Vector2D &operator+=(Vector2D const &other) {
    this->x += other.x;
    this->y += other.y;
    return *this;
  }

  Vector2D operator-() const { return (Vector2D(-x, -y)); }

  Vector2D operator+() const { return *this; }

  Vector2D &operator-=(Vector2D const &other) {
    this->x -= other.x;
    this->y -= other.y;
    return *this;
  }

  Vector2D &operator*=(libnum::num32 scale) {
    this->x *= scale;
    this->y *= scale;
    return *this;
  }

  Vector2D &operator/=(libnum::num32 scale) {
    this->x /= scale;
    this->y /= scale;
    return *this;
  }

  friend Vector2D operator*(libnum::num32 scale, Vector2D const &v) {
    Vector2D r;
    r.x = v.x * scale;
    r.y = v.y * scale;
    return r;
  }

  friend Vector2D operator*(Vector2D const &v, libnum::num32 scale) {
    Vector2D r;
    r.x = v.x * scale;
    r.y = v.y * scale;
    return r;
  }

  friend Vector2D operator/(Vector2D const &v, libnum::num32 scale) {
    Vector2D r;
    r.x = v.x / scale;
    r.y = v.y / scale;
    return r;
  }

  libnum::num32 x;
  libnum::num32 y;
};



Vector2D ClosestPointOnSegment(Vector2D P, Vector2D A, Vector2D B ) {
  Vector2D AB = B - A;

  libnum::num32 t = AB.Dot(AB);
  
  if (t == 0) return A;

  libnum::num32 t2 = (P.Dot(AB) - A.Dot(AB)) / t;

  if (t2 < libnum::num32(0)) t2 = libnum::num32(0);
  if (t2 > libnum::num32(1)) t2 = libnum::num32(1);

  Vector2D C = A.Clone();
  C.Add(AB, t2);

  return C;
}


#endif