From 291ee682b7e16f1e86ad4dd2a8a93526f34b78fd Mon Sep 17 00:00:00 2001 From: blueloveTH Date: Sun, 25 Aug 2024 01:40:31 +0800 Subject: [PATCH] add `linalg` module --- include/pocketpy/interpreter/modules.h | 3 +- include/pocketpy/{common => }/linalg.h | 3 +- include/pocketpy/objects/base.h | 3 +- include/pocketpy/objects/error.h | 2 - include/pocketpy/pocketpy.h | 21 + include/typings/linalg.pyi | 171 ++--- src/interpreter/ceval.c | 13 +- src/interpreter/heap.c | 5 +- src/interpreter/vm.c | 2 + src/modules/linalg.c | 860 +++++++++++++++++++++++++ src/public/cast.c | 8 + src/public/py_exception.c | 15 - tests/80_linalg.py | 197 ++---- 13 files changed, 1016 insertions(+), 287 deletions(-) rename include/pocketpy/{common => }/linalg.h (95%) create mode 100644 src/modules/linalg.c diff --git a/include/pocketpy/interpreter/modules.h b/include/pocketpy/interpreter/modules.h index f7588be2..a6b1e6ce 100644 --- a/include/pocketpy/interpreter/modules.h +++ b/include/pocketpy/interpreter/modules.h @@ -11,4 +11,5 @@ void pk__add_module_gc(); void pk__add_module_time(); void pk__add_module_easing(); void pk__add_module_traceback(); -void pk__add_module_enum(); \ No newline at end of file +void pk__add_module_enum(); +void pk__add_module_linalg(); \ No newline at end of file diff --git a/include/pocketpy/common/linalg.h b/include/pocketpy/linalg.h similarity index 95% rename from include/pocketpy/common/linalg.h rename to include/pocketpy/linalg.h index e8fabf9f..b25b39fa 100644 --- a/include/pocketpy/common/linalg.h +++ b/include/pocketpy/linalg.h @@ -29,7 +29,8 @@ typedef struct c11_mat3x3 { float _21, _22, _23; float _31, _32, _33; }; + float m[3][3]; - float v[9]; + float data[9]; }; } c11_mat3x3; diff --git a/include/pocketpy/objects/base.h b/include/pocketpy/objects/base.h index 28508a2c..740ae22f 100644 --- a/include/pocketpy/objects/base.h +++ b/include/pocketpy/objects/base.h @@ -22,7 +22,8 @@ typedef struct py_TValue { bool _bool; py_CFunction _cfunc; PyObject* _obj; - // Vec2 + c11_vec2 _vec2; + c11_vec2i _vec2i; }; } py_TValue; diff --git a/include/pocketpy/objects/error.h b/include/pocketpy/objects/error.h index ef3e0638..4ca63d35 100644 --- a/include/pocketpy/objects/error.h +++ b/include/pocketpy/objects/error.h @@ -13,6 +13,4 @@ typedef struct{ char msg[100]; } Error; -void py_BaseException__set_lineno(py_Ref, int lineno, const CodeObject* code); -int py_BaseException__get_lineno(py_Ref, const CodeObject* code); void py_BaseException__stpush(py_Ref, SourceData_ src, int lineno, const char* func_name); diff --git a/include/pocketpy/pocketpy.h b/include/pocketpy/pocketpy.h index 6990330f..37457567 100644 --- a/include/pocketpy/pocketpy.h +++ b/include/pocketpy/pocketpy.h @@ -7,6 +7,7 @@ #include "pocketpy/config.h" #include "pocketpy/export.h" +#include "pocketpy/linalg.h" #ifdef __cplusplus extern "C" { @@ -229,6 +230,8 @@ PK_EXPORT py_f64 py_tofloat(py_Ref); /// If successful, return true and set the value to `out`. /// Otherwise, return false and raise `TypeError`. PK_EXPORT bool py_castfloat(py_Ref, py_f64* out) PY_RAISE; +/// Cast a `int` object in python to `int64_t`. +PK_EXPORT bool py_castint(py_Ref, py_i64* out) PY_RAISE; /// Convert a `bool` object in python to `bool`. PK_EXPORT bool py_tobool(py_Ref); /// Convert a `type` object in python to `py_Type`. @@ -597,6 +600,18 @@ PK_EXPORT bool /// noexcept PK_EXPORT int py_dict_len(py_Ref self); +/************* linalg module *************/ +void py_newvec2(py_OutRef out, c11_vec2); +void py_newvec3(py_OutRef out, c11_vec3); +void py_newvec2i(py_OutRef out, c11_vec2i); +void py_newvec3i(py_OutRef out, c11_vec3i); +c11_mat3x3* py_newmat3x3(py_OutRef out); +c11_vec2 py_tovec2(py_Ref self); +c11_vec3 py_tovec3(py_Ref self); +c11_vec2i py_tovec2i(py_Ref self); +c11_vec3i py_tovec3i(py_Ref self); +c11_mat3x3* py_tomat3x3(py_Ref self); + /************* Others *************/ /// An utility function to read a line from stdin for REPL. @@ -678,6 +693,12 @@ enum py_PredefinedTypes { tp_ImportError, tp_AssertionError, tp_KeyError, + /* Extended */ + tp_vec2, + tp_vec3, + tp_vec2i, + tp_vec3i, + tp_mat3x3, }; #ifdef __cplusplus diff --git a/include/typings/linalg.pyi b/include/typings/linalg.pyi index 6be3e036..7d3a3221 100644 --- a/include/typings/linalg.pyi +++ b/include/typings/linalg.pyi @@ -1,25 +1,28 @@ from typing import overload -from c import _StructLike, float_p class vec2: - x: float - y: float + ZERO = vec2(0, 0) + ONE = vec2(1, 1) - ZERO: 'vec2' = ... - ONE: 'vec2' = ... + @property + def x(self) -> float: ... + @property + def y(self) -> float: ... + + def with_x(self, x: float) -> vec2: ... + def with_y(self, y: float) -> vec2: ... def __init__(self, x: float, y: float) -> None: ... + def __repr__(self) -> str: ... + def __add__(self, other: vec2) -> vec2: ... def __sub__(self, other: vec2) -> vec2: ... - def __getitem__(self, index: int) -> float: ... - @overload def __mul__(self, other: float) -> vec2: ... @overload def __mul__(self, other: vec2) -> vec2: ... - - def __rmul__(self, other: float) -> vec2: ... def __truediv__(self, other: float) -> vec2: ... + def dot(self, other: vec2) -> float: ... def cross(self, other: vec2) -> float: ... def length(self) -> float: ... @@ -44,108 +47,33 @@ class vec2: Returns a new value that is closer to the target and current velocity. """ -class vec3: - x: float - y: float - z: float - ZERO: 'vec3' = ... - ONE: 'vec3' = ... - - def __init__(self, x: float, y: float, z: float) -> None: ... - def __add__(self, other: vec3) -> vec3: ... - def __sub__(self, other: vec3) -> vec3: ... - def __getitem__(self, index: int) -> float: ... - - @overload - def __mul__(self, other: float) -> vec3: ... - @overload - def __mul__(self, other: vec3) -> vec3: ... - - def __rmul__(self, other: float) -> vec3: ... - def __truediv__(self, other: float) -> vec3: ... - def dot(self, other: vec3) -> float: ... - def cross(self, other: vec3) -> float: ... - def length(self) -> float: ... - def length_squared(self) -> float: ... - def normalize(self) -> vec3: ... - -class vec4(_StructLike['vec4']): - x: float - y: float - z: float - w: float - - ZERO: 'vec4' = ... - ONE: 'vec4' = ... - - def __init__(self, x: float, y: float, z: float, w: float) -> None: ... - def __add__(self, other: vec4) -> vec4: ... - def __sub__(self, other: vec4) -> vec4: ... - def __getitem__(self, index: int) -> float: ... - - @overload - def __mul__(self, other: float) -> vec4: ... - @overload - def __mul__(self, other: vec4) -> vec4: ... - - def __rmul__(self, other: float) -> vec4: ... - def __truediv__(self, other: float) -> vec4: ... - def dot(self, other: vec4) -> float: ... - def length(self) -> float: ... - def length_squared(self) -> float: ... - def normalize(self) -> vec4: ... - - def copy_(self, other: vec4) -> None: ... - def normalize_(self) -> None: ... - -class mat3x3(_StructLike['mat3x3']): - _11: float - _12: float - _13: float - _21: float - _22: float - _23: float - _31: float - _32: float - _33: float - - @overload - def __init__(self) -> None: ... - @overload +class mat3x3: def __init__(self, _11, _12, _13, _21, _22, _23, _31, _32, _33) -> None: ... - @overload - def __init__(self, a: list[float]): ... - - def determinant(self) -> float: ... - def inverse(self) -> mat3x3: ... - def transpose(self) -> mat3x3: ... + def __repr__(self) -> str: ... def __getitem__(self, index: tuple[int, int]) -> float: ... def __setitem__(self, index: tuple[int, int], value: float) -> None: ... - def __add__(self, other: mat3x3) -> mat3x3: ... - def __sub__(self, other: mat3x3) -> mat3x3: ... - def __mul__(self, other: float) -> mat3x3: ... - def __rmul__(self, other: float) -> mat3x3: ... - def __truediv__(self, other: float) -> mat3x3: ... - def __invert__(self) -> mat3x3: ... @overload def __matmul__(self, other: mat3x3) -> mat3x3: ... @overload def __matmul__(self, other: vec3) -> vec3: ... + def __invert__(self) -> mat3x3: ... + def matmul(self, other: mat3x3, out: mat3x3 = None) -> mat3x3 | None: ... + def determinant(self) -> float: ... + + def copy(self) -> mat3x3: ... + def inverse(self) -> mat3x3: ... def copy_(self, other: mat3x3) -> None: ... def inverse_(self) -> None: ... - def transpose_(self) -> None: ... @staticmethod def zeros() -> mat3x3: ... @staticmethod - def ones() -> mat3x3: ... - @staticmethod def identity() -> mat3x3: ... # affine transformations @@ -153,20 +81,55 @@ class mat3x3(_StructLike['mat3x3']): def trs(t: vec2, r: float, s: vec2) -> mat3x3: ... def copy_trs_(self, t: vec2, r: float, s: vec2) -> None: ... - def copy_t_(self, t: vec2) -> None: ... - def copy_r_(self, r: float) -> None: ... - def copy_s_(self, s: vec2) -> None: ... - def _t(self) -> vec2: ... - def _r(self) -> float: ... - def _s(self) -> vec2: ... - - def is_affine(self) -> bool: ... + def t(self) -> vec2: ... + def r(self) -> float: ... + def s(self) -> vec2: ... def transform_point(self, p: vec2) -> vec2: ... def transform_vector(self, v: vec2) -> vec2: ... - def inverse_transform_point(self, p: vec2) -> vec2: ... - def inverse_transform_vector(self, v: vec2) -> vec2: ... -vec4_p = float_p -mat3x3_p = float_p + +class vec2i: + @property + def x(self) -> int: ... + @property + def y(self) -> int: ... + + def with_x(self, x: int) -> vec2i: ... + def with_y(self, y: int) -> vec2i: ... + + def __init__(self, x: int, y: int) -> None: ... + def __repr__(self) -> str: ... + + +class vec3i: + @property + def x(self) -> int: ... + @property + def y(self) -> int: ... + @property + def z(self) -> int: ... + + def with_x(self, x: int) -> vec3i: ... + def with_y(self, y: int) -> vec3i: ... + def with_z(self, z: int) -> vec3i: ... + + def __init__(self, x: int, y: int, z: int) -> None: ... + def __repr__(self) -> str: ... + + +class vec3: + @property + def x(self) -> float: ... + @property + def y(self) -> float: ... + @property + def z(self) -> float: ... + + def with_x(self, x: float) -> vec3: ... + def with_y(self, y: float) -> vec3: ... + def with_z(self, z: float) -> vec3: ... + + def __init__(self, x: float, y: float, z: float) -> None: ... + def __repr__(self) -> str: ... diff --git a/src/interpreter/ceval.c b/src/interpreter/ceval.c index a73dae16..69f81c83 100644 --- a/src/interpreter/ceval.c +++ b/src/interpreter/ceval.c @@ -1011,18 +1011,13 @@ FrameResult VM__run_top_frame(VM* self) { c11__unreachedable(); __ERROR: - pk_print_stack(self, frame, (Bytecode){0}); - py_BaseException__set_lineno(&self->curr_exception, Frame__lineno(frame), frame->co); + py_BaseException__stpush(&self->curr_exception, + frame->co->src, + Frame__lineno(frame), + frame->has_function ? frame->co->name->data : NULL); __ERROR_RE_RAISE: do { } while(0); - // printf("error.op: %s, line: %d\n", pk_opname(byte.op), Frame__lineno(frame)); - int lineno = py_BaseException__get_lineno(&self->curr_exception, frame->co); - py_BaseException__stpush(&self->curr_exception, - frame->co->src, - lineno < 0 ? Frame__lineno(frame) : lineno, - frame->has_function ? frame->co->name->data : NULL); - int target = Frame__prepare_jump_exception_handler(frame, &self->stack); if(target >= 0) { // 1. Exception can be handled inside the current frame diff --git a/src/interpreter/heap.c b/src/interpreter/heap.c index 9840ac37..54afe05a 100644 --- a/src/interpreter/heap.c +++ b/src/interpreter/heap.c @@ -103,11 +103,10 @@ PyObject* PyObject__new(py_Type type, int slots, int size) { self->slots = slots; // initialize slots or dict - void* p = (char*)self + 8; if(slots >= 0) { - memset(p, 0, slots * sizeof(py_TValue)); + memset(self->flex, 0, slots * sizeof(py_TValue)); } else { - NameDict__ctor(p); + NameDict__ctor((void*)self->flex); } return self; } diff --git a/src/interpreter/vm.c b/src/interpreter/vm.c index 7ae275cb..29dde235 100644 --- a/src/interpreter/vm.c +++ b/src/interpreter/vm.c @@ -196,6 +196,8 @@ void VM__ctor(VM* self) { py_newnotimplemented(py_emplacedict(&self->builtins, py_name("NotImplemented"))); + pk__add_module_linalg(); + // add modules pk__add_module_pkpy(); pk__add_module_os(); diff --git a/src/modules/linalg.c b/src/modules/linalg.c new file mode 100644 index 00000000..b252f35e --- /dev/null +++ b/src/modules/linalg.c @@ -0,0 +1,860 @@ +#include "pocketpy/pocketpy.h" + +#include "pocketpy/common/utils.h" +#include "pocketpy/objects/object.h" +#include "pocketpy/common/sstream.h" +#include "pocketpy/interpreter/vm.h" +#include + +static bool isclose(float a, float b) { return fabs(a - b) < 1e-4; } + +#define DEFINE_VEC_FIELD(name, T, Tc, field) \ + static bool name##__##field(int argc, py_Ref argv) { \ + PY_CHECK_ARGC(1); \ + py_new##T(py_retval(), py_to##name(argv).field); \ + return true; \ + } \ + static bool name##__with_##field(int argc, py_Ref argv) { \ + PY_CHECK_ARGC(2); \ + Tc val; \ + if(!py_cast##T(&argv[1], &val)) return false; \ + c11_##name v = py_to##name(argv); \ + v.field = val; \ + py_new##name(py_retval(), v); \ + return true; \ + } + +#define DEFINE_BOOL_NE(name, f_eq) \ + static bool name##__ne__(int argc, py_Ref argv) { \ + f_eq(argc, argv); \ + py_Ref ret = py_retval(); \ + if(ret->type == tp_NotImplementedType) return true; \ + ret->_bool = !ret->_bool; \ + return true; \ + } + +void py_newvec2(py_OutRef out, c11_vec2 v) { + out->type = tp_vec2; + out->is_ptr = false; + out->_vec2 = v; +} + +c11_vec2 py_tovec2(py_Ref self) { + assert(self->type == tp_vec2); + return self->_vec2; +} + +void py_newvec2i(py_OutRef out, c11_vec2i v) { + out->type = tp_vec2i; + out->is_ptr = false; + out->_vec2i = v; +} + +c11_vec2i py_tovec2i(py_Ref self) { + assert(self->type == tp_vec2i); + return self->_vec2i; +} + +void py_newvec3(py_OutRef out, c11_vec3 v) { + out->type = tp_vec3; + out->is_ptr = false; + c11_vec3* data = (c11_vec3*)(&out->extra); + *data = v; +} + +c11_vec3 py_tovec3(py_Ref self) { + assert(self->type == tp_vec3); + return *(c11_vec3*)(&self->extra); +} + +void py_newvec3i(py_OutRef out, c11_vec3i v) { + out->type = tp_vec3i; + out->is_ptr = false; + c11_vec3i* data = (c11_vec3i*)(&out->extra); + *data = v; +} + +c11_vec3i py_tovec3i(py_Ref self) { + assert(self->type == tp_vec3i); + return *(c11_vec3i*)(&self->extra); +} + +c11_mat3x3* py_newmat3x3(py_OutRef out) { + return py_newobject(out, tp_mat3x3, 0, sizeof(c11_mat3x3)); +} + +c11_mat3x3* py_tomat3x3(py_Ref self) { + assert(self->type == tp_mat3x3); + return py_touserdata(self); +} + +static bool vec2__new__(int argc, py_Ref argv) { + PY_CHECK_ARGC(3); + py_f64 x, y; + if(!py_castfloat(&argv[1], &x) || !py_castfloat(&argv[2], &y)) return false; + py_newvec2(py_retval(), (c11_vec2){x, y}); + return true; +} + +static bool vec2__add__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec2) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec2 res; + res.x = argv[0]._vec2.x + argv[1]._vec2.x; + res.y = argv[0]._vec2.y + argv[1]._vec2.y; + py_newvec2(py_retval(), res); + return true; +} + +static bool vec2__sub__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec2) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec2 res; + res.x = argv[0]._vec2.x - argv[1]._vec2.x; + res.y = argv[0]._vec2.y - argv[1]._vec2.y; + py_newvec2(py_retval(), res); + return true; +} + +static bool vec2__mul__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + c11_vec2 res; + switch(argv[1].type) { + case tp_vec2: + res.x = argv[0]._vec2.x * argv[1]._vec2.x; + res.y = argv[0]._vec2.y * argv[1]._vec2.y; + py_newvec2(py_retval(), res); + return true; + case tp_int: + res.x = argv[0]._vec2.x * argv[1]._i64; + res.y = argv[0]._vec2.y * argv[1]._i64; + py_newvec2(py_retval(), res); + return true; + case tp_float: + res.x = argv[0]._vec2.x * argv[1]._f64; + res.y = argv[0]._vec2.y * argv[1]._f64; + py_newvec2(py_retval(), res); + return true; + default: py_newnotimplemented(py_retval()); return true; + } +} + +static bool vec2__truediv__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_float) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec2 res; + res.x = argv[0]._vec2.x / argv[1]._f64; + res.y = argv[0]._vec2.y / argv[1]._f64; + py_newvec2(py_retval(), res); + return true; +} + +static bool vec2__repr__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + char buf[64]; + int size = snprintf(buf, 64, "vec2(%.4f, %.4f)", argv[0]._vec2.x, argv[0]._vec2.y); + py_newstrn(py_retval(), buf, size); + return true; +} + +static bool vec2__eq__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec2) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec2 lhs = argv[0]._vec2; + c11_vec2 rhs = argv[1]._vec2; + py_newbool(py_retval(), isclose(lhs.x, rhs.x) && isclose(lhs.y, rhs.y)); + return true; +} + +DEFINE_BOOL_NE(vec2, vec2__eq__) + +static bool vec2_dot(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_vec2); + float x = argv[0]._vec2.x * argv[1]._vec2.x; + float y = argv[0]._vec2.y * argv[1]._vec2.y; + py_newfloat(py_retval(), x + y); + return true; +} + +static bool vec2_cross(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_vec2); + float x = argv[0]._vec2.x * argv[1]._vec2.y; + float y = argv[0]._vec2.y * argv[1]._vec2.x; + py_newfloat(py_retval(), x - y); + return true; +} + +static bool vec2_length(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + float x = argv[0]._vec2.x; + float y = argv[0]._vec2.y; + py_newfloat(py_retval(), sqrtf(x * x + y * y)); + return true; +} + +static bool vec2_length_squared(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + float x = argv[0]._vec2.x; + float y = argv[0]._vec2.y; + py_newfloat(py_retval(), x * x + y * y); + return true; +} + +static bool vec2_normalize(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + float x = argv[0]._vec2.x; + float y = argv[0]._vec2.y; + float len = sqrtf(x * x + y * y); + if(isclose(len, 0)) return ZeroDivisionError("cannot normalize zero vector"); + py_newvec2(py_retval(), (c11_vec2){x / len, y / len}); + return true; +} + +static bool vec2_rotate(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + py_f64 radians; + if(!py_castfloat(&argv[1], &radians)) return false; + float cr = cosf(radians); + float sr = sinf(radians); + c11_vec2 res; + res.x = argv[0]._vec2.x * cr - argv[0]._vec2.y * sr; + res.y = argv[0]._vec2.x * sr + argv[0]._vec2.y * cr; + py_newvec2(py_retval(), res); + return true; +} + +static bool vec2_angle_STATIC(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(0, tp_vec2); + PY_CHECK_ARG_TYPE(1, tp_vec2); + float val = atan2f(argv[1]._vec2.y, argv[1]._vec2.x) - atan2f(argv[0]._vec2.y, argv[0]._vec2.x); + const float PI = 3.1415926535897932384f; + if(val > PI) val -= 2 * PI; + if(val < -PI) val += 2 * PI; + py_newfloat(py_retval(), val); + return true; +} + +static bool vec2_smoothdamp_STATIC(int argc, py_Ref argv) { + PY_CHECK_ARGC(6); + PY_CHECK_ARG_TYPE(0, tp_vec2); // current: vec2 + PY_CHECK_ARG_TYPE(1, tp_vec2); // target: vec2 + PY_CHECK_ARG_TYPE(2, tp_vec2); // current_velocity: vec2 + PY_CHECK_ARG_TYPE(3, tp_float); // smooth_time: float + PY_CHECK_ARG_TYPE(4, tp_float); // max_speed: float + PY_CHECK_ARG_TYPE(5, tp_float); // delta_time: float + c11_vec2 current = argv[0]._vec2; + c11_vec2 target = argv[1]._vec2; + c11_vec2 currentVelocity = argv[2]._vec2; + float smoothTime = argv[3]._f64; + float maxSpeed = argv[4]._f64; + float deltaTime = argv[5]._f64; + + // https://github.com/Unity-Technologies/UnityCsReference/blob/master/Runtime/Export/Math/Vector2.cs#L289 + // Based on Game Programming Gems 4 Chapter 1.10 + smoothTime = c11__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_x = current.x - target.x; + float change_y = current.y - target.y; + c11_vec2 originalTo = target; + + // Clamp maximum speed + float maxChange = maxSpeed * smoothTime; + + float maxChangeSq = maxChange * maxChange; + float sqDist = change_x * change_x + change_y * change_y; + if(sqDist > maxChangeSq) { + float mag = sqrtf(sqDist); + change_x = change_x / mag * maxChange; + change_y = change_y / mag * maxChange; + } + + target.x = current.x - change_x; + target.y = current.y - change_y; + + float temp_x = (currentVelocity.x + omega * change_x) * deltaTime; + float temp_y = (currentVelocity.y + omega * change_y) * deltaTime; + + currentVelocity.x = (currentVelocity.x - omega * temp_x) * exp; + currentVelocity.y = (currentVelocity.y - omega * temp_y) * exp; + + float output_x = target.x + (change_x + temp_x) * exp; + float output_y = target.y + (change_y + temp_y) * exp; + + // Prevent overshooting + float origMinusCurrent_x = originalTo.x - current.x; + float origMinusCurrent_y = originalTo.y - current.y; + float outMinusOrig_x = output_x - originalTo.x; + float outMinusOrig_y = output_y - originalTo.y; + + if(origMinusCurrent_x * outMinusOrig_x + origMinusCurrent_y * outMinusOrig_y > 0) { + output_x = originalTo.x; + output_y = originalTo.y; + + currentVelocity.x = (output_x - originalTo.x) / deltaTime; + currentVelocity.y = (output_y - originalTo.y) / deltaTime; + } + + py_Ref ret = py_retval(); + py_newtuple(ret, 2); + py_newvec2(py_tuple_getitem(ret, 0), (c11_vec2){output_x, output_y}); + py_newvec2(py_tuple_getitem(ret, 1), currentVelocity); + return true; +} + +DEFINE_VEC_FIELD(vec2, float, py_f64, x) +DEFINE_VEC_FIELD(vec2, float, py_f64, y) + +/* mat3x3 */ +static bool mat3x3__new__(int argc, py_Ref argv) { + PY_CHECK_ARGC(10); + c11_mat3x3* m = py_newmat3x3(py_retval()); + for(int i = 0; i < 9; i++) { + py_f64 val; + if(!py_castfloat(&argv[i + 1], &val)) return false; + m->data[i] = val; + } + return true; +} + +static bool mat3x3__repr__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* m = py_tomat3x3(argv); + char buf[256]; + const char* fmt = + "mat3x3(%.4f, %.4f, %.4f,\n %.4f, %.4f, %.4f,\n %.4f, %.4f, %.4f)"; + int size = snprintf(buf, + 256, + fmt, + m->data[0], + m->data[1], + m->data[2], + m->data[3], + m->data[4], + m->data[5], + m->data[6], + m->data[7], + m->data[8]); + py_newstrn(py_retval(), buf, size); + return true; +} + +static bool mat3x3__getitem__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_tuple); + c11_mat3x3* ud = py_tomat3x3(argv); + if(py_tuple_len(&argv[1]) != 2) return IndexError("expected a tuple of length 2"); + py_Ref i = py_tuple_getitem(&argv[1], 0); + py_Ref j = py_tuple_getitem(&argv[1], 1); + if(!py_checktype(i, tp_int) || !py_checktype(j, tp_int)) return false; + if(i->_i64 < 0 || i->_i64 >= 3 || j->_i64 < 0 || j->_i64 >= 3) { + return IndexError("index out of range"); + } + py_newfloat(py_retval(), ud->m[i->_i64][j->_i64]); + return true; +} + +static bool mat3x3__setitem__(int argc, py_Ref argv) { + PY_CHECK_ARGC(3); + PY_CHECK_ARG_TYPE(1, tp_tuple); + c11_mat3x3* ud = py_tomat3x3(argv); + if(py_tuple_len(&argv[1]) != 2) return IndexError("expected a tuple of length 2"); + py_Ref i = py_tuple_getitem(&argv[1], 0); + py_Ref j = py_tuple_getitem(&argv[1], 1); + if(!py_checktype(i, tp_int) || !py_checktype(j, tp_int)) return false; + py_f64 val; + if(!py_castfloat(&argv[2], &val)) return false; + if(i->_i64 < 0 || i->_i64 >= 3 || j->_i64 < 0 || j->_i64 >= 3) { + return IndexError("index out of range"); + } + ud->m[i->_i64][j->_i64] = val; + py_newnone(py_retval()); + return true; +} + +static bool mat3x3__eq__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_mat3x3) { + py_newnotimplemented(py_retval()); + return true; + } + c11_mat3x3* lhs = py_tomat3x3(argv); + c11_mat3x3* rhs = py_tomat3x3(&argv[1]); + for(int i = 0; i < 9; i++) { + if(!isclose(lhs->data[i], rhs->data[i])) { + py_newbool(py_retval(), false); + return true; + } + } + py_newbool(py_retval(), true); + return true; +} + +DEFINE_BOOL_NE(mat3x3, mat3x3__eq__) + +static void matmul(const c11_mat3x3* lhs, const c11_mat3x3* rhs, c11_mat3x3* out) { + out->_11 = lhs->_11 * rhs->_11 + lhs->_12 * rhs->_21 + lhs->_13 * rhs->_31; + out->_12 = lhs->_11 * rhs->_12 + lhs->_12 * rhs->_22 + lhs->_13 * rhs->_32; + out->_13 = lhs->_11 * rhs->_13 + lhs->_12 * rhs->_23 + lhs->_13 * rhs->_33; + out->_21 = lhs->_21 * rhs->_11 + lhs->_22 * rhs->_21 + lhs->_23 * rhs->_31; + out->_22 = lhs->_21 * rhs->_12 + lhs->_22 * rhs->_22 + lhs->_23 * rhs->_32; + out->_23 = lhs->_21 * rhs->_13 + lhs->_22 * rhs->_23 + lhs->_23 * rhs->_33; + out->_31 = lhs->_31 * rhs->_11 + lhs->_32 * rhs->_21 + lhs->_33 * rhs->_31; + out->_32 = lhs->_31 * rhs->_12 + lhs->_32 * rhs->_22 + lhs->_33 * rhs->_32; + out->_33 = lhs->_31 * rhs->_13 + lhs->_32 * rhs->_23 + lhs->_33 * rhs->_33; +} + +static float determinant(const c11_mat3x3* m) { + return m->_11 * (m->_22 * m->_33 - m->_23 * m->_32) - + m->_12 * (m->_21 * m->_33 - m->_23 * m->_31) + + m->_13 * (m->_21 * m->_32 - m->_22 * m->_31); +} + +static bool inverse(const c11_mat3x3* m, c11_mat3x3* out) { + float det = determinant(m); + if(isclose(det, 0)) return false; + float invdet = 1.0f / det; + out->_11 = (m->_22 * m->_33 - m->_23 * m->_32) * invdet; + out->_12 = (m->_13 * m->_32 - m->_12 * m->_33) * invdet; + out->_13 = (m->_12 * m->_23 - m->_13 * m->_22) * invdet; + out->_21 = (m->_23 * m->_31 - m->_21 * m->_33) * invdet; + out->_22 = (m->_11 * m->_33 - m->_13 * m->_31) * invdet; + out->_23 = (m->_13 * m->_21 - m->_11 * m->_23) * invdet; + out->_31 = (m->_21 * m->_32 - m->_22 * m->_31) * invdet; + out->_32 = (m->_12 * m->_31 - m->_11 * m->_32) * invdet; + out->_33 = (m->_11 * m->_22 - m->_12 * m->_21) * invdet; + return true; +} + +static void trs(c11_vec2 t, float r, c11_vec2 s, c11_mat3x3* out) { + float cr = cosf(r); + float sr = sinf(r); + // clang-format off + *out = (c11_mat3x3){ + ._11 = s.x * cr, ._12 = -s.y * sr, ._13 = t.x, + ._21 = s.x * sr, ._22 = s.y * cr, ._23 = t.y, + ._31 = 0, ._32 = 0, ._33 = 1, + }; + // clang-format on +} + +static bool mat3x3__matmul__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + c11_mat3x3* lhs = py_tomat3x3(argv); + if(argv[1].type == tp_mat3x3) { + c11_mat3x3* rhs = py_tomat3x3(&argv[1]); + c11_mat3x3* out = py_newmat3x3(py_retval()); + matmul(lhs, rhs, out); + } else if(argv[1].type == tp_vec3) { + c11_vec3 rhs = py_tovec3(&argv[1]); + c11_vec3 res; + res.x = lhs->_11 * rhs.x + lhs->_12 * rhs.y + lhs->_13 * rhs.z; + res.y = lhs->_21 * rhs.x + lhs->_22 * rhs.y + lhs->_23 * rhs.z; + res.z = lhs->_31 * rhs.x + lhs->_32 * rhs.y + lhs->_33 * rhs.z; + py_newvec3(py_retval(), res); + } else { + py_newnotimplemented(py_retval()); + } + return true; +} + +static bool mat3x3__invert__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_mat3x3* out = py_newmat3x3(py_retval()); + if(inverse(ud, out)) return true; + return ZeroDivisionError("matrix is not invertible"); +} + +static bool mat3x3_matmul(int argc, py_Ref argv) { + PY_CHECK_ARGC(3); + PY_CHECK_ARG_TYPE(0, tp_mat3x3); + PY_CHECK_ARG_TYPE(1, tp_mat3x3); + PY_CHECK_ARG_TYPE(2, tp_mat3x3); + c11_mat3x3* lhs = py_tomat3x3(&argv[0]); + c11_mat3x3* rhs = py_tomat3x3(&argv[1]); + c11_mat3x3* out = py_tomat3x3(&argv[2]); + matmul(lhs, rhs, out); + py_newnone(py_retval()); + return true; +} + +static bool mat3x3_determinant(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + py_newfloat(py_retval(), determinant(ud)); + return true; +} + +static bool mat3x3_copy(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_mat3x3* out = py_newmat3x3(py_retval()); + *out = *ud; + return true; +} + +static bool mat3x3_inverse(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_mat3x3* out = py_newmat3x3(py_retval()); + if(inverse(ud, out)) return true; + return ZeroDivisionError("matrix is not invertible"); +} + +static bool mat3x3_copy_(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_mat3x3); + c11_mat3x3* self = py_tomat3x3(argv); + c11_mat3x3* other = py_tomat3x3(&argv[1]); + *self = *other; + py_newnone(py_retval()); + return true; +} + +static bool mat3x3_inverse_(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_mat3x3 res; + if(inverse(ud, &res)) { + *ud = res; + py_newnone(py_retval()); + return true; + } + return ZeroDivisionError("matrix is not invertible"); +} + +static bool mat3x3_zeros_STATIC(int argc, py_Ref argv) { + PY_CHECK_ARGC(0); + c11_mat3x3* out = py_newmat3x3(py_retval()); + memset(out, 0, sizeof(c11_mat3x3)); + return true; +} + +static bool mat3x3_identity_STATIC(int argc, py_Ref argv) { + PY_CHECK_ARGC(0); + c11_mat3x3* out = py_newmat3x3(py_retval()); + // clang-format off + *out = (c11_mat3x3){ + ._11 = 1, ._12 = 0, ._13 = 0, + ._21 = 0, ._22 = 1, ._23 = 0, + ._31 = 0, ._32 = 0, ._33 = 1, + }; + // clang-format on + return true; +} + +static bool mat3x3_trs_STATIC(int argc, py_Ref argv) { + PY_CHECK_ARGC(3); + py_f64 r; + if(!py_checktype(&argv[0], tp_vec2)) return false; + if(!py_castfloat(&argv[1], &r)) return false; + if(!py_checktype(&argv[2], tp_vec2)) return false; + c11_vec2 t = py_tovec2(&argv[0]); + c11_vec2 s = py_tovec2(&argv[2]); + c11_mat3x3* out = py_newmat3x3(py_retval()); + trs(t, r, s, out); + return true; +} + +static bool mat3x3_copy_trs_(int argc, py_Ref argv) { + PY_CHECK_ARGC(4); + c11_mat3x3* ud = py_tomat3x3(&argv[0]); + py_f64 r; + if(!py_checktype(&argv[1], tp_vec2)) return false; + if(!py_castfloat(&argv[2], &r)) return false; + if(!py_checktype(&argv[3], tp_vec2)) return false; + c11_vec2 t = py_tovec2(&argv[1]); + c11_vec2 s = py_tovec2(&argv[3]); + trs(t, r, s, ud); + py_newnone(py_retval()); + return true; +} + +static bool mat3x3_t(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_vec2 res; + res.x = ud->_13; + res.y = ud->_23; + py_newvec2(py_retval(), res); + return true; +} + +static bool mat3x3_r(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + float r = atan2f(ud->_21, ud->_11); + py_newfloat(py_retval(), r); + return true; +} + +static bool mat3x3_s(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_mat3x3* ud = py_tomat3x3(argv); + c11_vec2 res; + res.x = sqrtf(ud->_11 * ud->_11 + ud->_21 * ud->_21); + res.y = sqrtf(ud->_12 * ud->_12 + ud->_22 * ud->_22); + py_newvec2(py_retval(), res); + return true; +} + +static bool mat3x3_transform_point(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_vec2); + c11_mat3x3* ud = py_tomat3x3(&argv[0]); + c11_vec2 p = py_tovec2(&argv[1]); + c11_vec2 res; + res.x = ud->_11 * p.x + ud->_12 * p.y + ud->_13; + res.y = ud->_21 * p.x + ud->_22 * p.y + ud->_23; + py_newvec2(py_retval(), res); + return true; +} + +static bool mat3x3_transform_vector(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + PY_CHECK_ARG_TYPE(1, tp_vec2); + c11_mat3x3* ud = py_tomat3x3(&argv[0]); + c11_vec2 p = py_tovec2(&argv[1]); + c11_vec2 res; + res.x = ud->_11 * p.x + ud->_12 * p.y; + res.y = ud->_21 * p.x + ud->_22 * p.y; + py_newvec2(py_retval(), res); + return true; +} + +/* vec2i */ +static bool vec2i__new__(int argc, py_Ref argv) { + PY_CHECK_ARGC(3); + PY_CHECK_ARG_TYPE(1, tp_int); + PY_CHECK_ARG_TYPE(2, tp_int); + py_newvec2i(py_retval(), (c11_vec2i){argv[1]._i64, argv[2]._i64}); + return true; +} + +DEFINE_VEC_FIELD(vec2i, int, py_i64, x) +DEFINE_VEC_FIELD(vec2i, int, py_i64, y) + +static bool vec2i__repr__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_vec2i data = py_tovec2i(argv); + char buf[64]; + int size = snprintf(buf, 64, "vec2i(%d, %d)", data.x, data.y); + py_newstrn(py_retval(), buf, size); + return true; +} + +static bool vec2i__eq__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec2i) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec2i lhs = py_tovec2i(argv); + c11_vec2i rhs = py_tovec2i(&argv[1]); + py_newbool(py_retval(), lhs.x == rhs.x && lhs.y == rhs.y); + return true; +} + +DEFINE_BOOL_NE(vec2i, vec2i__eq__) + +/* vec3i */ +static bool vec3i__new__(int argc, py_Ref argv) { + PY_CHECK_ARGC(4); + PY_CHECK_ARG_TYPE(1, tp_int); + PY_CHECK_ARG_TYPE(2, tp_int); + PY_CHECK_ARG_TYPE(3, tp_int); + py_newvec3i(py_retval(), (c11_vec3i){argv[1]._i64, argv[2]._i64, argv[3]._i64}); + return true; +} + +static bool vec3i__repr__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_vec3i data = py_tovec3i(argv); + char buf[64]; + int size = snprintf(buf, 64, "vec3i(%d, %d, %d)", data.x, data.y, data.z); + py_newstrn(py_retval(), buf, size); + return true; +} + +static bool vec3i__eq__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec3i) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec3i lhs = py_tovec3i(argv); + c11_vec3i rhs = py_tovec3i(&argv[1]); + py_newbool(py_retval(), lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z); + return true; +} + +DEFINE_BOOL_NE(vec3i, vec3i__eq__) + +DEFINE_VEC_FIELD(vec3i, int, py_i64, x) +DEFINE_VEC_FIELD(vec3i, int, py_i64, y) +DEFINE_VEC_FIELD(vec3i, int, py_i64, z) + +/* vec3 */ +static bool vec3__new__(int argc, py_Ref argv) { + PY_CHECK_ARGC(4); + py_f64 x, y, z; + if(!py_castfloat(&argv[1], &x) || !py_castfloat(&argv[2], &y) || !py_castfloat(&argv[3], &z)) + return false; + py_newvec3(py_retval(), (c11_vec3){x, y, z}); + return true; +} + +static bool vec3__repr__(int argc, py_Ref argv) { + PY_CHECK_ARGC(1); + c11_vec3 data = py_tovec3(argv); + char buf[64]; + int size = snprintf(buf, 64, "vec3(%.4f, %.4f, %.4f)", data.x, data.y, data.z); + py_newstrn(py_retval(), buf, size); + return true; +} + +static bool vec3__eq__(int argc, py_Ref argv) { + PY_CHECK_ARGC(2); + if(argv[1].type != tp_vec3) { + py_newnotimplemented(py_retval()); + return true; + } + c11_vec3 lhs = py_tovec3(argv); + c11_vec3 rhs = py_tovec3(&argv[1]); + py_newbool(py_retval(), + isclose(lhs.x, rhs.x) && isclose(lhs.y, rhs.y) && isclose(lhs.z, rhs.z)); + return true; +} + +DEFINE_BOOL_NE(vec3, vec3__eq__) + +DEFINE_VEC_FIELD(vec3, float, py_f64, x) +DEFINE_VEC_FIELD(vec3, float, py_f64, y) +DEFINE_VEC_FIELD(vec3, float, py_f64, z) + +void pk__add_module_linalg() { + py_Ref mod = py_newmodule("linalg"); + + py_Type vec2 = pk_newtype("vec2", tp_object, mod, NULL, false, true); + py_Type vec3 = pk_newtype("vec3", tp_object, mod, NULL, false, true); + py_Type vec2i = pk_newtype("vec2i", tp_object, mod, NULL, false, true); + py_Type vec3i = pk_newtype("vec3i", tp_object, mod, NULL, false, true); + py_Type mat3x3 = pk_newtype("mat3x3", tp_object, mod, NULL, false, true); + + py_setdict(mod, py_name("vec2"), py_tpobject(vec2)); + py_setdict(mod, py_name("vec3"), py_tpobject(vec3)); + py_setdict(mod, py_name("vec2i"), py_tpobject(vec2i)); + py_setdict(mod, py_name("vec3i"), py_tpobject(vec3i)); + py_setdict(mod, py_name("mat3x3"), py_tpobject(mat3x3)); + + assert(vec2 == tp_vec2); + assert(vec3 == tp_vec3); + assert(vec2i == tp_vec2i); + assert(vec3i == tp_vec3i); + assert(mat3x3 == tp_mat3x3); + + /* vec2 */ + py_bindmagic(vec2, __new__, vec2__new__); + py_bindmagic(vec2, __add__, vec2__add__); + py_bindmagic(vec2, __sub__, vec2__sub__); + py_bindmagic(vec2, __mul__, vec2__mul__); + py_bindmagic(vec2, __truediv__, vec2__truediv__); + py_bindmagic(vec2, __repr__, vec2__repr__); + py_bindmagic(vec2, __eq__, vec2__eq__); + py_bindmagic(vec2, __ne__, vec2__ne__); + py_bindmethod(vec2, "dot", vec2_dot); + py_bindmethod(vec2, "cross", vec2_cross); + py_bindmethod(vec2, "length", vec2_length); + py_bindmethod(vec2, "length_squared", vec2_length_squared); + py_bindmethod(vec2, "normalize", vec2_normalize); + py_bindmethod(vec2, "rotate", vec2_rotate); + + py_newvec2(py_emplacedict(py_tpobject(vec2), py_name("ZERO")), (c11_vec2){0, 0}); + py_newvec2(py_emplacedict(py_tpobject(vec2), py_name("ONE")), (c11_vec2){1, 1}); + + py_bindmethod(vec2, "angle", vec2_angle_STATIC); + py_bindmethod(vec2, "smooth_damp", vec2_smoothdamp_STATIC); + + py_bindproperty(vec2, "x", vec2__x, NULL); + py_bindproperty(vec2, "y", vec2__y, NULL); + py_bindmethod(vec2, "with_x", vec2__with_x); + py_bindmethod(vec2, "with_y", vec2__with_y); + + /* mat3x3 */ + py_bindmagic(mat3x3, __new__, mat3x3__new__); + py_bindmagic(mat3x3, __repr__, mat3x3__repr__); + py_bindmagic(mat3x3, __getitem__, mat3x3__getitem__); + py_bindmagic(mat3x3, __setitem__, mat3x3__setitem__); + py_bindmagic(mat3x3, __matmul__, mat3x3__matmul__); + py_bindmagic(mat3x3, __invert__, mat3x3__invert__); + py_bindmagic(mat3x3, __eq__, mat3x3__eq__); + py_bindmagic(mat3x3, __ne__, mat3x3__ne__); + py_bindmethod(mat3x3, "matmul", mat3x3_matmul); + py_bindmethod(mat3x3, "determinant", mat3x3_determinant); + py_bindmethod(mat3x3, "copy", mat3x3_copy); + py_bindmethod(mat3x3, "inverse", mat3x3_inverse); + py_bindmethod(mat3x3, "copy_", mat3x3_copy_); + py_bindmethod(mat3x3, "inverse_", mat3x3_inverse_); + py_bindmethod(mat3x3, "zeros", mat3x3_zeros_STATIC); + py_bindmethod(mat3x3, "identity", mat3x3_identity_STATIC); + py_bindmethod(mat3x3, "trs", mat3x3_trs_STATIC); + py_bindmethod(mat3x3, "copy_trs_", mat3x3_copy_trs_); + py_bindmethod(mat3x3, "t", mat3x3_t); + py_bindmethod(mat3x3, "r", mat3x3_r); + py_bindmethod(mat3x3, "s", mat3x3_s); + py_bindmethod(mat3x3, "transform_point", mat3x3_transform_point); + py_bindmethod(mat3x3, "transform_vector", mat3x3_transform_vector); + + /* vec2i */ + py_bindmagic(vec2i, __new__, vec2i__new__); + py_bindmagic(vec2i, __repr__, vec2i__repr__); + py_bindmagic(vec2i, __eq__, vec2i__eq__); + py_bindmagic(vec2i, __ne__, vec2i__ne__); + py_bindproperty(vec2i, "x", vec2i__x, NULL); + py_bindproperty(vec2i, "y", vec2i__y, NULL); + py_bindmethod(vec2i, "with_x", vec2i__with_x); + py_bindmethod(vec2i, "with_y", vec2i__with_y); + + /* vec3i */ + py_bindmagic(vec3i, __new__, vec3i__new__); + py_bindmagic(vec3i, __repr__, vec3i__repr__); + py_bindmagic(vec3i, __eq__, vec3i__eq__); + py_bindmagic(vec3i, __ne__, vec3i__ne__); + py_bindproperty(vec3i, "x", vec3i__x, NULL); + py_bindproperty(vec3i, "y", vec3i__y, NULL); + py_bindproperty(vec3i, "z", vec3i__z, NULL); + py_bindmethod(vec3i, "with_x", vec3i__with_x); + py_bindmethod(vec3i, "with_y", vec3i__with_y); + py_bindmethod(vec3i, "with_z", vec3i__with_z); + + /* vec3 */ + py_bindmagic(vec3, __new__, vec3__new__); + py_bindmagic(vec3, __repr__, vec3__repr__); + py_bindmagic(vec3, __eq__, vec3__eq__); + py_bindmagic(vec3, __ne__, vec3__ne__); + py_bindproperty(vec3, "x", vec3__x, NULL); + py_bindproperty(vec3, "y", vec3__y, NULL); + py_bindproperty(vec3, "z", vec3__z, NULL); + py_bindmethod(vec3, "with_x", vec3__with_x); + py_bindmethod(vec3, "with_y", vec3__with_y); + py_bindmethod(vec3, "with_z", vec3__with_z); +} \ No newline at end of file diff --git a/src/public/cast.c b/src/public/cast.c index 45b6830b..4b5b7811 100644 --- a/src/public/cast.c +++ b/src/public/cast.c @@ -24,6 +24,14 @@ bool py_castfloat(py_Ref self, double* out) { } } +bool py_castint(py_Ref self, int64_t* out) { + if(self->type == tp_int) { + *out = self->_i64; + return true; + } + return TypeError("expected 'int', got '%t'", self->type); +} + bool py_tobool(py_Ref self) { assert(self->type == tp_bool); return self->_bool; diff --git a/src/public/py_exception.c b/src/public/py_exception.c index b1385547..b25d9928 100644 --- a/src/public/py_exception.c +++ b/src/public/py_exception.c @@ -14,22 +14,9 @@ typedef struct BaseExceptionFrame { } BaseExceptionFrame; typedef struct BaseException { - int lineno_backup; - const CodeObject* code_backup; c11_vector /*T=BaseExceptionFrame*/ stacktrace; } BaseException; -void py_BaseException__set_lineno(py_Ref self, int lineno, const CodeObject* code) { - BaseException* ud = py_touserdata(self); - ud->lineno_backup = lineno; - ud->code_backup = code; -} - -int py_BaseException__get_lineno(py_Ref self, const CodeObject* code) { - BaseException* ud = py_touserdata(self); - if(code != ud->code_backup) return -1; - return ud->lineno_backup; -} void py_BaseException__stpush(py_Ref self, SourceData_ src, int lineno, const char* func_name) { BaseException* ud = py_touserdata(self); @@ -54,8 +41,6 @@ static bool _py_BaseException__new__(int argc, py_Ref argv) { py_Type cls = py_totype(argv); BaseException* ud = py_newobject(py_retval(), cls, 2, sizeof(BaseException)); c11_vector__ctor(&ud->stacktrace, sizeof(BaseExceptionFrame)); - ud->lineno_backup = -1; - ud->code_backup = NULL; return true; } diff --git a/tests/80_linalg.py b/tests/80_linalg.py index e4820086..edb135d3 100644 --- a/tests/80_linalg.py +++ b/tests/80_linalg.py @@ -1,16 +1,11 @@ -exit() - -from linalg import mat3x3, vec2, vec3 +from linalg import mat3x3, vec2, vec3, vec2i, vec3i import random -import sys import math a = vec2(1.5, 2) assert a.x == 1.5 assert a.y == 2 -assert repr(math) == "" - # 出于对精度转换的考虑,在本测试中具体将采用str(floating_num)[:6]来比较两个浮点数是否相等 # test vec2-------------------------------------------------------------------- @@ -34,12 +29,6 @@ static_test_vec2_int = vec2(278, -1391) assert str(static_test_vec2_float).startswith('vec2(') assert str(static_test_vec2_int).startswith('vec2(') -# test copy -element_name_list = [e for e in dir(test_vec2) if e in 'x,y,z,w'] -element_value_list = [getattr(test_vec2, attr) for attr in element_name_list] -copy_element_value_list = [getattr(test_vec2, attr) for attr in element_name_list] -assert element_value_list == copy_element_value_list - # test rotate test_vec2_copy = test_vec2 radians = random.uniform(-10*math.pi, 10*math.pi) @@ -161,7 +150,7 @@ def row_operation(matrix, target_row, source_row, scale): # 生成随机测试目标 min_num = -10.0 max_num = 10.0 -test_mat = mat3x3([random.uniform(min_num, max_num) for _ in range(9)]) +test_mat = mat3x3(*[random.uniform(min_num, max_num) for _ in range(9)]) static_test_mat_float= mat3x3( 7.264189733952545, -5.432187523625671, 1.8765304152872613, -2.4910524352374734, 8.989660807513068, -0.7168824333280513, @@ -172,11 +161,7 @@ static_test_mat_float_inv = mat3x3( 0.32265243, 0.15808159, -0.09939472, 0.04199553, 0.13813096, 0.00408326, -0.59454451, -0.21208362, 0.39658464) -static_test_mat_int = mat3x3([ - 1, 2, 3, - 4, 5, 6, - 7, 8, 9] - ) +static_test_mat_int = mat3x3(1, 2, 3, 4, 5, 6, 7, 8, 9) # test incorrect number of parameters is passed for i in range(20): @@ -194,94 +179,45 @@ for i in range(20): except TypeError: pass -# test 9 floating parameters is passed -test_mat_copy = test_mat.copy() -element_name_list = [] -for i in range(3): - for j in range(3): - element_name_list.append(f'_{i+1}{j+1}') -element_value_list = [getattr(test_mat, attr) for attr in element_name_list] -assert mat3x3(*tuple(element_value_list)) == test_mat - # test copy test_mat_copy = test_mat.copy() assert test_mat is not test_mat_copy assert test_mat == test_mat_copy -# test __getitem__ -for i, element in enumerate([getattr(test_mat, e) for e in element_name_list]): - assert test_mat[int(i/3), i%3] == element - try: test_mat[1,2,3] - raise Exception('未能触发错误拦截, 此处应当报错 IndexError("index out of range")') -except: +except IndexError: pass try: - test_mat[-1][4] + test_mat[-1, 4] raise Exception('未能触发错误拦截, 此处应当报错 IndexError("index out of range")') -except: +except IndexError: pass -# test __setitem__ +# test __setitem__ and __getitem__ test_mat_copy = test_mat.copy() -for i, element in enumerate([getattr(test_mat_copy, e) for e in element_name_list]): - test_mat_copy[int(i/3), i%3] = list(range(9))[i] -assert test_mat_copy == mat3x3([0,1,2, - 3,4,5, - 6,7,8]) +test_mat_copy[1, 2] = 1 +assert test_mat_copy[1, 2] == 1 try: test_mat[1,2,3] = 1 raise Exception('未能触发错误拦截, 此处应当报错 TypeError("Mat3x3.__setitem__ takes a tuple of 2 integers")') -except: +except IndexError: pass try: - test_mat[-1][4] = 1 + test_mat[-1, 4] = 1 raise Exception('未能触发错误拦截, 此处应当报错 IndexError("index out of range")') -except: +except IndexError: pass -# test __add__ -test_mat_copy = test_mat.copy() -ones = mat3x3.ones() -result_mat = test_mat_copy.__add__(ones) -correct_result_mat = test_mat_copy.copy() -for i in range(3): - for j in range(3): - correct_result_mat[i, j] += 1 -assert result_mat == correct_result_mat - -# test __sub__ -test_mat_copy = test_mat.copy() -ones = mat3x3.ones() -result_mat = test_mat_copy.__sub__(ones) -correct_result_mat = test_mat_copy.copy() -for i in range(3): - for j in range(3): - correct_result_mat[i, j] -= 1 -assert result_mat == correct_result_mat - -# test __mul__ -test_mat_copy = test_mat.copy() -result_mat = test_mat_copy.__mul__(12.345) -correct_result_mat = test_mat_copy.copy() -for i in range(3): - for j in range(3): - correct_result_mat[i, j] *= 12.345 -# print(result_mat) -# print(correct_result_mat) -assert result_mat == correct_result_mat - - # test matmul test_mat_copy = test_mat.copy() test_mat_copy_2 = test_mat.copy() result_mat = test_mat_copy @ test_mat_copy_2 -correct_result_mat = mat3x3() +correct_result_mat = mat3x3.zeros() for i in range(3): for j in range(3): correct_result_mat[i, j] = sum([e1*e2 for e1, e2 in zip(get_row(test_mat_copy, i), get_col(test_mat_copy_2, j))]) @@ -295,27 +231,6 @@ test_mat_copy.determinant() assert str(static_test_mat_float) assert str(static_test_mat_int) -# test __truediv__ -test_mat_copy = test_mat.copy() -result_mat = test_mat_copy.__truediv__(12.345) -correct_result_mat = test_mat_copy.copy() -for i in range(3): - for j in range(3): - correct_result_mat[i, j] /= 12.345 -assert result_mat == correct_result_mat - - - -# test __rmul__ -test_mat_copy = test_mat.copy() -result_mat = 12.345 * test_mat_copy -correct_result_mat = test_mat_copy.copy() -for i in range(3): - for j in range(3): - correct_result_mat[i, j] *= 12.345 - -assert result_mat == correct_result_mat - # 此处测试不完全, 未验证正确性 # test interface of "@" "matmul" "__matmul__" with vec3 and error handling @@ -328,31 +243,22 @@ except TypeError: pass -# test transpose -test_mat_copy = test_mat.copy() -assert test_mat_copy.transpose_() is None -assert test_mat_copy == test_mat.transpose() -assert test_mat_copy.transpose() == test_mat_copy.transpose().transpose().transpose() - # test inverse assert ~static_test_mat_float == static_test_mat_float_inv == static_test_mat_float.inverse() assert static_test_mat_float.inverse_() is None assert static_test_mat_float == static_test_mat_float_inv try: - ~mat3x3([1, 2, 3, 2, 4, 6, 3, 6, 9]) + ~mat3x3(*[1, 2, 3, 2, 4, 6, 3, 6, 9]) raise Exception('未能拦截错误 ValueError("matrix is not invertible") 在 test_mat_copy 的行列式为0') -except ValueError: +except ZeroDivisionError: pass # test zeros -assert mat3x3([0 for _ in range(9)]) == mat3x3.zeros() - -# test ones -assert mat3x3([1 for _ in range(9)]) == mat3x3.ones() +assert mat3x3(*[0 for _ in range(9)]) == mat3x3.zeros() # test identity -assert mat3x3([1,0,0,0,1,0,0,0,1]) == mat3x3.identity() +assert mat3x3(*[1,0,0,0,1,0,0,0,1]) == mat3x3.identity() # test affine transformations----------------------------------------------- @@ -378,39 +284,20 @@ mat3x3.trs(test_vec2_copy, radian, test_vec2_2_copy) a = mat3x3.zeros() a.copy_trs_(test_vec2_copy, radian, test_vec2_2_copy) assert a == mat3x3.trs(test_vec2_copy, radian, test_vec2_2_copy) -b = mat3x3.identity() -b.copy_t_(test_vec2_copy) -b.copy_r_(radian) -b.copy_s_(test_vec2_2_copy) -assert a == b - -# test is_affine -def mat_is_affine(mat_list): - return mat_list[2][0] == 0 and mat_list[2][1] == 0 and mat_list[2][2] == 1 - -# 通过random.unifrom的返回值不可能是整数0或1, 因此认为test_mat不可能is_affine -test_mat_copy = test_mat.copy() -assert test_mat_copy.is_affine() == mat_is_affine(mat_to_list(test_mat_copy)) - -test_mat_copy[2,0] = 0 -test_mat_copy[2,1] = 0 -test_mat_copy[2,2] = 1 -assert test_mat_copy.is_affine() == mat_is_affine(mat_to_list(test_mat_copy)) - # test translation test_mat_copy = test_mat.copy() -assert test_mat_copy._t() == vec2(test_mat_copy[0, 2], test_mat_copy[1, 2]) +assert test_mat_copy.t() == vec2(test_mat_copy[0, 2], test_mat_copy[1, 2]) # 该方法的测试未验证计算的准确性 # test rotation test_mat_copy = test_mat.copy() -assert type(test_mat_copy._r()) is float +assert type(test_mat_copy.r()) is float # test scale test_mat_copy = test_mat.copy() -temp_vec2 = test_mat_copy._s() +temp_vec2 = test_mat_copy.s() # test transform_point test_mat_copy = test_mat.copy() @@ -424,32 +311,27 @@ test_mat_copy = test_mat.copy() test_vec2_copy = test_vec2 temp_vec2 = test_mat_copy.transform_vector(test_vec2_copy) -# test inverse_transform_point -assert test_mat_copy.inverse_transform_point(test_vec2_copy) == test_mat_copy.inverse().transform_point(test_vec2_copy) -# test inverse_transform_vector -assert test_mat_copy.inverse_transform_vector(test_vec2_copy) == test_mat_copy.inverse().transform_vector(test_vec2_copy) - val = vec2.angle(vec2(-1, 0), vec2(0, -1)) assert 1.57 < val < 1.58 # test about staticmethod -class mymat3x3(mat3x3): - def f(self): - _0 = self.zeros() - _1 = super().zeros() - _2 = mat3x3.zeros() - return _0 == _1 == _2 +# class mymat3x3(mat3x3): +# def f(self): +# _0 = self.zeros() +# _1 = super().zeros() +# _2 = mat3x3.zeros() +# return _0 == _1 == _2 -assert mymat3x3().f() +# assert mymat3x3().f() d = mat3x3.identity() assert d.copy_(mat3x3.zeros()) is None assert d == mat3x3.zeros() d = mat3x3.identity() -assert d.matmul(mat3x3.zeros()) == mat3x3.zeros() +assert d @ mat3x3.zeros() == mat3x3.zeros() assert d == mat3x3.identity() -assert d.matmul(mat3x3.zeros(), out=d) is None +assert d.matmul(mat3x3.zeros(), d) is None assert d == mat3x3.zeros() try: @@ -460,8 +342,21 @@ except IndexError: # test vec * vec assert vec2(1, 2) * vec2(3, 4) == vec2(3, 8) -assert vec3(1, 2, 3) * vec3(4, 5, 6) == vec3(4, 10, 18) -# test vec.__getitem__ -assert vec2(1, 2)[0] == 1 and vec2(1, 2)[1] == 2 -assert vec3(1, 2, 3)[0] == 1 and vec3(1, 2, 3)[1] == 2 and vec3(1, 2, 3)[2] == 3 \ No newline at end of file +# test vec2i and vec3i + +a = vec2i(1, 2) +assert a.x == 1 +assert a.y == 2 + +assert a == vec2i(1, 2) + +a = vec3i(1, 2, 3) +assert a.x == 1 +assert a.y == 2 +assert a.z == 3 + +assert a == vec3i(1, 2, 3) +assert a.with_x(2) == vec3i(2, 2, 3) +assert a.with_y(3) == vec3i(1, 3, 3) +assert a.with_z(4) == vec3i(1, 2, 4)