From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001
From: leochanj105 <leochanj@live.unc.edu>
Date: Mon, 19 Oct 2020 23:09:30 -0400
Subject: initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs
---
 SD-VBS/benchmarks/localization/src/c/quatRot.c | 47 ++++++++++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 SD-VBS/benchmarks/localization/src/c/quatRot.c

(limited to 'SD-VBS/benchmarks/localization/src/c/quatRot.c')

diff --git a/SD-VBS/benchmarks/localization/src/c/quatRot.c b/SD-VBS/benchmarks/localization/src/c/quatRot.c
new file mode 100644
index 0000000..4962b55
--- /dev/null
+++ b/SD-VBS/benchmarks/localization/src/c/quatRot.c
@@ -0,0 +1,47 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "localization.h"
+
+F2D* quatRot(F2D* vec, F2D* rQuat)
+{
+    F2D *ret;
+    int nr, i, j, k, rows, cols;
+    F2D *tv, *vQuat, *temp, *temp1;
+    F2D *retVec;
+
+    nr = vec->height;
+    tv = fSetArray(nr, 1, 0);
+    vQuat = fHorzcat(tv, vec);
+    temp = quatMul(rQuat, vQuat);
+    temp1 = quatConj(rQuat);
+    retVec = quatMul(temp, temp1);
+
+    rows = retVec->height;
+    cols = retVec->width;
+
+    ret = fSetArray(rows, 3, 0);
+
+    for(i=0; i<rows; i++)
+    {
+        k = 0;
+        for(j=1; j<4; j++)
+        {
+            subsref(ret,i,k) = subsref(retVec,i,j);
+            k++;
+        }
+    }
+
+    fFreeHandle(tv);
+    fFreeHandle(vQuat);
+    fFreeHandle(temp);
+    fFreeHandle(temp1);
+    fFreeHandle(retVec);
+        
+    return ret;
+}
+
+
-- 
cgit v1.2.2