// Based on: // http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm private static FQuat FindBetween_Helper(FVector a, FVector b, float normAB) { float w = normAB + FVector.DotProduct(a, b); FQuat result; if (w >= 1e-6f * normAB) { //Axis = FVector::CrossProduct(A, B); result = new FQuat( a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X, w); } else { // A and B point in opposite directions w = 0.0f; result = FMath.Abs(a.X) > FMath.Abs(a.Y) ? new FQuat(-a.Z, 0.0f, a.X, w) : new FQuat(0.0f, -a.Z, a.Y, w); } result.Normalize(); return(result); }
/// <summary> /// Check against another vector for equality, within specified error limits. /// </summary> /// <param name="v">The vector to check against.</param> /// <param name="tolerance">Error tolerance. (default to FMath.KindaSmallNumber)</param> /// <returns>true if the vectors are equal within tolerance limits, false otherwise.</returns> public bool Equals(FVector2D v, float tolerance) { // Change this function name so that we can make use of the default param? return (FMath.Abs(X - v.X) <= tolerance && FMath.Abs(Y - v.Y) <= tolerance); }
/// <summary> /// Checks whether two rotators are equal within specified tolerance, when treated as an orientation. /// This means that FRotator(0, 0, 360).Equals(FRotator(0,0,0)) is true, because they represent the same final orientation. /// </summary> /// <param name="other">The other rotator.</param> /// <param name="tolerance">Error Tolerance.</param> /// <returns>true if two rotators are equal, within specified tolerance, otherwise false.</returns> public bool Equals(FRotator other, float tolerance = FMath.KindaSmallNumber) { return ((FMath.Abs(NormalizeAxis(Pitch - other.Pitch)) <= tolerance) && (FMath.Abs(NormalizeAxis(Yaw - other.Yaw)) <= tolerance) && (FMath.Abs(NormalizeAxis(Roll - other.Roll)) <= tolerance)); }
/// <summary> /// @note Exp should really only be used after Log. /// Assumes a quaternion with W=0 and V=theta*v (where |v| = 1). /// Exp(q) = (sin(theta)*v, cos(theta)) /// </summary> public FQuat Exp() { float angle = FMath.Sqrt(X * X + Y * Y + Z * Z); float sinAngle = FMath.Sin(angle); FQuat result; result.W = FMath.Cos(angle); if (FMath.Abs(sinAngle) >= FMath.SmallNumber) { float scale = sinAngle / angle; result.X = scale * X; result.Y = scale * Y; result.Z = scale * Z; } else { result.X = X; result.Y = Y; result.Z = Z; } return(result); }
/// <summary> /// Checks whether rotator is nearly zero within specified tolerance, when treated as an orientation. /// This means that FRotator(0, 0, 360) is "zero", because it is the same final orientation as the zero rotator. /// </summary> /// <param name="tolerance">Error Tolerance.</param> /// <returns>true if rotator is nearly zero, within specified tolerance, otherwise false.</returns> public bool IsNearlyZero(float tolerance = FMath.KindaSmallNumber) { return (FMath.Abs(NormalizeAxis(Pitch)) <= tolerance && FMath.Abs(NormalizeAxis(Yaw)) <= tolerance && FMath.Abs(NormalizeAxis(Roll)) <= tolerance); }
/// <summary> /// Returns quaternion with W=0 and V=theta*v. /// </summary> public FQuat Log() { FQuat result; result.W = 0.0f; if (FMath.Abs(W) < 1.0f) { float angle = FMath.Acos(W); float sinAngle = FMath.Sin(angle); if (FMath.Abs(sinAngle) >= FMath.SmallNumber) { float scale = angle / sinAngle; result.X = scale * X; result.Y = scale * Y; result.Z = scale * Z; return(result); } } result.X = X; result.Y = Y; result.Z = Z; return(result); }
/// <summary> /// Error tolerant comparison. /// </summary> /// <param name="v">Vector to compare against.</param> /// <param name="tolerance">Error Tolerance.</param> /// <returns>true if the two vectors are equal within specified tolerance, otherwise false.</returns> public bool Equals(FVector4 v, float tolerance = FMath.KindaSmallNumber) { return (FMath.Abs(X - v.X) <= tolerance && FMath.Abs(Y - v.Y) <= tolerance && FMath.Abs(Z - v.Z) <= tolerance && FMath.Abs(W - v.W) <= tolerance); }
/// <summary> /// Checks whether two planes are equal within specified tolerance. /// </summary> /// <param name="other">The other plane.</param> /// <param name="tolerance">Error Tolerance.</param> /// <returns>true if the two planes are equal within specified tolerance, otherwise false.</returns> public bool Equals(FPlane other, float tolerance = FMath.KindaSmallNumber) { return ((FMath.Abs(X - other.X) < tolerance) && (FMath.Abs(Y - other.Y) < tolerance) && (FMath.Abs(Z - other.Z) < tolerance) && (FMath.Abs(W - other.W) < tolerance)); }
/// <summary> /// Find good arbitrary axis vectors to represent U and V axes of a plane, /// given just the normal. /// </summary> public void FindBestAxisVectors3(FVector4 axis1, FVector4 axis2) { float nx = FMath.Abs(X); float ny = FMath.Abs(Y); float nz = FMath.Abs(Z); // Find best basis vectors. if (nz > nx && nz > ny) { axis1 = new FVector4(1, 0, 0); } else { axis1 = new FVector4(0, 0, 1); } axis1 = (axis1 - this * Dot3(axis1, this)).GetSafeNormal(); axis2 = axis1 ^ this; }
/// <summary> /// Simpler Slerp that doesn't do any checks for 'shortest distance' etc. /// We need this for the cubic interpolation stuff so that the multiple Slerps dont go in different directions. /// Result is NOT normalized. /// </summary> public static FQuat SlerpFullPath_NotNormalized(FQuat quat1, FQuat quat2, float alpha) { float cosAngle = FMath.Clamp(quat1 | quat2, -1.0f, 1.0f); float angle = FMath.Acos(cosAngle); //UE_LOG(LogUnrealMath, Log, TEXT("CosAngle: %f Angle: %f"), CosAngle, Angle ); if (FMath.Abs(angle) < FMath.KindaSmallNumber) { return(quat1); } float sinAngle = FMath.Sin(angle); float invSinAngle = 1.0f / sinAngle; float scale0 = FMath.Sin((1.0f - alpha) * angle) * invSinAngle; float scale1 = FMath.Sin(alpha * angle) * invSinAngle; return(quat1 * scale0 + quat2 * scale1); }
// Special-case interpolation /// <summary> /// Interpolate a normal vector Current to Target, by interpolating the angle between those vectors with constant step. /// </summary> public static FVector VInterpNormalRotationTo(FVector current, FVector target, float deltaTime, float rotationSpeedDegrees) { // Find delta rotation between both normals. FQuat deltaQuat = FQuat.FindBetween(current, target); // Decompose into an axis and angle for rotation FVector deltaAxis; float deltaAngle; deltaQuat.ToAxisAndAngle(out deltaAxis, out deltaAngle); // Find rotation step for this frame float rotationStepRadians = rotationSpeedDegrees * (PI / 180) * deltaTime; if (FMath.Abs(deltaAngle) > rotationStepRadians) { deltaAngle = FMath.Clamp(deltaAngle, -rotationStepRadians, rotationStepRadians); deltaQuat = new FQuat(deltaAxis, deltaAngle); return(deltaQuat.RotateVector(current)); } return(target); }
/// <summary> /// Checks whether another Quaternion is equal to this, within specified tolerance. /// </summary> /// <param name="other">The other Quaternion.</param> /// <param name="tolerance">Error tolerance for comparison with other Quaternion.</param> /// <returns>true if two Quaternions are equal, within specified tolerance, otherwise false.</returns> public bool Equals(FQuat other, float tolerance = FMath.KindaSmallNumber) { return((FMath.Abs(X - other.X) <= tolerance && FMath.Abs(Y - other.Y) <= tolerance && FMath.Abs(Z - other.Z) <= tolerance && FMath.Abs(W - other.W) <= tolerance) || (FMath.Abs(X + other.X) <= tolerance && FMath.Abs(Y + other.Y) <= tolerance && FMath.Abs(Z + other.Z) <= tolerance && FMath.Abs(W + other.W) <= tolerance)); }
/// <summary> /// Creates and initializes a new quaternion from the given matrix. /// </summary> /// <param name="m">The rotation matrix to initialize from.</param> public FQuat(FMatrix m) { // If Matrix is NULL, return Identity quaternion. If any of them is 0, you won't be able to construct rotation // if you have two plane at least, we can reconstruct the frame using cross product, but that's a bit expensive op to do here // for now, if you convert to matrix from 0 scale and convert back, you'll lose rotation. Don't do that. if (m.GetScaledAxis(EAxis.X).IsNearlyZero() || m.GetScaledAxis(EAxis.Y).IsNearlyZero() || m.GetScaledAxis(EAxis.Z).IsNearlyZero()) { this = FQuat.Identity; return; } //#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) // Make sure the Rotation part of the Matrix is unit length. // Changed to this (same as RemoveScaling) from RotDeterminant as using two different ways of checking unit length matrix caused inconsistency. if (!FMessage.Ensure( (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.X).SizeSquared()) <= FMath.KindaSmallNumber) && (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Y).SizeSquared()) <= FMath.KindaSmallNumber) && (FMath.Abs(1.0f - m.GetScaledAxis(EAxis.Z).SizeSquared()) <= FMath.KindaSmallNumber) , "Make sure the Rotation part of the Matrix is unit length.")) { this = FQuat.Identity; return; } //#endif //const MeReal *const t = (MeReal *) tm; float s; // Check diagonal (trace) float tr = m[0, 0] + m[1, 1] + m[2, 2]; if (tr > 0.0f) { float invS = FMath.InvSqrt(tr + 1.0f); this.W = 0.5f * (1.0f / invS); s = 0.5f * invS; this.X = (m[1, 2] - m[2, 1]) * s; this.Y = (m[2, 0] - m[0, 2]) * s; this.Z = (m[0, 1] - m[1, 0]) * s; } else { // diagonal is negative int i = 0; if (m[1, 1] > m[0, 0]) { i = 1; } if (m[2, 2] > m[i, i]) { i = 2; } int[] nxt = { 1, 2, 0 }; int j = nxt[i]; int k = nxt[j]; s = m[i, i] - m[j, j] - m[k, k] + 1.0f; float InvS = FMath.InvSqrt(s); float[] qt = new float[4]; qt[i] = 0.5f * (1.0f / InvS); s = 0.5f * InvS; qt[3] = (m[j, k] - m[k, j]) * s; qt[j] = (m[i, j] + m[j, i]) * s; qt[k] = (m[i, k] + m[k, i]) * s; this.X = qt[0]; this.Y = qt[1]; this.Z = qt[2]; this.W = qt[3]; DiagnosticCheckNaN(); } }
/// <summary> /// Error measure (angle) between two quaternions, ranged [0..1]. /// Returns the hypersphere-angle between two quaternions; alignment shouldn't matter, though /// @note normalized input is expected. /// </summary> public static float Error(FQuat q1, FQuat q2) { float cosom = FMath.Abs(q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W); return((FMath.Abs(cosom) < 0.9999999f) ? FMath.Acos(cosom) * (1.0f / FMath.PI) : 0.0f); }
/// <summary> /// Return the manhattan distance in degrees between this Rotator and the passed in one. /// </summary> /// <param name="rotator">the Rotator we are comparing with.</param> /// <returns>Distance(Manhattan) between the two rotators. </returns> public float GetManhattanDistance(FRotator rotator) { return(FMath.Abs(Yaw - rotator.Yaw) + FMath.Abs(Pitch - rotator.Pitch) + FMath.Abs(Roll - rotator.Roll)); }
/// <summary> /// Check whether two spheres are the same within specified tolerance. /// </summary> /// <param name="other">The other sphere.</param> /// <param name="tolerance">Error Tolerance.</param> /// <returns>true if spheres are equal within specified tolerance, otherwise false.</returns> public bool Equals(FSphere other, float tolerance = FMath.KindaSmallNumber) { return(Center.Equals(other.Center, tolerance) && FMath.Abs(W - other.W) <= tolerance); }
/// <summary> /// Return true if this quaternion is normalized /// </summary> public bool IsNormalized() { return(FMath.Abs(1.0f - SizeSquared()) < FMath.THRESH_QUAT_NORMALIZED); }
/// <summary> /// Get the maximum absolute value of the vector's components. /// </summary> /// <returns>The maximum absolute value of the vector's components.</returns> public float GetAbsMax() { return(FMath.Max(FMath.Abs(X), FMath.Abs(Y))); }
/// <summary> /// Utility to check if all of the components of this vector are nearly zero given the tolerance. /// </summary> public bool IsNearlyZero3(float tolerance = FMath.KindaSmallNumber) { return(FMath.Abs(X) <= tolerance && FMath.Abs(Y) <= tolerance && FMath.Abs(Z) <= tolerance); }
/// <summary> /// Check if the vector is of unit length, with specified tolerance. /// </summary> /// <param name="lengthSquaredTolerance">Tolerance against squared length.</param> /// <returns>true if the vector is a unit vector within the specified tolerance.</returns> public bool IsUnit3(float lengthSquaredTolerance = FMath.KindaSmallNumber) { return(FMath.Abs(1.0f - SizeSquared3()) < lengthSquaredTolerance); }
/// <summary> /// Get a copy of this vector with absolute value of each component. /// </summary> /// <returns>A copy of this vector with absolute value of each component.</returns> public FVector2D GetAbs() { return(new FVector2D(FMath.Abs(X), FMath.Abs(Y))); }