Lucas de Vries
293 lines
6.9 KiB
C++
293 lines
6.9 KiB
C++
#pragma once
|
|
#include <math.h>
|
|
|
|
#if defined(_MSC_VER) && defined(_M_AMD64)
|
|
#include <emmintrin.h>
|
|
#endif
|
|
|
|
//3 Dimensional Vector structure
|
|
template<class T = double>
|
|
struct vec3 {
|
|
T x,y,z;
|
|
|
|
vec3() : x(0), y(0), z(0) {}
|
|
explicit vec3(T def) : x(def), y(def), z(def) {}
|
|
explicit vec3(T X, T Y, T Z) : x(X), y(Y), z(Z) {}
|
|
|
|
template<class O>
|
|
explicit vec3(const vec3<O>& other) : x((T)other.x), y((T)other.y), z((T)other.z) {}
|
|
|
|
static vec3 up(T len = (T)1.0) { return vec3(0,len,0); }
|
|
static vec3 front(T len = (T)1.0) { return vec3(len,0,0); }
|
|
static vec3 right(T len = (T)1.0) { return vec3(0,0,-len); }
|
|
|
|
#if defined(_MSC_VER) && defined(_M_AMD64)
|
|
|
|
double distanceTo(const vec3& other) const {
|
|
__m128d reg0 = _mm_set_sd(x), reg1 = _mm_set_sd(other.x);
|
|
reg0 = _mm_sub_sd(reg0, reg1);
|
|
__m128d reg2 = _mm_set_sd(y);
|
|
reg0 = _mm_mul_sd(reg0, reg0);
|
|
reg1 = _mm_set_sd(other.y);
|
|
reg1 = _mm_sub_sd(reg2, reg1);
|
|
__m128d reg3 = _mm_set_sd(z);
|
|
reg1 = _mm_mul_sd(reg1, reg1);
|
|
reg2 = _mm_set_sd(other.z);
|
|
reg0 = _mm_add_sd(reg0, reg1);
|
|
reg2 = _mm_sub_sd(reg3, reg2);
|
|
reg2 = _mm_mul_sd(reg2, reg2);
|
|
reg0 = _mm_add_sd(reg0, reg2);
|
|
reg0 = _mm_sqrt_sd(reg0, reg0);
|
|
return reg0.m128d_f64[0];
|
|
}
|
|
|
|
double distanceToSQ(const vec3& other) const {
|
|
__m128d reg0 = _mm_set_sd(x), reg1 = _mm_set_sd(other.x);
|
|
reg0 = _mm_sub_sd(reg0, reg1);
|
|
__m128d reg2 = _mm_set_sd(y);
|
|
reg0 = _mm_mul_sd(reg0, reg0);
|
|
reg1 = _mm_set_sd(other.y);
|
|
reg1 = _mm_sub_sd(reg2, reg1);
|
|
__m128d reg3 = _mm_set_sd(z);
|
|
reg1 = _mm_mul_sd(reg1, reg1);
|
|
reg2 = _mm_set_sd(other.z);
|
|
reg0 = _mm_add_sd(reg0, reg1);
|
|
reg2 = _mm_sub_sd(reg3, reg2);
|
|
reg2 = _mm_mul_sd(reg2, reg2);
|
|
reg0 = _mm_add_sd(reg0, reg2);
|
|
return reg0.m128d_f64[0];
|
|
}
|
|
|
|
T getLength() const {
|
|
__m128d reg0 = _mm_set_sd(x);
|
|
reg0 = _mm_mul_sd(reg0, reg0);
|
|
__m128d reg1 = _mm_set_sd(y);
|
|
reg1 = _mm_mul_sd(reg1, reg1);
|
|
__m128d reg2 = _mm_set_sd(z);
|
|
reg2 = _mm_mul_sd(reg2, reg2);
|
|
reg0 = _mm_add_sd(reg0, reg1);
|
|
reg0 = _mm_add_sd(reg0, reg2);
|
|
reg0 = _mm_sqrt_sd(reg0, reg0);
|
|
return reg0.m128d_f64[0];
|
|
}
|
|
|
|
T getLengthSQ() const {
|
|
__m128d reg0 = _mm_set_sd(x);
|
|
reg0 = _mm_mul_sd(reg0, reg0);
|
|
__m128d reg1 = _mm_set_sd(y);
|
|
reg1 = _mm_mul_sd(reg1, reg1);
|
|
__m128d reg2 = _mm_set_sd(z);
|
|
reg2 = _mm_mul_sd(reg2, reg2);
|
|
reg0 = _mm_add_sd(reg0, reg1);
|
|
reg0 = _mm_add_sd(reg0, reg2);
|
|
return reg0.m128d_f64[0];
|
|
}
|
|
|
|
double dot(const vec3& other) const {
|
|
__m128d reg0 = _mm_set_sd(x), reg1 = _mm_set_sd(other.x);
|
|
reg0 = _mm_mul_sd(reg0, reg1);
|
|
__m128d reg2 = _mm_set_sd(y);
|
|
reg1 = _mm_set_sd(other.y);
|
|
reg1 = _mm_mul_sd(reg2, reg1);
|
|
__m128d reg3 = _mm_set_sd(z);
|
|
reg0 = _mm_add_sd(reg0, reg1);
|
|
reg2 = _mm_set_sd(other.z);
|
|
reg2 = _mm_mul_sd(reg3, reg2);
|
|
reg0 = _mm_add_sd(reg0, reg2);
|
|
return reg0.m128d_f64[0];
|
|
}
|
|
#else
|
|
|
|
double distanceTo(const vec3& other) const {
|
|
double tx = x-other.x,
|
|
ty = y-other.y,
|
|
tz = z-other.z;
|
|
return sqrt((tx*tx)+(ty*ty)+(tz*tz));
|
|
}
|
|
|
|
double distanceToSQ(const vec3& other) const {
|
|
double tx = x-other.x,
|
|
ty = y-other.y,
|
|
tz = z-other.z;
|
|
return ((tx*tx)+(ty*ty)+(tz*tz));
|
|
}
|
|
|
|
T getLength() const {
|
|
return (T)sqrt((double)(x*x)+(double)(y*y)+(double)(z*z));
|
|
}
|
|
|
|
T getLengthSQ() const {
|
|
return (x*x)+(y*y)+(z*z);
|
|
}
|
|
|
|
double dot(const vec3& other) const {
|
|
return (double(x)*double(other.x)) + (double(y)*double(other.y)) + (double(z)*double(other.z));
|
|
}
|
|
#endif
|
|
|
|
double angleDistance(const vec3& other) const {
|
|
double _dot = dot(other);
|
|
if(_dot < -1.0)
|
|
_dot = -1.0;
|
|
else if(_dot > 1.0)
|
|
_dot = 1.0;
|
|
|
|
return acos(_dot);
|
|
}
|
|
|
|
vec3 cross(const vec3& other) const {
|
|
return vec3(
|
|
(y*other.z)-(z*other.y),
|
|
(z*other.x)-(x*other.z),
|
|
(x*other.y)-(y*other.x) );
|
|
}
|
|
|
|
vec3 operator+(const vec3& other) const {
|
|
return vec3(x+other.x, y+other.y, z+other.z);
|
|
}
|
|
|
|
vec3& operator+=(const vec3& other) {
|
|
x += other.x;
|
|
y += other.y;
|
|
z += other.z;
|
|
return *this;
|
|
}
|
|
|
|
vec3 operator-() const {
|
|
return vec3(-x, -y, -z);
|
|
}
|
|
|
|
vec3 operator-(const vec3& other) const {
|
|
return vec3(x-other.x, y-other.y, z-other.z);
|
|
}
|
|
|
|
vec3& operator-=(const vec3& other) {
|
|
x -= other.x;
|
|
y -= other.y;
|
|
z -= other.z;
|
|
return *this;
|
|
}
|
|
|
|
vec3 operator*(const vec3& other) const {
|
|
return vec3(x*other.x, y*other.y, z*other.z);
|
|
}
|
|
|
|
vec3 operator*(double scalar) const {
|
|
return vec3(T((double)x*scalar), T((double)y*scalar), T((double)z*scalar));
|
|
}
|
|
|
|
vec3& operator*=(double scalar) {
|
|
x = T((double)x * scalar);
|
|
y = T((double)y * scalar);
|
|
z = T((double)z * scalar);
|
|
return *this;
|
|
}
|
|
|
|
vec3 operator/(double scalar) const {
|
|
return vec3(T((double)x/scalar), T((double)y/scalar), T((double)z/scalar));
|
|
}
|
|
|
|
vec3& operator/=(double scalar) {
|
|
x = T((double)x / scalar);
|
|
y = T((double)y / scalar);
|
|
z = T((double)z / scalar);
|
|
return *this;
|
|
}
|
|
|
|
vec3& operator=(const vec3& other) {
|
|
x = other.x;
|
|
y = other.y;
|
|
z = other.z;
|
|
return *this;
|
|
}
|
|
|
|
bool operator==(const vec3& other) const {
|
|
return x == other.x && y == other.y && z == other.z;
|
|
}
|
|
|
|
bool operator!=(const vec3& other) const {
|
|
return x != other.x || y != other.y || z != other.z;
|
|
}
|
|
|
|
vec3 interpolate(const vec3& other, double pct) const {
|
|
return *this + (other - *this) * pct;
|
|
}
|
|
|
|
//Spherically interpolate between normalized vectors
|
|
vec3 slerp(const vec3& other, double pct) const {
|
|
if(pct >= 1.0)
|
|
return other;
|
|
if(pct <= 0.0)
|
|
return *this;
|
|
|
|
double _dot = dot(other);
|
|
if(_dot > 0.999)
|
|
return interpolate(other, pct);
|
|
if(_dot <= -1.0) {
|
|
//Normally this would reduce to lerp, but we know we want to rotate in some direction instead
|
|
// We find a vector perpendicular to our endpoints and interpolate based on our progress
|
|
vec3 temp = cross(vec3::up());
|
|
if(temp.zero())
|
|
return other;
|
|
else if(pct < 0.5)
|
|
return slerp(temp, pct * 2.0);
|
|
else
|
|
return temp.slerp(other, (pct - 0.5) * 2.0);
|
|
}
|
|
|
|
double omega = acos(_dot);
|
|
if(omega < 0.001)
|
|
return interpolate(other, pct);
|
|
|
|
double sinOmg = sin(omega);
|
|
return ((*this * sin((1.0 - pct)*omega)/sinOmg) + (other * sin(pct * omega)/sinOmg)).normalized();
|
|
}
|
|
|
|
vec3& normalize(T length = (T)1.0) {
|
|
double X = x, Y = y, Z = z,
|
|
L = (X*X)+(Y*Y)+(Z*Z);
|
|
if(L == 0.0)
|
|
return *this;
|
|
L = (double)length / sqrt(L);
|
|
|
|
x = (T)(X*L);
|
|
y = (T)(Y*L);
|
|
z = (T)(Z*L);
|
|
return *this;
|
|
}
|
|
|
|
vec3 normalized(T length = (T)1.0) const {
|
|
vec3 temp(*this);
|
|
temp.normalize(length);
|
|
return temp;
|
|
}
|
|
|
|
void set(T X, T Y, T Z) {
|
|
x = X;
|
|
y = Y;
|
|
z = Z;
|
|
}
|
|
|
|
vec3<T> elementMax(const vec3<T>& other) const {
|
|
return vec3<T>(
|
|
x > other.x ? x : other.x,
|
|
y > other.y ? y : other.y,
|
|
z > other.z ? z : other.z);
|
|
}
|
|
|
|
vec3<T> elementMin(const vec3<T>& other) const {
|
|
return vec3<T>(
|
|
x < other.x ? x : other.x,
|
|
y < other.y ? y : other.y,
|
|
z < other.z ? z : other.z);
|
|
}
|
|
|
|
bool zero() {
|
|
return x == 0.0 && y == 0.0 && z == 0.0;
|
|
}
|
|
};
|
|
|
|
typedef vec3<float> vec3f;
|
|
typedef vec3<double> vec3d;
|
|
typedef vec3<int> vec3i;
|