CAMLprim value ml_gsl_blas_sgemv(value transa, value alpha, value A,
value X, value beta, value Y)
{
_DECLARE_MATRIX(A);
_DECLARE_VECTOR2(X, Y);
_CONVERT_MATRIX(A);
_CONVERT_VECTOR2(X, Y);
gsl_blas_sgemv(CBLAS_TRANS_val(transa), Double_val(alpha),
&m_A, &v_X, Double_val(beta), &v_Y);
return Val_unit;
}
// We need to compute something like:
// sum_{k != i, j} \lambda_{i,k} x_i q_k(x_k = 1)
// = x_i (\sum \lambda_{i, k} q_k(x_k = 1) -
// \lambda_{i, j} q_j(x_j = 0))
// (note that lambda_{i, i} = 0)
unsigned int estimateExpectations() {
// estimates_{x,y} = lambda_{x,y}
gsl_matrix_float_memcpy(estimates_, lambda_);
// estimates_{x,y} = lambda_{x,y} + kappa_x
gsl_blas_sger(1.0, ones_, kappa_, estimates_);
// estimates_{x,y} = lambda_{x,y} + kappa_x + kappa_y
gsl_blas_sger(1.0, kappa_, ones_, estimates_);
gsl_vector_float_set_zero(q_lambda_);
gsl_blas_sgemv(CblasNoTrans,
1.0,
lambda_,
singleton_expectation_,
1.0,
q_lambda_);
// estimates_{x,y} = lambda_{x,y} + kappa_x + kappa_y
// + E_q [ sum lambda_{x, z} + lambda_{z, y} ]
gsl_blas_sger(1.0, ones_, q_lambda_, estimates_);
gsl_blas_sger(1.0, q_lambda_, ones_, estimates_);
}
/**
* C++ version of gsl_blas_sgemv().
* @param TransA Transpose type
* @param alpha A constant
* @param A A matrix
* @param X A vector
* @param beta Another constant
* @param Y A vector
* @return Error code on failure
*/
int sgemv( CBLAS_TRANSPOSE_t TransA, float alpha, matrix_float const& A,
vector_float const& X, float beta, vector_float& Y ){
return gsl_blas_sgemv( TransA, alpha, A.get(), X.get(), beta, Y.get() ); }
void Euler_init(double* accl, double* magn) {
norm = getNorm(accl);
for (i=0; i<3; ++i) {
acclNorm[i] = accl[i] / norm;
Euler[i] = acos(acclNorm[i]);
}
alpha = sqrt((acclNorm[2]+1)/2);
// q[0] = alpha;
// q[1] = sqrt(1-pow(alpha,2));
// for (i=2; i<4; ++i) q[i] = q[1];
// for (i=1; i<4; ++i) q[i] *= acclNorm[i];
// Euler[0] = atan2(2*(q[0]*q[1]+q[2]*q[3]) , 1-2*(q[1]*q[1]+q[2]*q[2]));
// Euler[1] = asin(2*(q[0]*q[2]-q[1]*q[3]));
// Euler[2] = atan2(2*(q[0]*q[3]+q[2]*q[1]) , 1-2*(q[2]*q[2]+q[3]*q[3]));
// Euler[2] = getAngle(magn);
double rotate[4][9] = { {1,0,0, 0, cos(Euler[0]), -sin(Euler[0]) , 0, sin(Euler[0]), cos(Euler[0]) } ,
{cos(Euler[1]), 0, -sin(Euler[1]) , 0,1,0 , sin(Euler[1]), 0, cos(Euler[1])},
{cos(Euler[2]), -sin(Euler[2]), 0 , sin(Euler[2]), cos(Euler[2]), 0 , 0,0,1},
{0,0,0,0,0,0,0,0,0}};
/*
double rotate[4][9] = { {1,0,0, 0, cos(Euler[0]), sin(Euler[0]) , 0, -sin(Euler[0]), cos(Euler[0]) } ,
{cos(Euler[1]), 0, sin(Euler[1]) , 0,1,0 , -sin(Euler[1]), 0, cos(Euler[1])},
{cos(Euler[2]), sin(Euler[2]), 0 , -sin(Euler[2]), cos(Euler[2]), 0 , 0,0,1},
{0,0,0,0,0,0,0,0,0}};
*/
gsl_matrix_float_view A = gsl_matrix_float_view_array(&rotate[0][0], 3, 3);
gsl_matrix_float_view B = gsl_matrix_float_view_array(&rotate[1][0], 3, 3);
gsl_matrix_float_view C = gsl_matrix_float_view_array(&rotate[2][0], 3, 3);
gsl_matrix_float_view D = gsl_matrix_float_view_array(&rotate[3][0], 3, 3);
gsl_matrix_float_view E = gsl_matrix_float_view_array(rotateAll, 3, 3);
gsl_blas_sgemm (CblasNoTrans, CblasNoTrans,1.0, &B.matrix, &A.matrix, 0.0, &D.matrix);
gsl_blas_sgemm (CblasNoTrans, CblasNoTrans,1.0, &D.matrix, &C.matrix, 0.0, &E.matrix);
// norm = getNorm(magn);
for (i=0; i<3; ++i) magNord[i] = 0;
magNord[2] = norm;
gsl_vector_float_view mm = gsl_vector_float_view_array(magNord, 3);
gsl_vector_float_view aa = gsl_vector_float_view_array(Angle, 3);
gsl_blas_sgemv (CblasNoTrans, 1.0, &E.matrix, &mm.vector, 0.0, &aa.vector);
/*
magNord[0] = (cos(Euler[1])*cos(Euler[0]) * magn[0] + (sin(Euler[2])*sin(Euler[1])*cos(Euler[0])-cos(Euler[2])*sin(Euler[0])) * magn[1]
+ (cos(Euler[2])*sin(Euler[1])*cos(Euler[0])+sin(Euler[2])*sin(Euler[0])) * magn[2])/norm;
magNord[1] = (cos(Euler[1])*sin(Euler[0]) * magn[0] + (sin(Euler[2])*sin(Euler[1])*sin(Euler[0])-cos(Euler[2])*cos(Euler[0])) * magn[1]
+ (cos(Euler[2])*sin(Euler[1])*sin(Euler[0])-sin(Euler[2])*cos(Euler[0])) * magn[2])/norm;
magNord[2] = (-sin(Euler[1]) * magn[0] + sin(Euler[2])*cos(Euler[1]) * magn[1] + cos(Euler[2])*cos(Euler[1]) * magn[2])/norm;
magNord[0] = (cos(Euler[0])*cos(Euler[1]) - cos(Euler[2])*sin(Euler[0])*sin(Euler[1])) * magn[0] +
(sin(Euler[0])*cos(Euler[1]) + cos(Euler[2])*cos(Euler[0])*sin(Euler[1])) * magn[1] +
sin(Euler[2])*sin(Euler[1]) * magn[2];
magNord[1] = (-cos(Euler[0])*sin(Euler[1]) - cos(Euler[2])*sin(Euler[0])*cos(Euler[1])) * magn[0] +
(-sin(Euler[0])*sin(Euler[1]) + cos(Euler[2])*cos(Euler[0])*cos(Euler[1])) * magn[1] +
sin(Euler[2])*cos(Euler[1]) * magn[2];
magNord[2] = sin(Euler[2])*sin(Euler[0]) * magn[0] - sin(Euler[2])*cos(Euler[0])*magn[1] + cos(Euler[2]) * magn[2];
*/
/*
for (i=0; i<3; ++i) {
magNord[i] = 0;
for (j=0; j=3; ++j) {
magNord[i] +=
}
magNord[i] /= norm;
}
*/
printf("%f, %f, %f\t",accl[0],accl[1],accl[2]);
printf("%f, %f, %f\t",magNord[0],magNord[1],magNord[2]);
printf("%f, %f, %f\n", Angle[0],Angle[1],Angle[2]);
}
