00001 #ifndef __J2K__Position_CPP__
00002 #define __J2K__Position_CPP__
00003
00004 #include <j2k/Fred/3d/pos.hpp>
00005 #include <alloc.h>
00006 #include <mem.h>
00007 #include <math.h>
00008 #include <string.h>
00009
00010 void MatSwapProd(
00011 realtype* m1,
00012 realtype* m2,
00013 int l,
00014 int m,
00015 int n,
00016 realtype* m3
00017 )
00018 {
00019 int i, j, k, in, im;
00020 realtype*m4,*m5, tmp;
00021
00022 if ( m1 == m3 ) {
00023 m4 =(realtype*) calloc ( l*m, sizeof(realtype) );
00024 memmove( m4,m1,l*m*sizeof(realtype) );
00025 }
00026 else m4 = m1;
00027
00028 if ( m2 == m3 )
00029 {
00030 m5 = (realtype*)calloc ( n*m, sizeof(realtype) );
00031 memmove( m5,m2,n*m*sizeof(realtype) );
00032 }
00033 else m5 = m2;
00034
00035 for ( i = 0; i < l; i++ ) {
00036 in = i* n;
00037 im = i* m;
00038 for ( j = 0; j < n; j++ ) {
00039 m3[in+j] = 0;
00040 for ( k = 0; k < m; k++ ) {
00041 tmp = m4[im+k]* m5[k*n+j];
00042 m3[in+j] += tmp;
00043 }
00044 }
00045 }
00046 if ( m1 == m3 ) free(m4);
00047 if ( m2 == m3 ) free(m5);
00048 }
00049
00050 void SetIdentityMat ( realtype*m1, int m )
00051 { int i;
00052 SetNullMat(m1,m,m);
00053 for ( i = 0; i != m; i ++ ) m1[ i*m + i] = 1; }
00054
00055 void SetNullMat ( realtype*m1, int m,int n )
00056 { memset( m1, 0x0000, m*n*sizeof(realtype) );}
00057
00058
00059 #define ENDOFSTRING '\x0'
00060 #define SEPARATEUR ';'
00061
00062 int ComputeStringTransform( char* s, Matrix44*transform )
00063 {
00064 int pos = -1, end;
00065 char *sbegin1 = s,*sbegin, commande[3];
00066
00067 SetIdentityMat ((realtype*)*transform, 4 );
00068
00069 if ( s == NULL ||*s == ENDOFSTRING ||
00070 strcmp(s,IDENTITY) == SUCCESS ) return SUCCESS;
00071
00072 end = strlen( s )-1;
00073 commande[2] = ENDOFSTRING;
00074
00075 while ( pos != end )
00076 {
00077 sbegin = strchr ( sbegin1, SEPARATEUR );
00078 pos += sbegin - sbegin1 + 1;
00079 *sbegin = ENDOFSTRING;
00080
00081
00082
00083 commande[0] = sbegin1[0];
00084 commande[1] = sbegin1[1];
00085
00086
00087
00088 __ComputeTransform( commande, sbegin1+2, transform );
00089
00090
00091 *sbegin = SEPARATEUR;
00092 sbegin1 = sbegin+1;
00093 }
00094
00095 return SUCCESS; }
00096
00097 void __ComputeTransform( char* com, char* val, Matrix44 *m )
00098 {
00099 double nv;
00100
00101 if ( com[0] == 'R' || com[0] == 'r' )
00102 {
00103 nv = atof( val );
00104 __Rotate( nv, com[1], m );
00105 return; }
00106
00107 if ( com[0] == 'T' || com[0] == 't' )
00108 {
00109 nv = atof( val );
00110 __Translate( nv, com[1], m );
00111 return; }
00112
00113 if ( com[0] == 'E' || com[0] == 'e' )
00114 {
00115 nv = atof( val );
00116 __Resize( nv, com[1], m );
00117 return; }
00118 }
00119
00120 void __Rotate( double nv, char axe, Matrix44*m )
00121 { Matrix44 matrix;
00122 realtype cos, sin;
00123
00124 SetIdentityMat ((realtype*) matrix, 4 );
00125 cos = cos ( DegreeToRad(nv) );
00126 sin = sin ( DegreeToRad(nv) );
00127
00128 do{
00129 if ( axe == 'X' || axe == 'x' )
00130 {
00131 matrix[_Y_][_Y_] = cos;
00132 matrix[_Z_][_Z_] = cos;
00133 matrix[_Z_][_Y_] = -sin;
00134 matrix[_Y_][_Z_] = sin;
00135
00136 break; }
00137
00138 if ( axe == 'Y' || axe == 'y' )
00139 {
00140 matrix[_X_][_X_] = cos;
00141 matrix[_Z_][_Z_] = cos;
00142 matrix[_X_][_Z_] = -sin;
00143 matrix[_Z_][_X_] = sin;
00144
00145 break; }
00146
00147 if ( axe == 'Z' || axe == 'z' )
00148 {
00149 matrix[_X_][_X_] = cos;
00150 matrix[_Y_][_Y_] = cos;
00151 matrix[_Y_][_X_] = -sin;
00152 matrix[_X_][_Y_] = sin;
00153
00154 break; }
00155
00156 } while (1);
00157
00158 MatSwapProd( (realtype*)*m, (realtype*)matrix, 4, 4, 4, (realtype*)*m );
00159 }
00160
00161 void __Translate( double nv, char axe, Matrix44*m )
00162 { Matrix44 matrix;
00163
00164 SetIdentityMat ((realtype*) matrix, 4 );
00165
00166 do{
00167 if ( axe == 'X' || axe == 'x' )
00168 {
00169 matrix[_W_][_X_] = nv;
00170 break; }
00171
00172 if ( axe == 'Y' || axe == 'y' )
00173 {
00174 matrix[_W_][_Y_] = nv;
00175 break; }
00176
00177 if ( axe == 'Z' || axe == 'z' )
00178 {
00179 matrix[_W_][_Z_] = nv;
00180 break; }
00181
00182 } while (1);
00183
00184 MatSwapProd( (realtype*)*m, (realtype*)matrix, 4, 4, 4, (realtype*)*m );
00185 }
00186
00187 void __Resize( double nv, char axe, Matrix44*m ) {
00188
00189 Matrix44 matrix;
00190
00191 SetIdentityMat ((realtype*) matrix, 4 );
00192
00193 do {
00194 if ( axe == 'X' || axe == 'x' )
00195 {
00196 matrix[_X_][_X_] = nv;
00197 break;
00198 }
00199
00200 if ( axe == 'Y' || axe == 'y' )
00201 {
00202 matrix[_Y_][_Y_] = nv;
00203 break;
00204 }
00205
00206 if ( axe == 'Z' || axe == 'z' )
00207 {
00208 matrix[_Z_][_Z_] = nv;
00209 break;
00210 }
00211
00212 } while (1);
00213
00214 MatSwapProd( (realtype*)*m, (realtype*)matrix, 4, 4, 4, (realtype*)*m );
00215 }
00216
00217 #endif