void ss_controller::update( matrix *R, struct matrix *Y)
{
matrix_minus(U_tmp, R, X_hat);
matrix_mult(U, K, U_tmp);
for(int i=0; i < num_outputs; i++) {
double* u_i = U->data+i;
double u_max = U_max->data[i];
double u_min = U_min->data[i];
if (*u_i > u_max) {
*u_i = u_max;
} else if (*u_i < u_min) {
*u_i = u_min;
}
}
matrix_mult(b_u, B, U);
matrix_mult(l_y, L, Y);
matrix_mult(l_c, L, C);
matrix_minus(a_lc, A, l_c);
matrix_mult(alc_xhat, a_lc, X_hat);
matrix_add(xhatp1, alc_xhat, l_y);
matrix_add(X_hat, xhatp1, b_u);
}
//matrix_add
static void matrix_add_TwoMatrixesWithElementsOneTwoThreeFour_MatrixWithElementsTwoFourSixEight(void ** state)
{
matrix_t * mat1 =matrix_test(1,2,3,4);
matrix_t * mat2 =matrix_test(1,2,3,4);
assert_int_equal(matrix_getElementOfMatrix(matrix_add(mat1,mat2),0,0),2);
assert_int_equal(matrix_getElementOfMatrix(matrix_add(mat1,mat2),0,1),4);
assert_int_equal(matrix_getElementOfMatrix(matrix_add(mat1,mat2),1,0),6);
assert_int_equal(matrix_getElementOfMatrix(matrix_add(mat1,mat2),1,1),8);
matrix_free(mat1);
matrix_free(mat2);
}
//Rotate a vector by a given angle about a given unit vector
void rotate_vector(double (&result)[3], double* vect, double* axis, double angle) {
double identity[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
double w_hat[] = {0, *(axis+2), -*(axis+1), -*(axis+2), 0, *(axis+0), *(axis+1), -*(axis+0), 0};
double temp[9], temp2[9], temp3[9];
scalar_multiply(temp, w_hat, 3, 3, sin(angle));
multiply(temp2, w_hat, 3, 3, w_hat, 3);
scalar_multiply(temp3, temp2, 3, 3, 1-cos(angle));
matrix_add(temp2, identity, temp, 3, 3);
matrix_add(temp, temp2, temp3, 3, 3);
multiply(result, temp, 3, 3, vect, 1);
}
int test_matrix_add_element(void) {
Matrix *matrix = new_matrix();
if(matrix == NULL) {
LOG_ERR("add element to matrix -> new matrix");
return 1;
}
Element *element = element_new(0,0,100);
if(element == NULL) {
safe_free(matrix);
LOG_ERR("add element to matrix -> new element");
return 1;
}
Status status = matrix_add(matrix, element);
if(status != STAT_SUCCESS) {
safe_free(matrix);
safe_free(element);
LOG_ERR("add element to matrix");
return 1;
}
safe_free(element);
safe_free(matrix);
LOG_SUCCESS("add element to matrix");
return 0;
}
void
correction_apply(NetworkState *network_state, Correction *correction) {
Matrix state_matrix, correction_matrix;
for (int32_t layer = 0; layer < correction->layers; layer += 1) {
state_matrix = network_state->biases[layer + 1];
correction_matrix = correction->biases[layer];
matrix_add(state_matrix, correction_matrix, state_matrix);
state_matrix = network_state->weights[layer + 1];
correction_matrix = correction->weights[layer];
matrix_add(state_matrix, correction_matrix, state_matrix);
}
}
/*
Launch the current matrix read, write, and add procedure
*/
void matrix_run(int block_size, int scalar, int size){
int curBuffer[block_size], previous[block_size];
int next[block_size];
int offset = 0; //Keep the current file offset
int writeOffset = 0, count=0, readOffset = 0;
time_t diffTime = time(NULL);
//Create the request object
struct aiocb* request = malloc(sizeof(struct aiocb));
request->aio_buf = next;
request->aio_fildes = 0;
request->aio_nbytes = block_size;
request->aio_offset = 0;
//Create the response object
struct aiocb* response = malloc(sizeof(struct aiocb));
response->aio_buf = previous;
response->aio_fildes = 1;
response->aio_nbytes = block_size;
response->aio_offset = 0;
//Get the return from the read request
do{
memcpy(curBuffer,next,block_size);
request->aio_offset = offset; // Set the offset
//Create a read request
if(aio_read(request)!=0){
//perror("Read Error");
}
//Change the offsets
offset = offset + readOffset;
matrix_add(curBuffer,block_size,scalar,previous);
//Write the results currently stored to a file
response->aio_offset = writeOffset;
response->aio_nbytes = readOffset;
aio_write(response);
while(aio_error(response)==EINPROGRESS);
aio_return(response);
writeOffset= writeOffset + readOffset;
//Wait for progress
//while(aio_error(request)==EINPROGRESS);
readOffset= aio_return(request);
}while(readOffset>0 && offset<=size);
//Print end time
diffTime = time(NULL)-diffTime;
fprintf(stderr,"Time Difference: %d\n",(int)diffTime);
}
inline void multiply_and_add(
orthotope<T> const& C_sub
, orthotope<T> const& A_sub
, orthotope<T> const& B_sub
, matrix_mutex const& mtx
)
{
orthotope<T> AB_sub = matrix_multiply(A_sub, B_sub);
{
matrix_mutex::scoped_lock l(mtx);
matrix_add(C_sub, AB_sub);
}
}
static int fill_matrix(Matrix *matrix, unsigned row, unsigned col) {
for(unsigned i =0; i <= row; i++) {
for(unsigned j = 0; j <= col; j++) {
Element *element = element_new(i, j, i*j);
if(element == NULL)
return 1;
Status status = matrix_add(matrix, element);
if(status != STAT_SUCCESS)
return 1;
}
}
return 0;
}
int main() {
int operation;
unsigned timestamp;
unsigned prev_checksum;
unsigned new_checksum;
matrix matrix1;
matrix matrix2;
matrix matrix3;
char line[1024];
while(!feof(stdin)) {
fgets(line, sizeof(line), stdin);
}
if(sscanf(line, "timestamp: 0x%x, operation: %d, checksum: 0x%x", ×tamp, &operation, &prev_checksum) == 0) {
fprintf(stderr, "[oracle] Invalid input\n");
exit(1);
}
srand(timestamp);
printf("[Starting oracle]\n");
printf("timestamp: 0x%04x, ", timestamp);
printf("operation: %d, ", operation);
printf("checksum: 0x%04x\n", prev_checksum);
/* init matrices */
printf("initializing matrices\n");
matrix_init(&matrix1);
matrix_init(&matrix2);
switch(operation) {
case 0: /* add matrices */
printf("adding VM matrices ");fflush(stdout);
matrix_add(&matrix3, &matrix1, &matrix2);
break;
case 1: /* multiply matrices */
printf("multiplying matrices ");fflush(stdout);
matrix_mult(&matrix3, &matrix1, &matrix2);
break;
default:
break;
}
new_checksum = matrix_checksum(&matrix3);
printf("new checksum: 0x%04x [%s]\n", new_checksum, new_checksum == prev_checksum ? "ok" : "ko");
return 0;
}
strategy_profile_t *new_point(strategy_profile_t *x, strategy_profile_t *xp, double eps)
{
//printf("=======\n");
strategy_profile_t *np = malloc(sizeof(strategy_profile_t));
np->players = xp->players;
np->strategies = malloc(sizeof(matrix_t *) * np->players);
int i;
for (i = 0; i < x->players; ++i) {
matrix_t *tmp = matrix_sub(xp->strategies[i], x->strategies[i]);
matrix_mul_const_in(tmp, eps);
np->strategies[i] = matrix_add(x->strategies[i], tmp);
matrix_free(tmp);
//matrix_print(matrix_trans(xp->strategies[i]));
//matrix_print(matrix_trans(x->strategies[i]));
//matrix_print(matrix_trans(np->strategies[i]));
}
return np;
}
int main(char argc, char ** argv){
int row = 2, col = 2;
int i, j;
double* Ma = (double *)malloc(row * col * sizeof(double));
double* Mb = (double *)malloc(row * col * sizeof(double));
//init Ma
printf("Now input Ma..\n");
for (i = 0; i < row; i++){
for (j = 0; j < col; j++)
scanf("%lf", Ma+i*col+j);
}
printf("Now input Mb..\n");
for (i = 0; i < row; i++){
for (j = 0; j < col; j++)
scanf("%lf", Mb+i*col+j);
}
matrix_add(Ma, Mb, row, col);
return 0;
}
int main() {
int rows, cols;
scanf("%d", &rows);
scanf("%d", &cols);
int** matrix_1 = malloc(sizeof(int*) * rows);
int** matrix_2 = malloc(sizeof(int*) * rows);
read_matrix(matrix_1, rows, cols);
read_matrix(matrix_2, rows, cols);
int** result;
result = matrix_add(matrix_1, matrix_2, rows, cols);
printf("\n");
print_matrix(result, rows, cols);
free_matrix(matrix_1, rows);
free_matrix(matrix_2, rows);
free_matrix(result, rows);
return 0;
}
void test1() {
Matrix m1 = matrix_create( 3, 3 );
Matrix m2 = matrix_create( 3, 3 );
Matrix m3 = matrix_create( 3, 3 );
int i,j;
for ( i=0; i<3; ++i ) { for ( j=0; j<3; ++j ) { m1[i][j] = i+j; } }
for ( i=0; i<3; ++i ) { for ( j=0; j<3; ++j ) { m2[i][j] = 5; } }
printf( "m1 =\n" );
matrix_print( m1, 3, 3 );
printf( "m2 =\n" );
matrix_print( m2, 3, 3 );
matrix_add( m1, m2, m3, 3, 3 );
printf( "m1 + m2 =\n" );
matrix_print( m3, 3, 3 );
matrix_delete( m1, 3 );
matrix_delete( m2, 3 );
matrix_delete( m3, 3 );
}
/*
void
matrix_get_row(Matrix *m, index_t i, Matrix *v)
{
// Extracts the ith row of m to the column vector v
index_t j;
const index_t nc=numcols(m);
#ifdef _DBG_
len=numrows(v),
if (len!=nc)
error("Incompatible dimensions in matrix_get_row()");
#endif
for (j=0; j<nc; j++)
matrix_set_element(v,j,0, matrix_get_element(m,i,j));
return;
}
*/
int
matrix_is_symmetric(Matrix *xx)
{
// Checks a matrix for symmetry, aindex_t with other basic checks.
// Returns 1 if the matrix is
// symmetric, 0 if not symmetric (if something else is wrong).
int retval = 1;
index_t ii, jj, nrow_xx = numrows(xx), ncol_xx = numcols(xx);
Matrix *yy = matrix_new(nrow_xx, ncol_xx);
matrix_transpose(xx, yy);
matrix_scalar_multiply(yy, -1.0, yy);
matrix_add(xx, yy, yy);
for (ii=0; ii<nrow_xx; ii++)
for (jj=0; jj<ncol_xx; jj++)
if (matrix_get_element(yy, ii, jj) > DBL_EPSILON)
retval = 0;
matrix_free(yy);
return retval;
}
int add_mat_input_parameters(char *pch, int size)
{
int **add_A = GetMatrixByName(pch, size, 0);
if (add_A == NULL)
return MARIX_ADD_FIRST_MATRIX_PARAMETER_IS_INCORRECT;
pch = strtok(NULL, " ");
int **add_B = GetMatrixByName(pch, size, 0);
if (add_B == NULL)
return MARIX_ADD_SECOND_MATRIX_PARAMETER_IS_INCORRECT;;
pch = strtok(NULL, " ");
// we need to allocate if needed
int **add_C = GetMatrixByName(pch, size, 1);
if (add_C == NULL)
return MARIX_ADD_RESULT_MATRIX_PARAMETER_IS_INCORRECT;
add_C = matrix_add(add_A, add_B, add_C, size);
return OK;
}
static ERL_NIF_TERM
add(ErlNifEnv *env, int32_t argc, const ERL_NIF_TERM *argv) {
ErlNifBinary first, second;
ERL_NIF_TERM result;
float *first_data, *second_data, *result_data;
int32_t data_size;
size_t result_size;
(void)(argc);
if (!enif_inspect_binary(env, argv[0], &first )) return enif_make_badarg(env);
if (!enif_inspect_binary(env, argv[1], &second)) return enif_make_badarg(env);
first_data = (float *) first.data;
second_data = (float *) second.data;
data_size = (int32_t) (first_data[0] * first_data[1] + 2);
result_size = sizeof(float) * data_size;
result_data = (float *) enif_make_new_binary(env, result_size, &result);
matrix_add(first_data, second_data, result_data);
return result;
}
matrix operator+(matrix A, matrix B){
return matrix_add(A, B);
}
std::vector<Task*> StrassenSingleProblem::split() {
int T_m = m/2, T_n = k/2, S_m = k/2, S_n = n/2;
float *A11 = A;
float *A21 = A + m/2;
float *A12 = A + lda*k/2;
float *A22 = A + lda*k/2 + m/2;
float *B11 = B;
float *B21 = B + k/2;
float *B12 = B + ldb*n/2;
float *B22 = B + ldb*n/2 + k/2;
float *C11 = C;
float *C21 = C + m/2;
float *C12 = C + ldc*n/2;
float *C22 = C + ldc*n/2 + m/2;
float *T0 = A11;
float *T1 = A12;
float *T2 = (float*) malloc(T_m * T_n * sizeof(float));
float *T3 = (float*) malloc(T_m * T_n * sizeof(float));
float *T4 = (float*) malloc(T_m * T_n * sizeof(float));
float *T5 = (float*) malloc(T_m * T_n * sizeof(float));
float *T6 = A22;
float *S0 = B11;
float *S1 = B21;
float *S2 = (float*) malloc(S_m * S_n * sizeof(float));
float *S3 = (float*) malloc(S_m * S_n * sizeof(float));
float *S4 = (float*) malloc(S_m * S_n * sizeof(float));
float *S5 = B22;
float *S6 = (float*) malloc(S_m * S_n * sizeof(float));
float *Q0 = C11;
float *Q1 = (float*) malloc(T_m * S_n * sizeof(float));
float *Q2 = C22;
float *Q3 = C12;
float *Q4 = C21;
float *Q5 = (float*) malloc(T_m * S_n * sizeof(float));
float *Q6 = (float*) malloc(T_m * S_n * sizeof(float));
matrix_add(T_m, T_n, A21, lda, A22, lda, T2, T_m);
matrix_subtract(T_m, T_n, T2, T_m, A11, lda, T3, T_m);
matrix_subtract(T_m, T_n, A11, lda, A21, lda, T4, T_m);
matrix_subtract(T_m, T_n, A12, lda, T3, T_m, T5, T_m);
matrix_subtract(S_m, S_n, B12, ldb, B11, ldb, S2, S_m);
matrix_subtract(S_m, S_n, B22, ldb, S2, S_m, S3, S_m);
matrix_subtract(S_m, S_n, B22, ldb, B12, ldb, S4, S_m);
matrix_subtract(S_m, S_n, S3, S_m, B21, ldb, S6, S_m);
std::vector<Task*> tasks (7);
tasks[0] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T0, lda, S0, ldb, Q0, ldc));
tasks[1] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T1, lda, S1, ldb, Q1, T_m));
tasks[2] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T2, T_m, S2, S_m, Q2, ldc));
tasks[3] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T3, T_m, S3, S_m, Q3, ldc));
tasks[4] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T4, T_m, S4, S_m, Q4, ldc));
tasks[5] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T5, T_m, S5, ldb, Q5, T_m));
tasks[6] = new Task(new StrassenSingleProblem(T_m, T_n, S_n, T6, lda, S6, S_m, Q6, T_m));
return tasks;
}
int altitude_estimator::update(float accelz, float baro, float dt)
{
if (dt > 0.2f || dt < 0)
return -1;
bool invalid_baro_data = isnan(baro);
float dtsq = dt*dt;
float dtsq2 = dtsq/2;
float F[16] =
{
1, dt, dtsq2, dtsq2,
0, 1, dt, dt,
0, 0, 1, 0,
0, 0, 0, 1,
};
float FT[16] =
{
1, 0, 0, 0,
dt, 1, 0, 0,
dtsq2, dt, 1, 0,
dtsq2, dt, 0, 1,
};
static float Hab[2*4] =
{
1, 0, 0, 0,
0, 0, 1, 0,
};
static float HabT[4*2] =
{
1, 0,
0, 0,
0, 1,
0, 0,
};
// accelerometer only
static float Ha[2*4] =
{
0, 0, 1, 0,
};
static float HaT[4*2] =
{
0,
0,
1,
0,
};
float zk_ab[2] = {baro, accelz};
float zk_a[2] = {accelz};
float *H = invalid_baro_data ? Ha : Hab;
float *HT = invalid_baro_data ? HaT : HabT;
float *zk = invalid_baro_data ? zk_a : zk_ab;
int observation_count = invalid_baro_data ? 1 : 2;
float state1[4];
float P1[16];
float tmp[16];
float tmp2[16];
float tmp3[16];
float kg[8];
// predict
matrix_mul(state1, F, 4, 4, state, 4, 1);
matrix_mul(tmp, P, 4, 4, FT, 4, 4);
matrix_mul(P1, F, 4, 4, tmp, 4, 4);
// covariance
float Q2[16];
for(int i=0; i<16; i++)
Q2[i] = Q[i] * dt / 0.003f; // normalize Q to dt = 0.003s
matrix_add(P1, Q2, 4, 4);
// controll vector
//state1[2] = state1[2] * 0.8f * 0.2f * (target_accel);
// if (invalid_baro_data)
// state1[1] *= 0.995f;
// update
// kg
matrix_mul(tmp, P1, 4, 4, HT, 4, observation_count);
matrix_mul(tmp2, H, observation_count, 4, P1, 4, 4);
matrix_mul(tmp3, tmp2, observation_count, 4, HT, 4, observation_count);
if (observation_count == 2)
{
matrix_add(tmp3, compensate_land_effect ? R2 : R, observation_count, observation_count);
inverse_matrix2x2(tmp3);
}
else
{
tmp3[0] += R[3];
tmp3[0] = 1.0f / tmp3[0];
}
matrix_mul(kg, tmp, 4, observation_count, tmp3, observation_count, observation_count);
// update state
// residual
matrix_mul(tmp, H, observation_count, 4, state1, 4, 1);
matrix_sub(zk, tmp, observation_count, 1);
matrix_mul(tmp, kg, 4, observation_count, zk, observation_count, 1);
matrix_mov(state, state1, 4, 1);
matrix_add(state, tmp, 4, 1);
// update P
float I[16] =
{
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
};
matrix_mul(tmp, kg, 4, observation_count, H, observation_count, 4);
matrix_sub(I, tmp, 4, 4);
matrix_mul(P, I, 4, 4, P1, 4, 4);
TRACE("time=%.3f,state:%.2f,%.2f,%.2f,%.2f, raw:%.3f, accelz:%.3f \n", systimer->gettime()/1000000.0f, state[0], state[1], state[2], state[3], baro, accelz);
return 0;
}
void operator_matrix() {
matrix result;
char in[USHRT_MAX];
int m;
int n;
while (1) {
printf("Entre com o número de linhas da 1ª matriz: ");
scanf("%s", in);
m = atoi(in);
printf("\n");
if (m == 0)
printf("Valor inválido!\n\n");
else
break;
}
while (1) {
printf("Entre com o número de colunas da 1ª matriz: ");
scanf("%s", in);
n = atoi(in);
printf("\n");
if (n == 0)
printf("Valor inválido!\n\n");
else
break;
}
result = matrix_constructor(m, n);
printf("Entre com os elementos da 1ª matriz, separando-os por espaços e/ou quebras de linha:\n");
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++) {
scanf("%s", in);
result.table[i][j] = atof(in);
}
printf("\n");
int keep_going = 1;
while (keep_going) {
matrix next;
switch (menu_matrix()) {
case 1:
// Add
while (1) {
while (1) {
printf("Entre com o número de linhas da proxima matriz: ");
scanf("%s", in);
m = atoi(in);
printf("\n");
if (m == 0)
printf("Valor inválido!\n\n");
else
break;
}
while (1) {
printf("Entre com o número de colunas da proxima matriz: ");
scanf("%s", in);
n = atoi(in);
printf("\n");
if (n == 0)
printf("Valor inválido!\n\n");
else
break;
}
next = matrix_constructor(m, n);
if (!matrix_can_add(result, next))
printf("Não é possivel fazer a operação desejada com as matrizes de ordens previamente informadas!\n\n");
else
break;
}
printf("Entre com os elementos da proxima matriz, separando-os por espaços e/ou quebras de linha:\n");
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++) {
scanf("%s", in);
next.table[i][j] = atof(in);
}
printf("\n");
matrix_add(&result, next);
break;
case 2:
// Subtract
while (1) {
while (1) {
printf("Entre com o número de linhas da proxima matriz: ");
scanf("%s", in);
m = atoi(in);
printf("\n");
if (m == 0)
printf("Valor inválido!\n\n");
else
break;
}
while (1) {
printf("Entre com o número de colunas da proxima matriz: ");
scanf("%s", in);
n = atoi(in);
printf("\n");
if (n == 0)
printf("Valor inválido!\n\n");
else
break;
}
next = matrix_constructor(m, n);
if (!matrix_can_subtract(result, next))
printf("Não é possivel fazer a operação desejada com as matrizes de ordens previamente informadas!\n\n");
else
break;
}
printf("Entre com os elementos da proxima matriz, separando-os por espaços e/ou quebras de linha:\n");
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++) {
scanf("%s", in);
next.table[i][j] = atof(in);
}
printf("\n");
matrix_subtract(&result, next);
break;
case 3:
// Multiply
while (1) {
while (1) {
printf("Entre com o número de linhas da proxima matriz: ");
scanf("%s", in);
m = atoi(in);
printf("\n");
if (m == 0)
printf("Valor inválido!\n\n");
else
break;
}
while (1) {
printf("Entre com o número de colunas da proxima matriz: ");
scanf("%s", in);
n = atoi(in);
printf("\n");
if (n == 0)
printf("Valor inválido!\n\n");
else
break;
}
next = matrix_constructor(m, n);
if (!matrix_can_multiply(result, next))
printf("Não é possivel fazer a operação desejada com as matrizes de ordens previamente informadas!\n\n");
else
break;
}
printf("Entre com os elementos da proxima matriz, separando-os por espaços e/ou quebras de linha:\n");
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++) {
scanf("%s", in);
next.table[i][j] = atof(in);
}
printf("\n");
matrix_multiply(&result, next);
break;
case 4:
// Power
if (matrix_can_power(result)) {
while (1) {
printf("Entre com o próximo valor: ");
scanf("%s", in);
printf("\n");
if (atoll(in) >= 0) {
matrix_power(&result, (unsigned long long int) atoll(in));
break;
} else
printf("Valor inválido!\n\n");
}
} else
printf("Não é possivel realizar a operação desejada com a matrix atual!\n\n");
break;
default:
keep_going = 0;
break;
}
matrix_destructor(&next);
printf("Resultado:\n\n");
matrix_print(result);
printf("\n");
if (!keep_going)
matrix_destructor(&result);
}
}
DECLARE_TEST(matrix, ops) {
matrix_t mat;
VECTOR_ALIGN float32_t aligned[] = {
1, -2, 3, -4,
-5, 6, -7, 8,
9, 10, 11, 12,
-13, -14, -15, -16
};
float32_t unaligned[] = {
0, 1, 2, 3,
4, 5, 6, 7,
8, 9, 10, 11,
12, 13, 14, 15
};
mat = matrix_transpose(matrix_aligned(aligned));
EXPECT_VECTOREQ(mat.row[0], vector(1, -5, 9, -13));
EXPECT_VECTOREQ(mat.row[1], vector(-2, 6, 10, -14));
EXPECT_VECTOREQ(mat.row[2], vector(3, -7, 11, -15));
EXPECT_VECTOREQ(mat.row[3], vector(-4, 8, 12, -16));
mat = matrix_add(matrix_zero(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector_zero());
EXPECT_VECTOREQ(mat.row[1], vector_zero());
EXPECT_VECTOREQ(mat.row[2], vector_zero());
EXPECT_VECTOREQ(mat.row[3], vector_zero());
mat = matrix_add(matrix_zero(), matrix_identity());
EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0));
EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0));
EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0));
EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1));
mat = matrix_add(matrix_identity(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0));
EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0));
EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0));
EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1));
mat = matrix_add(matrix_aligned(aligned), matrix_unaligned(unaligned));
EXPECT_VECTOREQ(mat.row[0], vector(1, -1, 5, -1));
EXPECT_VECTOREQ(mat.row[1], vector(-1, 11, -1, 15));
EXPECT_VECTOREQ(mat.row[2], vector(17, 19, 21, 23));
EXPECT_VECTOREQ(mat.row[3], vector(-1, -1, -1, -1));
mat = matrix_add(matrix_aligned(aligned), matrix_transpose(matrix_aligned(aligned)));
EXPECT_VECTOREQ(mat.row[0], vector(2, -7, 12, -17));
EXPECT_VECTOREQ(mat.row[1], vector(-7, 12, 3, -6));
EXPECT_VECTOREQ(mat.row[2], vector(12, 3, 22, -3));
EXPECT_VECTOREQ(mat.row[3], vector(-17, -6, -3, -32));
mat = matrix_sub(matrix_zero(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector_zero());
EXPECT_VECTOREQ(mat.row[1], vector_zero());
EXPECT_VECTOREQ(mat.row[2], vector_zero());
EXPECT_VECTOREQ(mat.row[3], vector_zero());
mat = matrix_sub(matrix_zero(), matrix_identity());
EXPECT_VECTOREQ(mat.row[0], vector(-1, 0, 0, 0));
EXPECT_VECTOREQ(mat.row[1], vector(0, -1, 0, 0));
EXPECT_VECTOREQ(mat.row[2], vector(0, 0, -1, 0));
EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, -1));
mat = matrix_add(matrix_identity(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0));
EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0));
EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0));
EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1));
mat = matrix_sub(matrix_aligned(aligned), matrix_unaligned(unaligned));
EXPECT_VECTOREQ(mat.row[0], vector(1, -3, 1, -7));
EXPECT_VECTOREQ(mat.row[1], vector(-9, 1, -13, 1));
EXPECT_VECTOREQ(mat.row[2], vector(1, 1, 1, 1));
EXPECT_VECTOREQ(mat.row[3], vector(-25, -27, -29, -31));
mat = matrix_sub(matrix_aligned(aligned), matrix_transpose(matrix_aligned(aligned)));
EXPECT_VECTOREQ(mat.row[0], vector(0, 3, -6, 9));
EXPECT_VECTOREQ(mat.row[1], vector(-3, 0, -17, 22));
EXPECT_VECTOREQ(mat.row[2], vector(6, 17, 0, 27));
EXPECT_VECTOREQ(mat.row[3], vector(-9, -22, -27, 0));
mat = matrix_mul(matrix_zero(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector_zero());
EXPECT_VECTOREQ(mat.row[1], vector_zero());
EXPECT_VECTOREQ(mat.row[2], vector_zero());
EXPECT_VECTOREQ(mat.row[3], vector_zero());
mat = matrix_mul(matrix_zero(), matrix_identity());
EXPECT_VECTOREQ(mat.row[0], vector_zero());
EXPECT_VECTOREQ(mat.row[1], vector_zero());
EXPECT_VECTOREQ(mat.row[2], vector_zero());
EXPECT_VECTOREQ(mat.row[3], vector_zero());
mat = matrix_mul(matrix_identity(), matrix_zero());
EXPECT_VECTOREQ(mat.row[0], vector_zero());
EXPECT_VECTOREQ(mat.row[1], vector_zero());
EXPECT_VECTOREQ(mat.row[2], vector_zero());
EXPECT_VECTOREQ(mat.row[3], vector_zero());
mat = matrix_mul(matrix_identity(), matrix_identity());
EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0));
EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0));
EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0));
EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1));
mat = matrix_mul(matrix_aligned(aligned), matrix_unaligned(unaligned));
EXPECT_VECTOREQ(mat.row[0], vector(-8 + 24 - 48, 1 - 10 + 27 - 4 * 13, 2 - 12 + 30 - 4 * 14,
3 - 14 + 33 - 4 * 15));
EXPECT_VECTOREQ(mat.row[1], vector(6 * 4 - 7 * 8 + 8 * 12, -5 * 1 + 6 * 5 - 7 * 9 + 8 * 13,
-5 * 2 + 6 * 6 - 7 * 10 + 8 * 14, -5 * 3 + 6 * 7 - 7 * 11 + 8 * 15));
EXPECT_VECTOREQ(mat.row[2], vector(10 * 4 + 11 * 8 + 12 * 12, 9 * 1 + 10 * 5 + 11 * 9 + 12 * 13,
9 * 2 + 10 * 6 + 11 * 10 + 12 * 14, 9 * 3 + 10 * 7 + 11 * 11 + 12 * 15));
EXPECT_VECTOREQ(mat.row[3], vector(-14 * 4 - 15 * 8 - 16 * 12, -13 * 1 - 14 * 5 - 15 * 9 - 16 * 13,
-13 * 2 - 14 * 6 - 15 * 10 - 16 * 14, -13 * 3 - 14 * 7 - 15 * 11 - 16 * 15));
return 0;
}
// Deform the vertex array in classic linear way.
void compute_linear_deformation(FbxAMatrix& pGlobalPosition,
FbxMesh* pMesh,
FbxTime& pTime,
FbxVector4* pVertexArray,
FbxPose* pPose)
{
// All the links must have the same link mode.
FbxCluster::ELinkMode lClusterMode = ((FbxSkin*)pMesh->GetDeformer(0, FbxDeformer::eSkin))->GetCluster(0)->GetLinkMode();
int lVertexCount = pMesh->GetControlPointsCount();
FbxAMatrix* lClusterDeformation = new FbxAMatrix[lVertexCount];
memset(lClusterDeformation, 0, lVertexCount * sizeof(FbxAMatrix));
double* lClusterWeight = new double[lVertexCount];
memset(lClusterWeight, 0, lVertexCount * sizeof(double));
if(lClusterMode == FbxCluster::eAdditive) {
for(int i = 0; i < lVertexCount; ++i) {
lClusterDeformation[i].SetIdentity();
}
}
// For all skins and all clusters, accumulate their deformation and weight
// on each vertices and store them in lClusterDeformation and lClusterWeight.
int lSkinCount = pMesh->GetDeformerCount(FbxDeformer::eSkin);
for(int lSkinIndex=0; lSkinIndex<lSkinCount; ++lSkinIndex) {
FbxSkin * lSkinDeformer = (FbxSkin *)pMesh->GetDeformer(lSkinIndex, FbxDeformer::eSkin);
int lClusterCount = lSkinDeformer->GetClusterCount();
for(int lClusterIndex=0; lClusterIndex<lClusterCount; ++lClusterIndex) {
FbxCluster* lCluster = lSkinDeformer->GetCluster(lClusterIndex);
if(!lCluster->GetLink()) {
continue;
}
FbxAMatrix lVertexTransformMatrix;
compute_cluster_deformation(pGlobalPosition, pMesh, lCluster, lVertexTransformMatrix, pTime, pPose);
int lVertexIndexCount = lCluster->GetControlPointIndicesCount();
for(int k = 0; k < lVertexIndexCount; ++k) {
int lIndex = lCluster->GetControlPointIndices()[k];
// Sometimes, the mesh can have less points than at the time of the skinning
// because a smooth operator was active when skinning but has been deactivated during export.
if(lIndex >= lVertexCount) {
continue;
}
double lWeight = lCluster->GetControlPointWeights()[k];
if(lWeight == 0.0) {
continue;
}
// Compute the influence of the link on the vertex.
FbxAMatrix lInfluence = lVertexTransformMatrix;
matrix_scale(lInfluence, lWeight);
if(lClusterMode == FbxCluster::eAdditive) {
// Multiply with the product of the deformations on the vertex.
matrix_add_to_diagonal(lInfluence, 1.0 - lWeight);
lClusterDeformation[lIndex] = lInfluence * lClusterDeformation[lIndex];
// Set the link to 1.0 just to know this vertex is influenced by a link.
lClusterWeight[lIndex] = 1.0;
} else { // lLinkMode == FbxCluster::eNormalize || lLinkMode == FbxCluster::eTotalOne
// Add to the sum of the deformations on the vertex.
matrix_add(lClusterDeformation[lIndex], lInfluence);
// Add to the sum of weights to either normalize or complete the vertex.
lClusterWeight[lIndex] += lWeight;
}
}//For each vertex
}//lClusterCount
}
//Actually deform each vertices here by information stored in lClusterDeformation and lClusterWeight
for(int i = 0; i < lVertexCount; i++) {
FbxVector4 lSrcVertex = pVertexArray[i];
FbxVector4& lDstVertex = pVertexArray[i];
double lWeight = lClusterWeight[i];
// Deform the vertex if there was at least a link with an influence on the vertex,
if(lWeight != 0.0) {
lDstVertex = lClusterDeformation[i].MultT(lSrcVertex);
if(lClusterMode == FbxCluster::eNormalize) {
// In the normalized link mode, a vertex is always totally influenced by the links.
lDstVertex /= lWeight;
} else if(lClusterMode == FbxCluster::eTotalOne) {
// In the total 1 link mode, a vertex can be partially influenced by the links.
lSrcVertex *= (1.0 - lWeight);
lDstVertex += lSrcVertex;
}
}
}
delete [] lClusterDeformation;
delete [] lClusterWeight;
}
int main(int argc, char **argv) {
if (argc < 3) {
fprintf(stderr,"Two arguments required.\n");
exit(-1);
}
srand(time(NULL));
int i, j;
int size = atoi(argv[1]); // side length of matrix
size *= size; // square side to get total size
int blocks = atoi(argv[2]); // number of blocks
if (size % blocks != 0) {
fprintf(stderr,"Matrix size can not be evenly divided into %d blocks.\n",blocks);
exit(-1);
}
int block_size = size / blocks; // size of a block
int scalar = rand() % 200 - 100; // random scalar
int *next = malloc(sizeof(int) * block_size); // read block
int *current = malloc(sizeof(int) * block_size); // add block
int *prev = malloc(sizeof(int) * block_size); // write block
struct aiocb async_cb_r;
async_cb_r.aio_fildes = STDIN_FILENO;
async_cb_r.aio_buf = next;
async_cb_r.aio_nbytes = sizeof(int) * block_size;
async_cb_r.aio_reqprio = 0;
struct aiocb async_cb_w;
async_cb_w.aio_fildes = STDOUT_FILENO;
async_cb_w.aio_buf = prev;
async_cb_w.aio_nbytes = sizeof(int) * block_size;
async_cb_w.aio_reqprio = 0;
int start_time = time(NULL); // start time
// prime the pump
async_cb_r.aio_offset = 0;
aio_read(&async_cb_r);
while (aio_error(&async_cb_r) == EINPROGRESS) {} // complete the first async request
aio_return(&async_cb_r);
memcpy(current, next, sizeof(int) * block_size); // copy next to current
for (i = 1; i < blocks; i++) {
async_cb_r.aio_offset = i * sizeof(int) * block_size;
aio_read(&async_cb_r); // read stdin to next
matrix_add(current, block_size, scalar); // do add on current while reading next
memcpy(prev, current, sizeof(int) * block_size);
async_cb_w.aio_offset = (i - 1) * sizeof(int) * block_size;
aio_write(&async_cb_w);
while (aio_error(&async_cb_w) == EINPROGRESS) {}
aio_return(&async_cb_w); // wait until write to prev is complete
while (aio_error(&async_cb_r) == EINPROGRESS) {}
aio_return(&async_cb_r); // wait until read is complete
memcpy(current, next, sizeof(int) * block_size); // copy next to current
}
matrix_add(current, block_size, scalar); // do final add
memcpy(prev, current, sizeof(int) * block_size);
async_cb_w.aio_offset = (i - 1) * sizeof(int) * block_size;
aio_write(&async_cb_w);
while (aio_error(&async_cb_w) == EINPROGRESS) {}
aio_return(&async_cb_w); // last write complete
int end_time = time(NULL); // end time
fprintf(stderr,"Start time: %d, End time: %d, Total time: %d\n", start_time, end_time, end_time - start_time);
free(next);
free(current);
free(prev);
return(0);
}
matrix calc_H(int l, double R, double M)
{
matrix H = matrix_add(calc_V(l, R), calc_T(l, M, R));
return H;
}
/**
* Set command
*/
void command_set(const char* line) {
char cmd[MAX_BUFFER];
char key[MAX_BUFFER];
char func[MAX_BUFFER];
char arg1[MAX_BUFFER];
char arg2[MAX_BUFFER];
int argc = sscanf(line, "%s %s = %s %s %s", cmd, key, func, arg1, arg2);
if (argc < 3) {
puts("invalid arguments");
return;
}
uint32_t* matrix = NULL;
switch (argc) {
case 3:
if (strcasecmp(func, "identity") == 0) {
matrix = identity_matrix();
} else {
goto invalid;
}
break;
case 4:
if (strcasecmp(func, "random") == 0) {
uint32_t seed = atoll(arg1);
matrix = random_matrix(seed);
} else if (strcasecmp(func, "uniform") == 0) {
uint32_t value = atoll(arg1);
matrix = uniform_matrix(value);
} else if (strcasecmp(func, "cloned") == 0) {
MATRIX_GUARD(arg1);
matrix = cloned(m);
} else if (strcasecmp(func, "reversed") == 0) {
MATRIX_GUARD(arg1);
matrix = reversed(m);
} else if (strcasecmp(func, "transposed") == 0) {
MATRIX_GUARD(arg1);
matrix = transposed(m);
} else {
goto invalid;
}
break;
case 5:
if (strcasecmp(func, "sequence") == 0) {
uint32_t start = atoll(arg1);
uint32_t step = atoll(arg2);
matrix = sequence_matrix(start, step);
} else if (strcasecmp(func, "scalar#add") == 0) {
MATRIX_GUARD(arg1);
uint32_t value = atoll(arg2);
matrix = scalar_add(m, value);
} else if (strcasecmp(func, "scalar#mul") == 0) {
MATRIX_GUARD(arg1);
uint32_t value = atoll(arg2);
matrix = scalar_mul(m, value);
} else if (strcasecmp(func, "matrix#add") == 0) {
MATRIX_GUARD_PAIR(arg1, arg2);
matrix = matrix_add(m1, m2);
} else if (strcasecmp(func, "matrix#mul") == 0) {
MATRIX_GUARD_PAIR(arg1, arg2);
matrix = matrix_mul(m1, m2);
} else if (strcasecmp(func, "matrix#pow") == 0) {
MATRIX_GUARD(arg1);
uint32_t exponent = atoll(arg2);
matrix = matrix_pow(m, exponent);
} else {
goto invalid;
}
break;
}
entry* e = find_entry(key);
if (e == NULL) {
e = add_entry(key);
} else {
free(e->matrix);
}
e->matrix = matrix;
puts("ok");
return;
invalid:
puts("invalid arguments");
}
void RPYSolver::orientationSolver(double* output, double phi, double theta, double psi, double yaw1, double pitch1, double roll1, double yaw2, double pitch2, double roll2, int attempt)
{
/**************************************************************************/
//Is the desired orientation possible to attain with the given joint limits?
/**************************************************************************/
double wrist_pitch_limit_max; //Used to express the wrist pitch limit in radians
double wrist_pitch_limit_min; //Used to express the wrist pitch limit in radians
//The wrist pitch limit in radians
wrist_pitch_limit_max=(PI/180.0)*WRIST_PITCH_LIMIT_MAX;
wrist_pitch_limit_min=(PI/180.0)*WRIST_PITCH_LIMIT_MIN;
//The PR2 FK is defined such that pitch is negative upward. Hence, the input pitch values are negated, because the following portion of the code was written assuming that pitch is positive upward.
theta=-theta;
pitch1=-pitch1;
pitch2=-pitch2;
double v1[] = {cos(theta)*cos(phi), cos(theta)*sin(phi), sin(theta)};
double v2[] = {cos(pitch2)*cos(yaw2), cos(pitch2)*sin(yaw2), sin(pitch2)};
double dp_v = dot_product(v1, v2, 3);
double check_flag=dp_v<0;
double check_flag2=fabs(dp_v)>fabs(cos(wrist_pitch_limit_max));
double check_flag3=dp_v>0;
double check_flag4=dp_v>cos(wrist_pitch_limit_min);
if((check_flag && check_flag2) || (check_flag3 && check_flag4)) {
// std::cout << "Impossible." << std::endl;
output[0]=0;
output[1]=0;
output[2]=0;
output[3]=0;
return;
}
/*****************************************************/
//Firstly, all variables are declared and some defined
/*****************************************************/
double hand1[]={0, -0.5, 0, 0, 0.5, 0};
double hand2[]={0, -0.5, 0, 0, 0.5, 0};
double hand1_fo[6];
double hand2_fo[6];
double hand1_vect[3], hand2_vect[3];
double grip1[]={0, 0, 0, 1, 0, 0};
double grip2[]={0, 0, 0, 1, 0, 0};
double grip1_fo[6];
double grip2_fo[6];
double grip1_vect[3], grip2_vect[3];
double indicator1[]={-2.5, 0, 0, -2.5, 0, 1};
double indicator2[]={-2.5, 0, 0, -2.5, 0, 1};
double indicator1_fo[6];
double indicator2_fo[6];
double ind1_vect[3],ind2_vect[3];
double comp1_vect[3], comp2_vect[3], project1_vect[3], project2_vect[3];
double rot1[9], rot2[9], rot3[9];
double temp[6], temp1[9], temp2[9], temp3[9], temp_var, temp_vect[3], temp2_vect[3];
double w[3], w_hat[9];
double rot_world[9], rot_world_trans[9];
double identity[]={1, 0, 0,
0, 1, 0,
0, 0, 1};
double unit_x[3]={1, 0, 0};
double zero_vect[3]={0, 0, 0};
double fo_arm_roll, wrist_pitch, wrist_roll, fs_angle, fe_angle;
double is_orient_possible_flag=1; //Flag returned indicates whether desired orientation is possible
//with wrist limits. 1-possible, 0-impossible
double c_alpha, c_beta, c_gamma, c_delta, c_eps;
/******************************/
//Accepting the input arguments
/******************************/
create_rotation_matrix(rot_world,phi,theta,psi);
transpose(rot_world_trans, rot_world, 3, 3);
//The start and the end orientations in the world frame
rot1[0]=cos(yaw1); rot1[3]=-sin(yaw1); rot1[6]=0;
rot1[1]=sin(yaw1); rot1[4]=cos(yaw1); rot1[7]=0;
rot1[2]=0; rot1[5]=0; rot1[8]=1;
rot2[0]=cos(pitch1); rot2[3]=0; rot2[6]=-sin(pitch1);
rot2[1]=0; rot2[4]=1; rot2[7]=0;
rot2[2]=sin(pitch1); rot2[5]=0; rot2[8]=cos(pitch1);
multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
multiply(temp,rot3,3,3,hand1,2);
equate(hand1,temp,3,2);
multiply(temp,rot3,3,3,grip1,2);
equate(grip1,temp,3,2);
w[0]=grip1[3]; //Unit vector along grip
w[1]=grip1[4];
w[2]=grip1[5];
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(roll1));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(roll1));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity, temp2,3,3);
multiply(temp1,rot1,3,3,hand1,2);
equate(hand1,temp1,3,2);
rot1[0]=cos(yaw2); rot1[3]=-sin(yaw2); rot1[6]=0;
rot1[1]=sin(yaw2); rot1[4]=cos(yaw2); rot1[7]=0;
rot1[2]=0; rot1[5]=0; rot1[8]=1;
rot2[0]=cos(pitch2); rot2[3]=0; rot2[6]=-sin(pitch2);
rot2[1]=0; rot2[4]=1; rot2[7]=0;
rot2[2]=sin(pitch2); rot2[5]=0; rot2[8]=cos(pitch2);
multiply(rot3,rot1,3,3,rot2,3); //Yaw and pitch rotations
multiply(temp,rot3,3,3,hand2,2);
equate(hand2,temp,3,2);
multiply(temp,rot3,3,3,grip2,2);
equate(grip2,temp,3,2);
w[0]=grip2[3]; //Unit vector along grip
w[1]=grip2[4];
w[2]=grip2[5];
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(roll2));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(roll2));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity,temp2,3,3);
multiply(temp1,rot1,3,3,hand2,2);
equate(hand2,temp1,3,2);
//The start and the end orientations in the forearm frame
multiply(hand1_fo,rot_world_trans,3,3,hand1,2);
multiply(hand2_fo,rot_world_trans,3,3,hand2,2);
multiply(grip1_fo,rot_world_trans,3,3,grip1,2);
multiply(grip2_fo,rot_world_trans,3,3,grip2,2);
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
temp_var=dot_product(grip1_vect, unit_x, 3);
scalar_multiply(comp1_vect, unit_x,3,1,temp_var);
subtract(project1_vect,grip1_vect,comp1_vect,3,1);
grip2_vect[0]=grip2_fo[3];
grip2_vect[1]=grip2_fo[4];
grip2_vect[2]=grip2_fo[5];
temp_var=dot_product(grip2_vect, unit_x, 3);
scalar_multiply(comp2_vect, unit_x,3,1,temp_var);
subtract(project2_vect,grip2_vect,comp2_vect,3,1);
ind1_vect[0]=indicator1[3]-indicator1[0];
ind1_vect[1]=indicator1[4]-indicator1[1];
ind1_vect[2]=indicator1[5]-indicator1[2];
ind2_vect[0]=indicator2[3]-indicator2[0];
ind2_vect[1]=indicator2[4]-indicator2[1];
ind2_vect[2]=indicator2[5]-indicator2[2];
if(!check_equality(grip1_vect, comp1_vect, 3)) {
c_delta=dot_product(ind1_vect, project1_vect, 3)/vect_norm(project1_vect,3);
fs_angle=acos(c_delta);
cross_product(temp_vect, ind1_vect, project1_vect);
if(vect_divide(temp_vect, unit_x, 3)<0) {
fs_angle=-fs_angle;
}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fs_angle); rot1[7]=-sin(fs_angle);
rot1[2]=0; rot1[5]=sin(fs_angle); rot1[8]=cos(fs_angle);
multiply(temp, rot1, 3,3, indicator1, 2);
equate(indicator1_fo, temp, 3, 2);
}
else {
equate(indicator1_fo, indicator1, 3,2);
}
if(!check_equality(grip2_vect, comp2_vect, 3)) {
c_eps=dot_product(ind2_vect, project2_vect, 3)/vect_norm(project2_vect, 3);
fe_angle=acos(c_eps);
cross_product(temp_vect, ind2_vect, project2_vect);
if(vect_divide(temp_vect, unit_x, 3)<0) {
fe_angle=-fe_angle;
}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fe_angle); rot1[7]=-sin(fe_angle);
rot1[2]=0; rot1[5]=sin(fe_angle); rot1[8]=cos(fe_angle);
multiply(temp, rot1, 3,3, indicator2, 2);
equate(indicator2_fo, temp, 3, 2);
}
else {
equate(indicator2_fo, indicator2, 3,2);
}
/*********************************/
//Forearm roll is calculated now
/*********************************/
ind1_vect[0]=indicator1_fo[3]-indicator1_fo[0];
ind1_vect[1]=indicator1_fo[4]-indicator1_fo[1];
ind1_vect[2]=indicator1_fo[5]-indicator1_fo[2];
ind2_vect[0]=indicator2_fo[3]-indicator2_fo[0];
ind2_vect[1]=indicator2_fo[4]-indicator2_fo[1];
ind2_vect[2]=indicator2_fo[5]-indicator2_fo[2];
c_gamma=dot_product(ind1_vect, ind2_vect, 3);
cross_product(temp_vect, ind1_vect, ind2_vect);
if(vect_divide(temp_vect, unit_x, 3)>0) {
fo_arm_roll=acos(c_gamma);
}
else {
fo_arm_roll=-acos(c_gamma);
}
//In the second attempt try rotating the other way around to the same orientation
if(attempt == 2) {fo_arm_roll = -(2*PI - fo_arm_roll);}
rot1[0]=1; rot1[3]=0; rot1[6]=0;
rot1[1]=0; rot1[4]=cos(fo_arm_roll); rot1[7]=-sin(fo_arm_roll);
rot1[2]=0; rot1[5]=sin(fo_arm_roll); rot1[8]=cos(fo_arm_roll);
multiply(temp1,rot1,3,3,hand1_fo,2);
equate(hand1_fo,temp1,3,2);
multiply(temp1,rot1,3,3,grip1_fo,2);
equate(grip1_fo,temp1,3,2);
/*********************************/
//Wrist pitch is calculated now
/*********************************/
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
grip2_vect[0]=grip2_fo[3];
grip2_vect[1]=grip2_fo[4];
grip2_vect[2]=grip2_fo[5];
cross_product(temp_vect, grip1_vect, grip2_vect);
if(!check_equality(temp_vect, zero_vect, 3)) {
c_alpha=dot_product(grip1_vect, grip2_vect, 3);
cross_product(temp2_vect,ind2_vect,temp_vect);
if(vect_divide(temp2_vect, unit_x, 3)>0) {
wrist_pitch=-acos(c_alpha);
temp_vect[0]=-temp_vect[0];
temp_vect[1]=-temp_vect[1];
temp_vect[2]=-temp_vect[2];
}
else {
wrist_pitch=acos(c_alpha);
}
scalar_multiply(w, temp_vect, 3,1,1/vect_norm(temp_vect, 3));
w_hat[0]=0; w_hat[3]=-w[2]; w_hat[6]=w[1];
w_hat[1]=w[2]; w_hat[4]=0; w_hat[7]=-w[0];
w_hat[2]=-w[1]; w_hat[5]=w[0]; w_hat[8]=0;
scalar_multiply(temp1,w_hat,3,3,sin(wrist_pitch));
multiply(temp2,w_hat,3,3,w_hat,3);
scalar_multiply(temp3,temp2,3,3,1-cos(wrist_pitch));
matrix_add(temp2,temp1,temp3,3,3);
matrix_add(rot1,identity, temp2,3,3);
multiply(temp1,rot1,3,3,hand1_fo,2);
equate(hand1_fo,temp1,3,2);
multiply(temp1,rot1,3,3,grip1_fo,2);
equate(grip1_fo,temp1,3,2);
}
else {
wrist_pitch=0;
}
/**Somehow the wrist_roll calculations from within this code don't seem to be right.
This problem has been temporarily solved by invoking FK from environment_robarm3d.cpp**/
/*********************************/
//Wrist roll is calculated now
/*********************************/
wrist_roll=0;
hand1_vect[0]=hand1_fo[3]-hand1_fo[0];
hand1_vect[1]=hand1_fo[4]-hand1_fo[1];
hand1_vect[2]=hand1_fo[5]-hand1_fo[2];
hand2_vect[0]=hand2_fo[3]-hand2_fo[0];
hand2_vect[1]=hand2_fo[4]-hand2_fo[1];
hand2_vect[2]=hand2_fo[5]-hand2_fo[2];
grip1_vect[0]=grip1_fo[3];
grip1_vect[1]=grip1_fo[4];
grip1_vect[2]=grip1_fo[5];
c_beta=dot_product(hand1_vect, hand2_vect, 3);
cross_product(temp_vect, hand1_vect, hand2_vect);
if(!check_equality(temp_vect, zero_vect, 3)) {
if(vect_divide(temp_vect, grip1_vect, 3)>0) {
wrist_roll=acos(c_beta);
}
else {
wrist_roll=-acos(c_beta);
}
}
//The output
output[0]=is_orient_possible_flag;
output[1]=fo_arm_roll;
output[2]=wrist_pitch;
output[3]=wrist_roll;
}
double NN_cost_function(int num_threads, matrix_t** gradient, matrix_t* rolled_theta, unsigned int layer_sizes[], unsigned int num_layers,
unsigned int num_labels, matrix_t* X, matrix_t* y, double lamda)
{
unsigned int theta_sizes[][2] = {{25, 401}, {10, 26}};
matrix_list_t* theta = unroll_matrix_list(rolled_theta, num_layers-1, theta_sizes);
unsigned int m = X->rows;
//unsigned int n = X->cols;
matrix_list_t* theta_gradient_total = matrix_list_constructor(theta->num);
unsigned int i, j;
for(i=0; i<theta_gradient_total->num; i++)
{
theta_gradient_total->matrix_list[i] = matrix_constructor(theta->matrix_list[i]->rows, theta->matrix_list[i]->cols);
}
omp_set_num_threads(num_threads);
int nthreads, tid;
#pragma omp parallel private(nthreads, tid)
{
int indexes[2];
tid = omp_get_thread_num();
nthreads = omp_get_num_threads();
get_indexes(m, nthreads, tid, indexes);
unsigned int i, j;
matrix_t* temp;
matrix_t* temp2;
matrix_t* temp3;
matrix_list_t* theta_gradient = matrix_list_constructor(theta->num);
for(i=0; i<theta_gradient->num; i++)
{
theta_gradient->matrix_list[i] = matrix_constructor(theta->matrix_list[i]->rows, theta->matrix_list[i]->cols);
}
for(i=indexes[0]; i<indexes[1]; i++)
{
matrix_list_t* A = matrix_list_constructor(num_layers);
matrix_list_t* Z = matrix_list_constructor(num_layers-1);
matrix_list_t* delta = matrix_list_constructor(num_layers-1);
A->matrix_list[0] = row_to_vector(X, i);
temp = matrix_prepend_col(A->matrix_list[0], 1.0);
free_matrix(A->matrix_list[0]);
A->matrix_list[0] = matrix_transpose(temp);
free_matrix(temp);
for(j=0; j<num_layers-1; j++)
{
Z->matrix_list[j] = matrix_multiply(theta->matrix_list[j], A->matrix_list[j]);
temp = matrix_sigmoid(Z->matrix_list[j]);
A->matrix_list[j+1] = matrix_prepend_row(temp, 1.0);
free_matrix(temp);
}
temp = matrix_remove_row(A->matrix_list[num_layers-1]);
free_matrix(A->matrix_list[num_layers-1]);
A->matrix_list[num_layers-1] = temp;
matrix_t* result_matrix = matrix_constructor(1, num_labels);
for(j = 0; j < num_labels; j++)
{
if(vector_get(y, i) == j)
{
vector_set(result_matrix, j, 1.0);
}
}
temp = matrix_transpose(result_matrix);
free_matrix(result_matrix);
result_matrix= temp;
delta->matrix_list[1] = matrix_subtract(A->matrix_list[num_layers-1], result_matrix);
free_matrix(result_matrix);
matrix_t* theta_transpose = matrix_transpose(theta->matrix_list[1]);
temp = matrix_multiply(theta_transpose, delta->matrix_list[1]);
matrix_t* sig_gradient = matrix_sigmoid_gradient(Z->matrix_list[0]);
temp2 = matrix_prepend_row(sig_gradient, 1.0);
temp3 = matrix_cell_multiply(temp, temp2);
delta->matrix_list[0] = matrix_remove_row(temp3);
free_matrix(temp);
free_matrix(temp2);
free_matrix(temp3);
free_matrix(sig_gradient);
free_matrix(theta_transpose);
for(j=0; j<num_layers-1; j++)
{
matrix_t* A_transpose = matrix_transpose(A->matrix_list[j]);
temp = matrix_multiply(delta->matrix_list[j], A_transpose);
temp2 = matrix_add(theta_gradient->matrix_list[j], temp);
free_matrix(theta_gradient->matrix_list[j]);
theta_gradient->matrix_list[j] = temp2;
free_matrix(A_transpose);
free_matrix(temp);
}
free_matrix_list(A);
free_matrix_list(Z);
free_matrix_list(delta);
}
#pragma omp critical
{
matrix_list_t* temp_list;
temp_list = matrix_list_add(theta_gradient_total, theta_gradient);
free_matrix_list(theta_gradient_total);
free_matrix_list(theta_gradient);
theta_gradient_total = temp_list;
}
}
for(i=0; i<num_layers-1; i++)
{
matrix_t* temp;
matrix_t* temp2;
matrix_t* temp3;
temp = matrix_scalar_multiply(theta_gradient_total->matrix_list[i], 1.0/m);
temp2 = copy_matrix(theta->matrix_list[i]);
for(j=0; j<theta->matrix_list[i]->rows; j++)
{
matrix_set(temp2, j, 0, 0.0);
}
free_matrix(theta_gradient_total->matrix_list[i]);
temp3 = matrix_scalar_multiply(temp2, lamda/m);
theta_gradient_total->matrix_list[i] = matrix_add(temp, temp3);
free_matrix(temp);
free_matrix(temp2);
free_matrix(temp3);
}
*gradient = roll_matrix_list(theta_gradient_total);
free_matrix_list(theta);
free_matrix_list(theta_gradient_total);
return 0.0;
}
/*
* Sample n Gaussians (with means X and covariances S) from HLL at sample points Q.
*/
void hll_sample(double **X, double ***S, double **Q, hll_t *hll, int n)
{
int i, j;
if (hll->cache.n > 0) {
for (i = 0; i < n; i++) {
j = kdtree_NN(hll->cache.Q_kdtree, Q[i]);
memcpy(X[i], hll->cache.X[j], hll->dx * sizeof(double));
matrix_copy(S[i], hll->cache.S[j], hll->dx, hll->dx);
}
return;
}
double r = hll->r;
int nx = hll->dx;
int nq = hll->dq;
double **WS = new_matrix2(nx, nx);
for (i = 0; i < n; i++) {
//printf("q = [%.2f %.2f %.2f %.2f]\n", Q[0][0], Q[0][1], Q[0][2], Q[0][3]);
//for (j = 0; j < hll->n; j++)
// printf("hll->Q[%d] = [%.2f %.2f %.2f %.2f]\n", j, hll->Q[j][0], hll->Q[j][1], hll->Q[j][2], hll->Q[j][3]);
// compute weights
double dq, qdot;
double w[hll->n];
//printf("w = [");
for (j = 0; j < hll->n; j++) {
qdot = fabs(dot(Q[i], hll->Q[j], nq));
dq = acos(MIN(qdot, 1.0));
w[j] = exp(-(dq/r)*(dq/r));
//printf("%.2f ", w[j]);
}
//printf("]\n");
// threshold weights
double wmax = arr_max(w, hll->n);
double wthresh = wmax/50; //dbug: make this a parameter?
for (j = 0; j < hll->n; j++)
if (w[j] < wthresh)
w[j] = 0;
double wtot = hll->w0 + sum(w, hll->n);
// compute posterior mean
mult(X[i], hll->x0, hll->w0, nx); // X[i] = w0*x0
for (j = 0; j < hll->n; j++) {
if (w[j] > 0) {
double wx[nx];
mult(wx, hll->X[j], w[j], nx);
add(X[i], X[i], wx, nx); // X[i] += wx
}
}
mult(X[i], X[i], 1/wtot, nx); // X[i] /= wtot
// compute posterior covariance matrix
mult(S[i][0], hll->S0[0], hll->w0, nx*nx); // S[i] = w0*S0
for (j = 0; j < hll->n; j++) {
if (w[j] > 0) {
double wdx[nx];
sub(wdx, hll->X[j], X[i], nx);
mult(wdx, wdx, w[j], nx);
outer_prod(WS, wdx, wdx, nx, nx); // WS = wdx'*wdx
matrix_add(S[i], S[i], WS, nx, nx); // S[i] += WS
}
}
mult(S[i][0], S[i][0], 1/wtot, nx*nx); // S[i] /= wtot
}
free_matrix2(WS);
}
