| 1 | #include "slalib.h"
|
|---|
| 2 | #include "slamac.h"
|
|---|
| 3 | void slaDcmpf ( double coeffs[6],
|
|---|
| 4 | double *xz, double *yz, double *xs,
|
|---|
| 5 | double *ys, double *perp, double *orient )
|
|---|
| 6 | /*
|
|---|
| 7 | ** - - - - - - - - -
|
|---|
| 8 | ** s l a D c m p f
|
|---|
| 9 | ** - - - - - - - - -
|
|---|
| 10 | **
|
|---|
| 11 | ** Decompose an [x,y] linear fit into its constituent parameters:
|
|---|
| 12 | ** zero points, scales, nonperpendicularity and orientation.
|
|---|
| 13 | **
|
|---|
| 14 | ** Given:
|
|---|
| 15 | ** coeffs double[6] transformation coefficients (see note)
|
|---|
| 16 | **
|
|---|
| 17 | ** Returned:
|
|---|
| 18 | ** *xz double x zero point
|
|---|
| 19 | ** *yz double y zero point
|
|---|
| 20 | ** *xs double x scale
|
|---|
| 21 | ** *ys double y scale
|
|---|
| 22 | ** *perp double nonperpendicularity (radians)
|
|---|
| 23 | ** *orient double orientation (radians)
|
|---|
| 24 | **
|
|---|
| 25 | ** The model relates two sets of [x,y] coordinates as follows.
|
|---|
| 26 | ** Naming the elements of coeffs:
|
|---|
| 27 | **
|
|---|
| 28 | ** coeffs[0] = a
|
|---|
| 29 | ** coeffs[1] = b
|
|---|
| 30 | ** coeffs[2] = c
|
|---|
| 31 | ** coeffs[3] = d
|
|---|
| 32 | ** coeffs[4] = e
|
|---|
| 33 | ** coeffs[5] = f
|
|---|
| 34 | **
|
|---|
| 35 | ** The model transforms coordinates [x1,y1] into coordinates
|
|---|
| 36 | ** [x2,y2] as follows:
|
|---|
| 37 | **
|
|---|
| 38 | ** x2 = a + b*x1 + c*y1
|
|---|
| 39 | ** y2 = d + e*x1 + f*y1
|
|---|
| 40 | **
|
|---|
| 41 | ** The transformation can be decomposed into four steps:
|
|---|
| 42 | **
|
|---|
| 43 | ** 1) Zero points:
|
|---|
| 44 | **
|
|---|
| 45 | ** x' = xz + x1
|
|---|
| 46 | ** y' = yz + y1
|
|---|
| 47 | **
|
|---|
| 48 | ** 2) Scales:
|
|---|
| 49 | **
|
|---|
| 50 | ** x'' = xs*x'
|
|---|
| 51 | ** y'' = ys*y'
|
|---|
| 52 | **
|
|---|
| 53 | ** 3) Nonperpendicularity:
|
|---|
| 54 | **
|
|---|
| 55 | ** x''' = cos(perp/2)*x'' + sin(perp/2)*y''
|
|---|
| 56 | ** y''' = sin(perp/2)*x'' + cos(perp/2)*y''
|
|---|
| 57 | **
|
|---|
| 58 | ** 4) Orientation:
|
|---|
| 59 | **
|
|---|
| 60 | ** x2 = cos(orient)*x''' + sin(orient)*y'''
|
|---|
| 61 | ** y2 =-sin(orient)*y''' + cos(orient)*y'''
|
|---|
| 62 | **
|
|---|
| 63 | ** See also slaFitxy, slaPxy, slaInvf, slaXy2xy
|
|---|
| 64 | **
|
|---|
| 65 | ** Last revision: 22 September 1995
|
|---|
| 66 | **
|
|---|
| 67 | ** Copyright P.T.Wallace. All rights reserved.
|
|---|
| 68 | */
|
|---|
| 69 | {
|
|---|
| 70 | double a, b, c, d, e, f, rb2e2, rc2f2, xsc, ysc, p,
|
|---|
| 71 | ws, wc, or, hp, shp, chp, sor, cor, det, x0, y0;
|
|---|
| 72 |
|
|---|
| 73 | /* Copy the six coefficients */
|
|---|
| 74 | a = coeffs[0];
|
|---|
| 75 | b = coeffs[1];
|
|---|
| 76 | c = coeffs[2];
|
|---|
| 77 | d = coeffs[3];
|
|---|
| 78 | e = coeffs[4];
|
|---|
| 79 | f = coeffs[5];
|
|---|
| 80 |
|
|---|
| 81 | /* Scales */
|
|---|
| 82 | rb2e2 = sqrt ( b * b + e * e );
|
|---|
| 83 | rc2f2 = sqrt ( c * c + f * f );
|
|---|
| 84 | if ( ( b * f - c * e ) >= 0.0 )
|
|---|
| 85 | xsc = rb2e2;
|
|---|
| 86 | else {
|
|---|
| 87 | b = -b;
|
|---|
| 88 | c = -c;
|
|---|
| 89 | xsc = -rb2e2;
|
|---|
| 90 | }
|
|---|
| 91 | ysc = rc2f2;
|
|---|
| 92 |
|
|---|
| 93 | /* Non-perpendicularity */
|
|---|
| 94 | p = ( ( c != 0.0 || f != 0.0 ) ? atan2 ( c, f ) : 0.0 ) +
|
|---|
| 95 | ( ( e != 0.0 || b != 0.0 ) ? atan2 ( e, b ) : 0.0 );
|
|---|
| 96 |
|
|---|
| 97 | /* Orientation */
|
|---|
| 98 | ws = ( c * rb2e2 ) - ( e * rc2f2 );
|
|---|
| 99 | wc = ( b * rc2f2 ) + ( f * rb2e2 );
|
|---|
| 100 | or = ( ws != 0.0 || wc != 0.0 ) ? atan2 ( ws, wc ) : 0.0;
|
|---|
| 101 |
|
|---|
| 102 | /* Zero corrections */
|
|---|
| 103 | hp = p / 2.0;
|
|---|
| 104 | shp = sin ( hp );
|
|---|
| 105 | chp = cos ( hp );
|
|---|
| 106 | sor = sin ( or );
|
|---|
| 107 | cor = cos ( or );
|
|---|
| 108 | det = xsc * ysc * ( chp + shp ) * ( chp - shp );
|
|---|
| 109 | if ( fabs ( det ) > 0.0 ) {
|
|---|
| 110 | x0 = ysc * ( a * ( ( chp * cor ) - ( shp * sor ) )
|
|---|
| 111 | - d * ( ( chp * sor ) + ( shp * cor ) ) ) / det;
|
|---|
| 112 | y0 = xsc * ( a * ( ( chp * sor ) - ( shp * cor ) )
|
|---|
| 113 | + d * ( ( chp * cor ) + ( shp * sor ) ) ) / det;
|
|---|
| 114 | }
|
|---|
| 115 | else {
|
|---|
| 116 | x0 = 0.0;
|
|---|
| 117 | y0 = 0.0;
|
|---|
| 118 | }
|
|---|
| 119 |
|
|---|
| 120 | /* Results */
|
|---|
| 121 | *xz = x0;
|
|---|
| 122 | *yz = y0;
|
|---|
| 123 | *xs = xsc;
|
|---|
| 124 | *ys = ysc;
|
|---|
| 125 | *perp = p;
|
|---|
| 126 | *orient = or;
|
|---|
| 127 | }
|
|---|
