106 lines
2.3 KiB
C++
106 lines
2.3 KiB
C++
#pragma once
|
|
#include "fp.h"
|
|
#include "rmath.h"
|
|
|
|
template <typename T>
|
|
struct vec3 {
|
|
T x;
|
|
T y;
|
|
T z;
|
|
inline vec3(){
|
|
x = 0;
|
|
y = 0;
|
|
z = 0;
|
|
}
|
|
inline vec3(T x, T y, T z){
|
|
this->x = x;
|
|
this->y = y;
|
|
this->z = z;
|
|
}
|
|
inline vec3<T> operator + (vec3<T> o){
|
|
return {x + o.x, y + o.y, z + o.z};
|
|
}
|
|
inline vec3<T> operator - (vec3<T> o){
|
|
return {x - o.x, y - o.y, z - o.z};
|
|
}
|
|
inline vec3<T> operator * (vec3<T> o){
|
|
return {x * o.x, y * o.y, z * o.z};
|
|
}
|
|
inline vec3<T> operator / (vec3<T> o){
|
|
return {x / o.x, y / o.y, z / o.z};
|
|
}
|
|
inline vec3<T> operator * (T o){
|
|
return {x * o, y * o, z * o};
|
|
}
|
|
inline vec3<T> operator / (T o){
|
|
return {x / o, y / o, z / o};
|
|
}
|
|
|
|
inline bool operator == (vec3<T> o){
|
|
return x == o.x && y == o.y && z == o.z;
|
|
}
|
|
|
|
inline vec3<T> normalized(){
|
|
T i_d = isqrt<T>(x*x + y*y + z*z);
|
|
return (*this) * i_d;
|
|
}
|
|
inline T i_length(){
|
|
return isqrt<T>(x*x + y*y + z*z);
|
|
}
|
|
inline T length2(){
|
|
return x*x + y*y + z*z;
|
|
}
|
|
template <typename U>
|
|
inline operator vec3<U>(){
|
|
return {x, y, z};
|
|
}
|
|
};
|
|
|
|
template <typename T>
|
|
inline vec3<T> smoothDamp(vec3<T> current, vec3<T> target, vec3<T> *currentVelocity, T smoothTime, T deltaTime, T maxSpeed){
|
|
if(smoothTime < 0.0001f) smoothTime = 0.0001f;
|
|
T omega = 2.0f / smoothTime;
|
|
|
|
T x = omega * deltaTime;
|
|
T exp = 1.0f / (1.0f + x + 0.48f * x * x + 0.235f * x * x * x);
|
|
|
|
vec3<T> change = current - target;
|
|
vec3<T> originalTo = target;
|
|
|
|
T maxChange = maxSpeed * smoothTime;
|
|
T maxChangeSq = maxChange * maxChange;
|
|
T sqDist = change.x * change.x + change.y * change.y + change.z * change.z;
|
|
if(sqDist > maxChangeSq){
|
|
T mag = 1.0f/isqrt(sqDist);
|
|
change = change / mag * maxChange;
|
|
}
|
|
|
|
target = current - change;
|
|
|
|
vec3<T> temp = ((*currentVelocity) + change * omega) * deltaTime;
|
|
|
|
(*currentVelocity) = ((*currentVelocity) - temp * omega) * exp;
|
|
|
|
vec3<T> output = target + (change + temp) * exp;
|
|
|
|
vec3<T> origMinusCurrent = originalTo - current;
|
|
vec3<T> outMinusOrig = output - originalTo;
|
|
|
|
if(origMinusCurrent.x * outMinusOrig.x + origMinusCurrent.y * outMinusOrig.y > 0){
|
|
output = originalTo;
|
|
|
|
(*currentVelocity) = (output - originalTo) / deltaTime;
|
|
}
|
|
return output;
|
|
}
|
|
|
|
template <typename T>
|
|
inline T dot(vec3<T> a, vec3<T> b){
|
|
return a.x*b.x + a.y*b.y + a.z*b.z;
|
|
}
|
|
|
|
template <typename T>
|
|
inline T cross(vec3<T> a, vec3<T> b){
|
|
return (a.x*b.y) - (a.y*b.x);
|
|
}
|