19016791193

Committed 02 Nov 2025 06:40PM UTC coverage: 57.167% (-2.6%) from 59.744%

Build # 19016791193

Build Type

Pull #406

github

Committed by

laurensvalk

Commit Message

bricks/virtualhub: Replace with embedded simulation.

Instead of using the newly introduced simhub alongside the virtualhub, we'll just replace the old one entirely now that it has reached feature parity. We can keep calling it the virtualhub.

Pull Request Pull Request #406: New virtual hub for more effective debugging

Run Details

41 of 48 new or added lines in 7 files covered. (85.42%)

414 existing lines in 53 files now uncovered.

4479 of 7835 relevant lines covered (57.17%)

17178392.75 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/pybricks/tools/pb_type_matrix.c

// SPDX-License-Identifier: MIT
// Copyright (c) 2020-2023 The Pybricks Authors

#include "py/mpconfig.h"

#include <inttypes.h>
#include <math.h>
#include <stdio.h>
#include <string.h>

#include <pybricks/tools/pb_type_matrix.h>

#include <pybricks/util_mp/pb_kwarg_helper.h>
#include <pybricks/util_mp/pb_obj_helper.h>
#include <pybricks/util_pb/pb_error.h>

#if MICROPY_PY_BUILTINS_FLOAT

// pybricks.tools.Matrix.__init__
static mp_obj_t pb_type_Matrix_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) {
    PB_PARSE_ARGS_CLASS(n_args, n_kw, args,
        PB_ARG_REQUIRED(rows));

    // If the input is already a matrix, just return it
    if (mp_obj_is_type(rows_in, &pb_type_Matrix)) {
        return rows_in;
    }

    // Before we allocate the object, check if it's a 1x1 matrix: C = [[c]],
    // in which case we should just return c as a float.

    // Unpack the main list of rows and get the requested sizes
    size_t m, n;
    mp_obj_t *row_objs, *scalar_objs;
    mp_obj_get_array(rows_in, &m, &row_objs);

    if (m == 0) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    mp_obj_get_array(row_objs[0], &n, &scalar_objs);

    if (n == 0) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    // It's a 1x1 object, return it as a float
    if (m == 1 && n == 1) {
        return mp_obj_new_float(mp_obj_get_float(scalar_objs[0]));
    }

    // Create objects and save dimensions
    pb_type_Matrix_obj_t *self = mp_obj_malloc(pb_type_Matrix_obj_t, type);
    self->m = m;
    self->n = n;
    self->data = m_new(float, m * n);

    // Iterate through each of the rows to get the scalars
    for (size_t r = 0; r < m; r++) {
        size_t n2;
        mp_obj_get_array(row_objs[r], &n2, &scalar_objs);

        if (n2 != n) {
            // TODO: raise dimension error, all rows must have same length
            pb_assert(PBIO_ERROR_INVALID_ARG);
        }

        // Unpack the scalars
        for (size_t c = 0; c < n2; c++) {
            self->data[r * n2 + c] = mp_obj_get_float_to_f(scalar_objs[c]);
        }
    }

    // Modifiers that allow basic modifications without moving data around
    self->scale = 1;
    self->transposed = false;

    return MP_OBJ_FROM_PTR(self);
}

// Get string representation of the form -123.456
static void print_float(char *buf, float x) {

    // Convert to positive int
    int32_t val = (int32_t)(x * 1000);
    bool negative = val < 0;
    if (negative) {
        val = -val;
    }

    // Limit to 999.999
    if (val >= 999999) {
        val = 999999;
    }

    // Get number of printed digits so we know where to add minus sign later
    int32_t c = snprintf(NULL, 0, "%" PRId32, val);

    // Print the integer
    snprintf(buf, 8, "%7" PRId32, val);

    // If the number was negative, add minus sign in the right place
    if (negative) {
        buf[6 - c <= 2 ? 6 - c : 2] = '-';
    }

    // If there is nothing before the decimal, ensure there is a zero
    buf[3] = buf[3] == ' ' ? '0' : buf[3];

    // Shift last 3 digits to make room for decimal point and insert 0 if empty
    buf[7] = buf[6] == ' ' ? '0' : buf[6];
    buf[6] = buf[5] == ' ' ? '0' : buf[5];
    buf[5] = buf[4] == ' ' ? '0' : buf[4];

    // Terminate and add decimal separator
    buf[4] = '.';
}

// pybricks.tools.Matrix.__repr__
void pb_type_Matrix_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {

    // Print class name
    mp_print_str(print, "Matrix([\n");

    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

    // Each "-123.456" is 8 chars. Each digit is appended with ", " or "]," so
    // 10 per digit. Each row starts with "    [" and ends with "\n" so we
    // add 6*rows. Plus 1 for null terminator.
    size_t len = self->m * self->n * 10 + self->m * 6 + 1;

    // Allocate the buffer
    char *buf = m_new(char, len);

    // Loop through the rows
    for (size_t r = 0; r < self->m; r++) {

        // Character starting index for this row
        size_t row_start = r * (self->n * 10 + 6);

        // Start row with "    ["
        strcpy(buf + row_start, "    [");

        // Loop through the columns, so the scalars in each row
        for (size_t c = 0; c < self->n; c++) {

            // Starting character index for this column
            size_t col_start = row_start + c * 10 + 5;

            // Get data index of the scalar we will print. Transposed attribute
            // tells us whether data is stored row by row or column by column.
            // (i, j) -> index:
            // regular:    i * self->n + j
            // transposed: j * self->m + i
            size_t idx = self->transposed ? c * self->m + r : r * self->n + c;

            // Get character representation of said value
            print_float(buf + col_start, self->data[idx] * self->scale);

            // Append ", " or "]," after the last value
            if (c < self->n - 1) {
                buf[col_start + 8] = ',';
                buf[col_start + 9] = ' ';
            } else {
                buf[col_start + 8] = ']';
                buf[col_start + 9] = ',';
            }
        }
        // New line at end of row
        buf[row_start + self->n * 10 + 5] = '\n';
    }
    // Terminate string
    buf[len - 1 ] = '\0';

    // Send the buffer to MicroPython
    mp_print_str(print, buf);
    mp_print_str(print, "])");
}

