Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 204 | chris | 1 | #include <stdio.h> |
| 2 | #include <math.h> |
||
| 3 | #include "util.h" |
||
| 4 | |||
| 5 | void get_rot( double a, double b , double c, double trans[3][4] ) |
||
| 6 | { |
||
| 7 | double sina, sinb, sinc, cosa, cosb, cosc; |
||
| 8 | |||
| 9 | sina = sin(a); cosa = cos(a); |
||
| 10 | sinb = sin(b); cosb = cos(b); |
||
| 11 | sinc = sin(c); cosc = cos(c); |
||
| 12 | trans[0][0] = cosa*cosa*cosb*cosc+sina*sina*cosc+sina*cosa*cosb*sinc-sina*cosa*sinc; |
||
| 13 | trans[0][1] = -cosa*cosa*cosb*sinc-sina*sina*sinc+sina*cosa*cosb*cosc-sina*cosa*cosc; |
||
| 14 | trans[0][2] = cosa*sinb; |
||
| 15 | trans[1][0] = sina*cosa*cosb*cosc-sina*cosa*cosc+sina*sina*cosb*sinc+cosa*cosa*sinc; |
||
| 16 | trans[1][1] = -sina*cosa*cosb*sinc+sina*cosa*sinc+sina*sina*cosb*cosc+cosa*cosa*cosc; |
||
| 17 | trans[1][2] = sina*sinb; |
||
| 18 | trans[2][0] = -cosa*sinb*cosc-sina*sinb*sinc; |
||
| 19 | trans[2][1] = cosa*sinb*sinc-sina*sinb*cosc; |
||
| 20 | trans[2][2] = cosb; |
||
| 21 | |||
| 22 | return; |
||
| 23 | } |
||
| 24 | |||
| 25 | int get_angle( double trans[3][4], double *wa, double *wb, double *wc ) |
||
| 26 | { |
||
| 27 | double a, b, c; |
||
| 28 | double sina, cosa, sinb, cosb, sinc, cosc; |
||
| 29 | |||
| 30 | if( trans[2][2] > 1.0 ) { |
||
| 31 | printf("cos(beta) = %f\n", trans[2][2]); |
||
| 32 | trans[2][2] = -1.0; |
||
| 33 | } |
||
| 34 | else if( trans[2][2] < -1.0 ) { |
||
| 35 | printf("cos(beta) = %f\n", trans[2][2]); |
||
| 36 | trans[2][2] = -1.0; |
||
| 37 | } |
||
| 38 | cosb = trans[2][2]; |
||
| 39 | b = acos( cosb ); |
||
| 40 | sinb = sin( b ); |
||
| 41 | if( b >= 0.000001 || b <= -0.000001) { |
||
| 42 | cosa = trans[0][2] / sinb; |
||
| 43 | sina = trans[1][2] / sinb; |
||
| 44 | if( cosa > 1.0 ) { |
||
| 45 | printf("cos(alph) = %f\n", cosa); |
||
| 46 | cosa = 1.0; |
||
| 47 | sina = 0.0; |
||
| 48 | } |
||
| 49 | if( cosa < -1.0 ) { |
||
| 50 | printf("cos(alph) = %f\n", cosa); |
||
| 51 | cosa = -1.0; |
||
| 52 | sina = 0.0; |
||
| 53 | } |
||
| 54 | if( sina > 1.0 ) { |
||
| 55 | printf("sin(alph) = %f\n", sina); |
||
| 56 | sina = 1.0; |
||
| 57 | cosa = 0.0; |
||
| 58 | } |
||
| 59 | if( sina < -1.0 ) { |
||
| 60 | printf("sin(alph) = %f\n", sina); |
||
| 61 | sina = -1.0; |
||
| 62 | cosa = 0.0; |
||
| 63 | } |
||
| 64 | a = acos( cosa ); |
||
| 65 | if( sina < 0 ) a = -a; |
||
| 66 | |||
| 67 | sinc = (trans[2][1]*trans[0][2]-trans[2][0]*trans[1][2]) |
||
| 68 | / (trans[0][2]*trans[0][2]+trans[1][2]*trans[1][2]); |
||
| 69 | cosc = -(trans[0][2]*trans[2][0]+trans[1][2]*trans[2][1]) |
||
| 70 | / (trans[0][2]*trans[0][2]+trans[1][2]*trans[1][2]); |
||
| 71 | if( cosc > 1.0 ) { |
||
| 72 | printf("cos(r) = %f\n", cosc); |
||
| 73 | cosc = 1.0; |
||
| 74 | sinc = 0.0; |
||
| 75 | } |
||
| 76 | if( cosc < -1.0 ) { |
||
| 77 | printf("cos(r) = %f\n", cosc); |
||
| 78 | cosc = -1.0; |
||
| 79 | sinc = 0.0; |
||
| 80 | } |
||
| 81 | if( sinc > 1.0 ) { |
||
| 82 | printf("sin(r) = %f\n", sinc); |
||
| 83 | sinc = 1.0; |
||
| 84 | cosc = 0.0; |
||
| 85 | } |
||
| 86 | if( sinc < -1.0 ) { |
||
| 87 | printf("sin(r) = %f\n", sinc); |
||
| 88 | sinc = -1.0; |
||
| 89 | cosc = 0.0; |
||
| 90 | } |
||
| 91 | c = acos( cosc ); |
||
| 92 | if( sinc < 0 ) c = -c; |
||
| 93 | } |
||
| 94 | else { |
||
| 95 | a = b = 0.0; |
||
| 96 | cosa = cosb = 1.0; |
||
| 97 | sina = sinb = 0.0; |
||
| 98 | cosc = trans[0][0]; |
||
| 99 | sinc = trans[1][0]; |
||
| 100 | if( cosc > 1.0 ) { |
||
| 101 | printf("cos(r) = %f\n", cosc); |
||
| 102 | cosc = 1.0; |
||
| 103 | sinc = 0.0; |
||
| 104 | } |
||
| 105 | if( cosc < -1.0 ) { |
||
| 106 | printf("cos(r) = %f\n", cosc); |
||
| 107 | cosc = -1.0; |
||
| 108 | sinc = 0.0; |
||
| 109 | } |
||
| 110 | if( sinc > 1.0 ) { |
||
| 111 | printf("sin(r) = %f\n", sinc); |
||
| 112 | sinc = 1.0; |
||
| 113 | cosc = 0.0; |
||
| 114 | } |
||
| 115 | if( sinc < -1.0 ) { |
||
| 116 | printf("sin(r) = %f\n", sinc); |
||
| 117 | sinc = -1.0; |
||
| 118 | cosc = 0.0; |
||
| 119 | } |
||
| 120 | c = acos( cosc ); |
||
| 121 | if( sinc < 0 ) c = -c; |
||
| 122 | } |
||
| 123 | |||
| 124 | *wa = a; |
||
| 125 | *wb = b; |
||
| 126 | *wc = c; |
||
| 127 | |||
| 128 | return(0); |
||
| 129 | } |
||
| 130 | |||
| 131 | double get_height( double px, double py, double trans[3][4], double boundary[3][2] ) |
||
| 132 | { |
||
| 133 | double x, x0, x1, x2; |
||
| 134 | double y, y0, y1, y2; |
||
| 135 | double d, m00, m01, m10, m11; |
||
| 136 | double rx, ry; |
||
| 137 | |||
| 138 | x = boundary[0][0]; |
||
| 139 | y = boundary[1][0]; |
||
| 140 | x0 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; |
||
| 141 | y0 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; |
||
| 142 | x = boundary[0][1]; |
||
| 143 | y = boundary[1][0]; |
||
| 144 | x1 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; |
||
| 145 | y1 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; |
||
| 146 | x = boundary[0][0]; |
||
| 147 | y = boundary[1][1]; |
||
| 148 | x2 = trans[0][0]*x + trans[0][1]*y + trans[0][3]; |
||
| 149 | y2 = trans[1][0]*x + trans[1][1]*y + trans[1][3]; |
||
| 150 | |||
| 151 | x1 -= x0; y1 -= y0; |
||
| 152 | x2 -= x0; y2 -= y0; |
||
| 153 | x0 = px - x0; |
||
| 154 | y0 = py - y0; |
||
| 155 | |||
| 156 | d = x1*y2 - x2*y1; |
||
| 157 | if( d == 0.0 ) return 0.0; |
||
| 158 | m00 = y2 / d; |
||
| 159 | m01 = -x2 / d; |
||
| 160 | m10 = -y1 / d; |
||
| 161 | m11 = x1 / d; |
||
| 162 | |||
| 163 | rx = m00*x0 + m01*y0; |
||
| 164 | ry = m10*x0 + m11*y0; |
||
| 165 | |||
| 166 | if( rx >= 0.0 && rx <= 1.0 && ry >= 0.0 && ry <= 1.0 ) { |
||
| 167 | return trans[2][3] + boundary[2][1]; |
||
| 168 | } |
||
| 169 | |||
| 170 | return 0.0; |
||
| 171 | } |