matrix.c (8490B)
1 /* matrix.c - operaciones con matrices y vectores (ver matrix.h). Las cuentas van con 2 * Num: con datos racionales el determinante y la inversa salen exactos. */ 3 #include "matrix.h" 4 5 static bool bad(const Num *v) { return num_bad(v); } 6 7 /* temporales compartidos (cada MVal ocupa ~1 KB y la RAM de la Pico es poca): 8 * T1 hojas (suma, escala, traspuesta, cruz, abs), T2 producto, T3/T4 determinante e 9 * inversa, T5/T6 potencia. Ninguna funcion usa un temporal de otra que la llame. */ 10 static MVal T1, T2, T3, T4, T5, T6; 11 12 void mv_scalar(MVal *o, Num v) 13 { 14 o->kind = MV_SCALAR; o->r = o->c = 1; 15 o->a[0] = v; 16 } 17 18 void mv_zero(MVal *o, int kind, int r, int c) 19 { 20 o->kind = (uint8_t)kind; o->r = (uint8_t)r; o->c = (uint8_t)c; 21 for (int i = 0; i < MAT_N * MAT_N; i++) o->a[i] = num_int(0); 22 } 23 24 bool mv_add(MVal *o, const MVal *a, const MVal *b, bool sub) 25 { 26 if (a->kind != b->kind || a->r != b->r || a->c != b->c) return false; 27 MVal *t_ = &T1; 28 *t_ = *a; 29 for (int i = 0; i < a->r; i++) 30 for (int j = 0; j < a->c; j++) { 31 Num *x = mv_at(t_, i, j); 32 *x = sub ? num_sub(*x, *mv_get(b, i, j)) : num_add(*x, *mv_get(b, i, j)); 33 if (bad(x)) return false; 34 } 35 *o = *t_; 36 return true; 37 } 38 39 bool mv_scale(MVal *o, const MVal *a, Num k, bool div) 40 { 41 MVal *t_ = &T1; 42 *t_ = *a; 43 for (int i = 0; i < a->r; i++) 44 for (int j = 0; j < a->c; j++) { 45 Num *x = mv_at(t_, i, j); 46 *x = div ? num_div(*x, k) : num_mul(*x, k); 47 if (bad(x)) return false; 48 } 49 *o = *t_; 50 return true; 51 } 52 53 bool mv_neg(MVal *o, const MVal *a) { return mv_scale(o, a, num_int(-1), false); } 54 55 /* producto: matriz x matriz, matriz x vector (columna) y vector x matriz (fila) */ 56 bool mv_mul(MVal *o, const MVal *a, const MVal *b) 57 { 58 if (a->kind == MV_SCALAR) return mv_scale(o, b, a->a[0], false); 59 if (b->kind == MV_SCALAR) return mv_scale(o, a, b->a[0], false); 60 if (a->kind == MV_VEC && b->kind == MV_VEC) return false; /* para eso estan dot y cross */ 61 MVal *const tp = &T2; 62 int n = a->c; 63 if (b->kind == MV_VEC) { /* M v: v como columna */ 64 if (b->c != n) return false; 65 mv_zero(tp, MV_VEC, 1, a->r); 66 for (int i = 0; i < a->r; i++) { 67 Num s = num_int(0); 68 for (int k = 0; k < n; k++) s = num_add(s, num_mul(*mv_get(a, i, k), *mv_get(b, 0, k))); 69 if (bad(&s)) return false; 70 *mv_at(tp, 0, i) = s; 71 } 72 } else { 73 if (b->r != n) return false; 74 mv_zero(tp, a->kind == MV_VEC ? MV_VEC : MV_MAT, a->r, b->c); 75 for (int i = 0; i < a->r; i++) 76 for (int j = 0; j < b->c; j++) { 77 Num s = num_int(0); 78 for (int k = 0; k < n; k++) s = num_add(s, num_mul(*mv_get(a, i, k), *mv_get(b, k, j))); 79 if (bad(&s)) return false; 80 *mv_at(tp, i, j) = s; 81 } 82 } 83 *o = *tp; 84 return true; 85 } 86 87 bool mv_det(Num *o, const MVal *a) 88 { 89 if (a->kind != MV_MAT || a->r != a->c) return false; 90 MVal *const tp = &T3; 91 *tp = *a; 92 int n = a->r; 93 Num det = num_int(1); 94 for (int col = 0; col < n; col++) { 95 int p = -1; 96 for (int r = col; r < n; r++) if (mv_at(tp, r, col)->v.m) { p = r; break; } 97 if (p < 0) { *o = num_int(0); return true; } 98 if (p != col) { 99 for (int j = 0; j < n; j++) { Num x = *mv_at(tp, p, j); *mv_at(tp, p, j) = *mv_at(tp, col, j); *mv_at(tp, col, j) = x; } 100 det = num_neg(det); 101 } 102 Num piv = *mv_at(tp, col, col); 103 det = num_mul(det, piv); 104 for (int r = col + 1; r < n; r++) { 105 Num f = num_div(*mv_at(tp, r, col), piv); 106 for (int j = col; j < n; j++) *mv_at(tp, r, j) = num_sub(*mv_at(tp, r, j), num_mul(f, *mv_at(tp, col, j))); 107 } 108 } 109 if (bad(&det)) return false; 110 if (!det.exact) det.v = dec_round_sig(det.v, 15); 111 *o = det; 112 return true; 113 } 114 115 /* Gauss-Jordan sobre [A | I] */ 116 bool mv_inv(MVal *o, const MVal *a) 117 { 118 if (a->kind != MV_MAT || a->r != a->c) return false; 119 MVal *const tp = &T3, *const iv = &T4; 120 int n = a->r; 121 *tp = *a; 122 mv_ident(iv, n); 123 for (int col = 0; col < n; col++) { 124 int p = -1; 125 for (int r = col; r < n; r++) if (mv_at(tp, r, col)->v.m) { p = r; break; } 126 if (p < 0) return false; /* singular */ 127 if (p != col) 128 for (int j = 0; j < n; j++) { 129 Num x = *mv_at(tp, p, j); *mv_at(tp, p, j) = *mv_at(tp, col, j); *mv_at(tp, col, j) = x; 130 x = *mv_at(iv, p, j); *mv_at(iv, p, j) = *mv_at(iv, col, j); *mv_at(iv, col, j) = x; 131 } 132 Num piv = *mv_at(tp, col, col); 133 for (int j = 0; j < n; j++) { 134 *mv_at(tp, col, j) = num_div(*mv_at(tp, col, j), piv); 135 *mv_at(iv, col, j) = num_div(*mv_at(iv, col, j), piv); 136 } 137 for (int r = 0; r < n; r++) { 138 if (r == col || !mv_at(tp, r, col)->v.m) continue; 139 Num f = *mv_at(tp, r, col); 140 for (int j = 0; j < n; j++) { 141 *mv_at(tp, r, j) = num_sub(*mv_at(tp, r, j), num_mul(f, *mv_at(tp, col, j))); 142 *mv_at(iv, r, j) = num_sub(*mv_at(iv, r, j), num_mul(f, *mv_at(iv, col, j))); 143 } 144 } 145 } 146 for (int i = 0; i < n * MAT_N; i++) { 147 if (bad(&iv->a[i])) return false; 148 if (!iv->a[i].exact) { 149 iv->a[i].v = dec_round_sig(iv->a[i].v, 15); 150 if (iv->a[i].v.m && iv->a[i].v.e < -14) iv->a[i] = num_int(0); 151 } 152 } 153 *o = *iv; 154 return true; 155 } 156 157 bool mv_ident(MVal *o, int n) 158 { 159 if (n < 1 || n > MAT_N) return false; 160 mv_zero(o, MV_MAT, n, n); 161 for (int i = 0; i < n; i++) *mv_at(o, i, i) = num_int(1); 162 return true; 163 } 164 165 bool mv_pow(MVal *o, const MVal *a, int64_t n) 166 { 167 if (a->kind != MV_MAT || a->r != a->c || n < -64 || n > 64) return false; 168 MVal *const bs = &T5, *const ac = &T6; 169 *bs = *a; 170 if (n < 0) { if (!mv_inv(bs, a)) return false; n = -n; } 171 mv_ident(ac, a->r); 172 while (n) { 173 if (n & 1) { if (!mv_mul(ac, ac, bs)) return false; } 174 n >>= 1; 175 if (n && !mv_mul(bs, bs, bs)) return false; 176 } 177 *o = *ac; 178 return true; 179 } 180 181 bool mv_trn(MVal *o, const MVal *a) 182 { 183 if (a->kind == MV_SCALAR) { *o = *a; return true; } 184 MVal *const tp = &T1; 185 mv_zero(tp, a->kind == MV_VEC ? MV_MAT : MV_MAT, a->c, a->r); 186 for (int i = 0; i < a->r; i++) 187 for (int j = 0; j < a->c; j++) *mv_at(tp, j, i) = *mv_get(a, i, j); 188 *o = *tp; 189 return true; 190 } 191 192 bool mv_dot(Num *o, const MVal *a, const MVal *b) 193 { 194 if (a->kind != MV_VEC || b->kind != MV_VEC || a->c != b->c) return false; 195 Num s = num_int(0); 196 for (int i = 0; i < a->c; i++) s = num_add(s, num_mul(*mv_get(a, 0, i), *mv_get(b, 0, i))); 197 *o = s; 198 return !bad(&s); 199 } 200 201 bool mv_cross(MVal *o, const MVal *a, const MVal *b) 202 { 203 if (a->kind != MV_VEC || b->kind != MV_VEC || a->c != b->c || a->c < 2) return false; 204 Num x1 = *mv_get(a, 0, 0), y1 = *mv_get(a, 0, 1), z1 = a->c == 3 ? *mv_get(a, 0, 2) : num_int(0); 205 Num x2 = *mv_get(b, 0, 0), y2 = *mv_get(b, 0, 1), z2 = b->c == 3 ? *mv_get(b, 0, 2) : num_int(0); 206 MVal *const tp = &T1; 207 mv_zero(tp, MV_VEC, 1, 3); 208 *mv_at(tp, 0, 0) = num_sub(num_mul(y1, z2), num_mul(z1, y2)); 209 *mv_at(tp, 0, 1) = num_sub(num_mul(z1, x2), num_mul(x1, z2)); 210 *mv_at(tp, 0, 2) = num_sub(num_mul(x1, y2), num_mul(y1, x2)); 211 *o = *tp; 212 return true; 213 } 214 215 bool mv_norm(Num *o, const MVal *a) 216 { 217 if (a->kind == MV_SCALAR) { *o = num_abs(a->a[0]); return true; } 218 if (a->kind != MV_VEC) return false; 219 Num s = num_int(0); 220 for (int i = 0; i < a->c; i++) s = num_add(s, num_mul(*mv_get(a, 0, i), *mv_get(a, 0, i))); 221 *o = num_sqrt(s); 222 return !bad(o); 223 } 224 225 bool mv_unit(MVal *o, const MVal *a) 226 { 227 Num n; 228 if (!mv_norm(&n, a) || !n.v.m) return false; 229 return mv_scale(o, a, n, true); 230 } 231 232 bool mv_angle(Num *o, const MVal *a, const MVal *b, int ang) 233 { 234 Num d, na, nb; 235 if (!mv_dot(&d, a, b) || !mv_norm(&na, a) || !mv_norm(&nb, b) || !na.v.m || !nb.v.m) return false; 236 *o = num_acos(num_div(d, num_mul(na, nb)), ang); 237 return !bad(o); 238 } 239 240 bool mv_abs(MVal *o, const MVal *a) 241 { 242 MVal *const tp = &T1; 243 *tp = *a; 244 for (int i = 0; i < a->r; i++) 245 for (int j = 0; j < a->c; j++) *mv_at(tp, i, j) = num_abs(*mv_get(a, i, j)); 246 *o = *tp; 247 return true; 248 }