// pybricks.tools.Matrix._add
static mp_obj_t pb_type_Matrix__add(mp_obj_t lhs_obj, mp_obj_t rhs_obj, bool add) {

    // Get left and right matrices
    pb_type_Matrix_obj_t *lhs = MP_OBJ_TO_PTR(lhs_obj);
    pb_type_Matrix_obj_t *rhs = MP_OBJ_TO_PTR(rhs_obj);

    // Verify matching dimensions else raise error
    if (lhs->n != rhs->n || lhs->m != rhs->m) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    // Result has same shape as both sides
    pb_type_Matrix_obj_t *ret = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);
    ret->m = lhs->m;
    ret->n = rhs->n;
    ret->data = m_new(float, ret->m * ret->n);

    // Scale must be reset; it has been and multiplied out above
    ret->scale = 1;
    ret->transposed = false;

    // Add the matrices by looping over rows and columns
    for (size_t r = 0; r < ret->m; r++) {
        for (size_t c = 0; c < ret->n; c++) {
            // This entry is obtained as the sum of scalars of both matrices
            // First find the index of sources and destination.
            size_t ret_idx = r * ret->n + c;
            size_t lhs_idx = lhs->transposed ? c * ret->m + r : ret_idx;
            size_t rhs_idx = rhs->transposed ? c * ret->m + r : ret_idx;

            // Either add or subtract to get result.
            if (add) {
                ret->data[ret_idx] = lhs->data[lhs_idx] * lhs->scale + rhs->data[rhs_idx] * rhs->scale;
            } else {
                ret->data[ret_idx] = lhs->data[lhs_idx] * lhs->scale - rhs->data[rhs_idx] * rhs->scale;
            }

        }
    }

    return MP_OBJ_FROM_PTR(ret);
}

// pybricks.tools.Matrix._mul
static mp_obj_t pb_type_Matrix__mul(mp_obj_t lhs_in, mp_obj_t rhs_in) {

    // Get left and right matrices
    pb_type_Matrix_obj_t *lhs = MP_OBJ_TO_PTR(lhs_in);
    pb_type_Matrix_obj_t *rhs = MP_OBJ_TO_PTR(rhs_in);

    // Verify matching dimensions else raise error
    if (lhs->n != rhs->m) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    // Result has as many rows as left hand side and as many columns as right hand side.
    pb_type_Matrix_obj_t *ret = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);
    ret->m = lhs->m;
    ret->n = rhs->n;
    ret->data = m_new(float, ret->m * ret->n);

    // Scale is commutative, so we can do it separately
    ret->scale = lhs->scale * rhs->scale;
    ret->transposed = false;

    // Multiply the matrices by looping over rows and columns
    for (size_t r = 0; r < ret->m; r++) {
        for (size_t c = 0; c < ret->n; c++) {
            // This entry is obtained as the sum of the products of the entries
            // of the r'th row of lhs and the c'th column of rhs, so size lhs->n.
            float sum = 0;
            for (size_t k = 0; k < lhs->n; k++) {
                // k'th entry on the r'th row (i = r, j = k)
                size_t lhs_idx = lhs->transposed ? k * lhs->m + r : r * lhs->n + k;
                // k'th entry on the c'th column  (i = k, j = c)
                size_t rhs_idx = rhs->transposed ? c * rhs->m + k : k * rhs->n + c;
                sum += lhs->data[lhs_idx] * rhs->data[rhs_idx];
            }
            ret->data[ret->n * r + c] = sum;
        }
    }

    // If the result is a 1x1, return as scalar. This solves all the
    // usual matrix library problems where you have to type things like
    // C[0][0] just to get the scalar, such as for the inner product of two
    // vectors. The same is done for 1x1 initialization above.
    if (ret->m == 1 && ret->n == 1) {
        return mp_obj_new_float_from_f(ret->data[0] * ret->scale);
    }

    return MP_OBJ_FROM_PTR(ret);
}

// pybricks.tools.Matrix._scale
static mp_obj_t pb_type_Matrix__scale(mp_obj_t self_in, float scale) {
    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

    pb_type_Matrix_obj_t *copy = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);

    // Point to the same data instead of copying
    copy->data = self->data;
    copy->n = self->n;
    copy->m = self->m;
    copy->scale = self->scale * scale;
    copy->transposed = self->transposed;

    return MP_OBJ_FROM_PTR(copy);
}

// pybricks.tools.Matrix._get_scalar
float pb_type_Matrix_get_scalar(mp_obj_t self_in, size_t r, size_t c) {
    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

    if (r >= self->m || c >= self->n) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    size_t idx = self->transposed ? c * self->m + r : r * self->n + c;
    return self->data[idx] * self->scale;
}

// pybricks.tools.Matrix._T
static mp_obj_t pb_type_Matrix__T(mp_obj_t self_in) {
    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

    pb_type_Matrix_obj_t *copy = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);

    // Point to the same data instead of copying
    copy->data = self->data;
    copy->n = self->m;
    copy->m = self->n;
    copy->scale = self->scale;
    copy->transposed = !self->transposed;

    return MP_OBJ_FROM_PTR(copy);
}

static void pb_type_Matrix_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest) {
    // Read only
    if (dest[0] == MP_OBJ_NULL) {
        // Create and return transpose
        if (attr == MP_QSTR_T) {
            dest[0] = pb_type_Matrix__T(self_in);
            return;
        }
        // Create and return shape tuple
        if (attr == MP_QSTR_shape) {
            pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

            mp_obj_t shape[] = {
                MP_OBJ_NEW_SMALL_INT(self->m),
                MP_OBJ_NEW_SMALL_INT(self->n)
            };

            dest[0] = mp_obj_new_tuple(2, shape);
            return;
        }
    }
}

