/// <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> /// 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); }
/// <summary> /// Returns a random unit vector, uniformly distributed, within the specified cone. /// </summary> /// <param name="dir">The center direction of the cone</param> /// <param name="horizontalConeHalfAngleRad">Horizontal half-angle of cone, in radians.</param> /// <param name="verticalConeHalfAngleRad">Vertical half-angle of cone, in radians.</param> /// <returns>Normalized vector within the specified cone.</returns> public FVector VRandCone(FVector dir, float horizontalConeHalfAngleRad, float verticalConeHalfAngleRad) { if ((verticalConeHalfAngleRad > 0.0f) && (horizontalConeHalfAngleRad > 0.0f)) { float randU = FRand(); float randV = FRand(); // Get spherical coords that have an even distribution over the unit sphere // Method described at http://mathworld.wolfram.com/SpherePointPicking.html float theta = 2.0f * FMath.PI * randU; float phi = FMath.Acos((2.0f * randV) - 1.0f); // restrict phi to [0, ConeHalfAngleRad] // where ConeHalfAngleRad is now a function of Theta // (specifically, radius of an ellipse as a function of angle) // function is ellipse function (x/a)^2 + (y/b)^2 = 1, converted to polar coords float coneHalfAngleRad = FMath.Square(FMath.Cos(theta) / verticalConeHalfAngleRad) + FMath.Square(FMath.Sin(theta) / horizontalConeHalfAngleRad); coneHalfAngleRad = FMath.Sqrt(1.0f / coneHalfAngleRad); // clamp to make a cone instead of a sphere phi = FMath.Fmod(phi, coneHalfAngleRad); // get axes we need to rotate around FMatrix dirMat = FMatrix.CreateRotation(dir.Rotation()); // note the axis translation, since we want the variation to be around X FVector dirZ = dirMat.GetUnitAxis(EAxis.X); FVector dirY = dirMat.GetUnitAxis(EAxis.Y); FVector result = dir.RotateAngleAxis(phi * 180.0f / FMath.PI, dirY); result = result.RotateAngleAxis(theta * 180.0f / FMath.PI, dirZ); // ensure it's a unit vector (might not have been passed in that way) result = result.GetSafeNormal(); return(result); } else { return(dir.GetSafeNormal()); } }
/// <summary> /// Spherical interpolation. Will correct alignment. Result is NOT normalized. /// </summary> public static FQuat Slerp_NotNormalized(FQuat quat1, FQuat quat2, float slerp) { // Get cosine of angle between quats. float rawCosom = quat1.X * quat2.X + quat1.Y * quat2.Y + quat1.Z * quat2.Z + quat1.W * quat2.W; // Unaligned quats - compensate, results in taking shorter route. float cosom = FMath.FloatSelect(rawCosom, rawCosom, -rawCosom); float scale0, scale1; if (cosom < 0.9999f) { float omega = FMath.Acos(cosom); float invSin = 1.0f / FMath.Sin(omega); scale0 = FMath.Sin((1.0f - slerp) * omega) * invSin; scale1 = FMath.Sin(slerp * omega) * invSin; } else { // Use linear interpolation. scale0 = 1.0f - slerp; scale1 = slerp; } // In keeping with our flipped Cosom: scale1 = FMath.FloatSelect(rawCosom, scale1, -scale1); FQuat result; result.X = scale0 * quat1.X + scale1 * quat2.X; result.Y = scale0 * quat1.Y + scale1 * quat2.Y; result.Z = scale0 * quat1.Z + scale1 * quat2.Z; result.W = scale0 * quat1.W + scale1 * quat2.W; return(result); }
/// <summary> /// Returns a random unit vector, uniformly distributed, within the specified cone. /// </summary> /// <param name="dir">The center direction of the cone</param> /// <param name="coneHalfAngleRad">Half-angle of cone, in radians.</param> /// <returns>Normalized vector within the specified cone.</returns> public FVector VRandCone(FVector dir, float coneHalfAngleRad) { if (coneHalfAngleRad > 0.0f) { float randU = FRand(); float randV = FRand(); // Get spherical coords that have an even distribution over the unit sphere // Method described at http://mathworld.wolfram.com/SpherePointPicking.html float theta = 2.0f * FMath.PI * randU; float phi = FMath.Acos((2.0f * randV) - 1.0f); // restrict phi to [0, ConeHalfAngleRad] // this gives an even distribution of points on the surface of the cone // centered at the origin, pointing upward (z), with the desired angle phi = FMath.Fmod(phi, coneHalfAngleRad); // get axes we need to rotate around FMatrix dirMat = FMatrix.CreateRotation(dir.Rotation()); // note the axis translation, since we want the variation to be around X FVector dirZ = dirMat.GetUnitAxis(EAxis.X); FVector dirY = dirMat.GetUnitAxis(EAxis.Y); FVector result = dir.RotateAngleAxis(phi * 180.0f / FMath.PI, dirY); result = result.RotateAngleAxis(theta * 180.0f / FMath.PI, dirZ); // ensure it's a unit vector (might not have been passed in that way) result = result.GetSafeNormal(); return(result); } else { return(dir.GetSafeNormal()); } }
/// <summary> /// Find the angular distance between two rotation quaternions (in radians) /// </summary> public float AngularDistance(FQuat q) { float InnerProd = X * q.X + Y * q.Y + Z * q.Z + W * q.W; return(FMath.Acos((2 * InnerProd * InnerProd) - 1.0f)); }
/// <summary> /// Get the angle of this quaternion /// </summary> public float GetAngle() { return(2.0f * FMath.Acos(W)); }
/// <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); }