void covTransform(float* cov, SimplePose offset) {
float c[9];
float sa = sin(offset.a);
float ca = cos(offset.a);
float rotation[9] = {
ca, -sa, 0,
sa, ca, 0,
0, 0, 1};
float rotationTranspose[9] = {
ca, sa, 0,
-sa, ca, 0,
0, 0, 1};
matProd(rotationTranspose, cov, c, 3, 3, 3);
matProd(c, rotation, cov, 3, 3, 3);
}
/**Assumed that vector W is of a valid length such that # of elements = # of elements in Up-Trig portion of dxd matrix*/
SEXP theta2w(SEXP W){
int p;
int *Wdims;
double *Wptr;
Wdims = getDims(W); //extract dimensions of W
p = Wdims[0]; //number of entries
PROTECT(W = coerceVector(W,REALSXP));
Wptr = REAL(W);
int d = (sqrt(8*p+1) + 1)/2;
SEXP ans = identity(&d); //initialize
SEXP interm; //intermediate matrix
int k = 0; //index for W
for(int j=2;j<=d;j++){
for(int i=j-1;i>=1;i--){
interm = subs(&d,&i,&j,&Wptr[k]);
ans = matProd(interm,ans);
k++;
}//end inner for
}//end outer for
UNPROTECT(1);
return ans;
}//end function theta2w
