// Given row and column permutations compute the QR decomposition of a
// sparse matrix.
void sparse_qr_decomp( const int *P1,
sparse_mat *A,
const int *P2,
sparse_mat **Q,
sparse_mat **R )
{
// Components of the R matrix:
int nnz, *is, *js;
double *vs;
// Allocate structures to store Q and R.
sparse_vec **rows = (sparse_vec**)calloc( A->m, sizeof(sparse_vec*) );
rotation *grots = alloc_rotation( A->m );
// Work variables
int i, j, *col;
double a, b, c, s, r, *val;
sparse_vec row, *row_i, *row_j;
grots -> n = A -> m;
permute_rows(P1, A);
permute_cols(A, P2);
for( j = 0; j < A->m; ++j ) {
row(*A,j,row);
row_j = rows[j] = copy_sparse_vec( &row );
if( row_j->nnz > 0 ) {
i = *(row_j->is);
b = *(row_j->vs);
while( i < j ) {
row_i = rows[i];
if( row_i->nnz == 0 || *(row_i->is) > i )
a = 0.0;
else
a = *(row_i->vs);
cblas_drotg( &a, &b, &c, &s );
sp_apply_grot( c, s, rows+i, rows+j );
left_comp_rotation( &grots, i, j, c, s );
row_j = rows[j];
if( row_j->nnz == 0 ) break;
i = *(row_j->is);
b = *(row_j->vs);
}
}
}
*Q = sparse_mat_rotation( grots );
*R = sparse_mat_rows( A->n, rows );
free_rotation( grots );
for( j = 0; j < A->m; ++j )
free( rows[j] );
free( rows );
}
//===========================================================================
int cHydraDevice::getRotation(cMatrix3d& a_rotation)
{
/************************************************************************
STEP 7:
Here you may implement code which reads the orientation frame from
your haptic device. The orientation frame is expressed by a 3x3
rotation matrix. The 1st column of this matrix corresponds to the
x-axis, the 2nd column to the y-axis and the 3rd column to the z-axis.
The length of each column vector should be of length 1 and vectors need
to be perpendicular to each other.
If the operation fails return an error code such as -1 for instance.
Note:
If your device is located in front of you, the x-axis is pointing
towards you (the operator). The y-axis points towards your right
hand side and the z-axis points up towards the sky.
If your device has a stylus, make sure that you set the reference frame
so that the x-axis corresponds to the axis of the stylus.
*************************************************************************/
int error = 0;
double r00, r01, r02, r10, r11, r12, r20, r21, r22;
cMatrix3d frame;
frame.identity();
// *** INSERT YOUR CODE HERE, MODIFY CODE BELLOW ACCORDINGLY ***
// if the device does not provide any rotation capabilities
// we set the rotation matrix equal to the identiy matrix.
sixenseAllControllerData acd;
sixenseGetAllNewestData( &acd );
// sixense is COLUMN MAJOR!
r00 = acd.controllers[a_deviceNumber].rot_mat[0][0];
r01 = acd.controllers[a_deviceNumber].rot_mat[1][0];
r02 = acd.controllers[a_deviceNumber].rot_mat[2][0];
r10 = acd.controllers[a_deviceNumber].rot_mat[0][1];
r11 = acd.controllers[a_deviceNumber].rot_mat[1][1];
r12 = acd.controllers[a_deviceNumber].rot_mat[2][1];
r20 = acd.controllers[a_deviceNumber].rot_mat[0][2];
r21 = acd.controllers[a_deviceNumber].rot_mat[1][2];
r22 = acd.controllers[a_deviceNumber].rot_mat[2][2];
frame.set(r00, r01, r02,
r10, r11, r12,
r20, r21, r22);
cMatrix3d permute_rows(0, 0, 1,
1, 0, 0,
0, 1, 0);
cMatrix3d permute_columns(0, 1, 0,
0, 0, 1,
1, 0, 0);
a_rotation = permute_rows*frame*permute_columns;
// estimate angular velocity
estimateAngularVelocity(a_rotation);
// exit
return (error);
}
