277 lines
11 KiB
C++
277 lines
11 KiB
C++
#pragma once
|
|
#include <cmath>
|
|
#include <Eigen/Dense>
|
|
#include <array>
|
|
#include <limits>
|
|
#include <optional>
|
|
#include <utility>
|
|
#include <iostream>
|
|
#include <numbers>
|
|
|
|
using namespace Eigen;
|
|
|
|
template <typename T> T color_mul_alpha(const T& c, float a) {
|
|
return T{c[0], c[1], c[2], c[3] * a};
|
|
}
|
|
|
|
template <typename T> T circular_mod(T a, T b) {
|
|
if(a >= 0)
|
|
return a % b;
|
|
else
|
|
return b - (std::abs(a) % b);
|
|
}
|
|
|
|
template <typename T> T circular_fmod(T a, T b) {
|
|
if(a >= 0)
|
|
return std::fmod(a, b);
|
|
else
|
|
return b - std::fmod(a, b);
|
|
}
|
|
|
|
template <typename T, typename MultipleType> T round_to_multiple(T a, MultipleType multiple) {
|
|
T toRet = round(a / multiple) * multiple;
|
|
if((toRet == +0.0 || toRet == -0.0) && std::signbit(toRet))
|
|
toRet = 0.0;
|
|
return toRet;
|
|
}
|
|
|
|
template <typename T, typename MultipleType> T round_vec_to_multiple(T a, MultipleType multiple) {
|
|
for(size_t i = 0; i < static_cast<size_t>(a.size()); i++)
|
|
a[i] = round_to_multiple(a[i], multiple);
|
|
return a;
|
|
}
|
|
|
|
template <typename T> Matrix<T, 2, 1> perpendicular_vec2(const Matrix<T, 2, 1>& vec) {
|
|
return Matrix<T, 2, 1>{-vec.y(), vec.x()};
|
|
}
|
|
|
|
template <typename T> T dist_point_line_segment(const Matrix<T, 2, 1>& point, const Matrix<T, 2, 1>& lineP1, const Matrix<T, 2, 1>& lineP2) {
|
|
T lenSqrd = (lineP1 - lineP2).squaredNorm();
|
|
if(lenSqrd == 0)
|
|
return (point - lineP1).norm();
|
|
T t = std::max<T>(0, std::min<T>(1, (point - lineP1).dot(lineP2 - lineP1) / lenSqrd));
|
|
return (point - (lineP1 + t * (lineP2 - lineP1))).norm();
|
|
}
|
|
|
|
template <typename T> bool collision_circle_line_segment(const Matrix<T, 2, 1>& circleCenter, T circleRadius, const Matrix<T, 2, 1>& lineP1, const Matrix<T, 2, 1>& lineP2) {
|
|
return dist_point_line_segment(circleCenter, lineP1, lineP2) <= circleRadius;
|
|
}
|
|
|
|
// https://stackoverflow.com/a/53896859
|
|
template <typename T> bool is_collision_ray_line_segment(const Vector<T, 2>& rayOrigin, const Vector<T, 2>& rayDirection, const Vector<T, 2>& point1, const Vector<T, 2>& point2) {
|
|
Vector<T, 2> v1 = rayOrigin - point1;
|
|
Vector<T, 2> v2 = point2 - point1;
|
|
Vector<T, 2> v3 = Vector<T, 2>{-rayDirection.y(), rayDirection.x()};
|
|
|
|
Vector<T, 3> v2three{v2.x(), v2.y(), 0.0};
|
|
Vector<T, 3> v1three{v1.x(), v1.y(), 0.0};
|
|
|
|
T dot = v2.dot(v3);
|
|
if (std::abs(dot) < 0.000001)
|
|
return false;
|
|
|
|
T t1 = v2three.cross(v1three) / dot;
|
|
T t2 = v1.dot(v3) / dot;
|
|
|
|
return (t1 >= 0.0 && (t2 >= 0.0 && t2 <= 1.0));
|
|
}
|
|
|
|
// https://www.geeksforgeeks.org/program-for-point-of-intersection-of-two-lines/
|
|
template <typename T> Vector<T, 2> line_line_intersection(const Vector<T, 2>& a, const Vector<T, 2>& b, const Vector<T, 2>& c, const Vector<T, 2>& d) {
|
|
T a1 = b.y() - a.y();
|
|
T b1 = a.x() - b.x();
|
|
T c1 = a1*(a.x()) + b1*(a.y());
|
|
|
|
T a2 = d.y() - c.y();
|
|
T b2 = c.x() - d.x();
|
|
T c2 = a2*(c.x())+ b2*(c.y());
|
|
|
|
T determinant = a1*b2 - a2*b1;
|
|
|
|
T x = (b2*c1 - b1*c2)/determinant;
|
|
T y = (a1*c2 - a2*c1)/determinant;
|
|
|
|
if(std::fabs(determinant) < 0.0000001f)
|
|
return Vector<T, 2>{std::numeric_limits<T>::max(), std::numeric_limits<T>::max()};
|
|
|
|
return Vector<T, 2>{x, y};
|
|
}
|
|
|
|
template <typename T> Vector<T, 2> project_point_on_vec(const Vector<T, 2>& point, const Vector<T, 2> unnormalizedVec) {
|
|
return ((point.dot(unnormalizedVec)) / unnormalizedVec.dot(unnormalizedVec)) * unnormalizedVec;
|
|
}
|
|
|
|
template <typename T> Vector<T, 2> project_point_on_normalized_vec(const Vector<T, 2>& point, const Vector<T, 2> normalizedVec) {
|
|
return point.dot(normalizedVec) * normalizedVec;
|
|
}
|
|
|
|
// https://stackoverflow.com/a/61342198
|
|
template <typename T> Vector<T, 2> project_point_on_line(const Vector<T, 2>& p, const Vector<T, 2>& b, const Vector<T, 2>& a) {
|
|
Vector<T, 2> ap = p - a;
|
|
Vector<T, 2> ab = b - a;
|
|
|
|
T t = ap.dot(ab) / ab.dot(ab);
|
|
|
|
return a + t * ab;
|
|
}
|
|
|
|
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
|
|
template <typename T> Vector<T, 2> line_line_intersection_inaccurate(const Matrix<T, 2, 1>& a, const Matrix<T, 2, 1>& b, const Matrix<T, 2, 1>& c, const Matrix<T, 2, 1>& d) {
|
|
T x1 = a.x(); T x2 = b.x(); T x3 = c.x(); T x4 = d.x();
|
|
T y1 = a.y(); T y2 = b.y(); T y3 = c.y(); T y4 = d.y();
|
|
T p1 = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4));
|
|
T p2 = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) / ((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4));
|
|
return Vector<T, 2>{p1, p2};
|
|
}
|
|
|
|
// https://www.reddit.com/r/algorithms/comments/9moad4/what_is_the_simplest_to_implement_line_segment/
|
|
// Taken from https://github.com/vlecomte/cp-geo
|
|
template <typename T> T cross_vals_vec(const Matrix<T, 2, 1>& a, const Matrix<T, 2, 1>& b) {
|
|
return a.x() * b.y() - a.y() * b.x();
|
|
}
|
|
|
|
template <typename T> T orient_vals_vec(const Matrix<T, 2, 1>& a, const Matrix<T, 2, 1>& b, const Matrix<T, 2, 1>& c) {
|
|
return cross_vals_vec((b - a).eval(), (c - a).eval());
|
|
}
|
|
|
|
template <typename T> bool is_collision_line_segment_line_segment(const Matrix<T, 2, 1>& a, const Matrix<T, 2, 1>& b, const Matrix<T, 2, 1>& c, const Matrix<T, 2, 1>& d) {
|
|
T oa = orient_vals_vec(c, d, a);
|
|
T ob = orient_vals_vec(c, d, b);
|
|
T oc = orient_vals_vec(a, b, c);
|
|
T od = orient_vals_vec(a, b, d);
|
|
return (oa * ob < T(0) && oc * od < T(0));
|
|
}
|
|
|
|
template <typename T> std::optional<Vector2d> collision_line_segment_line_segment_pos(const Matrix<T, 2, 1>& a, const Matrix<T, 2, 1>& b, const Matrix<T, 2, 1>& c, const Matrix<T, 2, 1>& d) {
|
|
T oa = orient_vals_vec(c, d, a);
|
|
T ob = orient_vals_vec(c, d, b);
|
|
T oc = orient_vals_vec(a, b, c);
|
|
T od = orient_vals_vec(a, b, d);
|
|
if(oa * ob < 0 && oc * od < 0)
|
|
return std::optional<Vector2d>((a * ob - b * oa) / (ob - oa));
|
|
return std::nullopt;
|
|
}
|
|
|
|
template <typename T> bool is_collision_aabb_line_segment(const AlignedBox<T, 2>& aabb, const Matrix<T, 2, 1>& lineP1, const Matrix<T, 2, 1>& lineP2) {
|
|
return aabb.contains(lineP1) ||
|
|
aabb.contains(lineP2) ||
|
|
is_collision_line_segment_line_segment(lineP1, lineP2, aabb.corner(AlignedBox<T, 2>::TopLeft), aabb.corner(AlignedBox<T, 2>::TopRight)) ||
|
|
is_collision_line_segment_line_segment(lineP1, lineP2, aabb.corner(AlignedBox<T, 2>::TopRight), aabb.corner(AlignedBox<T, 2>::BottomRight)) ||
|
|
is_collision_line_segment_line_segment(lineP1, lineP2, aabb.corner(AlignedBox<T, 2>::BottomLeft), aabb.corner(AlignedBox<T, 2>::BottomRight)) ||
|
|
is_collision_line_segment_line_segment(lineP1, lineP2, aabb.corner(AlignedBox<T, 2>::TopLeft), aabb.corner(AlignedBox<T, 2>::BottomLeft));
|
|
}
|
|
|
|
template <typename T> T vec_distance(const Vector<T, 2>& v1, const Vector<T, 2>& v2) {
|
|
return (v1 - v2).norm();
|
|
}
|
|
|
|
template <typename T> T vec_length(const Vector<T, 2>& v) {
|
|
return v.norm();
|
|
}
|
|
|
|
template <typename T> T cwise_vec_min(const T& v1, const T& v2) {
|
|
T toRet;
|
|
for(unsigned i = 0; i < v1.size(); i++)
|
|
toRet[i] = std::min(v1[i], v2[i]);
|
|
return toRet;
|
|
}
|
|
|
|
template <typename T> T cwise_vec_max(const T& v1, const T& v2) {
|
|
T toRet;
|
|
for(unsigned i = 0; i < v1.size(); i++)
|
|
toRet[i] = std::max(v1[i], v2[i]);
|
|
return toRet;
|
|
}
|
|
|
|
template <typename T> T cwise_vec_clamp(const T& v1, const T& v2, const T& v3) {
|
|
T toRet;
|
|
for(unsigned i = 0; i < v1.size(); i++)
|
|
toRet[i] = std::clamp(v1[i], v2[i], v3[i]);
|
|
return toRet;
|
|
}
|
|
|
|
template <typename T> T cwise_vec_modf(const T& v1, T& v2) {
|
|
T toRet;
|
|
for(unsigned i = 0; i < v1.size(); i++)
|
|
toRet[i] = std::modf(v1[i], &v2[i]);
|
|
return toRet;
|
|
}
|
|
|
|
|
|
template <typename T> std::vector<Vector<T, 2>> arc_vec(const Vector<T, 2>& arcCenter, const Vector<T, 2>& arcDirStart, const Vector<T, 2>& arcDirEnd, const Vector<T, 2>& arcDirCenter, T arcRadius, unsigned numOfDivisions) {
|
|
T a1 = std::atan2(arcDirStart.y(), arcDirStart.x());
|
|
T a2 = std::atan2(arcDirEnd.y(), arcDirEnd.x());
|
|
T a3 = std::atan2(arcDirCenter.y(), arcDirCenter.x());
|
|
|
|
T minAngle = std::min(a1, a2);
|
|
T maxAngle = std::max(a1, a2);
|
|
|
|
bool swapHappen = (a3 > minAngle && a3 > maxAngle) || (a3 < minAngle && a3 < maxAngle);
|
|
if(swapHappen) {
|
|
minAngle += 2.0 * std::numbers::pi;
|
|
std::swap(minAngle, maxAngle);
|
|
}
|
|
|
|
bool flipped = minAngle == a2;
|
|
|
|
std::vector<Vector<T, 2>> toRet;
|
|
T tStep = 1.0 / static_cast<T>(numOfDivisions);
|
|
if(flipped) {
|
|
for(unsigned i = numOfDivisions - 1; i > 0; i--) {
|
|
T newAngle = std::lerp(minAngle, maxAngle, tStep * i);
|
|
toRet.emplace_back(arcCenter + Vector<T, 2>{std::cos(newAngle), std::sin(newAngle)} * arcRadius);
|
|
}
|
|
}
|
|
else {
|
|
for(unsigned i = 1; i < numOfDivisions; i++) {
|
|
T newAngle = std::lerp(minAngle, maxAngle, tStep * i);
|
|
toRet.emplace_back(arcCenter + Vector<T, 2>{std::cos(newAngle), std::sin(newAngle)} * arcRadius);
|
|
}
|
|
}
|
|
return toRet;
|
|
}
|
|
|
|
template <typename T> std::vector<Vector<T, 2>> gen_circle_points(const Vector<T, 2>& center, T radius, unsigned numOfPoints) {
|
|
std::vector<Vector<T, 2>> toRet;
|
|
for(unsigned i = 0; i < numOfPoints; i++) {
|
|
T angle = (static_cast<T>(i) / static_cast<T>(numOfPoints)) * std::numbers::pi * 2;
|
|
toRet.emplace_back(std::cos(angle) * radius + center.x(), std::sin(angle) * radius + center.y());
|
|
}
|
|
return toRet;
|
|
}
|
|
|
|
// https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline
|
|
template <typename T> T catmull_rom_get_t(T t, T alpha, const Vector<T, 2>& p0, const Vector<T, 2>& p1) {
|
|
Vector<T, 2> d = p1 - p0;
|
|
T a = d.dot(d);
|
|
T b = std::pow(a, alpha * 0.5);
|
|
return (b + t);
|
|
}
|
|
template <typename T> Vector<T, 2> catmull_rom(const Vector<T, 2>& p0, const Vector<T, 2>& p1, const Vector<T, 2>& p2, const Vector<T, 2>& p3, T t, T alpha = 0.5) {
|
|
T t0 = 0.0;
|
|
T t1 = catmull_rom_get_t(t0, alpha, p0, p1);
|
|
T t2 = catmull_rom_get_t(t1, alpha, p1, p2);
|
|
T t3 = catmull_rom_get_t(t2, alpha, p2, p3);
|
|
t = std::lerp(t1, t2, t);
|
|
Vector<T, 2> A1 = ( t1-t )/( t1-t0 )*p0 + ( t-t0 )/( t1-t0 )*p1;
|
|
Vector<T, 2> A2 = ( t2-t )/( t2-t1 )*p1 + ( t-t1 )/( t2-t1 )*p2;
|
|
Vector<T, 2> A3 = ( t3-t )/( t3-t2 )*p2 + ( t-t2 )/( t3-t2 )*p3;
|
|
Vector<T, 2> B1 = ( t2-t )/( t2-t0 )*A1 + ( t-t0 )/( t2-t0 )*A2;
|
|
Vector<T, 2> B2 = ( t3-t )/( t3-t1 )*A2 + ( t-t1 )/( t3-t1 )*A3;
|
|
Vector<T, 2> C = ( t2-t )/( t2-t1 )*B1 + ( t-t1 )/( t2-t1 )*B2;
|
|
return C;
|
|
}
|
|
|
|
template <typename T, int S> T transform_scalar(const Transform<T, 2, S>& transform, T scalar) {
|
|
return (transform.linear() * Vector<T, 2>{scalar, scalar}).x();
|
|
}
|
|
|
|
template <typename T> std::array<Vector<T, 2>, 4> triangle_from_rect_points(const Vector<T, 2>& min, const Vector<T, 2>& max) {
|
|
return {min, {min.x(), max.y()}, max, {max.x(), min.y()}};
|
|
}
|
|
|
|
template <typename T> std::string vec_pretty(const Vector<T, 2>& a) {
|
|
std::stringstream ss;
|
|
ss << "[" << a.x() << ", " << a.y() << "]";
|
|
return ss.str();
|
|
}
|