2022-01-13 18:23:57 +01:00
|
|
|
#pragma once
|
2015-01-17 19:39:58 +01:00
|
|
|
#include <cmath>
|
2015-01-26 11:05:52 +01:00
|
|
|
#include <cassert>
|
2015-01-25 21:17:26 +01:00
|
|
|
#include <iostream>
|
2015-01-23 10:45:24 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> struct vec {
|
2023-01-05 23:05:46 +01:00
|
|
|
double data[n] = {0};
|
|
|
|
|
double& operator[](const int i) { assert(i>=0 && i<n); return data[i]; }
|
|
|
|
|
double operator[](const int i) const { assert(i>=0 && i<n); return data[i]; }
|
2015-01-26 17:26:16 +01:00
|
|
|
};
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> double operator*(const vec<n>& lhs, const vec<n>& rhs) {
|
2025-02-20 23:45:08 +01:00
|
|
|
double ret = 0; // N.B. Do not ever, ever use such for loops! They are highly confusing.
|
|
|
|
|
for (int i=n; i--; ret+=lhs[i]*rhs[i]); // Here I used them as a tribute to old-school game programmers fighting for every CPU cycle.
|
|
|
|
|
return ret; // Once upon a time reverse loops were faster than the normal ones, it is not the case anymore.
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
2015-01-22 09:51:42 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> vec<n> operator+(const vec<n>& lhs, const vec<n>& rhs) {
|
|
|
|
|
vec<n> ret = lhs;
|
|
|
|
|
for (int i=n; i--; ret[i]+=rhs[i]);
|
|
|
|
|
return ret;
|
2015-01-17 19:39:58 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> vec<n> operator-(const vec<n>& lhs, const vec<n>& rhs) {
|
|
|
|
|
vec<n> ret = lhs;
|
|
|
|
|
for (int i=n; i--; ret[i]-=rhs[i]);
|
|
|
|
|
return ret;
|
2015-01-26 20:35:34 +01:00
|
|
|
}
|
|
|
|
|
|
2025-02-20 23:45:08 +01:00
|
|
|
template<int n> vec<n> operator*(const vec<n>& lhs, const double& rhs) {
|
2020-08-25 15:42:18 +02:00
|
|
|
vec<n> ret = lhs;
|
|
|
|
|
for (int i=n; i--; ret[i]*=rhs);
|
|
|
|
|
return ret;
|
2015-01-17 19:39:58 +01:00
|
|
|
}
|
|
|
|
|
|
2025-02-20 23:45:08 +01:00
|
|
|
template<int n> vec<n> operator*(const double& lhs, const vec<n> &rhs) {
|
|
|
|
|
return rhs * lhs;
|
2015-01-26 20:35:34 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> vec<n> operator/(const vec<n>& lhs, const double& rhs) {
|
|
|
|
|
vec<n> ret = lhs;
|
|
|
|
|
for (int i=n; i--; ret[i]/=rhs);
|
2015-01-26 11:05:52 +01:00
|
|
|
return ret;
|
|
|
|
|
}
|
2015-01-22 21:32:31 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> std::ostream& operator<<(std::ostream& out, const vec<n>& v) {
|
|
|
|
|
for (int i=0; i<n; i++) out << v[i] << " ";
|
|
|
|
|
return out;
|
2015-01-26 20:35:34 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<> struct vec<2> {
|
2023-01-05 23:05:46 +01:00
|
|
|
double x = 0, y = 0;
|
2022-01-13 18:23:57 +01:00
|
|
|
double& operator[](const int i) { assert(i>=0 && i<2); return i ? y : x; }
|
|
|
|
|
double operator[](const int i) const { assert(i>=0 && i<2); return i ? y : x; }
|
2015-01-26 11:05:52 +01:00
|
|
|
};
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<> struct vec<3> {
|
2023-01-05 23:05:46 +01:00
|
|
|
double x = 0, y = 0, z = 0;
|
2022-01-13 18:23:57 +01:00
|
|
|
double& operator[](const int i) { assert(i>=0 && i<3); return i ? (1==i ? y : z) : x; }
|
|
|
|
|
double operator[](const int i) const { assert(i>=0 && i<3); return i ? (1==i ? y : z) : x; }
|
2025-02-20 23:45:08 +01:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
template<> struct vec<4> {
|
|
|
|
|
double x = 0, y = 0, z = 0, w = 0;
|
|
|
|
|
double& operator[](const int i) { assert(i>=0 && i<4); return i<2 ? (i ? y : x) : (2==i ? z : w); }
|
|
|
|
|
double operator[](const int i) const { assert(i>=0 && i<4); return i<2 ? (i ? y : x) : (2==i ? z : w); }
|
|
|
|
|
vec<2> xy() const { return {x, y}; }
|
|
|
|
|
vec<3> xyz() const { return {x, y, z}; }
|
2015-01-26 11:05:52 +01:00
|
|
|
};
|
|
|
|
|
|
2022-01-13 18:23:57 +01:00
|
|
|
typedef vec<2> vec2;
|
|
|
|
|
typedef vec<3> vec3;
|
|
|
|
|
typedef vec<4> vec4;
|
2025-02-20 23:45:08 +01:00
|
|
|
|
|
|
|
|
template<int n> double norm(const vec<n>& v) {
|
|
|
|
|
return std::sqrt(v*v);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
template<int n> vec<n> normalized(const vec<n>& v) {
|
|
|
|
|
return v / norm(v);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
inline vec3 cross(const vec3 &v1, const vec3 &v2) {
|
|
|
|
|
return {v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x};
|
|
|
|
|
}
|
2015-01-26 11:05:52 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int n> struct dt;
|
2015-01-26 11:05:52 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int nrows,int ncols> struct mat {
|
|
|
|
|
vec<ncols> rows[nrows] = {{}};
|
2015-01-26 11:05:52 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
vec<ncols>& operator[] (const int idx) { assert(idx>=0 && idx<nrows); return rows[idx]; }
|
|
|
|
|
const vec<ncols>& operator[] (const int idx) const { assert(idx>=0 && idx<nrows); return rows[idx]; }
|
2015-01-26 11:05:52 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
double det() const {
|
|
|
|
|
return dt<ncols>::det(*this);
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
double cofactor(const int row, const int col) const {
|
2025-02-21 15:55:15 +01:00
|
|
|
mat<nrows-1,ncols-1> submatrix;
|
|
|
|
|
for (int i=nrows-1; i--; )
|
|
|
|
|
for (int j=ncols-1;j--; submatrix[i][j]=rows[i+int(i>=row)][j+int(j>=col)]);
|
|
|
|
|
return submatrix.det() * ((row+col)%2 ? -1 : 1);
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
mat<nrows,ncols> invert_transpose() const {
|
2025-02-21 15:55:15 +01:00
|
|
|
mat<nrows,ncols> adjugate_transpose; // transpose to ease determinant computation, check the last line
|
|
|
|
|
for (int i=nrows; i--; )
|
|
|
|
|
for (int j=ncols; j--; adjugate_transpose[i][j]=cofactor(i,j));
|
|
|
|
|
return adjugate_transpose/(adjugate_transpose[0]*rows[0]);
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
2015-01-30 16:46:46 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
mat<nrows,ncols> invert() const {
|
2015-01-30 16:46:46 +01:00
|
|
|
return invert_transpose().transpose();
|
|
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
mat<ncols,nrows> transpose() const {
|
|
|
|
|
mat<ncols,nrows> ret;
|
2025-02-20 23:45:08 +01:00
|
|
|
for (int i=ncols; i--; )
|
|
|
|
|
for (int j=nrows; j--; ret[i][j]=rows[j][i]);
|
2015-01-30 16:46:46 +01:00
|
|
|
return ret;
|
|
|
|
|
}
|
2015-01-22 21:32:31 +01:00
|
|
|
};
|
|
|
|
|
|
2025-02-20 23:45:08 +01:00
|
|
|
template<int nrows,int ncols> vec<ncols> operator*(const vec<nrows>& lhs, const mat<nrows,ncols>& rhs) {
|
|
|
|
|
return (mat<1,nrows>{{lhs}}*rhs)[0];
|
|
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int nrows,int ncols> vec<nrows> operator*(const mat<nrows,ncols>& lhs, const vec<ncols>& rhs) {
|
|
|
|
|
vec<nrows> ret;
|
|
|
|
|
for (int i=nrows; i--; ret[i]=lhs[i]*rhs);
|
2015-01-26 17:26:16 +01:00
|
|
|
return ret;
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int R1,int C1,int C2>mat<R1,C2> operator*(const mat<R1,C1>& lhs, const mat<C1,C2>& rhs) {
|
|
|
|
|
mat<R1,C2> result;
|
|
|
|
|
for (int i=R1; i--; )
|
2025-02-20 23:45:08 +01:00
|
|
|
for (int j=C2; j--; )
|
|
|
|
|
for (int k=C1; k--; result[i][j]+=lhs[i][k]*rhs[k][j]);
|
2020-08-25 15:42:18 +02:00
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
template<int nrows,int ncols>mat<nrows,ncols> operator*(const mat<nrows,ncols>& lhs, const double& val) {
|
|
|
|
|
mat<nrows,ncols> result;
|
|
|
|
|
for (int i=nrows; i--; result[i] = lhs[i]*val);
|
2015-01-26 22:55:56 +01:00
|
|
|
return result;
|
|
|
|
|
}
|
2015-01-26 17:48:17 +01:00
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int nrows,int ncols>mat<nrows,ncols> operator/(const mat<nrows,ncols>& lhs, const double& val) {
|
|
|
|
|
mat<nrows,ncols> result;
|
|
|
|
|
for (int i=nrows; i--; result[i] = lhs[i]/val);
|
|
|
|
|
return result;
|
2015-01-26 17:48:17 +01:00
|
|
|
}
|
|
|
|
|
|
2020-08-25 15:42:18 +02:00
|
|
|
template<int nrows,int ncols>mat<nrows,ncols> operator+(const mat<nrows,ncols>& lhs, const mat<nrows,ncols>& rhs) {
|
|
|
|
|
mat<nrows,ncols> result;
|
|
|
|
|
for (int i=nrows; i--; )
|
|
|
|
|
for (int j=ncols; j--; result[i][j]=lhs[i][j]+rhs[i][j]);
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
template<int nrows,int ncols>mat<nrows,ncols> operator-(const mat<nrows,ncols>& lhs, const mat<nrows,ncols>& rhs) {
|
|
|
|
|
mat<nrows,ncols> result;
|
|
|
|
|
for (int i=nrows; i--; )
|
|
|
|
|
for (int j=ncols; j--; result[i][j]=lhs[i][j]-rhs[i][j]);
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
template<int nrows,int ncols> std::ostream& operator<<(std::ostream& out, const mat<nrows,ncols>& m) {
|
|
|
|
|
for (int i=0; i<nrows; i++) out << m[i] << std::endl;
|
2015-01-26 17:26:16 +01:00
|
|
|
return out;
|
2015-01-26 11:05:52 +01:00
|
|
|
}
|
|
|
|
|
|
2025-02-21 15:55:15 +01:00
|
|
|
template<int n> struct dt { // template metaprogramming to compute the determinant recursively
|
2020-08-25 15:42:18 +02:00
|
|
|
static double det(const mat<n,n>& src) {
|
|
|
|
|
double ret = 0;
|
2025-02-21 15:55:15 +01:00
|
|
|
for (int i=n; i--; ret += src[0][i] * src.cofactor(0,i));
|
2020-08-25 15:42:18 +02:00
|
|
|
return ret;
|
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
2025-02-21 15:55:15 +01:00
|
|
|
template<> struct dt<1> { // template specialization to stop the recursion
|
2020-08-25 15:42:18 +02:00
|
|
|
static double det(const mat<1,1>& src) {
|
|
|
|
|
return src[0][0];
|
|
|
|
|
}
|
|
|
|
|
};
|
|
|
|
|
|
