🏎️ Simplifies kinematics by removing matrix muls

app/src/lib/kinematic.ts

@@ -1,320 +1,125 @@
-
 export interface body_state_t {
-	omega: number;
+  omega: number
-	phi: number;
+  phi: number
-	psi: number;
+  psi: number
-	xm: number;
+  xm: number
-	ym: number;
+  ym: number
-	zm: number;
+  zm: number
-	feet: number[][];
+  feet: number[][]
 }
 
 export interface position {
-	x: number;
+  x: number
-	y: number;
+  y: number
-	z: number;
+  z: number
 }
 
 export interface target_position {
-	x: number;
+  x: number
-	z: number;
+  z: number
-	yaw: 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;
+  l1: number
-	l2: number;
+  l2: number
-	l3: number;
+  l3: number
-	l4: number;
+  l4: number
 
-	L: number;
+  L: number
-	W: number;
+  W: number
 
-	DEG2RAD = DEG2RAD;
+  DEG2RAD = DEG2RAD
 
-	sHp = sin(Math.PI / 2);
+  mountOffsets: number[][]
-	cHp = cos(Math.PI / 2);
 
-	Tlf: number[][] = [];
+  invMountRot = [
-	Trf: number[][] = [];
+    [0, 0, -1],
-	Tlb: number[][] = [];
+    [0, 1, 0],
-	Trb: number[][] = [];
+    [1, 0, 0]
-
+  ]
-	point_lf: number[][];
-	point_rf: number[][];
-	point_lb: number[][];
-	point_rb: number[][];
-	Ix: number[][];
 
   constructor() {
-		this.l1 = 60.5 / 100;
+    this.l1 = 60.5 / 100
-		this.l2 = 10 / 100;
+    this.l2 = 10 / 100
-		this.l3 = 100.7 / 100;
+    this.l3 = 100.7 / 100
-		this.l4 = 118.5 / 100;
+    this.l4 = 118.5 / 100
 
-		this.L = 207.5 / 100;
+    this.L = 230 / 100
-		this.W = 78 / 100;
+    this.W = 75 / 100
 
-		this.point_lf = [
+    this.mountOffsets = [
-			[this.cHp, 0, this.sHp, this.L / 2],
+      [this.L / 2, 0, this.W / 2],
-			[0, 1, 0, 0],
+      [this.L / 2, 0, -this.W / 2],
-			[-this.sHp, 0, this.cHp, this.W / 2],
+      [-this.L / 2, 0, this.W / 2],
-			[0, 0, 0, 1]
+      [-this.L / 2, 0, -this.W / 2]
-		];
+    ]
-
-		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[] {
+  calcIK(p: body_state_t): number[] {
-		this.bodyIK(body_state);
+    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 [
-			...this.legIK(this.multiplyVector(this.inverse(this.Tlf), body_state.feet[0])),
+      [cp * cy, -cp * sy, sp],
-			...this.legIK(
+      [sr * sp * cy + sy * cr, -sr * sp * sy + cr * cy, -sr * cp],
-				this.multiplyVector(
+      [sr * sy - sp * cr * cy, sr * cy + sp * sy * cr, cr * cp]
-					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;
   }
 }
-

esp32/lib/ESP32-sveltekit/kinematics.h

@@ -3,7 +3,7 @@
 
 #include <utils/math_utils.h>
 
-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);
+    static constexpr float mountOffsets[4][3] = {
-    const float cHp = cosf(PI_F / 2);
+        {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};
+    alignas(16) float rot[3][3] = {0};
-    float Trf[4][4] = {0};
+    alignas(16) float inv_rot[3][3] = {0};
-    float Tlb[4][4] = {0};
+    alignas(16) float inv_trans[3] = {0};
-    float Tlf[4][4] = {0};
-
-    float inv[4][4];
-    float point[4];
-    float Q1[4][4];
 
     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);
+        float roll = body_state.omega * DEG2RAD_F;
-        MAT_MULT(inv, body_state.feet[0], point, 4, 4, 1);
+        float pitch = body_state.phi * DEG2RAD_F;
-        legIK((float *)point, result);
+        float yaw = body_state.psi * DEG2RAD_F;
+        euler2R(roll, pitch, yaw, rot);
+        inverse(rot, inv_rot);
 
-        ret += inverse(Trf, inv);
+        inv_trans[0] =
-        MAT_MULT(Ix, inv, Q1, 4, 4, 4);
+            -inv_rot[0][0] * currentState.xm - inv_rot[0][1] * currentState.ym - inv_rot[0][2] * currentState.zm;
-        MAT_MULT(Q1, body_state.feet[1], point, 4, 4, 1);
+        inv_trans[1] =
-        legIK((float *)point, result + 3);
+            -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);
+        for (int i = 0; i < 4; i++) {
-        MAT_MULT(inv, body_state.feet[2], point, 4, 4, 1);
+            float wx = currentState.feet[i][0];
-        legIK((float *)point, result + 6);
+            float wy = currentState.feet[i][1];
+            float wz = currentState.feet[i][2];
 
-        ret += inverse(Trb, inv);
+            float bx = inv_rot[0][0] * wx + inv_rot[0][1] * wy + inv_rot[0][2] * wz + inv_trans[0];
-        MAT_MULT(Ix, inv, Q1, 4, 4, 4);
+            float by = inv_rot[1][0] * wx + inv_rot[1][1] * wy + inv_rot[1][2] * wz + inv_trans[1];
-        MAT_MULT(Q1, body_state.feet[3], point, 4, 4, 1);
+            float bz = inv_rot[2][0] * wx + inv_rot[2][1] * wy + inv_rot[2][2] * wz + inv_trans[2];
-        legIK((float *)point, result + 9);
+
+            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) {
+    inline void euler2R(float roll, float pitch, float yaw, float rot[3][3]) {
-        float cos_omega = COS_DEG_F(p.omega);
+        float cos_roll = std::cos(roll);
-        float sin_omega = SIN_DEG_F(p.omega);
+        float sin_roll = std::sin(roll);
-        float cos_phi = COS_DEG_F(p.phi);
+        float cos_pitch = std::cos(pitch);
-        float sin_phi = SIN_DEG_F(p.phi);
+        float sin_pitch = std::sin(pitch);
-        float cos_psi = COS_DEG_F(p.psi);
+        float cos_yaw = std::cos(yaw);
-        float sin_psi = SIN_DEG_F(p.psi);
+        float sin_yaw = std::sin(yaw);
 
-        float Tm[4][4] = {{cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm},
+        rot[0][0] = cos_pitch * cos_yaw;
-                          {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega,
+        rot[0][1] = -sin_yaw * cos_pitch;
-                           -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, -sin_omega * cos_phi, p.ym},
+        rot[0][2] = sin_pitch;
-                          {sin_omega * sin_psi - sin_phi * cos_omega * cos_psi,
+        rot[1][0] = sin_roll * sin_pitch * cos_yaw + sin_yaw * cos_roll;
-                           sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, cos_omega * cos_phi, p.zm},
+        rot[1][1] = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_yaw;
-                          {0, 0, 0, 1}};
+        rot[1][2] = -sin_roll * cos_pitch;
-
+        rot[2][0] = sin_roll * sin_yaw - sin_pitch * cos_roll * cos_yaw;
-        float point_lf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, W / 2}, {0, 0, 0, 1}};
+        rot[2][1] = sin_roll * cos_yaw + sin_pitch * sin_yaw * cos_roll;
-        float point_rf[4][4] = {{cHp, 0, sHp, L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}};
+        rot[2][2] = cos_roll * cos_pitch;
-        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;
     }
 
-    void legIK(float point[4], float result[3]) {
+    inline void inverse(float rot[3][3], float inv_rot[3][3]) {
-        float x = point[0], y = point[1], z = point[2];
+        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);
+    inline void legIK(float x, float y, float z, float result[3]) {
-        F = isnanf(F) ? l1 : F;
+        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);
+        float theta3 = acosf(max(-1.0f, min(1.0f, D)));
-        if (isnan(theta3)) theta3 = 0;
         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