diff --git a/include/J3ML/LinearAlgebra/Quaternion.h b/include/J3ML/LinearAlgebra/Quaternion.h index b29e152..c64f6cc 100644 --- a/include/J3ML/LinearAlgebra/Quaternion.h +++ b/include/J3ML/LinearAlgebra/Quaternion.h @@ -5,85 +5,211 @@ #include #include #include +#include #include - - namespace J3ML::LinearAlgebra { - - class Matrix3x3; - - class Quaternion : public Vector4 { + class Quaternion { public: + /// The identity quaternion performs no rotation when applied to a vector. + static const Quaternion Identity; + /// A compile-time constant Quaternion with the value (NAN, NAN, NAN, NAN). + /// For this constant, each element has the value of quiet NAN, or Not-A-Number. + /// @note Never compare a Quaternion to this value! Due to how IEEE floats work, "nan == nan" returns false! + /// That is, nothing is equal to NaN, not even NaN itself! + static const Quaternion NaN; + public: + /// The default constructor does not initialize any member values. Quaternion(); - + /// Copy constructor Quaternion(const Quaternion &rhs) = default; + /// Constructs a quaternion from the given data buffer. + /// @param data An array of four floats to use for the quaternion, in the order 'x,y,z,w.' (== 'i,j,k,w') + explicit Quaternion(const float *data); explicit Quaternion(const Matrix3x3 &rotationMtrx); - explicit Quaternion(const Matrix4x4 &rotationMtrx); - // @note The input data is not normalized after construction, this has to be done manually. + /// @param x The factor of i. + /// @param y The factor of j. + /// @param z The factor of k. + /// @param w The scalar factor (or 'w'). + /// @note The input data is not normalized after construction, this has to be done manually. Quaternion(float X, float Y, float Z, float W); - // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis - // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0. - Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween); - - Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween); - //void Inverted(); + /// Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis + /// @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0. + /// @param rotationAngleRadians The angle to rotate by, in radians. For example, Pi/4.f equals to 45 degrees, Pi/2.f is 90 degrees, etc. + /// @see DegToRad() + Quaternion(const Vector3 &rotationAxis, float rotationAngleRadians); + Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians); explicit Quaternion(Vector4 vector4); explicit Quaternion(const EulerAngle& angle); explicit Quaternion(const AxisAngle& angle); + /// Creates a LookAt quaternion. + /** A LookAt quaternion is a quaternion that orients an object to face towards a specified target direction. + @param localForward Specifies the forward direction in the local space of the object. This is the direction + the model is facing at in its own local/object space, often +X(1,0,0), +Y(0,1,0), or +Z(0,0,1). + The vector to pass in here depends on the conventions you or your modeling software is using, and it is best + to pick one convention for all your objects, and be consistent. + This input parameter must be a normalized vector. + @param targetDirection Specifies the desired world space direction the object should look at. This function + will compute a quaternion which will rotate the localForward vector to orient towards this targetDirection + vector. This input parameter must be a normalized vector. + @param localUp Specifies the up direction in the local space of the object. This is the up direction the model + was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you + or your modeling software is using, and it is best to pick one convention for all your objects, and be + consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the + vector localForward, i.e. localForward.Dot(localUp) == 0. + @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to + coincide with another (localForward->targetDirection) would cause the up direction of the resulting + orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep + the up direction straight, this function orients the localUp direction of the model to point towards + the specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be + collinear, but they do not need to be perpendicular either. + @return A quaternion that maps the given local space forward direction vector to point towards the given target + direction, and the given local up direction towards the given target world up direction. For the returned + quaternion Q it holds that M * localForward = targetDirection, and M * localUp lies in the plane spanned + by the vectors targetDirection and worldUp. + @see RotateFromTo() */ + static Quaternion LookAt(const Vector3& localForward, const Vector3& targetDirection, const Vector3& localUp, const Vector3& worldUp); + + /// Creates a new Quaternion that rotates about the given axis by the given angle. + static Quaternion RotateAxisAngle(const AxisAngle& axisAngle); + + /// Creates a new quaternion that rotates about the positive X axis by the given rotation. + static Quaternion RotateX(float angleRadians); + /// Creates a new quaternion that rotates about the positive Y axis by the given rotation. + static Quaternion RotateY(float angleRadians); + /// Creates a new quaternion that rotates about the positive Z axis by the given rotation. + static Quaternion RotateZ(float angleRadians); + + /// Creates a new quaternion that rotates sourceDirection vector (in world space) to coincide with the + /// targetDirection vector (in world space). + /// Rotation is performed about the origin. + /// The vectors sourceDirection and targetDirection are assumed to be normalized. + /// @note There are multiple such rotations - this function returns the rotation that has the shortest angle + /// (when decomposed to axis-angle notation). + static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection); + static Quaternion RotateFromTo(const Vector4& sourceDirection, const Vector4& targetDirection); + + /// Creates a new quaternion that + /// 1. rotates sourceDirection vector to coincide with the targetDirection vector, and then + /// 2. rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that + /// sourceDirection must look at targetDirection + static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection, const Vector3& sourceDirection2, const Vector3& targetDirection2); + + + /// Returns a uniformly random unitary quaternion. + static Quaternion RandomRotation(J3ML::Algorithm::RNG &rng); + public: void SetFromAxisAngle(const Vector3 &vector3, float between); void SetFromAxisAngle(const Vector4 &vector4, float between); void SetFrom(const AxisAngle& angle); - Quaternion Inverse() const; + /// Inverses this quaternion in-place. + /// @note For optimization purposes, this function assumes that the quaternion is unitary, in which + /// case the inverse of the quaternion is simply just the same as its conjugate. + /// This function does not detect whether the operation succeeded or failed. + void Inverse(); - Quaternion Conjugate() const; + /// Returns an inverted copy of this quaternion. + [[nodiscard]] Quaternion Inverted() const; + /// Computes the conjugate of this quaternion in-place. + void Conjugate(); + /// Returns a conjugated copy of this quaternion. + [[nodiscard]] Quaternion Conjugated() const; - //void Normalize(); - Vector3 GetWorldX() const; + /// Inverses this quaternion in-place. + /// Call this function when the quaternion is not known beforehand to be normalized. + /// This function computes the inverse proper, and normalizes the result. + /// @note Because of the normalization, it does not necessarily hold that q * q.InverseAndNormalize() == id. + /// @return Returns the old length of this quaternion (not the old length of the inverse quaternion). + float InverseAndNormalize(); - Vector3 GetWorldY() const; + /// Returns the local +X axis in the post-transformed coordinate space. This is the same as transforming the vector (1,0,0) by this quaternion. + [[nodiscard]] Vector3 WorldX() const; + /// Returns the local +Y axis in the post-transformed coordinate space. This is the same as transforming the vector (0,1,0) by this quaternion. + [[nodiscard]] Vector3 WorldY() const; + /// Returns the local +Z axis in the post-transformed coordinate space. This is the same as transforming the vector (0,0,1) by this quaternion. + [[nodiscard]] Vector3 WorldZ() const; - Vector3 GetWorldZ() const; + /// Returns the axis of rotation for this quaternion. + [[nodiscard]] Vector3 Axis() const; - Vector3 GetAxis() const; + /// Returns the angle of rotation for this quaternion, in radians. + [[nodiscard]] float Angle() const; - float GetAngle() const; + [[nodiscard]] float LengthSquared() const; + [[nodiscard]] float Length() const; - EulerAngle ToEulerAngle() const; + [[nodiscard]] EulerAngle ToEulerAngle() const; - Matrix3x3 ToMatrix3x3() const; + [[nodiscard]] Matrix3x3 ToMatrix3x3() const; + [[nodiscard]] Matrix4x4 ToMatrix4x4() const; - Matrix4x4 ToMatrix4x4() const; + [[nodiscard]] Matrix4x4 ToMatrix4x4(const Vector3 &translation) const; - Matrix4x4 ToMatrix4x4(const Vector3 &translation) const; - - Vector3 Transform(const Vector3& vec) const; - Vector3 Transform(float X, float Y, float Z) const; + [[nodiscard]] Vector3 Transform(const Vector3& vec) const; + [[nodiscard]] Vector3 Transform(float X, float Y, float Z) const; // Note: We only transform the x,y,z components of 4D vectors, w is left untouched - Vector4 Transform(const Vector4& vec) const; - Vector4 Transform(float X, float Y, float Z, float W) const; + [[nodiscard]] Vector4 Transform(const Vector4& vec) const; + [[nodiscard]] Vector4 Transform(float X, float Y, float Z, float W) const; - Quaternion GetInverse() const; - Quaternion Lerp(const Quaternion& b, float t) const; - Quaternion Slerp(const Quaternion& q2, float t) const; + [[nodiscard]] Quaternion Lerp(const Quaternion& b, float t) const; + static Quaternion Lerp(const Quaternion &source, const Quaternion& target, float t); + [[nodiscard]] Quaternion Slerp(const Quaternion& q2, float t) const; + static Quaternion Slerp(const Quaternion &source, const Quaternion& target, float t); - Quaternion Normalize() const; + /// Returns the 'from' vector rotated towards the 'to' vector by the given normalized time parameter. + /** This function slerps the given 'form' vector toward the 'to' vector. + @param from A normalized direction vector specifying the direction of rotation at t=0 + @param to A normalized direction vector specifying the direction of rotation at t=1 + @param t The interpolation time parameter, in the range [0, 1]. Input values outside this range are + silently clamped to the [0, 1] interval. + @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */ + static Vector3 SlerpVector(const Vector3& from, const Vector3& to, float t); + /// Returns the 'from' vector rotated towards the 'to' vector by the given absolute angle, in radians. + /** This function slerps the given 'from' vector towards the 'to' vector. + @param from A normalized direction vector specifying the direction of rotation at angleRadians=0. + @param to A normalized direction vector specifying the target direction to rotate towards. + @param angleRadians The maximum angle to rotate the 'from' vector by, in the range [0, pi]. If the + angle between 'from' and 'to' is smaller than this angle, then the vector 'to' is returned. + Input values outside this range are silently clamped to the [0, pi] interval. + @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */ + static Vector3 SlerpVectorAbs(const Vector3 &from, const Vector3& to, float angleRadians); - static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp); + /// Normalizes this quaternion in-place. + /// Returns the old length of this quaternion, or 0 if normalization failed. + float Normalize(); + /// Returns a normalized copy of this quaternion. + [[nodiscard]] Quaternion Normalized() const; + + /// Returns true if the length of this quaternion is one. + [[nodiscard]] bool IsNormalized(float epsilon = 1e-5f) const; + [[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const; + + /// Returns true if the entries of this quaternion are all finite. + [[nodiscard]] bool IsFinite() const; + + /// Returns true if this quaternion equals rhs, up to the given epsilon. + [[nodiscard]] bool Equals(const Quaternion& rhs, float epsilon = 1e-3f) const; + + /// Compares whether this Quaternion and the given Quaternion are identical bit-by-bit in the underlying representation. + /// @note Prefer using this over e.g. memcmp, since there can be SSE-related padding in the structures. + bool BitEquals(const Quaternion& rhs) const; + + /// @return A pointer to the first element (x). The data is contiguous in memory. + /// ptr[0] gives x, ptr[1] gives y, ptr[2] gives z, ptr[3] gives w. + inline float *ptr() { return &x; } + [[nodiscard]] inline const float *ptr() const { return &x; } - // TODO: Document (But do not override!) certain math functions that are the same for Vec4 - // TODO: Double Check which operations need to be overriden for correct behavior! // Multiplies two quaternions together. // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations. @@ -99,14 +225,37 @@ namespace J3ML::LinearAlgebra // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of Quaternion operator / (const Quaternion& rhs) const; Quaternion operator + (const Quaternion& rhs) const; + Quaternion operator / (float scalar) const + { + assert(!Math::EqualAbs(scalar, 0.f)); + } Quaternion operator + () const; Quaternion operator - () const; - float Dot(const Quaternion &quaternion) const; - float Angle() const { return acos(w) * 2.f;} + /// Computes the dot product of this and the given quaternion. + /// Dot product is commutative. + [[nodiscard]] float Dot(const Quaternion &quaternion) const; - float AngleBetween(const Quaternion& target) const; + /// Returns the angle between this and the target orientation (the shortest route) in radians. + [[nodiscard]] float AngleBetween(const Quaternion& target) const; + /// Returns the axis of rotation to get from this orientation to target orientation (the shortest route). + [[nodiscard]] Vector3 AxisFromTo(const Quaternion& target) const; - AxisAngle ToAxisAngle() const; + + [[nodiscard]] AxisAngle ToAxisAngle() const; + void SetFromAxisAngle(const AxisAngle& axisAngle); + + /// Sets this quaternion to represent the same rotation as the given matrix. + void Set(const Matrix3x3& matrix); + void Set(const Matrix4x4& matrix); + void Set(float x, float y, float z, float w); + void Set(const Quaternion& q); + void Set(const Vector4& v); + + public: + float x; + float y; + float z; + float w; }; } \ No newline at end of file diff --git a/src/J3ML/LinearAlgebra/Quaternion.cpp b/src/J3ML/LinearAlgebra/Quaternion.cpp index d28d378..33d5539 100644 --- a/src/J3ML/LinearAlgebra/Quaternion.cpp +++ b/src/J3ML/LinearAlgebra/Quaternion.cpp @@ -5,6 +5,10 @@ #include namespace J3ML::LinearAlgebra { + + const Quaternion Quaternion::Identity = Quaternion(0.f, 0.f, 0.f, 1.f); + const Quaternion Quaternion::NaN = Quaternion(NAN, NAN, NAN, NAN); + Quaternion Quaternion::operator-() const { return {-x, -y, -z, -w}; @@ -14,11 +18,11 @@ namespace J3ML::LinearAlgebra { Quaternion::Quaternion(const Matrix4x4 &rotationMtrx) {} - Vector3 Quaternion::GetWorldX() const { return Transform(1.f, 0.f, 0.f); } + Vector3 Quaternion::WorldX() const { return Transform(1.f, 0.f, 0.f); } - Vector3 Quaternion::GetWorldY() const { return Transform(0.f, 1.f, 0.f); } + Vector3 Quaternion::WorldY() const { return Transform(0.f, 1.f, 0.f); } - Vector3 Quaternion::GetWorldZ() const { return Transform(0.f, 0.f, 1.f); } + Vector3 Quaternion::WorldZ() const { return Transform(0.f, 0.f, 1.f); } Vector3 Quaternion::Transform(const Vector3 &vec) const { Matrix3x3 mat = this->ToMatrix3x3(); @@ -40,9 +44,9 @@ namespace J3ML::LinearAlgebra { Quaternion Quaternion::Lerp(const Quaternion &b, float t) const { float angle = this->Dot(b); if (angle >= 0.f) // Make sure we rotate the shorter arc - return (*this * (1.f - t) + b * t).Normalize(); + return (*this * (1.f - t) + b * t).Normalized(); else - return (*this * (t - 1.f) + b * t).Normalize(); + return (*this * (t - 1.f) + b * t).Normalized(); } void Quaternion::SetFromAxisAngle(const Vector3 &axis, float angle) { @@ -69,7 +73,7 @@ namespace J3ML::LinearAlgebra { Quaternion::Quaternion() {} - Quaternion::Quaternion(float X, float Y, float Z, float W) : Vector4(X,Y,Z,W) {} + Quaternion::Quaternion(float X, float Y, float Z, float W) : x(X), y(Y), z(Z), w(W) {} // TODO: implement float Quaternion::Dot(const Quaternion &rhs) const { @@ -80,25 +84,21 @@ namespace J3ML::LinearAlgebra { } - Quaternion Quaternion::Normalize() const { - float length = Length(); - if (length < 1e-4f) - return {0,0,0,0}; - float reciprocal = 1.f / length; - return { - x * reciprocal, - y * reciprocal, - z * reciprocal, - w * reciprocal - }; + Quaternion Quaternion::Normalized() const { + Quaternion copy = *this; + float success = copy.Normalize(); + assert(success > 0 && "Quaternion::Normalized failed!"); + return copy; } - Quaternion Quaternion::Conjugate() const { + Quaternion Quaternion::Conjugated() const { return { -x, -y, -z, w }; } - Quaternion Quaternion::Inverse() const { - return Conjugate(); + Quaternion Quaternion::Inverted() const { + assert(IsNormalized()); + assert(IsInvertible()); + return Conjugated(); } Quaternion Quaternion::Slerp(const Quaternion &q2, float t) const { @@ -131,7 +131,7 @@ namespace J3ML::LinearAlgebra { b = t; } // Lerp and renormalize. - return (*this * (a * sign) + q2 * b).Normalize(); + return (*this * (a * sign) + q2 * b).Normalized(); } AxisAngle Quaternion::ToAxisAngle() const { @@ -150,7 +150,7 @@ namespace J3ML::LinearAlgebra { float Quaternion::AngleBetween(const Quaternion &target) const { Quaternion delta = target / *this; - return delta.Normalize().Angle(); + return delta.Normalized().Angle(); } Quaternion Quaternion::operator/(const Quaternion &rhs) const { @@ -184,22 +184,22 @@ namespace J3ML::LinearAlgebra { return {*this, translation}; } - float Quaternion::GetAngle() const { + float Quaternion::Angle() const { return std::acos(w) * 2.f; } - Vector3 Quaternion::GetAxis() const { + Vector3 Quaternion::Axis() const { float rcpSinAngle = 1 - (std::sqrt(1 - w * w)); return Vector3(x, y, z) * rcpSinAngle; } - Quaternion::Quaternion(const Vector3 &rotationAxis, float rotationAngleBetween) { - SetFromAxisAngle(rotationAxis, rotationAngleBetween); + Quaternion::Quaternion(const Vector3 &rotationAxis, float rotationAngleRadians) { + SetFromAxisAngle(rotationAxis, rotationAngleRadians); } - Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleBetween) { - SetFromAxisAngle(rotationAxis, rotationAngleBetween); + Quaternion::Quaternion(const Vector4 &rotationAxis, float rotationAngleRadians) { + SetFromAxisAngle(rotationAxis, rotationAngleRadians); } Quaternion::Quaternion(const AxisAngle &angle) { @@ -236,4 +236,151 @@ namespace J3ML::LinearAlgebra { EulerAngle Quaternion::ToEulerAngle() const { return EulerAngle(*this); } + + Quaternion Quaternion::RandomRotation(RNG &rng) { + // Generate a random point on the 4D unitary hypersphere using the rejection sampling method. + for (int i = 0; i < 1000; ++i) + { + float x = rng.Float(-1, 1); + float y = rng.Float(-1, 1); + float z = rng.Float(-1, 1); + float w = rng.Float(-1, 1); + float lenSq = x*x + y*y + z*z + w*w; + if (lenSq >= 1e-6f && lenSq <= 1.f) + return Quaternion(x,y,z,w) / std::sqrt(lenSq); + } + assert(false && "Quaternion::RandomRotation failed!"); + return Quaternion::Identity; + } + + float Quaternion::Normalize() { + float length = Length(); + if (length < 1e-4f) + return 0.f; + float rcpLength = 1.f / length; + x *= rcpLength; + y *= rcpLength; + z *= rcpLength; + w *= rcpLength; + return length; + } + + bool Quaternion::IsNormalized(float epsilon) const { + return Math::EqualAbs(LengthSquared(), 1.f, epsilon); + } + + bool Quaternion::IsInvertible(float epsilon) const { + return LengthSquared() > epsilon && IsFinite(); + } + + bool Quaternion::IsFinite() const { + return std::isfinite(x) && std::isfinite(y) && std::isfinite(z) && std::isfinite(w); + } + + void Quaternion::Conjugate() { + x = -x; + y = -y; + z = -z; + } + + void Quaternion::Inverse() { + assert(IsNormalized()); + assert(IsInvertible()); + Conjugate(); + } + + float Quaternion::InverseAndNormalize() { + Conjugate(); + return Normalize(); + } + + Vector3 Quaternion::SlerpVector(const Vector3 &from, const Vector3 &to, float t) { + if (t <= 0.f) + return from; + if (t >= 1.f) + return to; + // TODO: the following chain can be greatly optimized. + Quaternion q = Quaternion::RotateFromTo(from, to); + q = Slerp(Quaternion::Identity, q, t); + return q.Transform(from); + } + + Quaternion Quaternion::LookAt(const Vector3 &localForward, const Vector3 &targetDirection, const Vector3 &localUp, + const Vector3 &worldUp) { + return Matrix3x3::LookAt(localForward, targetDirection, localUp, worldUp).ToQuat(); + } + + Quaternion Quaternion::RotateX(float angleRadians) { + return {{1,0,0}, angleRadians}; + } + + Quaternion Quaternion::RotateY(float angleRadians) { + return {{0,1,0}, angleRadians}; + } + + Quaternion Quaternion::RotateZ(float angleRadians) { + return {{0,0,1}, angleRadians}; + } + + Quaternion Quaternion::RotateAxisAngle(const AxisAngle &axisAngle) { + return {axisAngle.axis, axisAngle.angle}; + } + + Quaternion Quaternion::RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection) { + assert(sourceDirection.IsNormalized()); + assert(targetDirection.IsNormalized()); + // If sourceDirection == targetDirection, the cross product comes out zero, and normalization would fail. In that case, pick an arbitrary axis. + Vector3 axis = sourceDirection.Cross(targetDirection); + + float oldLength = axis.Normalize(); + + if (oldLength != 0.f) + { + float halfCosAngle = 0.5f * sourceDirection.Dot(targetDirection); + float cosHalfAngle = std::sqrt(0.5f + halfCosAngle); + float sinHalfAngle = std::sqrt(0.5f - halfCosAngle); + + return {axis.x * sinHalfAngle, axis.y * sinHalfAngle, axis.z * sinHalfAngle, cosHalfAngle}; + } else + return {1.f, 0, 0, 0}; + } + + Quaternion Quaternion::RotateFromTo(const Vector4 &sourceDirection, const Vector4 &targetDirection) { + assert(Math::EqualAbs(sourceDirection.w, 0.f)); + assert(Math::EqualAbs(targetDirection.w, 0.f)); + return Quaternion::RotateFromTo(sourceDirection.XYZ(), targetDirection.XYZ()); + } + + Quaternion::Quaternion(const float *data) { + assert(data); + x = data[0]; + y = data[1]; + z = data[2]; + w = data[3]; + } + + Quaternion Quaternion::Lerp(const Quaternion &source, const Quaternion &target, float t) { return source.Lerp(target, t);} + + Quaternion Quaternion::Slerp(const Quaternion &source, const Quaternion &target, float t) { return source.Slerp(target, t);} + + Quaternion Quaternion::RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection, + const Vector3 &sourceDirection2, const Vector3 &targetDirection2) { + return LookAt(sourceDirection, targetDirection, sourceDirection2, targetDirection2); + } + + Vector3 Quaternion::SlerpVectorAbs(const Vector3 &from, const Vector3 &to, float angleRadians) { + if (angleRadians <= 0.f) + return from; + Quaternion q = Quaternion::RotateFromTo(from, to); + float a = q.Angle(); + if (a <= angleRadians) + return to; + float t = angleRadians / a; + q = Slerp(Quaternion::Identity, q, t); + return q.Transform(from); + } + + float Quaternion::LengthSquared() const { return x*x + y*y + z*z + w*w;} + + float Quaternion::Length() const { return std::sqrt(LengthSquared()); } } \ No newline at end of file