commit eaa5702748c4e5f76a8217da356c773fe05be546 Author: Pascal Serrarens Date: Wed Jan 5 10:42:30 2022 +0100 first commit diff --git a/Quaternion.cpp b/Quaternion.cpp new file mode 100644 index 0000000..2327f22 --- /dev/null +++ b/Quaternion.cpp @@ -0,0 +1,391 @@ +//#include "stdafx.h" +#include "pch.h" +#include +#include +#include "Quaternion.h" +#include "Vector3.h" + +void CopyQuat(const Quat& q1, Quat& q2) { + q2.x = q1.x; + q2.y = q1.y; + q2.z = q1.z; + q2.w = q1.w; +} + +const float Deg2Rad = 0.0174532924F; +const float Rad2Deg = 57.29578F; + +Quaternion::Quaternion() { + x = 0; + y = 0; + z = 0; + w = 1; +} + +Quaternion::Quaternion(float _x, float _y, float _z, float _w) { + x = _x; + y = _y; + z = _z; + w = _w; +} + +Quaternion::Quaternion(Vector3 _xyz, float _w) { + x = _xyz.x; + y = _xyz.y; + z = _xyz.z; + w = _w; +} + +Quaternion::Quaternion(Quat q) { + x = q.x; + y = q.y; + z = q.z; + w = q.w; +} + +Quaternion::~Quaternion() {} + +const Quaternion Quaternion::identity = Quaternion(0, 0, 0, 1); + +Vector3 Quaternion::xyz() const { + return Vector3(x, y, z); +} + +float Quaternion::GetLength() const { + return sqrtf(x * x + y * y + z * z + w * w); +} + +float Quaternion::GetLengthSquared() const { + return x * x + y * y + z * z + w * w; +} +float Quaternion::GetLengthSquared(const Quaternion& q) { + return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; +} + +void Quaternion::Normalize() { + float scale = 1.0f / GetLength(); + x *= scale; + y *= scale; + z *= scale; + w *= scale; +} + +Quaternion Quaternion::Normalize(const Quaternion& q) { + Quaternion result; + Normalize(q, result); + return result; +}; + +void Quaternion::Normalize(const Quaternion& q, Quaternion& result) { + float scale = 1.0f / q.GetLength(); + result = Quaternion(q.x * scale, q.y * scale, q.z * scale, q.w * scale); +}; + +float Quaternion::Dot(Quaternion a, Quaternion b) { + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; +} + +Vector3 Quaternion::ToAngles(const Quaternion& q1) { + float test = q1.x * q1.y + q1.z * q1.w; + if (test > 0.499) { // singularity at north pole + return Vector3( + 0, + 2 * (float)atan2(q1.x, q1.w) * Rad2Deg, + 90 + ); + } + else if (test < -0.499) { // singularity at south pole + return Vector3( + 0, + -2 * (float)atan2(q1.x, q1.w) * Rad2Deg, + -90 + ); + } + else { + float sqx = q1.x * q1.x; + float sqy = q1.y * q1.y; + float sqz = q1.z * q1.z; + + return Vector3( + atan2f(2 * q1.x * q1.w - 2 * q1.y * q1.z, 1 - 2 * sqx - 2 * sqz) * Rad2Deg, + atan2f(2 * q1.y * q1.w - 2 * q1.x * q1.z, 1 - 2 * sqy - 2 * sqz) * Rad2Deg, + asinf(2 * test) * Rad2Deg + ); + } +} + +Quaternion Quaternion::operator *(const Quaternion& r2) const { + return Quaternion( + this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x, + -this->x * r2.z + this->y * r2.w + this->z * r2.x + this->w * r2.y, + this->x * r2.y - this->y * r2.x + this->z * r2.w + this->w * r2.z, + -this->x * r2.x - this->y * r2.y - this->z * r2.z + this->w * r2.w + ); +}; + +Vector3 Quaternion::operator *(const Vector3& p) const { + float num = this->x * 2; + float num2 = this->y * 2; + float num3 = this->z * 2; + float num4 = this->x * num; + float num5 = this->y * num2; + float num6 = this->z * num3; + float num7 = this->x * num2; + float num8 = this->x * num3; + float num9 = this->y * num3; + float num10 = this->w * num; + float num11 = this->w * num2; + float num12 = this->w * num3; + Vector3 result = Vector3::zero; + result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z; + result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z; + result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z; + return result; +} + +bool Quaternion::operator==(const Quaternion& q) { + return (this->x == q.x && this->y == q.y && this->z == q.z && this->w == q.w); +} + +Quaternion Quaternion::Inverse(Quaternion r) { + float n = sqrtf(r.x * r.x + r.y * r.y + r.z * r.z + r.w * r.w); + return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n); +} + + +//Quaternion Quaternion::LookRotation(const Vector3 &forward, const Vector3 &upwards) { +// return LookRotation(forward, upwards); +//} + + +// This is not correct when forward == Vector3::up +Quaternion Quaternion::LookRotation(const Vector3& forward) { + Vector3 up = Vector3(0, 1, 0); + if ((Vector3)forward == up) + up = Vector3(0, 0, 1); + return LookRotation(forward, up); +} +Quaternion Quaternion::LookRotation(const Vector3& forward, const Vector3& up) { + Vector3 nForward = Vector3::Normalize(forward); + Vector3 nRight = Vector3::Normalize(Vector3::Cross(up, nForward)); + Vector3 nUp = Vector3::Cross(nForward, nRight); + float m00 = nRight.x; + float m01 = nRight.y; + float m02 = nRight.z; + float m10 = nUp.x; + float m11 = nUp.y; + float m12 = nUp.z; + float m20 = nForward.x; + float m21 = nForward.y; + float m22 = nForward.z; + + + float num8 = (m00 + m11) + m22; + Quaternion quaternion = Quaternion(); + if (num8 > 0) { + float num = sqrtf(num8 + 1); + quaternion.w = num * 0.5f; + num = 0.5f / num; + quaternion.x = (m12 - m21) * num; + quaternion.y = (m20 - m02) * num; + quaternion.z = (m01 - m10) * num; + return quaternion; + } + if ((m00 >= m11) && (m00 >= m22)) { + float num7 = sqrtf(((1 + m00) - m11) - m22); + float num4 = 0.5F / num7; + quaternion.x = 0.5f * num7; + quaternion.y = (m01 + m10) * num4; + quaternion.z = (m02 + m20) * num4; + quaternion.w = (m12 - m21) * num4; + return quaternion; + } + if (m11 > m22) { + float num6 = sqrtf(((1 + m11) - m00) - m22); + float num3 = 0.5F / num6; + quaternion.x = (m10 + m01) * num3; + quaternion.y = 0.5F * num6; + quaternion.z = (m21 + m12) * num3; + quaternion.w = (m20 - m02) * num3; + return quaternion; + } + float num5 = sqrtf(((1 + m22) - m00) - m11); + float num2 = 0.5F / num5; + quaternion.x = (m20 + m02) * num2; + quaternion.y = (m21 + m12) * num2; + quaternion.z = 0.5F * num5; + quaternion.w = (m01 - m10) * num2; + return quaternion; +} + +Quaternion Quaternion::FromToRotation(Vector3 fromDirection, Vector3 toDirection) { + return RotateTowards(LookRotation(fromDirection), LookRotation(toDirection), FLT_MAX); +} + +Quaternion Quaternion::RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta) { + float num = Quaternion::Angle(from, to); + if (num == 0) { + return to; + } + float t = (float)fmin(1, maxDegreesDelta / num); + return SlerpUnclamped(from, to, t); +} + +Quaternion Quaternion::AngleAxis(float angle, const Vector3& axis) { + // return AngleAxis(angle, axis); + //} + //Quaternion AngleAxis(float angle, Vector3& axis) { + if (Vector3::SqrMagnitude(axis) == 0.0) + return Quaternion(); + + Quaternion result = Quaternion(); + float radians = angle * Deg2Rad; + radians *= 0.5; + //Vector3::Normalize(axis); + Vector3 axis2 = axis * (float)sin(radians); + result.x = axis2.x; + result.y = axis2.y; + result.z = axis2.z; + result.w = (float)cos(radians); + + return Quaternion::Normalize(result); +} + +float Quaternion::Angle(Quaternion a, Quaternion b) { + float f = Quaternion::Dot(a, b); + return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg; +} + +void Quaternion::ToAngleAxis(float* angle, Vector3* axis) +{ + Quaternion::ToAxisAngleRad(*this, axis, angle); + *angle *= Rad2Deg; +} + +void Quaternion::ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle) +{ + Quaternion q1 = (fabs(q.w) > 1.0f) ? Quaternion::Normalize(q) : q; + *angle = 2.0f * acosf(q1.w); // angle + float den = sqrtf(1.0F - q1.w * q1.w); + if (den > 0.0001f) + { + *axis = q1.xyz() / den; + } + else + { + // This occurs when the angle is zero. + // Not a problem: just set an arbitrary normalized axis. + *axis = Vector3(1, 0, 0); + } +} +Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) { + // if either input is zero, return the other. + if (Quaternion::GetLengthSquared(a) == 0.0) { + if (Quaternion::GetLengthSquared(b) == 0.0) { + return Quaternion(); + } + return b; + } + else if (Quaternion::GetLengthSquared(b) == 0.0f) { + return a; + } + + const Vector3 axyz = a.xyz(); + const Vector3 bxyz = b.xyz(); + float cosHalfAngle = a.w * b.w + Vector3::Dot(axyz, bxyz); + + Quaternion b2 = b; + if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { + // angle = 0.0f, so just return one input. + return a; + } + else if (cosHalfAngle < 0.0f) { + b2.x = -b.x; + b2.y = -b.y; + b2.z = -b.z; + b2.w = -b.w; + cosHalfAngle = -cosHalfAngle; + } + + float blendA; + float blendB; + if (cosHalfAngle < 0.99f) { + // do proper slerp for big angles + float halfAngle = acosf(cosHalfAngle); + float sinHalfAngle = sinf(halfAngle); + float oneOverSinHalfAngle = 1.0F / sinHalfAngle; + blendA = sinf(halfAngle * (1.0F - t)) * oneOverSinHalfAngle; + blendB = sinf(halfAngle * t) * oneOverSinHalfAngle; + } + else { + // do lerp if angle is really small. + blendA = 1.0f - t; + blendB = t; + } + + Quaternion result = Quaternion(axyz * blendA + b2.xyz() * blendB, blendA * a.w + blendB * b2.w); + if (result.GetLengthSquared() > 0.0f) + return Quaternion::Normalize(result); + else + return Quaternion(); +} +//Quaternion Quaternion::SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t) { +// return SlerpUnclamped(a, b, t); +//} +Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) { + if (t > 1) t = 1; + if (t < 0) t = 0; + return Quaternion::SlerpUnclamped(a, b, t); +} +//Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, float t) { +// return Slerp(a, b, t); +//} + +Quaternion Quaternion::Euler(float x, float y, float z) { + return Quaternion::Euler(Vector3(x, y, z)); +} +Quaternion Quaternion::Euler(Vector3 euler) { + return Quaternion::FromEulerRad(euler * Deg2Rad); +} + +Quaternion Quaternion::FromEulerRad(Vector3 euler) { + float yaw = euler.x; + float pitch = euler.y; + float roll = euler.z; + float rollOver2 = roll * 0.5f; + float sinRollOver2 = (float)sin((float)rollOver2); + float cosRollOver2 = (float)cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)sin((float)pitchOver2); + float cosPitchOver2 = (float)cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)sin((float)yawOver2); + float cosYawOver2 = (float)cos((float)yawOver2); + Quaternion result; + result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; + result.x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2; + result.y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2; + result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; + return result; +} + +float Quaternion::GetAngleAround(Vector3 axis, Quaternion rotation) { + Quaternion secondaryRotation = GetRotationAround(axis, rotation); + float rotationAngle; + Vector3 rotationAxis; + secondaryRotation.ToAngleAxis(&rotationAngle, &rotationAxis); + + // Do the axis point in opposite directions? + if (Vector3::Dot(axis, rotationAxis) < 0) + rotationAngle = -rotationAngle; + + return rotationAngle; +} + +Quaternion Quaternion::GetRotationAround(Vector3 axis, Quaternion rotation) { + Vector3 ra = Vector3(rotation.x, rotation.y, rotation.z); // rotation axis + Vector3 p = Vector3::Project(ra, axis); // return projection ra on to axis (parallel component) + Quaternion twist = Quaternion(p.x, p.y, p.z, rotation.w); + twist = Quaternion::Normalize(twist); + return twist; +} diff --git a/Quaternion.h b/Quaternion.h new file mode 100644 index 0000000..dba74bd --- /dev/null +++ b/Quaternion.h @@ -0,0 +1,71 @@ +#ifndef QUATERNION_H +#define QUATERNION_H + +#pragma once +struct Vector3; + +extern "C" { + typedef struct Quat { + float x; + float y; + float z; + float w; + } Quat; + void CopyQuat(const Quat& q1, Quat& q2); +} + + +struct Quaternion : Quat { +public: + Quaternion(); + Quaternion(float _x, float _y, float _z, float _w); + Quaternion(Vector3 _xyz, float _w); + Quaternion(Quat q); + ~Quaternion(); + + const static Quaternion identity; + + float GetLength() const; + float GetLengthSquared() const; + static float GetLengthSquared(const Quaternion& q); + void Normalize(); + static Quaternion Normalize(const Quaternion& q); + static void Normalize(const Quaternion& q, Quaternion& result); + + static float Dot(Quaternion a, Quaternion b); + + static Vector3 ToAngles(const Quaternion& q1); + + Vector3 operator *(const Vector3& p) const; + Quaternion operator *(const Quaternion& r2) const; + + bool operator ==(const Quaternion& q); + + static Quaternion Inverse(Quaternion r); + + static Quaternion LookRotation(const Vector3& forward, const Vector3& upwards); + static Quaternion LookRotation(const Vector3& forward); + + static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection); + + static Quaternion RotateTowards(const Quaternion& from, const Quaternion& to, float maxDegreesDelta); + static Quaternion AngleAxis(float angle, const Vector3& axis); + void ToAngleAxis(float* angle, Vector3* axis); + static void ToAxisAngleRad(const Quaternion& q, Vector3* const axis, float* angle); + static float Angle(Quaternion a, Quaternion b); + static Quaternion Slerp(const Quaternion& a, const Quaternion& b, float t); + static Quaternion SlerpUnclamped(const Quaternion& a, const Quaternion& b, float t); + + static Quaternion Euler(float x, float y, float z); + static Quaternion Euler(Vector3 euler); + static Quaternion FromEulerRad(Vector3 euler); + static float GetAngleAround(Vector3 axis, Quaternion rotation); + static Quaternion GetRotationAround(Vector3 axis, Quaternion rotation); +public: + Vector3 xyz() const; +}; + +//#include "Vector3.h" + + +#endif diff --git a/Vector3.cpp b/Vector3.cpp new file mode 100644 index 0000000..a5807c8 --- /dev/null +++ b/Vector3.cpp @@ -0,0 +1,205 @@ +//#include "stdafx.h" +#include "pch.h" +#include +#include "Vector3.h" +#include "Quaternion.h" + +const float Deg2Rad = 0.0174532924F; +const float Rad2Deg = 57.29578F; + +Vector3::Vector3() { + x = 0; + y = 0; + z = 0; +} + +Vector3::Vector3(float _x, float _y, float _z) { + x = _x; + y = _y; + z = _z; +} + +Vector3::Vector3(Vec3 v) { + x = v.x; + y = v.y; + z = v.z; +} + +Vector3::~Vector3() +{ +} + +const Vector3 Vector3::zero = Vector3(0, 0, 0); +const Vector3 Vector3::right = Vector3(1, 0, 0); +const Vector3 Vector3::left = Vector3(-1, 0, 0); +const Vector3 Vector3::up = Vector3(0, 1, 0); +const Vector3 Vector3::down = Vector3(0, -1, 0); +const Vector3 Vector3::forward = Vector3(0, 0, 1); +const Vector3 Vector3::back = Vector3(0, 0, -1); + +float Vector3::Magnitude(const Vector3& a) { + return sqrtf(a.x * a.x + a.y * a.y + a.z * a.z); +} +float Vector3::magnitude() const { + return (float)sqrtf(x * x + y * y + z * z); +} + +float Vector3::SqrMagnitude(const Vector3& a) { + return a.x * a.x + a.y * a.y + a.z * a.z; +} +float Vector3::sqrMagnitude() const { + return(x * x + y * y + z * z); +} + +Vector3 Vector3::Normalize(Vector3 v) { + float num = Vector3::Magnitude(v); + Vector3 result = Vector3::zero; + if (num > 1E-05f) { + result = v / num; + } + return result; +} + +Vector3 Vector3::normalized() const { + float num = this->magnitude(); + Vector3 result = Vector3::zero; + if (num > 1E-05f) { + result = ((Vector3)*this) / num; + } + return result; +} + +Vector3 Vector3::operator -(const Vector3& v2) const { + return Vector3(this->x - v2.x, this->y - v2.y, this->z - v2.z); +} +//Vector operator -(const Vector& v1, const Vector& v2) { +// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); +//} +//Vector Vector::operator -(const Vector& v1, const Vector& v2) { +// return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); +//} +Vector3 Vector3::operator -() { + return Vector3(-this->x, -this->y, -this->z); +} + +Vector3 Vector3::operator +(const Vector3& v2) const { + return Vector3(this->x + v2.x, this->y + v2.y, this->z + v2.z); +} +Vector3 Vector3::Scale(const Vector3& p1, const Vector3& p2) { + return Vector3(p1.x * p2.x, p1.y * p2.y, p1.z * p2.z); +} + +//Vector Vector::operator *(float f, const Vector& p) { +// return Vector(f * p.x, f * p.y, f * p.z); +//} +Vector3 Vector3::operator *(float f) const { + return Vector3(this->x * f, this->y * f, this->z * f); +} +//Vector Vector::operator*(const Vector& v, float f) { +// return Vector(v.x * f, v.y * f, v.z * f); +//} +//Vector Vector::operator*(float f, const Vector& v) { +// return Vector(f * v.x, f * v.y, f * v.z); +//} + +Vector3 operator *(const Quaternion& o, const Vector3& p) { + float num = o.x * 2; + float num2 = o.y * 2; + float num3 = o.z * 2; + float num4 = o.x * num; + float num5 = o.y * num2; + float num6 = o.z * num3; + float num7 = o.x * num2; + float num8 = o.x * num3; + float num9 = o.y * num3; + float num10 = o.w * num; + float num11 = o.w * num2; + float num12 = o.w * num3; + Vector3 result = Vector3::zero; + result.x = (1 - (num5 + num6)) * p.x + (num7 - num12) * p.y + (num8 + num11) * p.z; + result.y = (num7 + num12) * p.x + (1 - (num4 + num6)) * p.y + (num9 - num10) * p.z; + result.z = (num8 - num11) * p.x + (num9 + num10) * p.y + (1 - (num4 + num5)) * p.z; + return result; +} + +Vector3 Vector3::operator/(const float& d) { + return Vector3(this->x / d, this->y / d, this->z / d); +} + +float Vector3::Dot(const Vector3& v1, const Vector3& v2) { + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; +} + +bool Vector3::operator==(const Vector3& v) { + return (this->x == v.x && this->y == v.y && this->z == v.z); +} + +float Vector3::Distance(const Vector3& p1, const Vector3& p2) { + return Magnitude(p1 - p2); +} + +Vector3 Vector3::Cross(const Vector3& v1, const Vector3& v2) { + return Vector3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); +} + +// Projects a vector onto another vector. +Vector3 Vector3::Project(Vector3 vector, Vector3 onNormal) { + float sqrMag = Dot(onNormal, onNormal); + if (sqrMag < 1E-05f) + return Vector3::zero; + else + return onNormal * Dot(vector, onNormal) / sqrMag; +} + +// Projects a vector onto a plane defined by a normal orthogonal to the plane. +Vector3 Vector3::ProjectOnPlane(Vector3 vector, Vector3 planeNormal) { + return vector - Project(vector, planeNormal); +} + +float clamp(float x, float upper, float lower) { + return fminf(upper, fmaxf(x, lower)); +} + +// This function is buggy (0-1-0, 0-1-0) gives 180 degrees in return!!! +float Vector3::Angle(Vector3 from, Vector3 to) { + // sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers + float denominator = sqrtf(from.sqrMagnitude() * to.sqrMagnitude()); + if (denominator < 1E-05f) + return 0; + + float dot = clamp(Vector3::Dot(from, to) / denominator, -1.0, 1.0); + return ((float)acos(dot)) * Rad2Deg; +} + +float Vector3::SignedAngle(Vector3 from, Vector3 to, Vector3 axis) { + // angle in [0,180] + float angle = Vector3::Angle(from, to); + float signd = signbit(Vector3::Dot(axis, Vector3::Cross(from, to))); + + // angle in [-179,180] + float signed_angle = angle * signd; + + // angle in [0,360] (not used but included here for completeness) + //float angle360 = (signed_angle + 180) % 360; + + return signed_angle; +} + + +void CopyVec3(const Vec3& v1, Vec3& v2) { + v2.x = v1.x; + v2.y = v1.y; + v2.z = v1.z; +} + +void Vec3_Constructor(Vec3* v, float _x, float _y, float _z) { + v->x = _x; + v->y = _y; + v->z = _z; +} + +//Vec3 new_Vec3(float _x, float _y, float _z) { +// Vec3 v = Vec3(); +// v.x = _x; +// return v; +//} \ No newline at end of file diff --git a/Vector3.h b/Vector3.h new file mode 100644 index 0000000..2f87e46 --- /dev/null +++ b/Vector3.h @@ -0,0 +1,71 @@ +#ifndef VECTOR_H +#define VECTOR_H + +#pragma once +struct Quaternion; + +extern "C" { + typedef struct Vec3 { + float x; + float y; + float z; + + } Vec3; + + void CopyVec3(const Vec3& v1, Vec3& v2); +} + +struct Vector3 : Vec3 { +public: + Vector3(); + Vector3(float _x, float _y, float _z); + Vector3(Vec3 v); + ~Vector3(); + + const static Vector3 zero; + const static Vector3 right; + const static Vector3 left; + const static Vector3 up; + const static Vector3 down; + const static Vector3 forward; + const static Vector3 back; + + static float Magnitude(const Vector3& a); + float magnitude() const; + static float SqrMagnitude(const Vector3& a); + float sqrMagnitude() const; + static Vector3 Normalize(Vector3 v); + Vector3 normalized() const; + + Vector3 operator -(); + Vector3 operator -(const Vector3& p2) const; + + Vector3 operator +(const Vector3& t1) const; + static Vector3 Scale(const Vector3& p1, const Vector3& p2); + + Vector3 operator *(float f) const; + + Vector3 operator /(const float& d); + static float Dot(const Vector3& v1, const Vector3& v2); + + bool operator ==(const Vector3& v); + + static float Distance(const Vector3& p1, const Vector3& p2); + static Vector3 Cross(const Vector3& v1, const Vector3& v2); + + // Projects a vector onto another vector. + static Vector3 Project(Vector3 vector, Vector3 onNormal); + // Projects a vector onto a plane defined by a normal orthogonal to the plane. + static Vector3 ProjectOnPlane(Vector3 vector, Vector3 planeNormal); + + static float Angle(Vector3 from, Vector3 to); + static float SignedAngle(Vector3 from, Vector3 to, Vector3 axis); +}; + +void Vec3_Constructor(Vec3* v, float _x, float _y, float _z); +Vec3 new_Vec3(float _x, float _y, float _z); + + +//#include "Quaternion.h" + +#endif