private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
{
o_matAtA.loadZero();
double[][] t = o_matAtA.getArray();
for (int i = 0; i < i_num; i++)
{
//0
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
double wxwx = wx * wx;
double wywy = wy * wy;
double wxwy = wx * wy;
t[0][0] += wxwx;
t[0][1] += wxwy;
t[0][2] += wx;
t[1][2] += wy;
t[0][6] += (-wxwx) * (sx);
t[0][7] += (-wxwy) * (sx);
t[1][1] += wywy;
t[1][7] += (-wywy) * (sx);
t[2][6] += (-wx) * (sx);
t[2][7] += (-wy) * (sx);
t[3][6] += (-wxwx) * (sy);
t[3][7] += (-wxwy) * (sy);
t[4][7] += (-wywy) * (sy);
t[5][6] += (-wx) * (sy);
t[5][7] += (-wy) * (sy);
t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
}
t[1][0] = t[3][4] = t[4][3] = t[0][1];
t[2][0] = t[3][5] = t[5][3] = t[0][2];
t[2][1] = t[5][4] = t[4][5] = t[1][2];
t[7][0] = t[6][1] = t[1][6] = t[0][7];
t[4][6] = t[6][4] = t[7][3] = t[3][7];
t[2][2] = t[5][5] = i_num;
t[3][3] = t[0][0];
t[4][4] = t[1][1];
t[6][0] = t[0][6];
t[6][2] = t[2][6];
t[6][3] = t[3][6];
t[6][5] = t[5][6];
t[7][1] = t[1][7];
t[7][2] = t[2][7];
t[7][4] = t[4][7];
t[7][5] = t[5][7];
t[7][6] = t[6][7];
//先頭でゼロクリアしない場合。
//t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
return(o_matAtA);
}
public NyARMat makeJtJ(NyARMat o_dst)
{
o_dst.loadZero();
Item[] buf = this._items;
double[][] b = o_dst.getArray();
for (int k = 0; k < this._length; k++)
{
Item ptr = buf[k];
b[0][0] += (ptr.m00 * ptr.m00) + (ptr.m10 * ptr.m10);
b[0][1] += (ptr.m00 * ptr.m01) + (ptr.m10 * ptr.m11);
b[0][2] += (ptr.m00 * ptr.m02) + (ptr.m10 * ptr.m12);
b[0][3] += (ptr.m00 * ptr.m03) + (ptr.m10 * ptr.m13);
b[0][4] += (ptr.m00 * ptr.m04) + (ptr.m10 * ptr.m14);
b[0][5] += (ptr.m00 * ptr.m05) + (ptr.m10 * ptr.m15);
b[1][0] += (ptr.m01 * ptr.m00) + (ptr.m11 * ptr.m10);
b[1][1] += (ptr.m01 * ptr.m01) + (ptr.m11 * ptr.m11);
b[1][2] += (ptr.m01 * ptr.m02) + (ptr.m11 * ptr.m12);
b[1][3] += (ptr.m01 * ptr.m03) + (ptr.m11 * ptr.m13);
b[1][4] += (ptr.m01 * ptr.m04) + (ptr.m11 * ptr.m14);
b[1][5] += (ptr.m01 * ptr.m05) + (ptr.m11 * ptr.m15);
b[2][0] += (ptr.m02 * ptr.m00) + (ptr.m12 * ptr.m10);
b[2][1] += (ptr.m02 * ptr.m01) + (ptr.m12 * ptr.m11);
b[2][2] += (ptr.m02 * ptr.m02) + (ptr.m12 * ptr.m12);
b[2][3] += (ptr.m02 * ptr.m03) + (ptr.m12 * ptr.m13);
b[2][4] += (ptr.m02 * ptr.m04) + (ptr.m12 * ptr.m14);
b[2][5] += (ptr.m02 * ptr.m05) + (ptr.m12 * ptr.m15);
b[3][0] += (ptr.m03 * ptr.m00) + (ptr.m13 * ptr.m10);
b[3][1] += (ptr.m03 * ptr.m01) + (ptr.m13 * ptr.m11);
b[3][2] += (ptr.m03 * ptr.m02) + (ptr.m13 * ptr.m12);
b[3][3] += (ptr.m03 * ptr.m03) + (ptr.m13 * ptr.m13);
b[3][4] += (ptr.m03 * ptr.m04) + (ptr.m13 * ptr.m14);
b[3][5] += (ptr.m03 * ptr.m05) + (ptr.m13 * ptr.m15);
b[4][0] += (ptr.m04 * ptr.m00) + (ptr.m14 * ptr.m10);
b[4][1] += (ptr.m04 * ptr.m01) + (ptr.m14 * ptr.m11);
b[4][2] += (ptr.m04 * ptr.m02) + (ptr.m14 * ptr.m12);
b[4][3] += (ptr.m04 * ptr.m03) + (ptr.m14 * ptr.m13);
b[4][4] += (ptr.m04 * ptr.m04) + (ptr.m14 * ptr.m14);
b[4][5] += (ptr.m04 * ptr.m05) + (ptr.m14 * ptr.m15);
b[5][0] += (ptr.m05 * ptr.m00) + (ptr.m15 * ptr.m10);
b[5][1] += (ptr.m05 * ptr.m01) + (ptr.m15 * ptr.m11);
b[5][2] += (ptr.m05 * ptr.m02) + (ptr.m15 * ptr.m12);
b[5][3] += (ptr.m05 * ptr.m03) + (ptr.m15 * ptr.m13);
b[5][4] += (ptr.m05 * ptr.m04) + (ptr.m15 * ptr.m14);
b[5][5] += (ptr.m05 * ptr.m05) + (ptr.m15 * ptr.m15);
}
return(o_dst);
}
private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
{
o_matAtA.loadZero();
double[][] t = o_matAtA.getArray();
for (int i = 0; i < i_num; i++)
{
//0
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
double wxwx = wx * wx;
double wywy = wy * wy;
double wxwy = wx * wy;
t[0][0] += wxwx;
t[0][1] += wxwy;
t[0][2] += wx;
t[1][2] += wy;
t[0][6] += (-wxwx) * (sx);
t[0][7] += (-wxwy) * (sx);
t[1][1] += wywy;
t[1][7] += (-wywy) * (sx);
t[2][6] += (-wx) * (sx);
t[2][7] += (-wy) * (sx);
t[3][6] += (-wxwx) * (sy);
t[3][7] += (-wxwy) * (sy);
t[4][7] += (-wywy) * (sy);
t[5][6] += (-wx) * (sy);
t[5][7] += (-wy) * (sy);
t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
}
t[1][0] = t[3][4] = t[4][3] = t[0][1];
t[2][0] = t[3][5] = t[5][3] = t[0][2];
t[2][1] = t[5][4] = t[4][5] = t[1][2];
t[7][0] = t[6][1] = t[1][6] = t[0][7];
t[4][6] = t[6][4] = t[7][3] = t[3][7];
t[2][2] = t[5][5] = i_num;
t[3][3] = t[0][0];
t[4][4] = t[1][1];
t[6][0] = t[0][6];
t[6][2] = t[2][6];
t[6][3] = t[3][6];
t[6][5] = t[5][6];
t[7][1] = t[1][7];
t[7][2] = t[2][7];
t[7][4] = t[4][7];
t[7][5] = t[5][7];
t[7][6] = t[6][7];
//先頭でゼロクリアしない場合。
//t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
return o_matAtA;
}
