Robot

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers TransformationMatrix.cpp Source File

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