mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
646 lines
14 KiB
646 lines
14 KiB
/* |
|
Copyright (c) 2012, Broadcom Europe Ltd |
|
All rights reserved. |
|
|
|
Redistribution and use in source and binary forms, with or without |
|
modification, are permitted provided that the following conditions are met: |
|
* Redistributions of source code must retain the above copyright |
|
notice, this list of conditions and the following disclaimer. |
|
* Redistributions in binary form must reproduce the above copyright |
|
notice, this list of conditions and the following disclaimer in the |
|
documentation and/or other materials provided with the distribution. |
|
* Neither the name of the copyright holder nor the |
|
names of its contributors may be used to endorse or promote products |
|
derived from this software without specific prior written permission. |
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY |
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
|
*/ |
|
|
|
#include "interface/khronos/common/khrn_int_common.h" |
|
#include "interface/khronos/vg/vg_int_mat3x3.h" |
|
#include "interface/khronos/common/khrn_int_math.h" |
|
#include "interface/khronos/common/khrn_int_util.h" |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is the identity matrix. |
|
*/ |
|
|
|
void vg_mat3x3_set_identity(VG_MAT3X3_T *a) |
|
{ |
|
a->m[0][0] = 1.0f; a->m[0][1] = 0.0f; a->m[0][2] = 0.0f; |
|
a->m[1][0] = 0.0f; a->m[1][1] = 1.0f; a->m[1][2] = 0.0f; |
|
a->m[2][0] = 0.0f; a->m[2][1] = 0.0f; a->m[2][2] = 1.0f; |
|
} |
|
|
|
/* |
|
copies matrix (column-major) into a, "cleaning" the elements (see |
|
clean_float). forces the bottom row of a to (0, 0, 1) if force_affine is |
|
true. |
|
|
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
no elements of a are nan or infinity. |
|
if force_affine is true, a is affine. |
|
*/ |
|
|
|
void vg_mat3x3_set_clean(VG_MAT3X3_T *a, const float *matrix, bool force_affine) |
|
{ |
|
a->m[0][0] = clean_float(matrix[0]); |
|
a->m[0][1] = clean_float(matrix[3]); |
|
a->m[0][2] = clean_float(matrix[6]); |
|
|
|
a->m[1][0] = clean_float(matrix[1]); |
|
a->m[1][1] = clean_float(matrix[4]); |
|
a->m[1][2] = clean_float(matrix[7]); |
|
|
|
if (force_affine) { |
|
a->m[2][0] = 0.0f; |
|
a->m[2][1] = 0.0f; |
|
a->m[2][2] = 1.0f; |
|
} else { |
|
a->m[2][0] = clean_float(matrix[2]); |
|
a->m[2][1] = clean_float(matrix[5]); |
|
a->m[2][2] = clean_float(matrix[8]); |
|
} |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
matrix (column-major) is set to a. |
|
*/ |
|
|
|
void vg_mat3x3_get(const VG_MAT3X3_T *a, float *matrix) |
|
{ |
|
matrix[0] = a->m[0][0]; |
|
matrix[3] = a->m[0][1]; |
|
matrix[6] = a->m[0][2]; |
|
|
|
matrix[1] = a->m[1][0]; |
|
matrix[4] = a->m[1][1]; |
|
matrix[7] = a->m[1][2]; |
|
|
|
matrix[2] = a->m[2][0]; |
|
matrix[5] = a->m[2][1]; |
|
matrix[8] = a->m[2][2]; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
returns true iff a and b are bitwise identical. |
|
*/ |
|
|
|
bool vg_mat3x3_identical(const VG_MAT3X3_T *a, const VG_MAT3X3_T *b) |
|
{ |
|
return floats_identical(a->m[0][0], b->m[0][0]) && floats_identical(a->m[0][1], b->m[0][1]) && floats_identical(a->m[0][2], b->m[0][2]) && |
|
floats_identical(a->m[1][0], b->m[1][0]) && floats_identical(a->m[1][1], b->m[1][1]) && floats_identical(a->m[1][2], b->m[1][2]) && |
|
floats_identical(a->m[2][0], b->m[2][0]) && floats_identical(a->m[2][1], b->m[2][1]) && floats_identical(a->m[2][2], b->m[2][2]); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
a does not point to the same matrix as b or c. |
|
|
|
Postconditions: |
|
|
|
a is set to b * c. |
|
*/ |
|
|
|
void vg_mat3x3_mul(VG_MAT3X3_T *a, const VG_MAT3X3_T *b, const VG_MAT3X3_T *c) |
|
{ |
|
uint32_t j, i; |
|
for (j = 0; j != 3; ++j) { |
|
for (i = 0; i != 3; ++i) { |
|
a->m[j][i] = |
|
(b->m[j][0] * c->m[0][i]) + |
|
(b->m[j][1] * c->m[1][i]) + |
|
(b->m[j][2] * c->m[2][i]); |
|
} |
|
} |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to a * translation_matrix(x, y). |
|
*/ |
|
|
|
void vg_mat3x3_postmul_translate(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
a->m[0][2] += (a->m[0][0] * x) + (a->m[0][1] * y); |
|
a->m[1][2] += (a->m[1][0] * x) + (a->m[1][1] * y); |
|
a->m[2][2] += (a->m[2][0] * x) + (a->m[2][1] * y); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to a * scale_matrix(x, y). |
|
*/ |
|
|
|
void vg_mat3x3_postmul_scale(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
a->m[0][0] *= x; |
|
a->m[0][1] *= y; |
|
|
|
a->m[1][0] *= x; |
|
a->m[1][1] *= y; |
|
|
|
a->m[2][0] *= x; |
|
a->m[2][1] *= y; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to a * shear_matrix(x, y). |
|
*/ |
|
|
|
void vg_mat3x3_postmul_shear(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
float m00 = a->m[0][0], m10 = a->m[1][0], m20 = a->m[2][0]; |
|
|
|
a->m[0][0] += a->m[0][1] * y; |
|
a->m[0][1] += m00 * x; |
|
|
|
a->m[1][0] += a->m[1][1] * y; |
|
a->m[1][1] += m10 * x; |
|
|
|
a->m[2][0] += a->m[2][1] * y; |
|
a->m[2][1] += m20 * x; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
angle is in radians. |
|
|
|
Postconditions: |
|
|
|
a is set to a * rotation_matrix(angle). |
|
*/ |
|
|
|
void vg_mat3x3_postmul_rotate(VG_MAT3X3_T *a, float angle) |
|
{ |
|
float s, c; |
|
sin_cos_(&s, &c, angle); |
|
vg_mat3x3_postmul_rotate_sc(a, s, c); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
there is some angle such that: |
|
s = sin(angle) |
|
c = cos(angle) |
|
|
|
Postconditions: |
|
|
|
a is set to a * rotation_matrix(angle). |
|
*/ |
|
|
|
void vg_mat3x3_postmul_rotate_sc(VG_MAT3X3_T *a, float s, float c) |
|
{ |
|
float m00 = a->m[0][0], m10 = a->m[1][0], m20 = a->m[2][0]; |
|
|
|
a->m[0][0] = (m00 * c) + (a->m[0][1] * s); |
|
a->m[0][1] = (a->m[0][1] * c) - (m00 * s); |
|
|
|
a->m[1][0] = (m10 * c) + (a->m[1][1] * s); |
|
a->m[1][1] = (a->m[1][1] * c) - (m10 * s); |
|
|
|
a->m[2][0] = (m20 * c) + (a->m[2][1] * s); |
|
a->m[2][1] = (a->m[2][1] * c) - (m20 * s); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to translation_matrix(x, y) * a. |
|
*/ |
|
|
|
void vg_mat3x3_premul_translate(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
a->m[0][0] += a->m[2][0] * x; |
|
a->m[0][1] += a->m[2][1] * x; |
|
a->m[0][2] += a->m[2][2] * x; |
|
|
|
a->m[1][0] += a->m[2][0] * y; |
|
a->m[1][1] += a->m[2][1] * y; |
|
a->m[1][2] += a->m[2][2] * y; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to scale_matrix(x, y) * a. |
|
*/ |
|
|
|
void vg_mat3x3_premul_scale(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
a->m[0][0] *= x; |
|
a->m[0][1] *= x; |
|
a->m[0][2] *= x; |
|
|
|
a->m[1][0] *= y; |
|
a->m[1][1] *= y; |
|
a->m[1][2] *= y; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
a is set to shear_matrix(x, y) * a. |
|
*/ |
|
|
|
void vg_mat3x3_premul_shear(VG_MAT3X3_T *a, float x, float y) |
|
{ |
|
float m00 = a->m[0][0], m01 = a->m[0][1], m02 = a->m[0][2]; |
|
|
|
a->m[0][0] += a->m[1][0] * x; |
|
a->m[0][1] += a->m[1][1] * x; |
|
a->m[0][2] += a->m[1][2] * x; |
|
|
|
a->m[1][0] += m00 * y; |
|
a->m[1][1] += m01 * y; |
|
a->m[1][2] += m02 * y; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
angle is in radians. |
|
|
|
Postconditions: |
|
|
|
a is set to rotation_matrix(angle) * a. |
|
*/ |
|
|
|
void vg_mat3x3_premul_rotate(VG_MAT3X3_T *a, float angle) |
|
{ |
|
float s, c; |
|
sin_cos_(&s, &c, angle); |
|
vg_mat3x3_premul_rotate_sc(a, s, c); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
there is some angle such that: |
|
s = sin(angle) |
|
c = cos(angle) |
|
|
|
Postconditions: |
|
|
|
a is set to rotation_matrix(angle) * a. |
|
*/ |
|
|
|
void vg_mat3x3_premul_rotate_sc(VG_MAT3X3_T *a, float s, float c) |
|
{ |
|
float m00 = a->m[0][0], m01 = a->m[0][1], m02 = a->m[0][2]; |
|
|
|
a->m[0][0] = (m00 * c) - (a->m[1][0] * s); |
|
a->m[0][1] = (m01 * c) - (a->m[1][1] * s); |
|
a->m[0][2] = (m02 * c) - (a->m[1][2] * s); |
|
|
|
a->m[1][0] = (m00 * s) + (a->m[1][0] * c); |
|
a->m[1][1] = (m01 * s) + (a->m[1][1] * c); |
|
a->m[1][2] = (m02 * s) + (a->m[1][2] * c); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
returns true iff a is affine. |
|
*/ |
|
|
|
bool vg_mat3x3_is_affine(const VG_MAT3X3_T *a) |
|
{ |
|
return (a->m[2][0] == 0.0f) && (a->m[2][1] == 0.0f) && (a->m[2][2] == 1.0f); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
returns true iff a is affine or has nans in the bad elements. |
|
*/ |
|
|
|
bool vg_mat3x3_is_affine_or_nans(const VG_MAT3X3_T *a) |
|
{ |
|
return !nan_ne_(a->m[2][0], 0.0f) && !nan_ne_(a->m[2][1], 0.0f) && !nan_ne_(a->m[2][2], 1.0f); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
returns the determinant of a. |
|
*/ |
|
|
|
float vg_mat3x3_det(const VG_MAT3X3_T *a) |
|
{ |
|
return (a->m[0][0] * ((a->m[2][2] * a->m[1][1]) - (a->m[2][1] * a->m[1][2]))) + |
|
(a->m[1][0] * ((a->m[0][2] * a->m[2][1]) - (a->m[0][1] * a->m[2][2]))) + |
|
(a->m[2][0] * ((a->m[1][2] * a->m[0][1]) - (a->m[1][1] * a->m[0][2]))); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
a must be affine (or have nans in the bad elements). |
|
|
|
Postconditions: |
|
|
|
returns the determinant of a. |
|
*/ |
|
|
|
float vg_mat3x3_affine_det(const VG_MAT3X3_T *a) |
|
{ |
|
vcos_assert(vg_mat3x3_is_affine_or_nans(a)); |
|
return (a->m[0][0] * a->m[1][1]) - (a->m[1][0] * a->m[0][1]); |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
returns false iff a is not invertible (or very close to not being |
|
invertible). |
|
*/ |
|
|
|
bool vg_mat3x3_is_invertible(const VG_MAT3X3_T *a) |
|
{ |
|
return absf_(vg_mat3x3_det(a)) >= EPS; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
a must be affine (or have nans in the bad elements). |
|
|
|
Postconditions: |
|
|
|
returns false iff a is not invertible (or very close to not being |
|
invertible). |
|
*/ |
|
|
|
bool vg_mat3x3_affine_is_invertible(const VG_MAT3X3_T *a) |
|
{ |
|
return absf_(vg_mat3x3_affine_det(a)) >= EPS; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
a must be invertible, according to vg_mat3x3_is_invertible. |
|
|
|
Postconditions: |
|
|
|
a is inverted. |
|
*/ |
|
|
|
void vg_mat3x3_invert(VG_MAT3X3_T *a) |
|
{ |
|
float oo_det; |
|
VG_MAT3X3_T b; |
|
|
|
vcos_assert(vg_mat3x3_is_invertible(a)); |
|
|
|
oo_det = recip_(vg_mat3x3_det(a)); |
|
|
|
b.m[0][0] = ((a->m[2][2] * a->m[1][1]) - (a->m[2][1] * a->m[1][2])) * oo_det; |
|
b.m[0][1] = ((a->m[0][2] * a->m[2][1]) - (a->m[0][1] * a->m[2][2])) * oo_det; |
|
b.m[0][2] = ((a->m[1][2] * a->m[0][1]) - (a->m[1][1] * a->m[0][2])) * oo_det; |
|
|
|
b.m[1][0] = ((a->m[2][0] * a->m[1][2]) - (a->m[2][2] * a->m[1][0])) * oo_det; |
|
b.m[1][1] = ((a->m[0][0] * a->m[2][2]) - (a->m[0][2] * a->m[2][0])) * oo_det; |
|
b.m[1][2] = ((a->m[1][0] * a->m[0][2]) - (a->m[1][2] * a->m[0][0])) * oo_det; |
|
|
|
b.m[2][0] = ((a->m[2][1] * a->m[1][0]) - (a->m[2][0] * a->m[1][1])) * oo_det; |
|
b.m[2][1] = ((a->m[0][1] * a->m[2][0]) - (a->m[0][0] * a->m[2][1])) * oo_det; |
|
b.m[2][2] = ((a->m[1][1] * a->m[0][0]) - (a->m[1][0] * a->m[0][1])) * oo_det; |
|
|
|
*a = b; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
a must be affine (or have nans in the bad elements). |
|
a must be invertible, according to vg_mat3x3_affine_is_invertible. |
|
|
|
Postconditions: |
|
|
|
a is inverted. |
|
*/ |
|
|
|
void vg_mat3x3_affine_invert(VG_MAT3X3_T *a) |
|
{ |
|
float oo_det; |
|
float m00, m01, m02, m10; |
|
|
|
vcos_assert(vg_mat3x3_affine_is_invertible(a)); |
|
|
|
oo_det = recip_(vg_mat3x3_affine_det(a)); |
|
m00 = a->m[0][0]; |
|
m01 = a->m[0][1]; |
|
m02 = a->m[0][2]; |
|
m10 = a->m[1][0]; |
|
|
|
a->m[0][0] = a->m[1][1] * oo_det; |
|
a->m[0][1] = -m01 * oo_det; |
|
a->m[0][2] = ((a->m[1][2] * m01) - (a->m[1][1] * m02)) * oo_det; |
|
|
|
a->m[1][0] = -m10 * oo_det; |
|
a->m[1][1] = m00 * oo_det; |
|
a->m[1][2] = ((m10 * m02) - (a->m[1][2] * m00)) * oo_det; |
|
|
|
a->m[2][0] = 0.0f; |
|
a->m[2][1] = 0.0f; |
|
a->m[2][2] = 1.0f; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
(x, y, -) is set to a * (x, y, 1). |
|
*/ |
|
|
|
void vg_mat3x3_affine_transform(const VG_MAT3X3_T *a, float *x_io, float *y_io) |
|
{ |
|
float x = (a->m[0][0] * *x_io) + (a->m[0][1] * *y_io) + a->m[0][2]; |
|
float y = (a->m[1][0] * *x_io) + (a->m[1][1] * *y_io) + a->m[1][2]; |
|
*x_io = x; |
|
*y_io = y; |
|
} |
|
|
|
/* |
|
Preconditions: |
|
|
|
- |
|
|
|
Postconditions: |
|
|
|
(x, y, -) is set to a * (x, y, 0). |
|
*/ |
|
|
|
void vg_mat3x3_affine_transform_t(const VG_MAT3X3_T *a, float *x_io, float *y_io) |
|
{ |
|
float x = (a->m[0][0] * *x_io) + (a->m[0][1] * *y_io); |
|
float y = (a->m[1][0] * *x_io) + (a->m[1][1] * *y_io); |
|
*x_io = x; |
|
*y_io = y; |
|
} |
|
|
|
/* |
|
the top-left 2x2 submatrix of a is decomposed into (R * S * Q), where R is a |
|
rotation matrix, S is a scale matrix, and Q is a rotation/flip matrix. note |
|
that this decomposition is not unique. |
|
|
|
Preconditions: |
|
|
|
r may be NULL. |
|
s0 may be NULL. |
|
s1 may be NULL. |
|
|
|
Postconditions: |
|
|
|
if r is non-NULL, it is set to the angle of R (so R = rotation_matrix(r)) in |
|
radians. |
|
if s0 is non-NULL, it is set to S[0][0]. |
|
if s1 is non-NULL, it is set to S[1][1]. |
|
so if we set both, S = scale_matrix(s0, s1). |
|
*/ |
|
|
|
void vg_mat3x3_rsq(const VG_MAT3X3_T *a, |
|
float *r, float *s0, float *s1) |
|
{ |
|
/* |
|
a = R * S * Q (svd, R is rotation, S is scale, Q is rotation/flip) |
|
a^T = Q^T * S * R^T |
|
a * a^T = R * S * Q * Q^T * S * R^T = R * S^2 * R^T |
|
|
|
eigenvalues of a * a^T will give S^2 |
|
eigenvectors of a * a^T will give R |
|
*/ |
|
|
|
/* |
|
( b c ) = a * a^T |
|
( d e ) |
|
*/ |
|
|
|
float b = (a->m[0][0] * a->m[0][0]) + (a->m[0][1] * a->m[0][1]); |
|
float c = (a->m[0][0] * a->m[1][0]) + (a->m[0][1] * a->m[1][1]); |
|
/* d = c */ |
|
float e = (a->m[1][0] * a->m[1][0]) + (a->m[1][1] * a->m[1][1]); |
|
|
|
float bpe = b + e; |
|
float bme = b - e; |
|
|
|
/* |
|
solve: |
|
|
|
bx + cy = sx |
|
dx + ey = sy |
|
|
|
cy * dx = (s - b)x * (s - e)y |
|
c^2 = (s - b) * (s - e) |
|
s^2 - (b + e)s + (be - c^2) = 0 |
|
s = (b + e +/- sqrt((b + e)^2 - 4(be - c^2))) / 2 |
|
s = (b + e +/- sqrt(b^2 + e^2 - 2be + 4c^2)) / 2 |
|
s = (b + e +/- sqrt((b - e)^2 + 4c^2)) / 2 |
|
*/ |
|
|
|
float t = sqrt_((bme * bme) + (4.0f * c * c)); |
|
float v = (bpe + t) * 0.5f; /* first eigenvalue */ |
|
if (s0) { |
|
*s0 = sqrt_(v); |
|
} |
|
if (s1) { |
|
*s1 = sqrt_( |
|
/* second eigenvalue */ |
|
_maxf(bpe - t, 0.0f) * 0.5f); |
|
} |
|
|
|
/* |
|
angle of eigenvector corresponds to r |
|
*/ |
|
|
|
if (r) { |
|
/* first eigenvector is (c, v - b) / (v - e, c) */ |
|
float x = (v - e) + c; |
|
float y = (v - b) + c; |
|
*r = ((absf_(x) < EPS) && (absf_(y) < EPS)) ? 0.0f : atan2_(y, x); |
|
} |
|
}
|
|
|