278 lines
6.0 KiB
C++
278 lines
6.0 KiB
C++
#ifndef VECTOR2D_H
|
|
#define VECTOR2D_H
|
|
|
|
#include <num/num.h>
|
|
#include <stdint.h>
|
|
|
|
#define numPI 3.14159265
|
|
|
|
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;
|
|
}
|
|
|
|
/* TO DO : rework these functions for sine and cosine calculation */
|
|
|
|
libnum::num32 cos_num32(libnum::num32 angle) {
|
|
// Taylor serie for cos(x) = 1 - x²/2! + x⁴/4! + x⁶/6! + x⁸/8! + ...
|
|
|
|
// Cosine function is even
|
|
if (angle < libnum::num32(0))
|
|
return cos_num32(-angle);
|
|
|
|
// Look for an angle in the range [0, 2*pi [
|
|
libnum::num32 anglereduced = angle;
|
|
while (anglereduced >= libnum::num32(2 * numPI))
|
|
anglereduced -= libnum::num32(2 * numPI);
|
|
|
|
// Exploit the symetry for angle and angle+Pi to reduce the order of the
|
|
// limited developpement
|
|
if (anglereduced >= libnum::num(numPI))
|
|
return -cos_num32(anglereduced - libnum::num(numPI));
|
|
|
|
libnum::num32 sum = libnum::num32(1);
|
|
libnum::num32 angle2 = anglereduced * anglereduced;
|
|
|
|
// set first value of the Taylor serie : x⁰/0! = 1/1
|
|
libnum::num32 numerator = libnum::num32(1);
|
|
libnum::num32 denominator = libnum::num32(1);
|
|
|
|
for (int i = 2; i <= 8; i += 2) {
|
|
numerator *= (-angle2);
|
|
denominator *= libnum::num32(i - 1) * libnum::num32(i);
|
|
sum += (numerator / denominator);
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
libnum::num32 sin_num32(libnum::num32 angle) {
|
|
// Taylor serie for cos(x) = x/1! - x³/3! + x⁵/5! - x⁷/7! + x⁹/9! + ...
|
|
|
|
// Sine function is odd
|
|
if (angle < libnum::num32(0))
|
|
return -sin_num32(-angle);
|
|
|
|
// Look for an angle in the range [0, 2*pi [
|
|
libnum::num32 anglereduced = angle;
|
|
while (anglereduced >= libnum::num32(2 * numPI))
|
|
anglereduced -= libnum::num32(2 * numPI);
|
|
|
|
// Exploit the symetry for angle and angle+Pi to reduce the order of the
|
|
// limited developpement
|
|
if (anglereduced >= libnum::num(numPI))
|
|
return -sin_num32(anglereduced - libnum::num(numPI));
|
|
|
|
libnum::num32 sum = anglereduced;
|
|
libnum::num32 angle2 = anglereduced * anglereduced;
|
|
|
|
// set first value of the Taylor serie : x¹/1! = x/1
|
|
libnum::num32 numerator = anglereduced;
|
|
libnum::num32 denominator = libnum::num32(1);
|
|
|
|
for (int i = 2; i <= 8; i += 2) {
|
|
numerator *= (-angle2);
|
|
denominator *= libnum::num32(i) * libnum::num32(i + 1);
|
|
sum += (numerator / denominator);
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
|
|
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 |