// 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); }
public FRotator GroundVUYawPitchRow(string rotData) { if (rotData.Length < 1) { return(new FRotator(0, 0, 0)); } char[] charSeparators = new char[] { ' ' }; var values = rotData.Split(charSeparators, StringSplitOptions.RemoveEmptyEntries); FVector XAxis = new FVector(Convert.ToDouble(values[0]), Convert.ToDouble(values[1]), Convert.ToDouble(values[2])); //XAxis = XAxis.GetSafeNormal(); FVector YAxis = new FVector(-1.0 * Convert.ToDouble(values[3]), -1.0 * Convert.ToDouble(values[4]), -1.0 * Convert.ToDouble(values[5])); // YAxis = YAxis.GetSafeNormal(); FVector ZAxis = new FVector(1.0 * Convert.ToDouble(values[6]), 1.0 * Convert.ToDouble(values[7]), -1.0 * Convert.ToDouble(values[8])); //ZAxis = ZAxis.GetSafeNormal(); //double sy = Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y); //FRotator Rotator = new FRotator(Math.Atan2(-YAxis.Z, YAxis.Y) * 180.0 / PI,Math.Atan2(-ZAxis.X, sy) * 180.0 / PI, 0); FRotator Rotator = new FRotator(Math.Atan2(XAxis.Z, Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y)) * 180.0 / PI, Math.Atan2(XAxis.Y, XAxis.X) * 180.0 / PI, 0); //FRotator Rotator = new FRotator( // Math.Atan2(XAxis.Y, XAxis.X) * 180.0 / PI, Math.Atan2(XAxis.Z, Math.Sqrt(XAxis.X * XAxis.X + XAxis.Y * XAxis.Y)) * 180.0 / PI, 0); // double[] mt = BuildRotationMatrix(Rotator); FVector SYAxis = new FVector(mt[3], mt[4], mt[5]);// FRotationMatrix(Rotator).GetScaledAxis(EAxis::Y); Rotator.Roll = Math.Atan2(ZAxis.DotProduct(SYAxis), YAxis.DotProduct(SYAxis)) * 180.0 / PI; Rotator.Pitch *= -1.0; return(Rotator); }
/// <summary> /// Gets a bounding volume transformed by a matrix. /// </summary> /// <param name="m">The matrix.</param> /// <returns>The transformed volume.</returns> public FBoxSphereBounds TransformBy(FMatrix m) { FBoxSphereBounds result; FVector vecOrigin = Origin; FVector vecExtent = BoxExtent; FVector m0 = m.GetRow(0); FVector m1 = m.GetRow(1); FVector m2 = m.GetRow(2); FVector m3 = m.GetRow(3); FVector newOrigin = FVector.Replicate(vecOrigin, 0) * m0; newOrigin += (FVector.Replicate(vecOrigin, 1) * m1); newOrigin += (FVector.Replicate(vecOrigin, 2) * m2); newOrigin = newOrigin + m3; FVector newExtent = (FVector.Replicate(vecExtent, 0) * m0).GetAbs(); newExtent += (FVector.Replicate(vecExtent, 1) * m1).GetAbs(); newExtent += (FVector.Replicate(vecExtent, 2) * m2).GetAbs(); result.BoxExtent = newExtent; result.Origin = newOrigin; FVector maxRadius = m0 * m0; maxRadius += m1 * m1; maxRadius += m2 * m2; maxRadius = FVector.ComponentMax(FVector.ComponentMax(maxRadius, FVector.Replicate(maxRadius, 1)), FVector.Replicate(maxRadius, 2)); result.SphereRadius = FMath.Sqrt(maxRadius[0]) * SphereRadius; // For non-uniform scaling, computing sphere radius from a box results in a smaller sphere. float boxExtentMagnitude = FMath.Sqrt(FVector.DotProduct(newExtent, newExtent)); result.SphereRadius = FMath.Min(result.SphereRadius, boxExtentMagnitude); result.DiagnosticCheckNaN(); return(result); }
/// <summary> /// Get the swing and twist decomposition for a specified axis (assumes normalised quaternion and twist axis). /// </summary> /// <param name="twistAxis">Axis to use for decomposition</param> /// <param name="swing">swing component quaternion</param> /// <param name="twist">Twist component quaternion</param> public void ToSwingTwist(FVector twistAxis, out FQuat swing, out FQuat twist) { // Vector part projected onto twist axis FVector Projection = FVector.DotProduct(twistAxis, new FVector(X, Y, Z)) * twistAxis; // Twist quaternion twist = new FQuat(Projection.X, Projection.Y, Projection.Z, W); // Singularity close to 180deg if (twist.SizeSquared() == 0.0f) { twist = FQuat.Identity; } else { twist.Normalize(); } // Set swing swing = this * twist.Inverse(); }