diff options
Diffstat (limited to 'sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp')
| -rw-r--r-- | sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp | 205 |
1 files changed, 205 insertions, 0 deletions
diff --git a/sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp b/sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp new file mode 100644 index 0000000..fab0075 --- /dev/null +++ b/sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp @@ -0,0 +1,205 @@ +/*************************************************** +Author : Tanmay Chaudhari + **************************************************/ + +#include <bits/stdc++.h> +using namespace std; +extern "C" +{ +#include "api_scilab.h" +#include "Scierror.h" +#include "BOOL.h" +#include <localization.h> +#include "sciprint.h" + + int opencv_rotationVectorToMatrix(char *fname, unsigned long fname_len) + { + + //Error management variable + SciErr sciErr; + + //Variable declaration + int iComplex = 0; + int iType = 0; + int iRows = 0; + int iCols = 0; + int *piAddr = NULL; + double sinOfangle = 0; + double cosOfangle = 0; + double t, x, y, z; + double theta; + double *pdbl = NULL; + double *matrix; + double *identity; + double *A; + double *B; + + //Check input output arguments + checkInputArgument(pvApiCtx, 1, 1); + checkOutputArgument(pvApiCtx, 1, 1); + + //Get variable address of the first input argument + sciErr = getVarAddressFromPosition(pvApiCtx, 1, &piAddr); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + + //Check type + sciErr = getVarType(pvApiCtx, piAddr, &iType); + if(sciErr.iErr || iType != sci_matrix) + { + printError(&sciErr, 0); + return 0; + } + + //Get complexity + iComplex = isVarComplex(pvApiCtx, piAddr); + + //Check complexity + if(iComplex) + { + Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n"); + return 0; + } + + sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &iRows, &iCols, &pdbl); + if(sciErr.iErr || iCols != 3 || iRows != 1) + { + printError(&sciErr, 0); + return 0; + } + + x = pdbl[0]; + y = pdbl[1]; + z = pdbl[2]; + + theta = sqrt(x * x + y * y + z * z); + + sinOfangle = sin(theta); + cosOfangle = cos(theta); + t = 1 - cosOfangle; + x = pdbl[0] / theta; + y = pdbl[1] / theta; + z = pdbl[2] / theta; + + matrix = (double*)malloc(sizeof(double) * 9); + A = (double*)malloc(sizeof(double) * 9); + B = (double*)malloc(sizeof(double) * 9); + identity = (double*)malloc(sizeof(double) * 9); + + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + if(i == 0) + { + if(j == 0) + A[j * 3 + i] = x * x; + else if(j == 1) + A[j * 3 + i] = x * y; + else if(j == 2) + A[j * 3 + i] = x * z; + + } + else if(i == 1) + { + if(j == 0) + A[j * 3 + i] = x * y; + else if(j == 1) + A[j * 3 + i] = y * y; + else if(j == 2) + A[j * 3 + i] = y * z; + } + else + { + if(j == 0) + A[j * 3 + i] = x * z; + else if(j == 1) + A[j * 3 + i] = y * z; + else if(j == 2) + A[j * 3 + i] = z * z; + } + + } + } + + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + if(i == 0) + { + if(j == 0) + B[j * 3 + i] = 0; + else if(j == 1) + B[j * 3 + i] = -z; + else if(j == 2) + B[j * 3 + i] = y; + + } + else if(i == 1) + { + if(j == 0) + B[j * 3 + i] = z; + else if(j == 1) + B[j * 3 + i] = 0; + else if(j == 2) + B[j * 3 + i] = -x; + } + else + { + if(j == 0) + B[j * 3 + i] = -y; + else if(j == 1) + B[j * 3 + i] = x; + else if(j == 2) + B[j * 3 + i] = 0; + } + + } + } + + for(int i = 0; i < 3; i++) + { + for(int j = 0;j < 3; j++) + { + if(i == j) + identity[j * 3 + i] = 1; + else + identity[j * 3 + i] = 0; + } + } + + for(int i = 0; i < 3; i++) + { + for(int j = 0; j < 3; j++) + { + matrix[i * 3 + j] = identity[j * 3 + i] * cosOfangle + t * A[j * 3 + i] + sinOfangle * B[j * 3 + i]; + } + } + + sciErr = createList(pvApiCtx, nbInputArgument(pvApiCtx) + 1, 1, &piAddr); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + + sciErr = createMatrixOfDoubleInList(pvApiCtx, nbInputArgument(pvApiCtx) + 1, piAddr, 1, 3, 3, matrix); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + + //Return output arguments + AssignOutputVariable(pvApiCtx, 1) = nbInputArgument(pvApiCtx) + 1; + ReturnArguments(pvApiCtx); + return 0; + } +} + + + |