(mkosarek) Fix for wrong timestep

2 files changed, 36 insertions, 2 deletions

diff --git a/shared_cpp/mathlib.cpp b/shared_cpp/mathlib.cpp
index b7748fe..fb09cd9 100644
--- a/shared_cpp/mathlib.cpp
+++ b/shared_cpp/mathlib.cpp
@@ -155,6 +155,13 @@ Vector3 Vector3::operator+(const Vector3& v2) {
 	return add(v2);
 }
 
+Vector3& Vector3::operator+=(Vector3 other) {
+    x += other.x;
+    y += other.y;
+    z += other.z;
+    return *this;
+}
+
 Vector3 Vector3::operator-(const Vector3& v2) {
 	return subtract(v2);
 }
@@ -167,6 +174,14 @@ Vector3 Vector3::operator*(float value) {
 	return scale(value);
 }
 
+Vector3 Vector3::operator/(const Vector3& v2) {
+    return {
+        x / v2.x,
+        y / v2.y,
+        z / v2.z        
+    };
+}
+
 Vector3 Vector3::operator*(const Vector3& v2) {
 	return {
 	    x * v2.x,
@@ -623,3 +638,16 @@ Mat4x4 Quaternion::toMatrix() const {
 float Quaternion::dot(const Quaternion& other) const {
 	return w * other.w + x * other.x + y * other.y + z * other.z;
 }
+
+Quaternion quaternionFromRotation(Vector3 axis, float angleRadians) {
+    float halfAngleRadians = angleRadians / 2.f;
+    float cosHalfAngRad = cosf(halfAngleRadians);
+    float sinHalfAngRad = sinf(halfAngleRadians);
+
+    return {
+        cosHalfAngRad,
+        axis.x * sinHalfAngRad,
+        axis.y * sinHalfAngRad,
+        axis.z * sinHalfAngRad
+    };
+}
diff --git a/shared_cpp/mathlib.h b/shared_cpp/mathlib.h
index d550e90..e3c6875 100644
--- a/shared_cpp/mathlib.h
+++ b/shared_cpp/mathlib.h
@@ -18,6 +18,8 @@
 #define ABS(x) (x < 0 ? -x : x)
 #define SIGN(x) (x < 0 ? -1 : 1)
 #define PI 3.141592653589793238463
+#define DEG_TO_RAD(x) (x * (PI / 180.f))
+#define RAD_TO_DEG(x) (x * (180.f / PI))
 
 struct Vector2 {
     float x = 0;
@@ -58,10 +60,12 @@ struct Vector3 {
     Vector3 normalize();
 
     Vector3 operator+(const Vector3& v2);
+    Vector3& operator+=(Vector3 other);
     Vector3 operator-(const Vector3& v2);
     Vector3 operator-();
     Vector3 operator*(float value);
     Vector3 operator*(const Vector3& v2);
+    Vector3 operator/(const Vector3& v2);
     float operator[](int index);
     
     void printDebug(const char* name);
@@ -75,7 +79,7 @@ struct Vector4 {
 
     Vector4();
     Vector4(float value);
-    Vector4(float inX = 0.f, float inY = 0.f, float inZ = 0.f, float inW = 1.f);
+    Vector4(float inX, float inY, float inZ, float inW);
     Vector4 fromColor(float r, float g, float b, float a);
 	Vector4 toNormalizedColor();
 
@@ -126,10 +130,10 @@ struct Mat4x4 {
 };
 
 struct Quaternion {
+    float w = 0;
     float x = 0;
     float y = 0;
     float z = 0;
-    float w = 0;
 
     float operator [](int index);
 	Quaternion operator*(const Quaternion& other) const;
@@ -142,3 +146,5 @@ struct Quaternion {
 	float length() const;
 	float dot(const Quaternion& other) const;
 };
+
+Quaternion quaternionFromRotation(Vector3 axis, float angleRadians);