From e156b732eb47b7486f21bf6903445cad2585013c Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sun, 20 Apr 2025 13:55:41 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8F=8E=EF=B8=8F=20Simplifies=20kinematics?= =?UTF-8?q?=20by=20removing=20matrix=20muls?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/kinematic.ts | 419 +++++++------------------ esp32/lib/ESP32-sveltekit/kinematics.h | 163 +++++----- 2 files changed, 186 insertions(+), 396 deletions(-) diff --git a/app/src/lib/kinematic.ts b/app/src/lib/kinematic.ts index af85eb3..d4c6069 100644 --- a/app/src/lib/kinematic.ts +++ b/app/src/lib/kinematic.ts @@ -1,320 +1,125 @@ - export interface body_state_t { - omega: number; - phi: number; - psi: number; - xm: number; - ym: number; - zm: number; - feet: number[][]; + omega: number + phi: number + psi: number + xm: number + ym: number + zm: number + feet: number[][] } export interface position { - x: number; - y: number; - z: number; + x: number + y: number + z: number } export interface target_position { - x: number; - z: number; - yaw: number; + x: number + z: number + yaw: number } -const { cos, sin, atan2, sqrt } = Math; +const { cos, sin, atan2, acos, sqrt, max, min } = Math -const DEG2RAD = 0.017453292519943; +const DEG2RAD = 0.017453292519943 export default class Kinematic { - l1: number; - l2: number; - l3: number; - l4: number; - - L: number; - W: number; - - DEG2RAD = DEG2RAD; - - sHp = sin(Math.PI / 2); - cHp = cos(Math.PI / 2); - - Tlf: number[][] = []; - Trf: number[][] = []; - Tlb: number[][] = []; - Trb: number[][] = []; - - point_lf: number[][]; - point_rf: number[][]; - point_lb: number[][]; - point_rb: number[][]; - Ix: number[][]; - - constructor() { - this.l1 = 60.5 / 100; - this.l2 = 10 / 100; - this.l3 = 100.7 / 100; - this.l4 = 118.5 / 100; - - this.L = 207.5 / 100; - this.W = 78 / 100; - - this.point_lf = [ - [this.cHp, 0, this.sHp, this.L / 2], - [0, 1, 0, 0], - [-this.sHp, 0, this.cHp, this.W / 2], - [0, 0, 0, 1] - ]; - - this.point_rf = [ - [this.cHp, 0, this.sHp, this.L / 2], - [0, 1, 0, 0], - [-this.sHp, 0, this.cHp, -this.W / 2], - [0, 0, 0, 1] - ]; - - this.point_lb = [ - [this.cHp, 0, this.sHp, -this.L / 2], - [0, 1, 0, 0], - [-this.sHp, 0, this.cHp, this.W / 2], - [0, 0, 0, 1] - ]; - - this.point_rb = [ - [this.cHp, 0, this.sHp, -this.L / 2], - [0, 1, 0, 0], - [-this.sHp, 0, this.cHp, -this.W / 2], - [0, 0, 0, 1] - ]; - this.Ix = [ - [-1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]; - } - - public calcIK(body_state: body_state_t): number[] { - this.bodyIK(body_state); - - return [ - ...this.legIK(this.multiplyVector(this.inverse(this.Tlf), body_state.feet[0])), - ...this.legIK( - this.multiplyVector( - this.Ix, - this.multiplyVector(this.inverse(this.Trf), body_state.feet[1]) - ) - ), - ...this.legIK(this.multiplyVector(this.inverse(this.Tlb), body_state.feet[2])), - ...this.legIK( - this.multiplyVector( - this.Ix, - this.multiplyVector(this.inverse(this.Trb), body_state.feet[3]) - ) - ) - ]; - } - - bodyIK(p: body_state_t) { - const cos_omega = cos(p.omega * this.DEG2RAD); - const sin_omega = sin(p.omega * this.DEG2RAD); - const cos_phi = cos(p.phi * this.DEG2RAD); - const sin_phi = sin(p.phi * this.DEG2RAD); - const cos_psi = cos(p.psi * this.DEG2RAD); - const sin_psi = sin(p.psi * this.DEG2RAD); - - const Tm: number[][] = [ - [cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm], - [ - sin_omega * sin_phi * cos_psi + sin_psi * cos_omega, - -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, - -sin_omega * cos_phi, - p.ym - ], - [ - sin_omega * sin_psi - sin_phi * cos_omega * cos_psi, - sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, - cos_omega * cos_phi, - p.zm - ], - [0, 0, 0, 1] - ]; - - this.Tlf = this.matrixMultiply(Tm, this.point_lf); - this.Trf = this.matrixMultiply(Tm, this.point_rf); - this.Tlb = this.matrixMultiply(Tm, this.point_lb); - this.Trb = this.matrixMultiply(Tm, this.point_rb); - } - - public legIK(point: number[]): number[] { - const [x, y, z] = point; - - let F = sqrt(x ** 2 + y ** 2 - this.l1 ** 2); - if (isNaN(F)) F = this.l1; - - const G = F - this.l2; - const H = sqrt(G ** 2 + z ** 2); - - const theta1 = -atan2(y, x) - atan2(F, -this.l1); - const D = (H ** 2 - this.l3 ** 2 - this.l4 ** 2) / (2 * this.l3 * this.l4); - let theta3 = atan2(sqrt(1 - D ** 2), D); - if (isNaN(theta3)) theta3 = 0; - - const theta2 = atan2(z, G) - atan2(this.l4 * sin(theta3), this.l3 + this.l4 * cos(theta3)); - - return [theta1, theta2, theta3]; - } - - matrixMultiply(a: number[][], b: number[][]): number[][] { - const result: number[][] = []; - - for (let i = 0; i < a.length; i++) { - const row: number[] = []; - - for (let j = 0; j < b[0].length; j++) { - let sum = 0; - - for (let k = 0; k < a[i].length; k++) { - sum += a[i][k] * b[k][j]; - } - - row.push(sum); - } - - result.push(row); - } - - return result; - } - - multiplyVector(matrix: number[][], vector: number[]): number[] { - const rows = matrix.length; - const cols = matrix[0].length; - const vectorLength = vector.length; - - if (cols !== vectorLength) { - throw new Error('Matrix and vector dimensions do not match for multiplication.'); - } - - const result = []; - - for (let i = 0; i < rows; i++) { - let sum = 0; - - for (let j = 0; j < cols; j++) { - sum += matrix[i][j] * vector[j]; - } - - result.push(sum); - } - - return result; - } - - private inverse(matrix: number[][]): number[][] { - const det = this.determinant(matrix); - const adjugate = this.adjugate(matrix); - const scalar = 1 / det; - const inverse: number[][] = []; - - for (let i = 0; i < matrix.length; i++) { - const row: number[] = []; - - for (let j = 0; j < matrix[i].length; j++) { - row.push(adjugate[i][j] * scalar); - } - - inverse.push(row); - } - - return inverse; - } - - private determinant(matrix: number[][]): number { - if (matrix.length !== matrix[0].length) { - throw new Error('The matrix is not square.'); - } - - if (matrix.length === 2) { - return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]; - } - - let det = 0; - - for (let i = 0; i < matrix.length; i++) { - const sign = i % 2 === 0 ? 1 : -1; - const subMatrix: number[][] = []; - - for (let j = 1; j < matrix.length; j++) { - const row: number[] = []; - - for (let k = 0; k < matrix.length; k++) { - if (k !== i) { - row.push(matrix[j][k]); - } - } - - subMatrix.push(row); - } - - det += sign * matrix[0][i] * this.determinant(subMatrix); - } - - return det; - } - - private adjugate(matrix: number[][]): number[][] { - if (matrix.length !== matrix[0].length) { - throw new Error('The matrix is not square.'); - } - - const adjugate: number[][] = []; - - for (let i = 0; i < matrix.length; i++) { - const row: number[] = []; - - for (let j = 0; j < matrix[i].length; j++) { - const sign = (i + j) % 2 === 0 ? 1 : -1; - const subMatrix: number[][] = []; - - for (let k = 0; k < matrix.length; k++) { - if (k !== i) { - const subRow: number[] = []; - - for (let l = 0; l < matrix.length; l++) { - if (l !== j) { - subRow.push(matrix[k][l]); - } - } - - subMatrix.push(subRow); - } - } - - const cofactor = sign * this.determinant(subMatrix); - row.push(cofactor); - } - - adjugate.push(row); - } - - return this.transpose(adjugate); - } - - private transpose(matrix: number[][]): number[][] { - const transposed: number[][] = []; - - for (let i = 0; i < matrix.length; i++) { - const row: number[] = []; - - for (let j = 0; j < matrix[i].length; j++) { - row.push(matrix[j][i]); - } - - transposed.push(row); - } - - return transposed; - } + l1: number + l2: number + l3: number + l4: number + + L: number + W: number + + DEG2RAD = DEG2RAD + + mountOffsets: number[][] + + invMountRot = [ + [0, 0, -1], + [0, 1, 0], + [1, 0, 0] + ] + + constructor() { + this.l1 = 60.5 / 100 + this.l2 = 10 / 100 + this.l3 = 100.7 / 100 + this.l4 = 118.5 / 100 + + this.L = 230 / 100 + this.W = 75 / 100 + + this.mountOffsets = [ + [this.L / 2, 0, this.W / 2], + [this.L / 2, 0, -this.W / 2], + [-this.L / 2, 0, this.W / 2], + [-this.L / 2, 0, -this.W / 2] + ] + } + + calcIK(p: body_state_t): number[] { + const roll = p.omega * this.DEG2RAD + const pitch = p.phi * this.DEG2RAD + const yaw = p.psi * this.DEG2RAD + const rot = this.euler2R(roll, pitch, yaw) + const inv_rot = [ + [rot[0][0], rot[1][0], rot[2][0]], + [rot[0][1], rot[1][1], rot[2][1]], + [rot[0][2], rot[1][2], rot[2][2]] + ] + const inv_trans = [ + -inv_rot[0][0] * p.xm - inv_rot[0][1] * p.ym - inv_rot[0][2] * p.zm, + -inv_rot[1][0] * p.xm - inv_rot[1][1] * p.ym - inv_rot[1][2] * p.zm, + -inv_rot[2][0] * p.xm - inv_rot[2][1] * p.ym - inv_rot[2][2] * p.zm + ] + return p.feet.flatMap((foot, i) => { + const [wx, wy, wz] = foot + const bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0] + const by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1] + const bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2] + + const [mx, my, mz] = this.mountOffsets[i] + const px = bx - mx, + py = by - my, + pz = bz - mz + + const lx = + this.invMountRot[0][0] * px + this.invMountRot[0][1] * py + this.invMountRot[0][2] * pz + const ly = + this.invMountRot[1][0] * px + this.invMountRot[1][1] * py + this.invMountRot[1][2] * pz + const lz = + this.invMountRot[2][0] * px + this.invMountRot[2][1] * py + this.invMountRot[2][2] * pz + + const xLocal = i % 2 === 1 ? -lx : lx + return this.legIK(xLocal, ly, lz) + }) + } + + private legIK(x: number, y: number, z: number): [number, number, number] { + const F = sqrt(max(0, x * x + y * y - this.l1 * this.l1)) + const G = F - this.l2 + const H = sqrt(G * G + z * z) + const t1 = -atan2(y, x) - atan2(F, -this.l1) + const D = (H * H - this.l3 * this.l3 - this.l4 * this.l4) / (2 * this.l3 * this.l4) + const t3 = acos(max(-1, min(1, D))) + const t2 = atan2(z, G) - atan2(this.l4 * sin(t3), this.l3 + this.l4 * cos(t3)) + return [t1, t2, t3] + } + + private euler2R(roll: number, pitch: number, yaw: number): number[][] { + const cr = cos(roll), + sr = sin(roll) + const cp = cos(pitch), + sp = sin(pitch) + const cy = cos(yaw), + sy = sin(yaw) + return [ + [cp * cy, -cp * sy, sp], + [sr * sp * cy + sy * cr, -sr * sp * sy + cr * cy, -sr * cp], + [sr * sy - sp * cr * cy, sr * cy + sp * sy * cr, cr * cp] + ] + } } - diff --git a/esp32/lib/ESP32-sveltekit/kinematics.h b/esp32/lib/ESP32-sveltekit/kinematics.h index 348ae35..deb1834 100644 --- a/esp32/lib/ESP32-sveltekit/kinematics.h +++ b/esp32/lib/ESP32-sveltekit/kinematics.h @@ -3,7 +3,7 @@ #include -struct body_state_t { +struct alignas(16) body_state_t { float omega, phi, psi, xm, ym, zm; float feet[4][4]; @@ -15,7 +15,7 @@ struct body_state_t { !IS_ALMOST_EQUAL(zm, other.zm)) { return false; } - return arrayEqual(feet, other.feet, 0.1); + return arrayEqual(feet, other.feet, 0.1f); } }; @@ -23,25 +23,20 @@ class Kinematics { private: static constexpr float l1 = 60.5f / 100.0f; static constexpr float l2 = 10.0f / 100.0f; - static constexpr float l3 = 111.1f / 100.0f; + static constexpr float l3 = 111.2f / 100.0f; static constexpr float l4 = 118.5f / 100.0f; static constexpr float L = 207.5f / 100.0f; static constexpr float W = 78.0f / 100.0f; - const float sHp = sinf(PI_F / 2); - const float cHp = cosf(PI_F / 2); + static constexpr float mountOffsets[4][3] = { + {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}}; - static constexpr float Ix[4][4] = {{-1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; + static constexpr float invMountRot[3][3] = {{0, 0, -1}, {0, 1, 0}, {1, 0, 0}}; - float Trb[4][4] = {0}; - float Trf[4][4] = {0}; - float Tlb[4][4] = {0}; - float Tlf[4][4] = {0}; - - float inv[4][4]; - float point[4]; - float Q1[4][4]; + alignas(16) float rot[3][3] = {0}; + alignas(16) float inv_rot[3][3] = {0}; + alignas(16) float inv_trans[3] = {0}; body_state_t currentState; @@ -51,7 +46,6 @@ class Kinematics { if (currentState == body_state) return ESP_OK; - ret = bodyIK(body_state); currentState.omega = body_state.omega; currentState.phi = body_state.phi; currentState.psi = body_state.psi; @@ -60,100 +54,91 @@ class Kinematics { currentState.zm = body_state.zm; currentState.updateFeet(body_state.feet); - ret += inverse(Tlf, inv); - MAT_MULT(inv, body_state.feet[0], point, 4, 4, 1); - legIK((float *)point, result); + float roll = body_state.omega * DEG2RAD_F; + float pitch = body_state.phi * DEG2RAD_F; + float yaw = body_state.psi * DEG2RAD_F; + euler2R(roll, pitch, yaw, rot); + inverse(rot, inv_rot); - ret += inverse(Trf, inv); - MAT_MULT(Ix, inv, Q1, 4, 4, 4); - MAT_MULT(Q1, body_state.feet[1], point, 4, 4, 1); - legIK((float *)point, result + 3); + inv_trans[0] = + -inv_rot[0][0] * currentState.xm - inv_rot[0][1] * currentState.ym - inv_rot[0][2] * currentState.zm; + inv_trans[1] = + -inv_rot[1][0] * currentState.xm - inv_rot[1][1] * currentState.ym - inv_rot[1][2] * currentState.zm; + inv_trans[2] = + -inv_rot[2][0] * currentState.xm - inv_rot[2][1] * currentState.ym - inv_rot[2][2] * currentState.zm; - ret += inverse(Tlb, inv); - MAT_MULT(inv, body_state.feet[2], point, 4, 4, 1); - legIK((float *)point, result + 6); + for (int i = 0; i < 4; i++) { + float wx = currentState.feet[i][0]; + float wy = currentState.feet[i][1]; + float wz = currentState.feet[i][2]; - ret += inverse(Trb, inv); - MAT_MULT(Ix, inv, Q1, 4, 4, 4); - MAT_MULT(Q1, body_state.feet[3], point, 4, 4, 1); - legIK((float *)point, result + 9); + float bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0]; + float by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1]; + float bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2]; + + float mx = mountOffsets[i][0]; + float my = mountOffsets[i][1]; + float mz = mountOffsets[i][2]; + + float px = bx - mx; + float py = by - my; + float pz = bz - mz; + + float lx = invMountRot[0][0] * px + invMountRot[0][1] * py + invMountRot[0][2] * pz; + float ly = invMountRot[1][0] * px + invMountRot[1][1] * py + invMountRot[1][2] * pz; + float lz = invMountRot[2][0] * px + invMountRot[2][1] * py + invMountRot[2][2] * pz; + + float xLocal = (i % 2 == 1) ? -lx : lx; + legIK(xLocal, ly, lz, result + i * 3); + } return ret; } - esp_err_t bodyIK(const body_state_t p) { - float cos_omega = COS_DEG_F(p.omega); - float sin_omega = SIN_DEG_F(p.omega); - float cos_phi = COS_DEG_F(p.phi); - float sin_phi = SIN_DEG_F(p.phi); - float cos_psi = COS_DEG_F(p.psi); - float sin_psi = SIN_DEG_F(p.psi); + inline void euler2R(float roll, float pitch, float yaw, float rot[3][3]) { + float cos_roll = std::cos(roll); + float sin_roll = std::sin(roll); + float cos_pitch = std::cos(pitch); + float sin_pitch = std::sin(pitch); + float cos_yaw = std::cos(yaw); + float sin_yaw = std::sin(yaw); - float Tm[4][4] = {{cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm}, - {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega, - -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, -sin_omega * cos_phi, p.ym}, - {sin_omega * sin_psi - sin_phi * cos_omega * cos_psi, - sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, cos_omega * cos_phi, p.zm}, - {0, 0, 0, 1}}; - - float point_lf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, W / 2}, {0, 0, 0, 1}}; - float point_rf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}}; - float point_lb[4][4] = {{cHp, 0, sHp, -L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, W / 2}, {0, 0, 0, 1}}; - float point_rb[4][4] = {{cHp, 0, sHp, -L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}}; - - MAT_MULT(Tm, point_lf, Tlf, 4, 4, 4); - MAT_MULT(Tm, point_rf, Trf, 4, 4, 4); - MAT_MULT(Tm, point_lb, Tlb, 4, 4, 4); - MAT_MULT(Tm, point_rb, Trb, 4, 4, 4); - return ESP_OK; + rot[0][0] = cos_pitch * cos_yaw; + rot[0][1] = -sin_yaw * cos_pitch; + rot[0][2] = sin_pitch; + rot[1][0] = sin_roll * sin_pitch * cos_yaw + sin_yaw * cos_roll; + rot[1][1] = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw; + rot[1][2] = -sin_roll * cos_pitch; + rot[2][0] = sin_roll * sin_yaw - sin_pitch * cos_roll * cos_yaw; + rot[2][1] = sin_roll * cos_yaw + sin_pitch * sin_yaw * cos_roll; + rot[2][2] = cos_roll * cos_pitch; } - void legIK(float point[4], float result[3]) { - float x = point[0], y = point[1], z = point[2]; + inline void inverse(float rot[3][3], float inv_rot[3][3]) { + inv_rot[0][0] = rot[0][0]; + inv_rot[0][1] = rot[1][0]; + inv_rot[0][2] = rot[2][0]; + inv_rot[1][0] = rot[0][1]; + inv_rot[1][1] = rot[1][1]; + inv_rot[1][2] = rot[2][1]; + inv_rot[2][0] = rot[0][2]; + inv_rot[2][1] = rot[1][2]; + inv_rot[2][2] = rot[2][2]; + } - float F = sqrtf(x * x + y * y - l1 * l1); - F = isnanf(F) ? l1 : F; + inline void legIK(float x, float y, float z, float result[3]) { + float F = sqrt(max(0.0f, x * x + y * y - l1 * l1)); float G = F - l2; - float H = sqrtf(G * G + z * z); + float H = sqrt(G * G + z * z); float theta1 = -atan2f(y, x) - atan2f(F, -l1); float D = (H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4); - float theta3 = atan2(sqrt(1 - D * D), D); - if (isnan(theta3)) theta3 = 0; + float theta3 = acosf(max(-1.0f, min(1.0f, D))); float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3)); result[0] = RAD_TO_DEG_F(theta1); result[1] = RAD_TO_DEG_F(theta2); result[2] = RAD_TO_DEG_F(theta3); } - - esp_err_t inverse(float a[4][4], float b[4][4]) { - float s[] = {a[0][0] * a[1][1] - a[1][0] * a[0][1], a[0][0] * a[1][2] - a[1][0] * a[0][2], - a[0][0] * a[1][3] - a[1][0] * a[0][3], a[0][1] * a[1][2] - a[1][1] * a[0][2], - a[0][1] * a[1][3] - a[1][1] * a[0][3], a[0][2] * a[1][3] - a[1][2] * a[0][3]}; - float c[] = {a[2][0] * a[3][1] - a[3][0] * a[2][1], a[2][0] * a[3][2] - a[3][0] * a[2][2], - a[2][0] * a[3][3] - a[3][0] * a[2][3], a[2][1] * a[3][2] - a[3][1] * a[2][2], - a[2][1] * a[3][3] - a[3][1] * a[2][3], a[2][2] * a[3][3] - a[3][2] * a[2][3]}; - float det = s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] - s[4] * c[1] + s[5] * c[0]; - if (det == 0.0) return ESP_FAIL; - float invdet = 1.0 / det; - b[0][0] = (a[1][1] * c[5] - a[1][2] * c[4] + a[1][3] * c[3]) * invdet; - b[0][1] = (-a[0][1] * c[5] + a[0][2] * c[4] - a[0][3] * c[3]) * invdet; - b[0][2] = (a[3][1] * s[5] - a[3][2] * s[4] + a[3][3] * s[3]) * invdet; - b[0][3] = (-a[2][1] * s[5] + a[2][2] * s[4] - a[2][3] * s[3]) * invdet; - b[1][0] = (-a[1][0] * c[5] + a[1][2] * c[2] - a[1][3] * c[1]) * invdet; - b[1][1] = (a[0][0] * c[5] - a[0][2] * c[2] + a[0][3] * c[1]) * invdet; - b[1][2] = (-a[3][0] * s[5] + a[3][2] * s[2] - a[3][3] * s[1]) * invdet; - b[1][3] = (a[2][0] * s[5] - a[2][2] * s[2] + a[2][3] * s[1]) * invdet; - b[2][0] = (a[1][0] * c[4] - a[1][1] * c[2] + a[1][3] * c[0]) * invdet; - b[2][1] = (-a[0][0] * c[4] + a[0][1] * c[2] - a[0][3] * c[0]) * invdet; - b[2][2] = (a[3][0] * s[4] - a[3][1] * s[2] + a[3][3] * s[0]) * invdet; - b[2][3] = (-a[2][0] * s[4] + a[2][1] * s[2] - a[2][3] * s[0]) * invdet; - b[3][0] = (-a[1][0] * c[3] + a[1][1] * c[1] - a[1][2] * c[0]) * invdet; - b[3][1] = (a[0][0] * c[3] - a[0][1] * c[1] + a[0][2] * c[0]) * invdet; - b[3][2] = (-a[3][0] * s[3] + a[3][1] * s[1] - a[3][2] * s[0]) * invdet; - b[3][3] = (a[2][0] * s[3] - a[2][1] * s[1] + a[2][2] * s[0]) * invdet; - return ESP_OK; - } }; #endif \ No newline at end of file