static mp_obj_t pb_type_Matrix_unary_op(mp_unary_op_t op, mp_obj_t o_in) {

    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(o_in);

    switch (op) {
        // Hash is the same as in generic unary op
        case MP_UNARY_OP_HASH:
            return MP_OBJ_NEW_SMALL_INT((mp_uint_t)o_in);
        // Positive just returns the original object
        case MP_UNARY_OP_POSITIVE:
            return o_in;
        // Negative returns a scaled copy
        case MP_UNARY_OP_NEGATIVE:
            return pb_type_Matrix__scale(o_in, -1);
        // Get absolute vale (magnitude)
        case MP_UNARY_OP_ABS: {
            // For vectors, this is the norm
            if (self->m == 1 || self->n == 1) {
                size_t len = self->m * self->n;
                float squares = 0;
                for (size_t i = 0; i < len; i++) {
                    squares += self->data[i] * self->data[i];
                }
                return mp_obj_new_float_from_f(sqrtf(squares) * self->scale);
            }
            // Determinant not implemented
            return MP_OBJ_NULL;
        }
        default:
            // Other operations are not supported
            return MP_OBJ_NULL;
    }
}

static mp_obj_t pb_type_Matrix_binary_op(mp_binary_op_t op, mp_obj_t lhs_in, mp_obj_t rhs_in) {

    switch (op) {
        case MP_BINARY_OP_ADD:
        case MP_BINARY_OP_INPLACE_ADD:
            return pb_type_Matrix__add(lhs_in, rhs_in, true);
        case MP_BINARY_OP_SUBTRACT:
        case MP_BINARY_OP_INPLACE_SUBTRACT:
            return pb_type_Matrix__add(lhs_in, rhs_in, false);
        case MP_BINARY_OP_MULTIPLY:
        case MP_BINARY_OP_INPLACE_MULTIPLY:
            // If right of operand is a number, just scale to be faster
            if (mp_obj_is_float(rhs_in) || mp_obj_is_int(rhs_in)) {
                return pb_type_Matrix__scale(lhs_in, mp_obj_get_float_to_f(rhs_in));
            }
            // Otherwise we have to do full multiplication.
            return pb_type_Matrix__mul(lhs_in, rhs_in);
        case MP_BINARY_OP_REVERSE_MULTIPLY:
            // This gets called for c*A, so scale A by c (rhs/lhs is meaningless here)
            return pb_type_Matrix__scale(lhs_in, mp_obj_get_float_to_f(rhs_in));
        case MP_BINARY_OP_TRUE_DIVIDE:
        case MP_BINARY_OP_INPLACE_TRUE_DIVIDE:
            // Scalar division by c is scalar multiplication by 1/c
            return pb_type_Matrix__scale(lhs_in, 1 / mp_obj_get_float_to_f(rhs_in));
        default:
            // Other operations not supported
            return MP_OBJ_NULL;
    }
}

static mp_obj_t pb_type_Matrix_subscr(mp_obj_t self_in, mp_obj_t index_in, mp_obj_t value_in) {

    pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);

    if (value_in == MP_OBJ_SENTINEL) {
        // Integer index is just reading straight from self->data[idx].
        // But we need to do some checks to make sure we read within bounds.
        mp_int_t len = self->m * self->n;
        mp_int_t idx = -1;

        if (mp_obj_is_int(index_in)) {
            // Get requested index as int
            idx = mp_obj_get_int(index_in);

            // This is Python, so allow for negative index
            if (idx < 0) {
                idx += len;
            }

            // Data may be stored as transposed for efficiency reasons,
            // but the user will still expect a consistent value by index.
            if (self->transposed) {
                idx = idx / self->n + (idx % self->n) * self->m;
            }
        } else {
            // Get requested value at (row, col) pair
            size_t s;
            mp_obj_t *shape;
            mp_obj_get_array(index_in, &s, &shape);

            // Only proceed if the index is indeed of shape (row, col)
            if (s == 2) {
                // Get row index, allowing for negative
                mp_int_t r = mp_obj_get_int(shape[0]);
                if (r < 0) {
                    r += self->m;
                }
                // Get col index, allowing for negative
                mp_int_t c = mp_obj_get_int(shape[1]);
                if (c < 0) {
                    c += self->n;
                }
                // Make sure requested row/col exist within (m, n)
                if (c >= 0 && r >= 0 && (size_t)c < self->n && (size_t)r < self->m) {
                    idx = self->transposed ? c * self->m + r : r * self->n + c;
                }
            }
        }

        // Make sure we have meanwhile a valid index by now
        if (idx < 0 || idx >= len) {
            // FIXME: raise dimension error
            mp_raise_msg(&mp_type_IndexError, MP_ERROR_TEXT("index out of range"));
        }

        // Return result
        return mp_obj_new_float_from_f(self->scale * self->data[idx]);
    }
    return MP_OBJ_NULL;
}

typedef struct {
    mp_obj_base_t base;
    mp_fun_1_t iternext;
    mp_obj_t matrix;
    size_t cur;
} pb_type_Matrix_it_t;

_Static_assert(sizeof(pb_type_Matrix_it_t) <= sizeof(mp_obj_iter_buf_t),
    "pb_type_Matrix_it_t uses memory allocated for mp_obj_iter_buf_t");

static mp_obj_t pb_type_Matrix_it_iternext(mp_obj_t self_in) {
    pb_type_Matrix_it_t *self = MP_OBJ_TO_PTR(self_in);
    pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(self->matrix);

    if (self->cur < matrix->m * matrix->n) {
        return mp_obj_new_float_from_f(matrix->data[self->cur++] * matrix->scale);
    }

    return MP_OBJ_STOP_ITERATION;
}

static mp_obj_t pb_type_Matrix_it_iterprev(mp_obj_t self_in) {
    pb_type_Matrix_it_t *self = MP_OBJ_TO_PTR(self_in);
    pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(self->matrix);

    if (self->cur > 0) {
        return mp_obj_new_float_from_f(matrix->data[--self->cur] * matrix->scale);
    }

    return MP_OBJ_STOP_ITERATION;
}

