Skip to content

Commit

Permalink
Merge pull request #80225 from ettiSurreal/rotate-toward
Browse files Browse the repository at this point in the history 
Add `rotate_toward` and `angle_difference` methods.
  • Loading branch information
@akien-mga
akien-mga committed Oct 2, 2023
2 parents ce236a6 + 3a39de4 commit 7588e3f
Show file tree
Hide file tree
Showing 6 changed files with 158 additions and 12 deletions.
31 changes: 25 additions & 6 deletions core/math/math_funcs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -399,15 +399,20 @@ class Math {
return d;
}

static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
static _ALWAYS_INLINE_ double angle_difference(double p_from, double p_to) {
double difference = fmod(p_to - p_from, Math_TAU);
double distance = fmod(2.0 * difference, Math_TAU) - difference;
return p_from + distance * p_weight;
return fmod(2.0 * difference, Math_TAU) - difference;
}
static _ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) {
static _ALWAYS_INLINE_ float angle_difference(float p_from, float p_to) {
float difference = fmod(p_to - p_from, (float)Math_TAU);
float distance = fmod(2.0f * difference, (float)Math_TAU) - difference;
return p_from + distance * p_weight;
return fmod(2.0f * difference, (float)Math_TAU) - difference;
}

static _ALWAYS_INLINE_ double lerp_angle(double p_from, double p_to, double p_weight) {
return p_from + Math::angle_difference(p_from, p_to) * p_weight;
}
static _ALWAYS_INLINE_ float lerp_angle(float p_from, float p_to, float p_weight) {
return p_from + Math::angle_difference(p_from, p_to) * p_weight;
}

static _ALWAYS_INLINE_ double inverse_lerp(double p_from, double p_to, double p_value) {
Expand Down Expand Up @@ -438,13 +443,27 @@ class Math {
float s = CLAMP((p_s - p_from) / (p_to - p_from), 0.0f, 1.0f);
return s * s * (3.0f - 2.0f * s);
}

static _ALWAYS_INLINE_ double move_toward(double p_from, double p_to, double p_delta) {
return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
}
static _ALWAYS_INLINE_ float move_toward(float p_from, float p_to, float p_delta) {
return abs(p_to - p_from) <= p_delta ? p_to : p_from + SIGN(p_to - p_from) * p_delta;
}

static _ALWAYS_INLINE_ double rotate_toward(double p_from, double p_to, double p_delta) {
double difference = Math::angle_difference(p_from, p_to);
double abs_difference = Math::abs(difference);
// When `p_delta < 0` move no further than to PI radians away from `p_to` (as PI is the max possible angle distance).
return p_from + CLAMP(p_delta, abs_difference - Math_PI, abs_difference) * (difference >= 0.0 ? 1.0 : -1.0);
}
static _ALWAYS_INLINE_ float rotate_toward(float p_from, float p_to, float p_delta) {
float difference = Math::angle_difference(p_from, p_to);
float abs_difference = Math::abs(difference);
// When `p_delta < 0` move no further than to PI radians away from `p_to` (as PI is the max possible angle distance).
return p_from + CLAMP(p_delta, abs_difference - (float)Math_PI, abs_difference) * (difference >= 0.0f ? 1.0f : -1.0f);
}

static _ALWAYS_INLINE_ double linear_to_db(double p_linear) {
return Math::log(p_linear) * 8.6858896380650365530225783783321;
}
Expand Down
10 changes: 10 additions & 0 deletions core/variant/variant_utility.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -451,6 +451,10 @@ double VariantUtilityFunctions::bezier_derivative(double p_start, double p_contr
return Math::bezier_derivative(p_start, p_control_1, p_control_2, p_end, p_t);
}

double VariantUtilityFunctions::angle_difference(double from, double to) {
return Math::angle_difference(from, to);
}

double VariantUtilityFunctions::lerp_angle(double from, double to, double weight) {
return Math::lerp_angle(from, to, weight);
}
Expand All @@ -471,6 +475,10 @@ double VariantUtilityFunctions::move_toward(double from, double to, double delta
return Math::move_toward(from, to, delta);
}

double VariantUtilityFunctions::rotate_toward(double from, double to, double delta) {
return Math::rotate_toward(from, to, delta);
}

double VariantUtilityFunctions::deg_to_rad(double angle_deg) {
return Math::deg_to_rad(angle_deg);
}
Expand Down Expand Up @@ -1653,12 +1661,14 @@ void Variant::_register_variant_utility_functions() {
FUNCBINDR(cubic_interpolate_angle_in_time, sarray("from", "to", "pre", "post", "weight", "to_t", "pre_t", "post_t"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(bezier_interpolate, sarray("start", "control_1", "control_2", "end", "t"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(bezier_derivative, sarray("start", "control_1", "control_2", "end", "t"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(angle_difference, sarray("from", "to"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(lerp_angle, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(inverse_lerp, sarray("from", "to", "weight"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(remap, sarray("value", "istart", "istop", "ostart", "ostop"), Variant::UTILITY_FUNC_TYPE_MATH);

FUNCBINDR(smoothstep, sarray("from", "to", "x"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(move_toward, sarray("from", "to", "delta"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(rotate_toward, sarray("from", "to", "delta"), Variant::UTILITY_FUNC_TYPE_MATH);

FUNCBINDR(deg_to_rad, sarray("deg"), Variant::UTILITY_FUNC_TYPE_MATH);
FUNCBINDR(rad_to_deg, sarray("rad"), Variant::UTILITY_FUNC_TYPE_MATH);
Expand Down
2 changes: 2 additions & 0 deletions core/variant/variant_utility.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,11 +90,13 @@ struct VariantUtilityFunctions {
double to_t, double pre_t, double post_t);
static double bezier_interpolate(double p_start, double p_control_1, double p_control_2, double p_end, double p_t);
static double bezier_derivative(double p_start, double p_control_1, double p_control_2, double p_end, double p_t);
static double angle_difference(double from, double to);
static double lerp_angle(double from, double to, double weight);
static double inverse_lerp(double from, double to, double weight);
static double remap(double value, double istart, double istop, double ostart, double ostop);
static double smoothstep(double from, double to, double val);
static double move_toward(double from, double to, double delta);
static double rotate_toward(double from, double to, double delta);
static double deg_to_rad(double angle_deg);
static double rad_to_deg(double angle_rad);
static double linear_to_db(double linear);
Expand Down
19 changes: 19 additions & 0 deletions doc/classes/@GlobalScope.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,14 @@
[/codeblock]
</description>
</method>
<method name="angle_difference">
<return type="float" />
<param index="0" name="from" type="float" />
<param index="1" name="to" type="float" />
<description>
Returns the difference between the two angles, in the range of [code][-PI, +PI][/code]. When [param from] and [param to] are opposite, returns [code]-PI[/code] if [param from] is smaller than [param to], or [code]PI[/code] otherwise.
</description>
</method>
<method name="asin">
<return type="float" />
<param index="0" name="x" type="float" />
Expand Down Expand Up @@ -1110,6 +1118,17 @@
Creates a RID from a [param base]. This is used mainly from native extensions to build servers.
</description>
</method>
<method name="rotate_toward">
<return type="float" />
<param index="0" name="from" type="float" />
<param index="1" name="to" type="float" />
<param index="2" name="delta" type="float" />
<description>
Rotates [param from] toward [param to] by the [param delta] amount. Will not go past [param to].
Similar to [method move_toward], but interpolates correctly when the angles wrap around [constant @GDScript.TAU].
If [param delta] is negative, this function will rotate away from [param to], toward the opposite angle, and will not go past the opposite angle.
</description>
</method>
<method name="round">
<return type="Variant" />
<param index="0" name="x" type="Variant" />
Expand Down
72 changes: 66 additions & 6 deletions modules/mono/glue/GodotSharp/GodotSharp/Core/Mathf.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,38 @@ public static double Acosh(double s)
return Math.Acosh(s);
}

/// <summary>
/// Returns the difference between the two angles,
/// in range of -<see cref="Pi"/>, <see cref="Pi"/>.
/// When <paramref name="from"/> and <paramref name="to"/> are opposite,
/// returns -<see cref="Pi"/> if <paramref name="from"/> is smaller than <paramref name="to"/>,
/// or <see cref="Pi"/> otherwise.
/// </summary>
/// <param name="from">The start angle.</param>
/// <param name="to">The destination angle.</param>
/// <returns>The difference between the two angles.</returns>
public static float AngleDifference(float from, float to)
{
float difference = (to - from) % MathF.Tau;
return ((2.0f * difference) % MathF.Tau) - difference;
}

/// <summary>
/// Returns the difference between the two angles,
/// in range of -<see cref="Pi"/>, <see cref="Pi"/>.
/// When <paramref name="from"/> and <paramref name="to"/> are opposite,
/// returns -<see cref="Pi"/> if <paramref name="from"/> is smaller than <paramref name="to"/>,
/// or <see cref="Pi"/> otherwise.
/// </summary>
/// <param name="from">The start angle.</param>
/// <param name="to">The destination angle.</param>
/// <returns>The difference between the two angles.</returns>
public static double AngleDifference(double from, double to)
{
double difference = (to - from) % Math.Tau;
return ((2.0 * difference) % Math.Tau) - difference;
}

/// <summary>
/// Returns the arc sine of <paramref name="s"/> in radians.
/// Use to get the angle of sine <paramref name="s"/>.
Expand Down Expand Up @@ -1093,9 +1125,7 @@ public static double Lerp(double from, double to, double weight)
/// <returns>The resulting angle of the interpolation.</returns>
public static float LerpAngle(float from, float to, float weight)
{
float difference = (to - from) % MathF.Tau;
float distance = ((2 * difference) % MathF.Tau) - difference;
return from + (distance * weight);
return from + AngleDifference(from, to) * weight;
}

/// <summary>
Expand All @@ -1110,9 +1140,7 @@ public static float LerpAngle(float from, float to, float weight)
/// <returns>The resulting angle of the interpolation.</returns>
public static double LerpAngle(double from, double to, double weight)
{
double difference = (to - from) % Math.Tau;
double distance = ((2 * difference) % Math.Tau) - difference;
return from + (distance * weight);
return from + AngleDifference(from, to) * weight;
}

/// <summary>
Expand Down Expand Up @@ -1428,6 +1456,38 @@ public static double Remap(double value, double inFrom, double inTo, double outF
return Lerp(outFrom, outTo, InverseLerp(inFrom, inTo, value));
}

/// <summary>
/// Rotates <paramref name="from"/> toward <paramref name="to"/> by the <paramref name="delta"/> amount. Will not go past <paramref name="to"/>.
/// Similar to <see cref="MoveToward(float, float, float)"/> but interpolates correctly when the angles wrap around <see cref="Tau"/>.
/// If <paramref name="delta"/> is negative, this function will rotate away from <paramref name="to"/>, toward the opposite angle, and will not go past the opposite angle.
/// </summary>
/// <param name="from">The start angle.</param>
/// <param name="to">The angle to move towards.</param>
/// <param name="delta">The amount to move by.</param>
/// <returns>The angle after moving.</returns>
public static float RotateToward(float from, float to, float delta)
{
float difference = AngleDifference(from, to);
float absDifference = Math.Abs(difference);
return from + Math.Clamp(delta, absDifference - MathF.PI, absDifference) * (difference >= 0.0f ? 1.0f : -1.0f);
}

/// <summary>
/// Rotates <paramref name="from"/> toward <paramref name="to"/> by the <paramref name="delta"/> amount. Will not go past <paramref name="to"/>.
/// Similar to <see cref="MoveToward(double, double, double)"/> but interpolates correctly when the angles wrap around <see cref="Tau"/>.
/// If <paramref name="delta"/> is negative, this function will rotate away from <paramref name="to"/>, toward the opposite angle, and will not go past the opposite angle.
/// </summary>
/// <param name="from">The start angle.</param>
/// <param name="to">The angle to move towards.</param>
/// <param name="delta">The amount to move by.</param>
/// <returns>The angle after moving.</returns>
public static double RotateToward(double from, double to, double delta)
{
double difference = AngleDifference(from, to);
double absDifference = Math.Abs(difference);
return from + Math.Clamp(delta, absDifference - Math.PI, absDifference) * (difference >= 0.0 ? 1.0 : -1.0);
}

/// <summary>
/// Rounds <paramref name="s"/> to the nearest whole number,
/// with halfway cases rounded towards the nearest multiple of two.
Expand Down
36 changes: 36 additions & 0 deletions tests/core/math/test_math_funcs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -360,6 +360,25 @@ TEST_CASE_TEMPLATE("[Math] remap", T, float, double) {
CHECK(Math::remap((T)-250.0, (T)-100.0, (T)-200.0, (T)0.0, (T)-1000.0) == doctest::Approx((T)-1500.0));
}

TEST_CASE_TEMPLATE("[Math] angle_difference", T, float, double) {
// Loops around, should return 0.0.
CHECK(Math::angle_difference((T)0.0, (T)Math_TAU) == doctest::Approx((T)0.0));
CHECK(Math::angle_difference((T)Math_PI, (T)-Math_PI) == doctest::Approx((T)0.0));
CHECK(Math::angle_difference((T)0.0, (T)Math_TAU * (T)4.0) == doctest::Approx((T)0.0));

// Rotation is clockwise, so it should return -PI.
CHECK(Math::angle_difference((T)0.0, (T)Math_PI) == doctest::Approx((T)-Math_PI));
CHECK(Math::angle_difference((T)0.0, (T)-Math_PI) == doctest::Approx((T)Math_PI));
CHECK(Math::angle_difference((T)Math_PI, (T)0.0) == doctest::Approx((T)Math_PI));
CHECK(Math::angle_difference((T)-Math_PI, (T)0.0) == doctest::Approx((T)-Math_PI));

CHECK(Math::angle_difference((T)0.0, (T)3.0) == doctest::Approx((T)3.0));
CHECK(Math::angle_difference((T)1.0, (T)-2.0) == doctest::Approx((T)-3.0));
CHECK(Math::angle_difference((T)-1.0, (T)2.0) == doctest::Approx((T)3.0));
CHECK(Math::angle_difference((T)-2.0, (T)-4.5) == doctest::Approx((T)-2.5));
CHECK(Math::angle_difference((T)100.0, (T)102.5) == doctest::Approx((T)2.5));
}

TEST_CASE_TEMPLATE("[Math] lerp_angle", T, float, double) {
// Counter-clockwise rotation.
CHECK(Math::lerp_angle((T)0.24 * Math_TAU, 0.75 * Math_TAU, 0.5) == doctest::Approx((T)-0.005 * Math_TAU));
Expand Down Expand Up @@ -390,6 +409,23 @@ TEST_CASE_TEMPLATE("[Math] move_toward", T, float, double) {
CHECK(Math::move_toward(-2.0, -5.0, 4.0) == doctest::Approx((T)-5.0));
}

TEST_CASE_TEMPLATE("[Math] rotate_toward", T, float, double) {
// Rotate toward.
CHECK(Math::rotate_toward((T)0.0, (T)Math_PI * (T)0.75, (T)1.5) == doctest::Approx((T)1.5));
CHECK(Math::rotate_toward((T)-2.0, (T)1.0, (T)2.5) == doctest::Approx((T)0.5));
CHECK(Math::rotate_toward((T)-2.0, (T)Math_PI, (T)Math_PI) == doctest::Approx((T)-Math_PI));
CHECK(Math::rotate_toward((T)1.0, (T)Math_PI, (T)20.0) == doctest::Approx((T)Math_PI));

// Rotate away.
CHECK(Math::rotate_toward((T)0.0, (T)0.0, (T)-1.5) == doctest::Approx((T)-1.5));
CHECK(Math::rotate_toward((T)0.0, (T)0.0, (T)-Math_PI) == doctest::Approx((T)-Math_PI));
CHECK(Math::rotate_toward((T)3.0, (T)Math_PI, (T)-Math_PI) == doctest::Approx((T)0.0));
CHECK(Math::rotate_toward((T)2.0, (T)Math_PI, (T)-1.5) == doctest::Approx((T)0.5));
CHECK(Math::rotate_toward((T)1.0, (T)2.0, (T)-0.5) == doctest::Approx((T)0.5));
CHECK(Math::rotate_toward((T)2.5, (T)2.0, (T)-0.5) == doctest::Approx((T)3.0));
CHECK(Math::rotate_toward((T)-1.0, (T)1.0, (T)-1.0) == doctest::Approx((T)-2.0));
}

TEST_CASE_TEMPLATE("[Math] smoothstep", T, float, double) {
CHECK(Math::smoothstep((T)0.0, (T)2.0, (T)-5.0) == doctest::Approx((T)0.0));
CHECK(Math::smoothstep((T)0.0, (T)2.0, (T)0.5) == doctest::Approx((T)0.15625));
Expand Down

0 comments on commit 7588e3f

Please sign in to comment.