From a6df67e8bcd5159cde27556f4f6a315f8dc2215f Mon Sep 17 00:00:00 2001
From: shamikam
Date: Mon, 16 Jan 2017 02:56:17 +0530
Subject: First Commit

---
 sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp | 205 +++++++++++++++++++++
 1 file changed, 205 insertions(+)
 create mode 100644 sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp

(limited to 'sci_gateway1/cpp/opencv_rotationVectorToMatrix.cpp')

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;
+	}
+}
+
+
+
-- 
cgit