static mp_obj_t pb_type_Matrix_getiter(mp_obj_t o_in, mp_obj_iter_buf_t *iter_buf) {
    pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(o_in);
    pb_type_Matrix_it_t *matrix_it = (pb_type_Matrix_it_t *)iter_buf;

    matrix_it->base.type = &mp_type_polymorph_iter;
    matrix_it->matrix = MP_OBJ_FROM_PTR(matrix);

    if (matrix->transposed) {
        matrix_it->iternext = pb_type_Matrix_it_iterprev;
        matrix_it->cur = matrix->m * matrix->n;
    } else {
        matrix_it->iternext = pb_type_Matrix_it_iternext;
        matrix_it->cur = 0;
    }

    return MP_OBJ_FROM_PTR(matrix_it);
}

// type(pybricks.tools.Matrix)
MP_DEFINE_CONST_OBJ_TYPE(pb_type_Matrix,
    MP_QSTR_Matrix,
    MP_TYPE_FLAG_ITER_IS_GETITER,
    print, pb_type_Matrix_print,
    make_new, pb_type_Matrix_make_new,
    attr, pb_type_Matrix_attr,
    unary_op, pb_type_Matrix_unary_op,
    binary_op, pb_type_Matrix_binary_op,
    subscr, pb_type_Matrix_subscr,
    iter, pb_type_Matrix_getiter);

// pybricks.tools._make_vector
mp_obj_t pb_type_Matrix_make_vector(size_t m, float *data, bool normalize) {

    // Create object and save dimensions
    pb_type_Matrix_obj_t *mat = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);
    mat->m = m;
    mat->n = 1;
    mat->data = m_new(float, m);

    // Copy data and compute norm
    float squares = 0;
    for (size_t i = 0; i < m; i++) {
        mat->data[i] = data[i];
        squares += data[i] * data[i];
    }
    mat->scale = normalize ? 1 / sqrtf(squares) : 1;

    return MP_OBJ_FROM_PTR(mat);
}

// pybricks.tools._make_bitmap
mp_obj_t pb_type_Matrix_make_bitmap(size_t m, size_t n, float scale, uint32_t src) {

    // Create object and save dimensions
    pb_type_Matrix_obj_t *mat = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);
    mat->m = m;
    mat->n = n;
    mat->scale = scale;
    mat->data = m_new(float, m * n);

    for (size_t i = 0; i < m * n; i++) {
        mat->data[m * n - i - 1] = (src & (1 << i)) != 0;
    }

    return MP_OBJ_FROM_PTR(mat);
}

// pybricks.tools.vector
static mp_obj_t pb_geometry_vector(size_t n_args, const mp_obj_t *args) {

    // Convert user object to floats
    float data[3];
    for (size_t i = 0; i < n_args; i++) {
        data[i] = mp_obj_get_float_to_f(args[i]);
    }

    // Create and return Matrix object with vector shape
    return pb_type_Matrix_make_vector(n_args, data, false);
}
MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(pb_geometry_vector_obj, 2, 3, pb_geometry_vector);

// pybricks.tools.cross
static mp_obj_t pb_type_matrix_cross(mp_obj_t a_in, mp_obj_t b_in) {
    if (!mp_obj_is_type(a_in, &pb_type_Matrix) || !mp_obj_is_type(b_in, &pb_type_Matrix)) {
        mp_raise_TypeError(MP_ERROR_TEXT("arguments must be Matrix objects"));
    }

    // Get a and b vectors
    pb_type_Matrix_obj_t *a = MP_OBJ_TO_PTR(a_in);
    pb_type_Matrix_obj_t *b = MP_OBJ_TO_PTR(b_in);

    // Verify matching dimensions else raise error
    if (a->n * a->m != 3 || b->n * b->m != 3) {
        pb_assert(PBIO_ERROR_INVALID_ARG);
    }

    // Create c vector.
    pb_type_Matrix_obj_t *c = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);
    c->m = 3;
    c->n = 1;
    c->data = m_new(float, 3);

    // Evaluate cross product
    c->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1];
    c->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2];
    c->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0];
    c->scale = a->scale * b->scale;

    return MP_OBJ_FROM_PTR(c);
}
MP_DEFINE_CONST_FUN_OBJ_2(pb_type_matrix_cross_obj, pb_type_matrix_cross);

