- matrix[ 0] = c*cstd[ 0];
- matrix[ 1] = c*cstd[ 1]*s*cosf(h) - c*cstd[ 2]*s*sinf(h);
- matrix[ 2] = c*cstd[ 2]*s*cosf(h) + c*cstd[ 1]*s*sinf(h);
- matrix[ 3] = cstd[ 3] + cstd[ 0]*(b + c*ybias) + cstd[ 1]*(c*cbbias*s*cosf(h) + c*crbias*s*sinf(h)) + cstd[ 2]*(c*crbias*s*cosf(h) - c*cbbias*s*sinf(h));
-
- matrix[ 4] = c*cstd[ 4];
- matrix[ 5] = c*cstd[ 5]*s*cosf(h) - c*cstd[ 6]*s*sinf(h);
- matrix[ 6] = c*cstd[ 6]*s*cosf(h) + c*cstd[ 5]*s*sinf(h);
- matrix[ 7] = cstd[ 7] + cstd[ 4]*(b + c*ybias) + cstd[ 5]*(c*cbbias*s*cosf(h) + c*crbias*s*sinf(h)) + cstd[ 6]*(c*crbias*s*cosf(h) - c*cbbias*s*sinf(h));
-
- matrix[ 8] = c*cstd[ 8];
- matrix[ 9] = c*cstd[ 9]*s*cosf(h) - c*cstd[10]*s*sinf(h);
- matrix[10] = c*cstd[10]*s*cosf(h) + c*cstd[ 9]*s*sinf(h);
- matrix[11] = cstd[11] + cstd[ 8]*(b + c*ybias) + cstd[ 9]*(c*cbbias*s*cosf(h) + c*crbias*s*sinf(h)) + cstd[10]*(c*crbias*s*cosf(h) - c*cbbias*s*sinf(h));
-
- matrix[12] = c*cstd[12];
- matrix[13] = c*cstd[13]*s*cos(h) - c*cstd[14]*s*sin(h);
- matrix[14] = c*cstd[14]*s*cos(h) + c*cstd[13]*s*sin(h);
- matrix[15] = cstd[15] + cstd[12]*(b + c*ybias) + cstd[13]*(c*cbbias*s*cos(h) + c*crbias*s*sin(h)) + cstd[14]*(c*crbias*s*cos(h) - c*cbbias*s*sin(h));
+ (*matrix)[0][0] = c * (*cstd)[0][0];
+ (*matrix)[0][1] = (*cstd)[0][1] * x - (*cstd)[0][2] * y;
+ (*matrix)[0][2] = (*cstd)[0][2] * x + (*cstd)[0][1] * y;
+ (*matrix)[0][3] = (*cstd)[0][3] + (*cstd)[0][0] * b +
+ (*cstd)[0][1] * (x * cbbias + y * crbias) +
+ (*cstd)[0][2] * (x * crbias - y * cbbias);
+
+ (*matrix)[1][0] = c * (*cstd)[1][0];
+ (*matrix)[1][1] = (*cstd)[1][1] * x - (*cstd)[1][2] * y;
+ (*matrix)[1][2] = (*cstd)[1][2] * x + (*cstd)[1][1] * y;
+ (*matrix)[1][3] = (*cstd)[1][3] + (*cstd)[1][0] * b +
+ (*cstd)[1][1] * (x * cbbias + y * crbias) +
+ (*cstd)[1][2] * (x * crbias - y * cbbias);
+
+ (*matrix)[2][0] = c * (*cstd)[2][0];
+ (*matrix)[2][1] = (*cstd)[2][1] * x - (*cstd)[2][2] * y;
+ (*matrix)[2][2] = (*cstd)[2][2] * x + (*cstd)[2][1] * y;
+ (*matrix)[2][3] = (*cstd)[2][3] + (*cstd)[2][0] * b +
+ (*cstd)[2][1] * (x * cbbias + y * crbias) +
+ (*cstd)[2][2] * (x * crbias - y * cbbias);