Robot
Embed:
(wiki syntax)
Show/hide line numbers
TransformationMatrix.cpp
00001 // 00002 // Created by m007 on 29/06/2020. 00003 // 00004 00005 #include "Ro/TransformationMatrix.h" 00006 void TransformationMatrix::Clone(Matrix<float> Mat){ 00007 for (int i = 0; i <this->rows ; i++){ 00008 for (int j = 0; j < this->columns; j++){ 00009 this->n[i][j] = Mat(i,j); 00010 } 00011 } 00012 } 00013 TransformationMatrix::TransformationMatrix():Matrix<float>(4,4){} 00014 00015 Matrix<float>& TransformationMatrix::RotX(float thetaX){ 00016 float STheta = sinf(thetaX); 00017 float CTheta = cosf( thetaX); 00018 for(int x=0;x<4;x++){ 00019 for(int y=0; y<4; y++){ 00020 if ((x == 0 && y == 0) || (x == 3 && y == 3) ) { 00021 this->n[x][y]=1; 00022 } else if (x == 1 && y == 1) { 00023 this->n[x][y]=CTheta; 00024 } else if (x == 2 && y == 1) { 00025 this->n[x][y]=STheta; 00026 } else if (x == 1 && y == 2) { 00027 this->n[x][y]=-STheta; 00028 } else if (x == 2 && y == 2) { 00029 this->n[x][y]=CTheta; 00030 }else{ 00031 this->n[x][y]=0; 00032 } 00033 if( (this->n[x][y]>-0.000001F)&&(this->n[x][y]<0.001F) ){ 00034 this->n[x][y]=0; 00035 } 00036 } 00037 } 00038 return *this; 00039 } 00040 Matrix<float>& TransformationMatrix::RotY(float thetaY){ 00041 float STheta = sinf(thetaY); 00042 float CTheta = cosf( thetaY); 00043 for(int x=0;x<4;x++){ 00044 for(int y=0; y<4; y++){ 00045 if ((x == 1 && y == 1) || (x == 3 && y == 3) ) { 00046 this->n[x][y]=1; 00047 } else if (x == 0 && y == 0) { 00048 this->n[x][y]=CTheta; 00049 } else if (x == 0 && y == 2) { 00050 this->n[x][y]=STheta; 00051 } else if (x == 2 && y == 0) { 00052 this->n[x][y]=-STheta; 00053 } else if (x == 2 && y == 2) { 00054 this->n[x][y]=CTheta; 00055 }else{ 00056 this->n[x][y]=0; 00057 } 00058 if( (this->n[x][y]>-0.000001F)&&(this->n[x][y]<0.001F) ){ 00059 this->n [x][y]=0; 00060 } 00061 } 00062 } 00063 return *this; 00064 } 00065 Matrix<float>& TransformationMatrix::RotZ(float thetaZ){ 00066 float STheta = sinf(thetaZ); 00067 float CTheta = cosf( thetaZ); 00068 for(int x=0;x<4;x++){ 00069 for(int y=0; y<4; y++){ 00070 if ((x == 2 && y == 2) || (x == 3 && y == 3) ) { 00071 this->n[x][y]=1; 00072 } else if (x == 0 && y == 0) { 00073 this->n[x][y]=CTheta; 00074 } else if (x == 0 && y == 1) { 00075 this->n[x][y]=-STheta; 00076 } else if (x == 1 && y == 0) { 00077 this->n[x][y]=STheta; 00078 } else if (x == 1 && y == 1) { 00079 this->n[x][y]=CTheta; 00080 }else{ 00081 this->n[x][y]=0; 00082 } 00083 if( (this->n[x][y]>-0.000001F)&&(this->n[x][y]<0.001F) ){ 00084 this->n [x][y]=0; 00085 } 00086 } 00087 } 00088 return *this; 00089 } 00090 00091 00092 Matrix<float> TransformationMatrix::RotXInner(float thetaX){ 00093 Matrix<float> Mat(4,4); 00094 float STheta = sinf(thetaX); 00095 float CTheta = cosf( thetaX); 00096 for(int x=0;x<4;x++){ 00097 for(int y=0; y<4; y++){ 00098 if ((x == 0 && y == 0) || (x == 3 && y == 3) ) { 00099 Mat(x,y)=1; 00100 } else if (x == 1 && y == 1) { 00101 Mat(x,y)=CTheta; 00102 } else if (x == 2 && y == 1) { 00103 Mat(x,y)=STheta; 00104 } else if (x == 1 && y == 2) { 00105 Mat(x,y)=-STheta; 00106 } else if (x == 2 && y == 2) { 00107 Mat(x,y)=CTheta; 00108 }else{ 00109 Mat(x,y)=0; 00110 } 00111 if( (Mat(x,y)>-0.000001F)&&(Mat(x,y)<0.001F) ){ 00112 Mat(x,y)=0; 00113 } 00114 } 00115 } 00116 return Mat; 00117 } 00118 Matrix<float> TransformationMatrix::RotYInner(float thetaY){ 00119 Matrix<float> Mat(4,4); 00120 float STheta = sinf(thetaY); 00121 float CTheta = cosf( thetaY); 00122 for(int x=0;x<4;x++){ 00123 for(int y=0; y<4; y++){ 00124 if ((x == 1 && y == 1) || (x == 3 && y == 3) ) { 00125 Mat(x,y)=1; 00126 } else if (x == 0 && y == 0) { 00127 Mat(x,y)=CTheta; 00128 } else if (x == 0 && y == 2) { 00129 Mat(x,y)=STheta; 00130 } else if (x == 2 && y == 0) { 00131 Mat(x,y)=-STheta; 00132 } else if (x == 2 && y == 2) { 00133 Mat(x,y)=CTheta; 00134 }else{ 00135 Mat(x,y)=0; 00136 } 00137 if( (Mat(x,y)>-0.000001F)&&(Mat(x,y)<0.001F) ){ 00138 this->n [x][y]=0; 00139 } 00140 } 00141 } 00142 return Mat; 00143 } 00144 Matrix<float> TransformationMatrix::RotZInner(float thetaZ){ 00145 Matrix<float> Mat(4,4); 00146 float STheta = sinf(thetaZ); 00147 float CTheta = cosf( thetaZ); 00148 for(int x=0;x<4;x++){ 00149 for(int y=0; y<4; y++){ 00150 if ((x == 2 && y == 2) || (x == 3 && y == 3) ) { 00151 Mat(x,y)=1; 00152 } else if (x == 0 && y == 0) { 00153 Mat(x,y)=CTheta; 00154 } else if (x == 0 && y == 1) { 00155 Mat(x,y)=-STheta; 00156 } else if (x == 1 && y == 0) { 00157 Mat(x,y)=STheta; 00158 } else if (x == 1 && y == 1) { 00159 Mat(x,y)=CTheta; 00160 }else{ 00161 Mat(x,y)=0; 00162 } 00163 if( (Mat(x,y)>-0.000001F)&&(Mat(x,y)<0.001F) ){ 00164 Mat(x,y)=0; 00165 } 00166 } 00167 } 00168 return Mat; 00169 } 00170 00171 Matrix<float>& TransformationMatrix::EulerXZX(float thetaX1,float thetaZ2,float thetaX3){ 00172 Clone(RotXInner(thetaX1)*RotZInner(thetaZ2)*RotXInner(thetaX3)); 00173 return *this; 00174 } 00175 Matrix<float>& TransformationMatrix::EulerXYX(float thetaX1,float thetaY2,float thetaX3){ 00176 Clone(RotZInner(thetaX1)*RotXInner(thetaY2)*RotZInner(thetaX3)); 00177 return *this; 00178 } 00179 Matrix<float>& TransformationMatrix::EulerYXY(float thetaY1,float thetaX2,float thetaY3){ 00180 Clone (RotZInner(thetaY1)*RotXInner(thetaX2)*RotZInner(thetaY3)); 00181 return *this; 00182 } 00183 Matrix<float>& TransformationMatrix::EulerYZY(float thetaY1,float thetaZ2,float thetaY3){ 00184 Clone (RotZInner(thetaY1)*RotXInner(thetaZ2)*RotZInner(thetaY3)); 00185 return *this; 00186 } 00187 Matrix<float>& TransformationMatrix::EulerZYZ(float thetaZ1,float thetaY2,float thetaZ3){ 00188 Clone (RotZInner(thetaZ1)*RotXInner(thetaY2)*RotZInner(thetaZ3)); 00189 return *this; 00190 } 00191 Matrix<float>& TransformationMatrix::EulerZXZ(float thetaZ1,float thetaX2,float thetaZ3){ 00192 Clone (RotZInner(thetaZ1)*RotXInner(thetaX2)*RotZInner(thetaZ3)); 00193 return *this; 00194 } 00195 00196 Matrix<float>& TransformationMatrix::EulerXZY(float thetaX1,float thetaZ2,float thetaY3){ 00197 Clone (RotZInner(thetaX1)*RotXInner(thetaZ2)*RotZInner(thetaY3)); 00198 return *this; 00199 } 00200 Matrix<float>& TransformationMatrix::EulerXYZ(float thetaX1,float thetaY2,float thetaZ3){ 00201 Clone (RotZInner(thetaX1)*RotXInner(thetaY2)*RotZInner(thetaZ3)); 00202 return *this; 00203 } 00204 Matrix<float>& TransformationMatrix::EulerYXZ(float thetaY1,float thetaX2,float thetaZ3){ 00205 Clone (RotZInner(thetaY1)*RotXInner(thetaX2)*RotZInner(thetaZ3)); 00206 return *this; 00207 } 00208 Matrix<float>& TransformationMatrix::EulerYZX(float thetaY1,float thetaZ2,float thetaX3){ 00209 Clone (RotZInner(thetaY1)*RotXInner(thetaZ2)*RotZInner(thetaX3)); 00210 } 00211 Matrix<float>& TransformationMatrix::EulerZYX(float thetaZ1,float thetaY2,float thetaX3){ 00212 Clone (RotZInner(thetaZ1)*RotXInner(thetaY2)*RotZInner(thetaX3)); 00213 return *this; 00214 } 00215 Matrix<float>& TransformationMatrix::EulerZXY(float thetaZ1,float thetaX2,float thetaY3){ 00216 Clone (RotZInner(thetaZ1)*RotXInner(thetaX2)*RotZInner(thetaY3)); 00217 return *this; 00218 } 00219 00220 00221 Matrix<float>& TransformationMatrix::Translation(float dx, float dy, float dz){ 00222 for(int x=0;x<4;x++){ 00223 for(int y=0; y<4; y++){ 00224 if ( (x == 0 && y == 0) || (x == 1 && y == 1) || (x == 2 && y == 2) || (x == 3 && y == 3) ) { 00225 this->n[x][y]=1; 00226 } else if (x == 0 && y == 3) { 00227 this->n[x][y]=dx; 00228 } else if (x == 1 && y == 3) { 00229 this->n[x][y]=dy; 00230 } else if (x == 2 && y == 3) { 00231 this->n[x][y]=dz; 00232 }else{ 00233 this->n[x][y]=0; 00234 } 00235 } 00236 } 00237 return *this; 00238 } 00239 00240
Generated on Thu Jul 14 2022 08:46:33 by
1.7.2