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

/lib/pbio/src/geometry.c

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

#include <assert.h>

#include <math.h>

#include <pbio/geometry.h>

/**
 * Gets @p index and @p sign of the axis that passes through given @p side of
 * a box.
 * @param [in]  side    The requested side of the box.
 * @param [out] index   The index 0 (x), 1 (y), or 2 (z) of the axis.
 * @param [out] sign    The sign of the axis: 1 or -1.
 */
void pbio_geometry_side_get_axis(pbio_geometry_side_t side, uint8_t *index, int8_t *sign) {
    *index = side & 0x03;
    *sign = (side & 0x04) ? -1 : 1;
}

/**
 * Given index and sign of two base axes, finds the axis to complete a right
 * handed coordinate system.
 * @param [in] index   Array of three axis indexes. First and last are given,
 *                     middle axis index is computed.
 * @param [in] sign    Array of three axis signs. First and last are given,
 *                     middle axis sign is computed.
 */
void pbio_geometry_get_complementary_axis(uint8_t *index, int8_t *sign) {

    // Inputs must have valid axis index and sign.
    assert(index[0] < 3);
    assert(index[2] < 3);
    assert(sign[0] == 1 || sign[2] == 1);

    // Two axis cannot be parallel.
    assert(index[2] != index[0]);

    // The final axis is determined as the cross product v_1 = v_2 (x) v_0.
    // Since we know the axes are just base axis up to a sign, the result C
    // simply has the index that isn't used yet:
    index[1] = 3 - index[2] - index[0];

    // We still have so evaluate the cross product to get the sign. With inputs
    // j = {0, 2}, both input vectors v_0 and v_2 can be written in
    // terms of known information as:
    //
    //                 [ i_j == 0 ]
    //    v_j = s_j *  [ i_j == 1 ]
    //                 [ i_j == 2 ]
    //
    // where s_j is the sign of vector j, and i_j is the axis index of j.
    //
    // For example, if the first input has sign -1 and index 2, it is
    //
    //         [ 0]
    //   v_0 = [ 0]
    //         [-1]
    //
    // So it is the negative Z vector.
    //
    // Given the vectors v_0 and v_2, we evaluate the cross product formula:
    //
    // v_1 = + s_0 * s_2 * ((i_2==1)*(i_0==2)-(i_0==1)*(i_2==2)) * E_0
    //       - s_0 * s_2 * ((i_2==0)*(i_0==2)-(i_0==0)*(i_2==2)) * E_1
    //       + s_0 * s_2 * ((i_2==0)*(i_0==1)-(i_0==0)*(i_2==1)) * E_2
    //
    // But we already know which of these three vectors is nonzero, so we just
    // need to take the scalar value in front of it. This leaves us with one of
    // three results. Since the formulas are so similar, we can generalize it
    // to one result that depends on the known final axis index. This gives:
    sign[1] = sign[0] * sign[2] * (
        (index[0] == (index[1] + 2) % 3) * (index[2] == (index[1] + 1) % 3) -
        (index[0] == (index[1] + 1) % 3) * (index[2] == (index[1] + 2) % 3)
        );
}

/**
 * Gets the side of a unit-sized box through which a given vector passes first.
 *
 * @param [in]  vector  The input vector.
 * @return              The side through which the vector passes first.
 */
pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) {

    // Find index and sign of maximum component
    float abs_max = 0;
    uint8_t axis = 0;
    bool negative = true;
    for (uint8_t i = 0; i < 3; i++) {
        if (vector->values[i] > abs_max) {
            abs_max = vector->values[i];
            negative = false;
            axis = i;
        } else if (-vector->values[i] > abs_max) {
            abs_max = -vector->values[i];
            negative = true;
            axis = i;
        }
    }

    // Return as side enum value.
    return axis | (negative << 2);
}

/**
 * Gets the norm of a vector.
 *
 * @param [in]  input   The vector.
 * @return              The norm of the vector.
 */
float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input) {
    return sqrtf(input->x * input->x + input->y * input->y + input->z * input->z);
}

/**
 * Normalizes a vector so it has unit length.
 *
 * Output is allowed to be same as input, in which case input is normalized.
 *
 * @param [in]  input   The vector to normalize.
 * @param [out] output  The normalized vector.
 * @return              ::PBIO_ERROR_INVALID_ARG if the input has zero length, otherwise ::PBIO_SUCCESS.
 */
pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) {

    // Compute the norm.
    float norm = pbio_geometry_vector_norm(input);

    // If the vector norm is zero, do nothing.
    if (norm == 0.0f) {
        return PBIO_ERROR_INVALID_ARG;
    }

    // Otherwise, normalize.
    output->x = input->x / norm;
    output->y = input->y / norm;
    output->z = input->z / norm;
    return PBIO_SUCCESS;
}

/**
 * Gets the cross product of two vectors.
 *
 * @param [in]  a       The first vector.
 * @param [in]  b       The first vector.
 * @param [out] output  The result.
 */
void pbio_geometry_vector_cross_product(pbio_geometry_xyz_t *a, pbio_geometry_xyz_t *b, pbio_geometry_xyz_t *output) {
    output->x = a->y * b->z - a->z * b->y;
    output->y = a->z * b->x - a->x * b->z;
    output->z = a->x * b->y - a->y * b->x;
}

/**
 * Gets the scalar projection of one vector onto the line spanned by another.
 *
 * @param [in]  axis        The axis on which to project.
 * @param [in]  input       The input vector.
 * @param [out] projection  The projection.
 * @return                  ::PBIO_ERROR_INVALID_ARG if the axis has zero length, otherwise ::PBIO_SUCCESS.
 */
pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t *axis, pbio_geometry_xyz_t *input, float *projection) {

    // Normalize the given axis so its magnitude does not matter.
    pbio_geometry_xyz_t unit_axis;
    pbio_error_t err = pbio_geometry_vector_normalize(axis, &unit_axis);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    // Compute the projection.
    *projection = unit_axis.x * input->x + unit_axis.y * input->y + unit_axis.z * input->z;
    return PBIO_SUCCESS;
}

/**
 * Maps the input vector using: output = map * input
 *
 * @param [in]  map     The 3x3 map, such as a rotation matrix.
 * @param [in]  input   The input vector
 * @param [out] output  The resulting output vector.
 */
void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t *map, pbio_geometry_xyz_t *input, pbio_geometry_xyz_t *output) {
    output->x = input->x * map->m11 + input->y * map->m12 + input->z * map->m13;
    output->y = input->x * map->m21 + input->y * map->m22 + input->z * map->m23;
    output->z = input->x * map->m31 + input->y * map->m32 + input->z * map->m33;
}

/**
 * Multiplies two 3x3 matrices.
 *
 * @param [in]  a       The first 3x3 matrix.
 * @param [in]  b       The second 3x3 matrix.
 * @param [out] output  The resulting 3x3 matrix after multiplication.
 */
void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t *a, pbio_geometry_matrix_3x3_t *b, pbio_geometry_matrix_3x3_t *output) {
    output->m11 = a->m11 * b->m11 + a->m12 * b->m21 + a->m13 * b->m31;
    output->m12 = a->m11 * b->m12 + a->m12 * b->m22 + a->m13 * b->m32;
    output->m13 = a->m11 * b->m13 + a->m12 * b->m23 + a->m13 * b->m33;
    output->m21 = a->m21 * b->m11 + a->m22 * b->m21 + a->m23 * b->m31;
    output->m22 = a->m21 * b->m12 + a->m22 * b->m22 + a->m23 * b->m32;
    output->m23 = a->m21 * b->m13 + a->m22 * b->m23 + a->m23 * b->m33;
    output->m31 = a->m31 * b->m11 + a->m32 * b->m21 + a->m33 * b->m31;
    output->m32 = a->m31 * b->m12 + a->m32 * b->m22 + a->m33 * b->m32;
    output->m33 = a->m31 * b->m13 + a->m32 * b->m23 + a->m33 * b->m33;
}

/**
 * Gets a mapping (a rotation matrix) from two orthogonal base axes.
 *
 * @param [in]  x_axis  The X axis. Need not be normalized.
 * @param [in]  z_axis  The Z axis. Need not be normalized.
 * @param [out] map     The completed map, including the computed y_axis.
 * @return              ::PBIO_ERROR_INVALID_ARG if the axes are zero or not orthogonal, otherwise ::PBIO_SUCCESS.
 */
pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t *x_axis, pbio_geometry_xyz_t *z_axis, pbio_geometry_matrix_3x3_t *map) {

    pbio_geometry_xyz_t x_axis_normal;
    pbio_error_t err = pbio_geometry_vector_normalize(x_axis, &x_axis_normal);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    pbio_geometry_xyz_t z_axis_normal;
    err = pbio_geometry_vector_normalize(z_axis, &z_axis_normal);
    if (err != PBIO_SUCCESS) {
        return err;
    }

    // Assert that X and Z are orthogonal.
    float inner_product = x_axis_normal.x * z_axis_normal.x + x_axis_normal.y * z_axis_normal.y + x_axis_normal.z * z_axis_normal.z;
    if (inner_product > 0.001f || inner_product < -0.001f) {
        return PBIO_ERROR_INVALID_ARG;
    }

    pbio_geometry_xyz_t y_axis_normal;
    pbio_geometry_vector_cross_product(&z_axis_normal, &x_axis_normal, &y_axis_normal);

    // Build the resulting matrix as [x_axis y_axis z_axis]
    *map = (pbio_geometry_matrix_3x3_t) {
        .m11 = x_axis_normal.x, .m12 = y_axis_normal.x, .m13 = z_axis_normal.x,
        .m21 = x_axis_normal.y, .m22 = y_axis_normal.y, .m23 = z_axis_normal.y,
        .m31 = x_axis_normal.z, .m32 = y_axis_normal.z, .m33 = z_axis_normal.z,
    };

    return PBIO_SUCCESS;
}

/**
 * Computes a rotation matrix for a quaternion.
 *
 * @param [in]  q       The quaternion.
 * @param [out] R       The rotation matrix.
 */
void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t *q, pbio_geometry_matrix_3x3_t *R) {
    R->m11 = 1 - 2 * (q->q2 * q->q2 + q->q3 * q->q3);
    R->m21 = 2 * (q->q1 * q->q2 + q->q3 * q->q4);
    R->m31 = 2 * (q->q1 * q->q3 - q->q2 * q->q4);
    R->m12 = 2 * (q->q1 * q->q2 - q->q3 * q->q4);
    R->m22 = 1 - 2 * (q->q1 * q->q1 + q->q3 * q->q3);
    R->m32 = 2 * (q->q2 * q->q3 + q->q1 * q->q4);
    R->m13 = 2 * (q->q1 * q->q3 + q->q2 * q->q4);
    R->m23 = 2 * (q->q2 * q->q3 - q->q1 * q->q4);
    R->m33 = 1 - 2 * (q->q1 * q->q1 + q->q2 * q->q2);
}

/**
 * Computes a quaternion from a gravity unit vector.
 *
 * @param [in]     g  The gravity unit vector.
 * @param [out]    q  The resulting quaternion.
 */
void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t *g, pbio_geometry_quaternion_t *q) {
    if (g->z >= 0) {
        q->q4 = sqrtf((g->z + 1) / 2);
        q->q1 = g->y / sqrtf(2 * (g->z + 1));
        q->q2 = -g->x / sqrtf(2 * (g->z + 1));
        q->q3 = 0;
    } else {
        q->q4 = -g->y / sqrtf(2 * (1 - g->z));
        q->q1 = -sqrtf((1 - g->z) / 2);
        q->q2 = 0;
        q->q3 = -g->x / sqrtf(2 * (1 - g->z));
    }
}

/**
 * Computes the rate of change of a quaternion given the angular velocity vector.
 *
 * @param [in]     q                 Quaternion of the current orientation.
 * @param [in]     angular_velocity  The angular velocity vector in the body frame.
 * @param [out]    dq                The rate of change of the quaternion.
 */
void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t *q, pbio_geometry_xyz_t *angular_velocity, pbio_geometry_quaternion_t *dq) {

    pbio_geometry_xyz_t w = {
        .x = pbio_geometry_degrees_to_radians(angular_velocity->x),
        .y = pbio_geometry_degrees_to_radians(angular_velocity->y),
        .z = pbio_geometry_degrees_to_radians(angular_velocity->z),
    };

    dq->q1 = 0.5f * (w.z * q->q2 - w.y * q->q3 + w.x * q->q4);
    dq->q2 = 0.5f * (-w.z * q->q1 + w.x * q->q3 + w.y * q->q4);
    dq->q3 = 0.5f * (w.y * q->q1 - w.x * q->q2 + w.z * q->q4);
    dq->q4 = 0.5f * (-w.x * q->q1 - w.y * q->q2 - w.z * q->q3);
}

/**
 * Normalizes a quaternion so it has unit length.
 *
 * @param [inout] q  The quaternion to normalize.
 */