#endif // MICROPY_PY_BUILTINS_FLOAT

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2020-2023 The Pybricks Authors
3
4	#include "py/mpconfig.h"
5
6	#include <inttypes.h>
7	#include <math.h>
8	#include <stdio.h>
9	#include <string.h>
10
11	#include <pybricks/tools/pb_type_matrix.h>
12
13	#include <pybricks/util_mp/pb_kwarg_helper.h>
14	#include <pybricks/util_mp/pb_obj_helper.h>
15	#include <pybricks/util_pb/pb_error.h>
16
17	#if MICROPY_PY_BUILTINS_FLOAT
18
19	// pybricks.tools.Matrix.__init__
20	static mp_obj_t pb_type_Matrix_make_new(const mp_obj_type_t type, size_t n_args, size_t n_kw, const mp_obj_t args) {	×
21	PB_PARSE_ARGS_CLASS(n_args, n_kw, args,	×
22	PB_ARG_REQUIRED(rows));
23
24	// If the input is already a matrix, just return it
25	if (mp_obj_is_type(rows_in, &pb_type_Matrix)) {	×
UNCOV 26	return rows_in;	×
27	}
28
29	// Before we allocate the object, check if it's a 1x1 matrix: C = [[c]],
30	// in which case we should just return c as a float.
31
32	// Unpack the main list of rows and get the requested sizes
33	size_t m, n;
34	mp_obj_t row_objs, scalar_objs;
35	mp_obj_get_array(rows_in, &m, &row_objs);	×
36
37	if (m == 0) {	×
38	pb_assert(PBIO_ERROR_INVALID_ARG);	×
39	}
40
41	mp_obj_get_array(row_objs[0], &n, &scalar_objs);	×
42
43	if (n == 0) {	×
44	pb_assert(PBIO_ERROR_INVALID_ARG);	×
45	}
46
47	// It's a 1x1 object, return it as a float
48	if (m == 1 && n == 1) {	×
49	return mp_obj_new_float(mp_obj_get_float(scalar_objs[0]));	×
50	}
51
52	// Create objects and save dimensions
53	pb_type_Matrix_obj_t *self = mp_obj_malloc(pb_type_Matrix_obj_t, type);	×
54	self->m = m;	×
55	self->n = n;	×
56	self->data = m_new(float, m * n);	×
57
58	// Iterate through each of the rows to get the scalars
59	for (size_t r = 0; r < m; r++) {	×
60	size_t n2;
61	mp_obj_get_array(row_objs[r], &n2, &scalar_objs);	×
62
63	if (n2 != n) {	×
64	// TODO: raise dimension error, all rows must have same length
65	pb_assert(PBIO_ERROR_INVALID_ARG);	×
66	}
67
68	// Unpack the scalars
69	for (size_t c = 0; c < n2; c++) {	×
70	self->data[r * n2 + c] = mp_obj_get_float_to_f(scalar_objs[c]);	×
71	}
72	}
73
74	// Modifiers that allow basic modifications without moving data around
75	self->scale = 1;	×
76	self->transposed = false;	×
77
78	return MP_OBJ_FROM_PTR(self);	×
79	}
80
81	// Get string representation of the form -123.456
82	static void print_float(char *buf, float x) {	×
83
84	// Convert to positive int
85	int32_t val = (int32_t)(x * 1000);	×
86	bool negative = val < 0;	×
87	if (negative) {	×
UNCOV 88	val = -val;	×
89	}
90
91	// Limit to 999.999
92	if (val >= 999999) {	×
UNCOV 93	val = 999999;	×
94	}
95
96	// Get number of printed digits so we know where to add minus sign later
97	int32_t c = snprintf(NULL, 0, "%" PRId32, val);	×
98
99	// Print the integer
100	snprintf(buf, 8, "%7" PRId32, val);	×
101
102	// If the number was negative, add minus sign in the right place
103	if (negative) {	×
104	buf[6 - c <= 2 ? 6 - c : 2] = '-';	×
105	}
106
107	// If there is nothing before the decimal, ensure there is a zero
108	buf[3] = buf[3] == ' ' ? '0' : buf[3];	×
109
110	// Shift last 3 digits to make room for decimal point and insert 0 if empty
111	buf[7] = buf[6] == ' ' ? '0' : buf[6];	×
112	buf[6] = buf[5] == ' ' ? '0' : buf[5];	×
113	buf[5] = buf[4] == ' ' ? '0' : buf[4];	×
114
115	// Terminate and add decimal separator
116	buf[4] = '.';	×
117	}	×
118
119	// pybricks.tools.Matrix.__repr__
120	void pb_type_Matrix_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind) {	×
121
122	// Print class name
123	mp_print_str(print, "Matrix([\n");	×
124
125	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
126
127	// Each "-123.456" is 8 chars. Each digit is appended with ", " or "]," so
128	// 10 per digit. Each row starts with " [" and ends with "\n" so we
129	// add 6*rows. Plus 1 for null terminator.
130	size_t len = self->m * self->n * 10 + self->m * 6 + 1;	×
131
132	// Allocate the buffer
133	char *buf = m_new(char, len);	×
134
135	// Loop through the rows
136	for (size_t r = 0; r < self->m; r++) {	×
137
138	// Character starting index for this row
139	size_t row_start = r * (self->n * 10 + 6);	×
140
141	// Start row with " ["
142	strcpy(buf + row_start, " [");	×
143
144	// Loop through the columns, so the scalars in each row
145	for (size_t c = 0; c < self->n; c++) {	×
146
147	// Starting character index for this column
148	size_t col_start = row_start + c * 10 + 5;	×
149
150	// Get data index of the scalar we will print. Transposed attribute
151	// tells us whether data is stored row by row or column by column.
152	// (i, j) -> index:
153	// regular: i * self->n + j
154	// transposed: j * self->m + i
155	size_t idx = self->transposed ? c * self->m + r : r * self->n + c;	×
156
157	// Get character representation of said value
158	print_float(buf + col_start, self->data[idx] * self->scale);	×
159
160	// Append ", " or "]," after the last value
161	if (c < self->n - 1) {	×
162	buf[col_start + 8] = ',';	×
163	buf[col_start + 9] = ' ';	×
164	} else {
165	buf[col_start + 8] = ']';	×
166	buf[col_start + 9] = ',';	×
167	}
168	}
169	// New line at end of row
170	buf[row_start + self->n * 10 + 5] = '\n';	×
171	}
172	// Terminate string
173	buf[len - 1 ] = '\0';	×
174
175	// Send the buffer to MicroPython
176	mp_print_str(print, buf);	×
177	mp_print_str(print, "])");	×
178	}	×
179
180	// pybricks.tools.Matrix._add
181	static mp_obj_t pb_type_Matrix__add(mp_obj_t lhs_obj, mp_obj_t rhs_obj, bool add) {	×
182
183	// Get left and right matrices
184	pb_type_Matrix_obj_t *lhs = MP_OBJ_TO_PTR(lhs_obj);	×
185	pb_type_Matrix_obj_t *rhs = MP_OBJ_TO_PTR(rhs_obj);	×
186
187	// Verify matching dimensions else raise error
188	if (lhs->n != rhs->n \|\| lhs->m != rhs->m) {	×
189	pb_assert(PBIO_ERROR_INVALID_ARG);	×
190	}
191
192	// Result has same shape as both sides
193	pb_type_Matrix_obj_t *ret = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
194	ret->m = lhs->m;	×
195	ret->n = rhs->n;	×
196	ret->data = m_new(float, ret->m * ret->n);	×
197
198	// Scale must be reset; it has been and multiplied out above
199	ret->scale = 1;	×
200	ret->transposed = false;	×
201
202	// Add the matrices by looping over rows and columns
203	for (size_t r = 0; r < ret->m; r++) {	×
204	for (size_t c = 0; c < ret->n; c++) {	×
205	// This entry is obtained as the sum of scalars of both matrices
206	// First find the index of sources and destination.
207	size_t ret_idx = r * ret->n + c;	×
208	size_t lhs_idx = lhs->transposed ? c * ret->m + r : ret_idx;	×
209	size_t rhs_idx = rhs->transposed ? c * ret->m + r : ret_idx;	×
210
211	// Either add or subtract to get result.
212	if (add) {	×
213	ret->data[ret_idx] = lhs->data[lhs_idx] * lhs->scale + rhs->data[rhs_idx] * rhs->scale;	×
214	} else {
215	ret->data[ret_idx] = lhs->data[lhs_idx] * lhs->scale - rhs->data[rhs_idx] * rhs->scale;	×
216	}
217
218	}
219	}
220
221	return MP_OBJ_FROM_PTR(ret);	×
222	}
223
224	// pybricks.tools.Matrix._mul
225	static mp_obj_t pb_type_Matrix__mul(mp_obj_t lhs_in, mp_obj_t rhs_in) {	×
226
227	// Get left and right matrices
228	pb_type_Matrix_obj_t *lhs = MP_OBJ_TO_PTR(lhs_in);	×
229	pb_type_Matrix_obj_t *rhs = MP_OBJ_TO_PTR(rhs_in);	×
230
231	// Verify matching dimensions else raise error
232	if (lhs->n != rhs->m) {	×
233	pb_assert(PBIO_ERROR_INVALID_ARG);	×
234	}
235
236	// Result has as many rows as left hand side and as many columns as right hand side.
237	pb_type_Matrix_obj_t *ret = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
238	ret->m = lhs->m;	×
239	ret->n = rhs->n;	×
240	ret->data = m_new(float, ret->m * ret->n);	×
241
242	// Scale is commutative, so we can do it separately
243	ret->scale = lhs->scale * rhs->scale;	×
244	ret->transposed = false;	×
245
246	// Multiply the matrices by looping over rows and columns
247	for (size_t r = 0; r < ret->m; r++) {	×
248	for (size_t c = 0; c < ret->n; c++) {	×
249	// This entry is obtained as the sum of the products of the entries
250	// of the r'th row of lhs and the c'th column of rhs, so size lhs->n.
UNCOV 251	float sum = 0;	×
252	for (size_t k = 0; k < lhs->n; k++) {	×
253	// k'th entry on the r'th row (i = r, j = k)
254	size_t lhs_idx = lhs->transposed ? k * lhs->m + r : r * lhs->n + k;	×
255	// k'th entry on the c'th column (i = k, j = c)
256	size_t rhs_idx = rhs->transposed ? c * rhs->m + k : k * rhs->n + c;	×
257	sum += lhs->data[lhs_idx] * rhs->data[rhs_idx];	×
258	}
259	ret->data[ret->n * r + c] = sum;	×
260	}
261	}
262
263	// If the result is a 1x1, return as scalar. This solves all the
264	// usual matrix library problems where you have to type things like
265	// C[0][0] just to get the scalar, such as for the inner product of two
266	// vectors. The same is done for 1x1 initialization above.
267	if (ret->m == 1 && ret->n == 1) {	×
268	return mp_obj_new_float_from_f(ret->data[0] * ret->scale);	×
269	}
270
UNCOV 271	return MP_OBJ_FROM_PTR(ret);	×
272	}
273
274	// pybricks.tools.Matrix._scale
275	static mp_obj_t pb_type_Matrix__scale(mp_obj_t self_in, float scale) {	×
276	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
277
278	pb_type_Matrix_obj_t *copy = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
279
280	// Point to the same data instead of copying
281	copy->data = self->data;	×
282	copy->n = self->n;	×
283	copy->m = self->m;	×
284	copy->scale = self->scale * scale;	×
285	copy->transposed = self->transposed;	×
286
287	return MP_OBJ_FROM_PTR(copy);	×
288	}
289
290	// pybricks.tools.Matrix._get_scalar
291	float pb_type_Matrix_get_scalar(mp_obj_t self_in, size_t r, size_t c) {	×
292	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
293
294	if (r >= self->m \|\| c >= self->n) {	×
295	pb_assert(PBIO_ERROR_INVALID_ARG);	×
296	}
297
298	size_t idx = self->transposed ? c * self->m + r : r * self->n + c;	×
299	return self->data[idx] * self->scale;	×
300	}
301
302	// pybricks.tools.Matrix._T
303	static mp_obj_t pb_type_Matrix__T(mp_obj_t self_in) {	×
304	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
305
306	pb_type_Matrix_obj_t *copy = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
307
308	// Point to the same data instead of copying
309	copy->data = self->data;	×
310	copy->n = self->m;	×
311	copy->m = self->n;	×
312	copy->scale = self->scale;	×
313	copy->transposed = !self->transposed;	×
314
315	return MP_OBJ_FROM_PTR(copy);	×
316	}
317
318	static void pb_type_Matrix_attr(mp_obj_t self_in, qstr attr, mp_obj_t *dest) {	×
319	// Read only
320	if (dest[0] == MP_OBJ_NULL) {	×
321	// Create and return transpose
322	if (attr == MP_QSTR_T) {	×
323	dest[0] = pb_type_Matrix__T(self_in);	×
324	return;	×
325	}
326	// Create and return shape tuple
327	if (attr == MP_QSTR_shape) {	×
328	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
329
330	mp_obj_t shape[] = {	×
331	MP_OBJ_NEW_SMALL_INT(self->m),	×
332	MP_OBJ_NEW_SMALL_INT(self->n)	×
333	};
334
335	dest[0] = mp_obj_new_tuple(2, shape);	×
336	return;	×
337	}
338	}
339	}
340
341	static mp_obj_t pb_type_Matrix_unary_op(mp_unary_op_t op, mp_obj_t o_in) {	×
342
343	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(o_in);	×
344
345	switch (op) {	×
346	// Hash is the same as in generic unary op
347	case MP_UNARY_OP_HASH:	×
348	return MP_OBJ_NEW_SMALL_INT((mp_uint_t)o_in);	×
349	// Positive just returns the original object
UNCOV 350	case MP_UNARY_OP_POSITIVE:	×
UNCOV 351	return o_in;	×
352	// Negative returns a scaled copy
353	case MP_UNARY_OP_NEGATIVE:	×
354	return pb_type_Matrix__scale(o_in, -1);	×
355	// Get absolute vale (magnitude)
356	case MP_UNARY_OP_ABS: {	×
357	// For vectors, this is the norm
358	if (self->m == 1 \|\| self->n == 1) {	×
359	size_t len = self->m * self->n;	×
360	float squares = 0;	×
361	for (size_t i = 0; i < len; i++) {	×
362	squares += self->data[i] * self->data[i];	×
363	}
364	return mp_obj_new_float_from_f(sqrtf(squares) * self->scale);	×
365	}
366	// Determinant not implemented
UNCOV 367	return MP_OBJ_NULL;	×
368	}
UNCOV 369	default:	×
370	// Other operations are not supported
UNCOV 371	return MP_OBJ_NULL;	×
372	}
373	}
374
375	static mp_obj_t pb_type_Matrix_binary_op(mp_binary_op_t op, mp_obj_t lhs_in, mp_obj_t rhs_in) {	×
376
377	switch (op) {	×
378	case MP_BINARY_OP_ADD:	×
379	case MP_BINARY_OP_INPLACE_ADD:
380	return pb_type_Matrix__add(lhs_in, rhs_in, true);	×
381	case MP_BINARY_OP_SUBTRACT:	×
382	case MP_BINARY_OP_INPLACE_SUBTRACT:
383	return pb_type_Matrix__add(lhs_in, rhs_in, false);	×
UNCOV 384	case MP_BINARY_OP_MULTIPLY:	×
385	case MP_BINARY_OP_INPLACE_MULTIPLY:
386	// If right of operand is a number, just scale to be faster
387	if (mp_obj_is_float(rhs_in) \|\| mp_obj_is_int(rhs_in)) {	×
388	return pb_type_Matrix__scale(lhs_in, mp_obj_get_float_to_f(rhs_in));	×
389	}
390	// Otherwise we have to do full multiplication.
391	return pb_type_Matrix__mul(lhs_in, rhs_in);	×
UNCOV 392	case MP_BINARY_OP_REVERSE_MULTIPLY:	×
393	// This gets called for c*A, so scale A by c (rhs/lhs is meaningless here)
394	return pb_type_Matrix__scale(lhs_in, mp_obj_get_float_to_f(rhs_in));	×
UNCOV 395	case MP_BINARY_OP_TRUE_DIVIDE:	×
396	case MP_BINARY_OP_INPLACE_TRUE_DIVIDE:
397	// Scalar division by c is scalar multiplication by 1/c
398	return pb_type_Matrix__scale(lhs_in, 1 / mp_obj_get_float_to_f(rhs_in));	×
UNCOV 399	default:	×
400	// Other operations not supported
UNCOV 401	return MP_OBJ_NULL;	×
402	}
403	}
404
405	static mp_obj_t pb_type_Matrix_subscr(mp_obj_t self_in, mp_obj_t index_in, mp_obj_t value_in) {	×
406
407	pb_type_Matrix_obj_t *self = MP_OBJ_TO_PTR(self_in);	×
408
409	if (value_in == MP_OBJ_SENTINEL) {	×
410	// Integer index is just reading straight from self->data[idx].
411	// But we need to do some checks to make sure we read within bounds.
412	mp_int_t len = self->m * self->n;	×
413	mp_int_t idx = -1;	×
414
415	if (mp_obj_is_int(index_in)) {	×
416	// Get requested index as int
417	idx = mp_obj_get_int(index_in);	×
418
419	// This is Python, so allow for negative index
420	if (idx < 0) {	×
421	idx += len;	×
422	}
423
424	// Data may be stored as transposed for efficiency reasons,
425	// but the user will still expect a consistent value by index.
426	if (self->transposed) {	×
427	idx = idx / self->n + (idx % self->n) * self->m;	×
428	}
429	} else {
430	// Get requested value at (row, col) pair
431	size_t s;
432	mp_obj_t *shape;
433	mp_obj_get_array(index_in, &s, &shape);	×
434
435	// Only proceed if the index is indeed of shape (row, col)
436	if (s == 2) {	×
437	// Get row index, allowing for negative
438	mp_int_t r = mp_obj_get_int(shape[0]);	×
439	if (r < 0) {	×
440	r += self->m;	×
441	}
442	// Get col index, allowing for negative
443	mp_int_t c = mp_obj_get_int(shape[1]);	×
444	if (c < 0) {	×
445	c += self->n;	×
446	}
447	// Make sure requested row/col exist within (m, n)
448	if (c >= 0 && r >= 0 && (size_t)c < self->n && (size_t)r < self->m) {	×
449	idx = self->transposed ? c * self->m + r : r * self->n + c;	×
450	}
451	}
452	}
453
454	// Make sure we have meanwhile a valid index by now
455	if (idx < 0 \|\| idx >= len) {	×
456	// FIXME: raise dimension error
457	mp_raise_msg(&mp_type_IndexError, MP_ERROR_TEXT("index out of range"));	×
458	}
459
460	// Return result
461	return mp_obj_new_float_from_f(self->scale * self->data[idx]);	×
462	}
UNCOV 463	return MP_OBJ_NULL;	×
464	}
465
466	typedef struct {
467	mp_obj_base_t base;
468	mp_fun_1_t iternext;
469	mp_obj_t matrix;
470	size_t cur;
471	} pb_type_Matrix_it_t;
472
473	_Static_assert(sizeof(pb_type_Matrix_it_t) <= sizeof(mp_obj_iter_buf_t),
474	"pb_type_Matrix_it_t uses memory allocated for mp_obj_iter_buf_t");
475
476	static mp_obj_t pb_type_Matrix_it_iternext(mp_obj_t self_in) {	×
477	pb_type_Matrix_it_t *self = MP_OBJ_TO_PTR(self_in);	×
478	pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(self->matrix);	×
479
480	if (self->cur < matrix->m * matrix->n) {	×
481	return mp_obj_new_float_from_f(matrix->data[self->cur++] * matrix->scale);	×
482	}
483
UNCOV 484	return MP_OBJ_STOP_ITERATION;	×
485	}
486
487	static mp_obj_t pb_type_Matrix_it_iterprev(mp_obj_t self_in) {	×
488	pb_type_Matrix_it_t *self = MP_OBJ_TO_PTR(self_in);	×
489	pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(self->matrix);	×
490
491	if (self->cur > 0) {	×
492	return mp_obj_new_float_from_f(matrix->data[--self->cur] * matrix->scale);	×
493	}
494
UNCOV 495	return MP_OBJ_STOP_ITERATION;	×
496	}
497
498	static mp_obj_t pb_type_Matrix_getiter(mp_obj_t o_in, mp_obj_iter_buf_t *iter_buf) {	×
499	pb_type_Matrix_obj_t *matrix = MP_OBJ_TO_PTR(o_in);	×
500	pb_type_Matrix_it_t matrix_it = (pb_type_Matrix_it_t )iter_buf;	×
501
502	matrix_it->base.type = &mp_type_polymorph_iter;	×
503	matrix_it->matrix = MP_OBJ_FROM_PTR(matrix);	×
504
505	if (matrix->transposed) {	×
506	matrix_it->iternext = pb_type_Matrix_it_iterprev;	×
507	matrix_it->cur = matrix->m * matrix->n;	×
508	} else {
509	matrix_it->iternext = pb_type_Matrix_it_iternext;	×
510	matrix_it->cur = 0;	×
511	}
512
513	return MP_OBJ_FROM_PTR(matrix_it);	×
514	}
515
516	// type(pybricks.tools.Matrix)
517	MP_DEFINE_CONST_OBJ_TYPE(pb_type_Matrix,
518	MP_QSTR_Matrix,
519	MP_TYPE_FLAG_ITER_IS_GETITER,
520	print, pb_type_Matrix_print,
521	make_new, pb_type_Matrix_make_new,
522	attr, pb_type_Matrix_attr,
523	unary_op, pb_type_Matrix_unary_op,
524	binary_op, pb_type_Matrix_binary_op,
525	subscr, pb_type_Matrix_subscr,
526	iter, pb_type_Matrix_getiter);
527
528	// pybricks.tools._make_vector
529	mp_obj_t pb_type_Matrix_make_vector(size_t m, float *data, bool normalize) {	×
530
531	// Create object and save dimensions
532	pb_type_Matrix_obj_t *mat = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
533	mat->m = m;	×
534	mat->n = 1;	×
535	mat->data = m_new(float, m);	×
536
537	// Copy data and compute norm
538	float squares = 0;	×
539	for (size_t i = 0; i < m; i++) {	×
540	mat->data[i] = data[i];	×
541	squares += data[i] * data[i];	×
542	}
543	mat->scale = normalize ? 1 / sqrtf(squares) : 1;	×
544
545	return MP_OBJ_FROM_PTR(mat);	×
546	}
547
548	// pybricks.tools._make_bitmap
549	mp_obj_t pb_type_Matrix_make_bitmap(size_t m, size_t n, float scale, uint32_t src) {	×
550
551	// Create object and save dimensions
552	pb_type_Matrix_obj_t *mat = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
553	mat->m = m;	×
554	mat->n = n;	×
555	mat->scale = scale;	×
556	mat->data = m_new(float, m * n);	×
557
558	for (size_t i = 0; i < m * n; i++) {	×
559	mat->data[m * n - i - 1] = (src & (1 << i)) != 0;	×
560	}
561
562	return MP_OBJ_FROM_PTR(mat);	×
563	}
564
565	// pybricks.tools.vector
566	static mp_obj_t pb_geometry_vector(size_t n_args, const mp_obj_t *args) {	×
567
568	// Convert user object to floats
569	float data[3];
570	for (size_t i = 0; i < n_args; i++) {	×
571	data[i] = mp_obj_get_float_to_f(args[i]);	×
572	}
573
574	// Create and return Matrix object with vector shape
575	return pb_type_Matrix_make_vector(n_args, data, false);	×
576	}
577	MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(pb_geometry_vector_obj, 2, 3, pb_geometry_vector);
578
579	// pybricks.tools.cross
580	static mp_obj_t pb_type_matrix_cross(mp_obj_t a_in, mp_obj_t b_in) {	×
581	if (!mp_obj_is_type(a_in, &pb_type_Matrix) \|\| !mp_obj_is_type(b_in, &pb_type_Matrix)) {	×
582	mp_raise_TypeError(MP_ERROR_TEXT("arguments must be Matrix objects"));	×
583	}
584
585	// Get a and b vectors
586	pb_type_Matrix_obj_t *a = MP_OBJ_TO_PTR(a_in);	×
587	pb_type_Matrix_obj_t *b = MP_OBJ_TO_PTR(b_in);	×
588
589	// Verify matching dimensions else raise error
590	if (a->n * a->m != 3 \|\| b->n * b->m != 3) {	×
591	pb_assert(PBIO_ERROR_INVALID_ARG);	×
592	}
593
594	// Create c vector.
595	pb_type_Matrix_obj_t *c = mp_obj_malloc(pb_type_Matrix_obj_t, &pb_type_Matrix);	×
596	c->m = 3;	×
597	c->n = 1;	×
598	c->data = m_new(float, 3);	×
599
600	// Evaluate cross product
601	c->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1];	×
602	c->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2];	×
603	c->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0];	×
604	c->scale = a->scale * b->scale;	×
605
606	return MP_OBJ_FROM_PTR(c);	×
607	}
608	MP_DEFINE_CONST_FUN_OBJ_2(pb_type_matrix_cross_obj, pb_type_matrix_cross);
609
610	#endif // MICROPY_PY_BUILTINS_FLOAT

pybricks / pybricks-micropython / 19016791193

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous