Vector3 nerror = n1iw.cross(n2jw);
u.assign( u.add(posError.multiply(Kcor)));
linear1.assign(
bi, bj,
Bi.column(0), Bangi.column(0), Bj.column(0), Bangj.column(0),
Ji.row(0), Jangi.row(0), Jj.row(0), Jangj.row(0),
Double.NEGATIVE_INFINITY,
Double.POSITIVE_INFINITY,
null,
u.x, 0 );