public static void constraintMatrix3x6 ( DMatrixRMaj L , DMatrixRMaj y , FastQueue < Point3D_F64 > controlWorldPts , List < Point3D_F64 > nullPts [ ] ) { int row = 0 ; for ( int i = 0 ; i < 3 ; i ++ ) { Point3D_F64 ci = controlWorldPts . get ( i ) ; Point3D_F64 vai = nullPts [ 0 ] . get ( i ) ; Point3D_F64 vbi = nullPts [ 1 ] . get ( i ) ; Point3D_F64 vci = nullPts [ 2 ] . get ( i ) ; for ( int j = i + 1 ; j < 3 ; j ++ , row ++ ) { Point3D_F64 cj = controlWorldPts . get ( j ) ; Point3D_F64 vaj = nullPts [ 0 ] . get ( j ) ; Point3D_F64 vbj = nullPts [ 1 ] . get ( j ) ; Point3D_F64 vcj = nullPts [ 2 ] . get ( j ) ; y . set ( row , 0 , ci . distance2 ( cj ) ) ; double xa = vai . x - vaj . x ; double ya = vai . y - vaj . y ; double za = vai . z - vaj . z ; double xb = vbi . x - vbj . x ; double yb = vbi . y - vbj . y ; double zb = vbi . z - vbj . z ; double xc = vci . x - vcj . x ; double yc = vci . y - vcj . y ; double zc = vci . z - vcj . z ; double da = xa * xa + ya * ya + za * za ; double db = xb * xb + yb * yb + zb * zb ; double dc = xc * xc + yc * yc + zc * zc ; double dab = xa * xb + ya * yb + za * zb ; double dac = xa * xc + ya * yc + za * zc ; double dbc = xb * xc + yb * yc + zb * zc ; L . set ( row , 0 , da ) ; L . set ( row , 1 , 2 * dab ) ; L . set ( row , 2 , 2 * dac ) ; L . set ( row , 3 , db ) ; L . set ( row , 4 , 2 * dbc ) ; L . set ( row , 5 , dc ) ; } } }
Linear constraint matrix for case 4 in the planar case