solve method

Vec3 solve(Vec3 b, [ Vec3? target ])

Solve Ax=b @param b The right hand side @param target Optional. Target vector to save in. @return The solution x @todo should reuse arrays

Implementation

Vec3 solve(Vec3 b,[ Vec3? target]) {
  target ??= Vec3();
  // finalruct equations
  const nr = 3; // num rows
  const nc = 4; // num cols
  List<double> eqns = [];
  int i;
  int j;
  for (i = 0; i < nr * nc; i++) {
    eqns.add(0);
  }
  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      eqns[i + nc * j] = elements[i + 3 * j];
    }
  }
  eqns[3 + 4 * 0] = b.x;
  eqns[3 + 4 * 1] = b.y;
  eqns[3 + 4 * 2] = b.z;

  // Compute right upper triangular version of the matrix - Gauss elimination
  int n = 3;

  final k = n;
  int np;
  const kp = 4; // num rows
  int p;
  for(;n>0;n--) {
    i = k - n;
    if (eqns[i + nc * i] == 0) {
      // the pivot is null, swap lines
      for (j = i + 1; j < k; j++) {
        if (eqns[i + nc * j] != 0) {
          for(np = kp;np > 0 ;np--){
            // do ligne( i ) = ligne( i ) + ligne( k )
            p = kp - np;
            eqns[p + nc * i] += eqns[p + nc * j];
          }
          //break;
        }
      }
    }
    if (eqns[i + nc * i] != 0) {
      for (j = i + 1; j < k; j++) {
        final num multiplier = eqns[i + nc * j] / eqns[i + nc * i];
        for (np = kp; np > 0; --np) {
          // do ligne( k ) = ligne( k ) - multiplier * ligne( i )
          p = kp - np;
          eqns[p + nc * j] = p <= i ? 0 : eqns[p + nc * j] - eqns[p + nc * i] * multiplier;
        }
      }
    }
  }

  // Get the solution
  target.z = eqns[2 * nc + 3] / eqns[2 * nc + 2];
  target.y = (eqns[1 * nc + 3] - eqns[1 * nc + 2] * target.z) / eqns[1 * nc + 1];
  target.x = (eqns[0 * nc + 3] - eqns[0 * nc + 2] * target.z - eqns[0 * nc + 1] * target.y) / eqns[0 * nc + 0];

  if (
    target.x.isNaN ||
    target.y.isNaN ||
    target.z.isNaN ||
    target.x == double.infinity ||
    target.y == double.infinity ||
    target.z == double.infinity
  ) {
    throw('Could not solve equation! Got x=[${target.toString()}], b=[${b.toString()}], A=[${toString()}]');
  }

  return target;
}