diff --git a/docs/modules/linalg.md b/docs/modules/linalg.md index 9ded67a0..13635769 100644 --- a/docs/modules/linalg.md +++ b/docs/modules/linalg.md @@ -27,7 +27,6 @@ class vec2(_StructLike['vec2']): def length_squared(self) -> float: ... def normalize(self) -> vec2: ... def rotate(self, radians: float) -> vec2: ... - def rotate_(self, radians: float) -> None: ... @staticmethod def angle(__from: vec2, __to: vec2) -> float: @@ -39,6 +38,10 @@ class vec2(_StructLike['vec2']): + if y axis is bottom to top, positive value means counter-clockwise """ + @staticmethod + def smooth_damp(current: vec2, target: vec2, current_velocity: vec2, smooth_time: float, max_speed: float, delta_time: float) -> vec2: + ... + class vec3(_StructLike['vec3']): x: float y: float diff --git a/include/typings/linalg.pyi b/include/typings/linalg.pyi index 0694e342..c391b686 100644 --- a/include/typings/linalg.pyi +++ b/include/typings/linalg.pyi @@ -17,7 +17,6 @@ class vec2(_StructLike['vec2']): def length_squared(self) -> float: ... def normalize(self) -> vec2: ... def rotate(self, radians: float) -> vec2: ... - def rotate_(self, radians: float) -> None: ... @staticmethod def angle(__from: vec2, __to: vec2) -> float: @@ -29,6 +28,10 @@ class vec2(_StructLike['vec2']): + if y axis is bottom to top, positive value means counter-clockwise """ + @staticmethod + def smooth_damp(current: vec2, target: vec2, current_velocity: vec2, smooth_time: float, max_speed: float, delta_time: float) -> vec2: + ... + class vec3(_StructLike['vec3']): x: float y: float diff --git a/src/linalg.cpp b/src/linalg.cpp index 068d4ffa..74d9bb0d 100644 --- a/src/linalg.cpp +++ b/src/linalg.cpp @@ -41,6 +41,37 @@ namespace pkpy{ }); +// https://github.com/Unity-Technologies/UnityCsReference/blob/master/Runtime/Export/Math/Mathf.cs#L309 +static float SmoothDamp(float current, float target, float& currentVelocity, float smoothTime, float maxSpeed, float deltaTime) +{ + // Based on Game Programming Gems 4 Chapter 1.10 + smoothTime = std::max(0.0001F, smoothTime); + float omega = 2.0F / smoothTime; + + float x = omega * deltaTime; + float exp = 1.0F / (1.0F + x + 0.48F * x * x + 0.235F * x * x * x); + float change = current - target; + float originalTo = target; + + // Clamp maximum speed + float maxChange = maxSpeed * smoothTime; + change = std::clamp(change, -maxChange, maxChange); + target = current - change; + + float temp = (currentVelocity + omega * change) * deltaTime; + currentVelocity = (currentVelocity - omega * temp) * exp; + float output = target + (change + temp) * exp; + + // Prevent overshooting + if (originalTo - current > 0.0F == output > originalTo) + { + output = originalTo; + currentVelocity = (output - originalTo) / deltaTime; + } + + return output; +} + void PyVec2::_register(VM* vm, PyObject* mod, PyObject* type){ PY_STRUCT_LIKE(PyVec2) @@ -55,6 +86,20 @@ namespace pkpy{ return VAR(Tuple({ VAR(self.x), VAR(self.y) })); }); + // @staticmethod + vm->bind(type, "smooth_damp(current: vec2, target: vec2, current_velocity: vec2, smooth_time: float, max_speed: float, delta_time: float) -> vec2", [](VM* vm, ArgsView args){ + PyVec2 current = CAST(PyVec2, args[0]); + PyVec2 target = CAST(PyVec2, args[1]); + PyVec2& current_velocity = CAST(PyVec2&, args[2]); + float smooth_time = CAST_F(args[3]); + float max_speed = CAST_F(args[4]); + float delta_time = CAST_F(args[5]); + float x = SmoothDamp(current.x, target.x, current_velocity.x, smooth_time, max_speed, delta_time); + float y = SmoothDamp(current.y, target.y, current_velocity.y, smooth_time, max_speed, delta_time); + return VAR(Vec2(x, y)); + }); + + // @staticmethod vm->bind(type, "angle(__from: vec2, __to: vec2) -> float", [](VM* vm, ArgsView args){ PyVec2 __from = CAST(PyVec2, args[0]); PyVec2 __to = CAST(PyVec2, args[1]); @@ -85,18 +130,6 @@ namespace pkpy{ return VAR(self); }); - vm->bind_method<1>(type, "rotate_", [](VM* vm, ArgsView args){ - Vec2& self = _CAST(PyVec2&, args[0]); - float radian = CAST(f64, args[1]); - float cr = cosf(radian); - float sr = sinf(radian); - Mat3x3 rotate(cr, -sr, 0.0f, - sr, cr, 0.0f, - 0.0f, 0.0f, 1.0f); - self = rotate.transform_vector(self); - return vm->None; - }); - BIND_VEC_VEC_OP(2, __add__, +) BIND_VEC_VEC_OP(2, __sub__, -) BIND_VEC_FLOAT_OP(2, __mul__, *) diff --git a/tests/80_linalg.py b/tests/80_linalg.py index 9e7c50b9..b9781aab 100644 --- a/tests/80_linalg.py +++ b/tests/80_linalg.py @@ -40,12 +40,9 @@ radians = random.uniform(-10*math.pi, 10*math.pi) test_vec2_copy = rotated_vec2(test_vec2_copy, radians) assert test_vec2.rotate(radians).__dict__ == test_vec2_copy.__dict__ -# test rotate_ -a = test_vec2.rotate(0.7) -assert a is not test_vec2 -b = test_vec2.rotate_(0.7) -assert b is None -assert a == test_vec2 +# test smooth_damp +ret = vec2.smooth_damp(vec2(1, 2), vec2(3, 4), vec2(5, 6), 7, 8, 9) +assert isinstance(ret, vec2) # test vec3-------------------------------------------------------------------- # 生成随机测试目标