void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q) {
    float norm = sqrtf(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3 + q->q4 * q->q4);

    if (norm < 0.0001f && norm > -0.0001f) {
        return;
    }

    q->q1 /= norm;
    q->q2 /= norm;
    q->q3 /= norm;
    q->q4 /= norm;
}

/**
 * Returns the maximum of two floating-point numbers.
 *
 * @param a The first floating-point number.
 * @param b The second floating-point number.
 * @return  The maximum of the two floating-point numbers.
 */
float pbio_geometry_maxf(float a, float b) {
    return a > b ? a : b;
}

/**
 * Returns the absolute value of a floating-point number.
 *
 * @param a The floating-point number.
 * @return  The absolute value of the floating-point number.
 */
float pbio_geometry_absf(float a) {
    return a < 0 ? -a : a;
}

1	// SPDX-License-Identifier: MIT
2	// Copyright (c) 2023 The Pybricks Authors
3
4	#include <assert.h>
5
6	#include <math.h>
7
8	#include <pbio/geometry.h>
9
10	/**
11	* Gets @p index and @p sign of the axis that passes through given @p side of
12	* a box.
13	* @param [in] side The requested side of the box.
14	* @param [out] index The index 0 (x), 1 (y), or 2 (z) of the axis.
15	* @param [out] sign The sign of the axis: 1 or -1.
16	*/
17	void pbio_geometry_side_get_axis(pbio_geometry_side_t side, uint8_t index, int8_t sign) {	×
18	*index = side & 0x03;	×
19	*sign = (side & 0x04) ? -1 : 1;	×
20	}	×
21
22	/**
23	* Given index and sign of two base axes, finds the axis to complete a right
24	* handed coordinate system.
25	* @param [in] index Array of three axis indexes. First and last are given,
26	* middle axis index is computed.
27	* @param [in] sign Array of three axis signs. First and last are given,
28	* middle axis sign is computed.
29	*/
30	void pbio_geometry_get_complementary_axis(uint8_t index, int8_t sign) {	×
31
32	// Inputs must have valid axis index and sign.
33	assert(index[0] < 3);	×
34	assert(index[2] < 3);	×
35	assert(sign[0] == 1 \|\| sign[2] == 1);	×
36
37	// Two axis cannot be parallel.
38	assert(index[2] != index[0]);	×
39
40	// The final axis is determined as the cross product v_1 = v_2 (x) v_0.
41	// Since we know the axes are just base axis up to a sign, the result C
42	// simply has the index that isn't used yet:
43	index[1] = 3 - index[2] - index[0];	×
44
45	// We still have so evaluate the cross product to get the sign. With inputs
46	// j = {0, 2}, both input vectors v_0 and v_2 can be written in
47	// terms of known information as:
48	//
49	// [ i_j == 0 ]
50	// v_j = s_j * [ i_j == 1 ]
51	// [ i_j == 2 ]
52	//
53	// where s_j is the sign of vector j, and i_j is the axis index of j.
54	//
55	// For example, if the first input has sign -1 and index 2, it is
56	//
57	// [ 0]
58	// v_0 = [ 0]
59	// [-1]
60	//
61	// So it is the negative Z vector.
62	//
63	// Given the vectors v_0 and v_2, we evaluate the cross product formula:
64	//
65	// v_1 = + s_0 * s_2 * ((i_2==1)(i_0==2)-(i_0==1)(i_2==2)) * E_0
66	// - s_0 * s_2 * ((i_2==0)(i_0==2)-(i_0==0)(i_2==2)) * E_1
67	// + s_0 * s_2 * ((i_2==0)(i_0==1)-(i_0==0)(i_2==1)) * E_2
68	//
69	// But we already know which of these three vectors is nonzero, so we just
70	// need to take the scalar value in front of it. This leaves us with one of
71	// three results. Since the formulas are so similar, we can generalize it
72	// to one result that depends on the known final axis index. This gives:
73	sign[1] = sign[0] * sign[2] * (	×
74	(index[0] == (index[1] + 2) % 3) * (index[2] == (index[1] + 1) % 3) -	×
75	(index[0] == (index[1] + 1) % 3) * (index[2] == (index[1] + 2) % 3)	×
76	);
77	}	×
78
79	/**
80	* Gets the side of a unit-sized box through which a given vector passes first.
81	*
82	* @param [in] vector The input vector.
83	* @return The side through which the vector passes first.
84	*/
85	pbio_geometry_side_t pbio_geometry_side_from_vector(pbio_geometry_xyz_t *vector) {	×
86
87	// Find index and sign of maximum component
88	float abs_max = 0;	×
89	uint8_t axis = 0;	×
90	bool negative = true;	×
91	for (uint8_t i = 0; i < 3; i++) {	×
92	if (vector->values[i] > abs_max) {	×
UNCOV 93	abs_max = vector->values[i];	×
UNCOV 94	negative = false;	×
UNCOV 95	axis = i;	×
96	} else if (-vector->values[i] > abs_max) {	×
97	abs_max = -vector->values[i];	×
98	negative = true;	×
99	axis = i;	×
100	}
101	}
102
103	// Return as side enum value.
104	return axis \| (negative << 2);	×
105	}
106
107	/**
108	* Gets the norm of a vector.
109	*
110	* @param [in] input The vector.
111	* @return The norm of the vector.
112	*/
113	float pbio_geometry_vector_norm(pbio_geometry_xyz_t *input) {	×
114	return sqrtf(input->x * input->x + input->y * input->y + input->z * input->z);	×
115	}
116
117	/**
118	* Normalizes a vector so it has unit length.
119	*
120	* Output is allowed to be same as input, in which case input is normalized.
121	*
122	* @param [in] input The vector to normalize.
123	* @param [out] output The normalized vector.
124	* @return ::PBIO_ERROR_INVALID_ARG if the input has zero length, otherwise ::PBIO_SUCCESS.
125	*/
126	pbio_error_t pbio_geometry_vector_normalize(pbio_geometry_xyz_t input, pbio_geometry_xyz_t output) {	×
127
128	// Compute the norm.
129	float norm = pbio_geometry_vector_norm(input);	×
130
131	// If the vector norm is zero, do nothing.
132	if (norm == 0.0f) {	×
UNCOV 133	return PBIO_ERROR_INVALID_ARG;	×
134	}
135
136	// Otherwise, normalize.
137	output->x = input->x / norm;	×
138	output->y = input->y / norm;	×
139	output->z = input->z / norm;	×
140	return PBIO_SUCCESS;	×
141	}
142
143	/**
144	* Gets the cross product of two vectors.
145	*
146	* @param [in] a The first vector.
147	* @param [in] b The first vector.
148	* @param [out] output The result.
149	*/
150	void pbio_geometry_vector_cross_product(pbio_geometry_xyz_t a, pbio_geometry_xyz_t b, pbio_geometry_xyz_t *output) {	×
151	output->x = a->y * b->z - a->z * b->y;	×
152	output->y = a->z * b->x - a->x * b->z;	×
153	output->z = a->x * b->y - a->y * b->x;	×
154	}	×
155
156	/**
157	* Gets the scalar projection of one vector onto the line spanned by another.
158	*
159	* @param [in] axis The axis on which to project.
160	* @param [in] input The input vector.
161	* @param [out] projection The projection.
162	* @return ::PBIO_ERROR_INVALID_ARG if the axis has zero length, otherwise ::PBIO_SUCCESS.
163	*/
164	pbio_error_t pbio_geometry_vector_project(pbio_geometry_xyz_t axis, pbio_geometry_xyz_t input, float *projection) {	×
165
166	// Normalize the given axis so its magnitude does not matter.
167	pbio_geometry_xyz_t unit_axis;
168	pbio_error_t err = pbio_geometry_vector_normalize(axis, &unit_axis);	×
169	if (err != PBIO_SUCCESS) {	×
UNCOV 170	return err;	×
171	}
172
173	// Compute the projection.
174	projection = unit_axis.x input->x + unit_axis.y * input->y + unit_axis.z * input->z;	×
175	return PBIO_SUCCESS;	×
176	}
177
178	/**
179	* Maps the input vector using: output = map * input
180	*
181	* @param [in] map The 3x3 map, such as a rotation matrix.
182	* @param [in] input The input vector
183	* @param [out] output The resulting output vector.
184	*/
185	void pbio_geometry_vector_map(pbio_geometry_matrix_3x3_t map, pbio_geometry_xyz_t input, pbio_geometry_xyz_t *output) {	×
186	output->x = input->x * map->m11 + input->y * map->m12 + input->z * map->m13;	×
187	output->y = input->x * map->m21 + input->y * map->m22 + input->z * map->m23;	×
188	output->z = input->x * map->m31 + input->y * map->m32 + input->z * map->m33;	×
189	}	×
190
191	/**
192	* Multiplies two 3x3 matrices.
193	*
194	* @param [in] a The first 3x3 matrix.
195	* @param [in] b The second 3x3 matrix.
196	* @param [out] output The resulting 3x3 matrix after multiplication.
197	*/
198	void pbio_geometry_matrix_multiply(pbio_geometry_matrix_3x3_t a, pbio_geometry_matrix_3x3_t b, pbio_geometry_matrix_3x3_t *output) {	×
199	output->m11 = a->m11 * b->m11 + a->m12 * b->m21 + a->m13 * b->m31;	×
200	output->m12 = a->m11 * b->m12 + a->m12 * b->m22 + a->m13 * b->m32;	×
201	output->m13 = a->m11 * b->m13 + a->m12 * b->m23 + a->m13 * b->m33;	×
202	output->m21 = a->m21 * b->m11 + a->m22 * b->m21 + a->m23 * b->m31;	×
203	output->m22 = a->m21 * b->m12 + a->m22 * b->m22 + a->m23 * b->m32;	×
204	output->m23 = a->m21 * b->m13 + a->m22 * b->m23 + a->m23 * b->m33;	×
205	output->m31 = a->m31 * b->m11 + a->m32 * b->m21 + a->m33 * b->m31;	×
206	output->m32 = a->m31 * b->m12 + a->m32 * b->m22 + a->m33 * b->m32;	×
207	output->m33 = a->m31 * b->m13 + a->m32 * b->m23 + a->m33 * b->m33;	×
208	}	×
209
210	/**
211	* Gets a mapping (a rotation matrix) from two orthogonal base axes.
212	*
213	* @param [in] x_axis The X axis. Need not be normalized.
214	* @param [in] z_axis The Z axis. Need not be normalized.
215	* @param [out] map The completed map, including the computed y_axis.
216	* @return ::PBIO_ERROR_INVALID_ARG if the axes are zero or not orthogonal, otherwise ::PBIO_SUCCESS.
217	*/
218	pbio_error_t pbio_geometry_map_from_base_axes(pbio_geometry_xyz_t x_axis, pbio_geometry_xyz_t z_axis, pbio_geometry_matrix_3x3_t *map) {	×
219
220	pbio_geometry_xyz_t x_axis_normal;
221	pbio_error_t err = pbio_geometry_vector_normalize(x_axis, &x_axis_normal);	×
222	if (err != PBIO_SUCCESS) {	×
UNCOV 223	return err;	×
224	}
225
226	pbio_geometry_xyz_t z_axis_normal;
227	err = pbio_geometry_vector_normalize(z_axis, &z_axis_normal);	×
228	if (err != PBIO_SUCCESS) {	×
UNCOV 229	return err;	×
230	}
231
232	// Assert that X and Z are orthogonal.
233	float inner_product = x_axis_normal.x * z_axis_normal.x + x_axis_normal.y * z_axis_normal.y + x_axis_normal.z * z_axis_normal.z;	×
234	if (inner_product > 0.001f \|\| inner_product < -0.001f) {	×
UNCOV 235	return PBIO_ERROR_INVALID_ARG;	×
236	}
237
238	pbio_geometry_xyz_t y_axis_normal;
239	pbio_geometry_vector_cross_product(&z_axis_normal, &x_axis_normal, &y_axis_normal);	×
240
241	// Build the resulting matrix as [x_axis y_axis z_axis]
242	*map = (pbio_geometry_matrix_3x3_t) {	×
243	.m11 = x_axis_normal.x, .m12 = y_axis_normal.x, .m13 = z_axis_normal.x,	×
244	.m21 = x_axis_normal.y, .m22 = y_axis_normal.y, .m23 = z_axis_normal.y,	×
245	.m31 = x_axis_normal.z, .m32 = y_axis_normal.z, .m33 = z_axis_normal.z,	×
246	};
247
248	return PBIO_SUCCESS;	×
249	}
250
251	/**
252	* Computes a rotation matrix for a quaternion.
253	*
254	* @param [in] q The quaternion.
255	* @param [out] R The rotation matrix.
256	*/
257	void pbio_geometry_quaternion_to_rotation_matrix(pbio_geometry_quaternion_t q, pbio_geometry_matrix_3x3_t R) {	×
258	R->m11 = 1 - 2 * (q->q2 * q->q2 + q->q3 * q->q3);	×
259	R->m21 = 2 * (q->q1 * q->q2 + q->q3 * q->q4);	×
260	R->m31 = 2 * (q->q1 * q->q3 - q->q2 * q->q4);	×
261	R->m12 = 2 * (q->q1 * q->q2 - q->q3 * q->q4);	×
262	R->m22 = 1 - 2 * (q->q1 * q->q1 + q->q3 * q->q3);	×
263	R->m32 = 2 * (q->q2 * q->q3 + q->q1 * q->q4);	×
264	R->m13 = 2 * (q->q1 * q->q3 + q->q2 * q->q4);	×
265	R->m23 = 2 * (q->q2 * q->q3 - q->q1 * q->q4);	×
266	R->m33 = 1 - 2 * (q->q1 * q->q1 + q->q2 * q->q2);	×
267	}	×
268
269	/**
270	* Computes a quaternion from a gravity unit vector.
271	*
272	* @param [in] g The gravity unit vector.
273	* @param [out] q The resulting quaternion.
274	*/
275	void pbio_geometry_quaternion_from_gravity_unit_vector(pbio_geometry_xyz_t g, pbio_geometry_quaternion_t q) {	×
276	if (g->z >= 0) {	×
277	q->q4 = sqrtf((g->z + 1) / 2);	×
278	q->q1 = g->y / sqrtf(2 * (g->z + 1));	×
279	q->q2 = -g->x / sqrtf(2 * (g->z + 1));	×
280	q->q3 = 0;	×
281	} else {
282	q->q4 = -g->y / sqrtf(2 * (1 - g->z));	×
283	q->q1 = -sqrtf((1 - g->z) / 2);	×
284	q->q2 = 0;	×
285	q->q3 = -g->x / sqrtf(2 * (1 - g->z));	×
286	}
287	}	×
288
289	/**
290	* Computes the rate of change of a quaternion given the angular velocity vector.
291	*
292	* @param [in] q Quaternion of the current orientation.
293	* @param [in] angular_velocity The angular velocity vector in the body frame.
294	* @param [out] dq The rate of change of the quaternion.
295	*/
296	void pbio_geometry_quaternion_get_rate_of_change(pbio_geometry_quaternion_t q, pbio_geometry_xyz_t angular_velocity, pbio_geometry_quaternion_t *dq) {	×
297
298	pbio_geometry_xyz_t w = {	×
299	.x = pbio_geometry_degrees_to_radians(angular_velocity->x),	×
300	.y = pbio_geometry_degrees_to_radians(angular_velocity->y),	×
301	.z = pbio_geometry_degrees_to_radians(angular_velocity->z),	×
302	};
303
304	dq->q1 = 0.5f * (w.z * q->q2 - w.y * q->q3 + w.x * q->q4);	×
305	dq->q2 = 0.5f * (-w.z * q->q1 + w.x * q->q3 + w.y * q->q4);	×
306	dq->q3 = 0.5f * (w.y * q->q1 - w.x * q->q2 + w.z * q->q4);	×
307	dq->q4 = 0.5f * (-w.x * q->q1 - w.y * q->q2 - w.z * q->q3);	×
308	}	×
309
310	/**
311	* Normalizes a quaternion so it has unit length.
312	*
313	* @param [inout] q The quaternion to normalize.
314	*/
315	void pbio_geometry_quaternion_normalize(pbio_geometry_quaternion_t *q) {	×
316	float norm = sqrtf(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3 + q->q4 * q->q4);	×
317
318	if (norm < 0.0001f && norm > -0.0001f) {	×
UNCOV 319	return;	×
320	}
321
322	q->q1 /= norm;	×
323	q->q2 /= norm;	×
324	q->q3 /= norm;	×
325	q->q4 /= norm;	×
326	}
327
328	/**
329	* Returns the maximum of two floating-point numbers.
330	*
331	* @param a The first floating-point number.
332	* @param b The second floating-point number.
333	* @return The maximum of the two floating-point numbers.
334	*/
335	float pbio_geometry_maxf(float a, float b) {	×
336	return a > b ? a : b;	×
337	}
338
339	/**
340	* Returns the absolute value of a floating-point number.
341	*
342	* @param a The floating-point number.
343	* @return The absolute value of the floating-point number.
344	*/
345	float pbio_geometry_absf(float a) {	×
346	return a < 0 ? -a : a;	×
347	}

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