void grab_self(char* Direction){
if(!strcmp(Direction,"N")){
update_matrix(train_number, 1, '1');
printf("\nTrain<%d>: Requests for North-Lock\n", pid);
grab(1);
printf("\nTrain<%d>: Acquires North-Lock\n", pid);
update_matrix(train_number, 1, '2');
}
else if(!strcmp(Direction, "E")){
update_matrix(train_number, 2, '1');
printf("\nTrain<%d>: Requests for East-Lock\n", pid);
grab(2);
printf("\nTrain<%d>: Acquires East-Lock\n", pid);
update_matrix(train_number, 2, '2');
}
else if(!strcmp(Direction, "W")){
update_matrix(train_number, 4, '1');
printf("\nTrain<%d>: Requests for West-Lock\n", pid);
grab(4);
printf("\nTrain<%d>: Acquires West-Lock\n", pid);
update_matrix(train_number, 4, '2');
}
else{
update_matrix(train_number, 3, '1');
printf("\nTrain<%d>: Requests for South-Lock\n", pid);
grab(3);
printf("\nTrain<%d>: Acquires South-Lock\n", pid);
update_matrix(train_number, 3, '2');
}
}
// Add a knot to the bspline
void BSpline::add_knot(int row, int col) {
// add a new knot to the end of the knot list
_knots.push_back(Knot(row, col));
// store the current knot coordinates in a newmat matrix
update_matrix();
}
void GameObject::update(Level* lvl) {
if (heading == target_heading) return;
if (target_heading > heading) {
heading = start_heading + (float)rotation_timer.get_msec() / 100;
if (target_heading < heading) {
heading = target_heading;
if (!symmetric) {
body->DestroyFixture(body->GetFixtureList());
b2FixtureDef fixtureDef;
fixtureDef.shape = &mi->GetMesh().getShape();
fixtureDef.density = 1.0f;
body->CreateFixture(&fixtureDef);
}
}
} else {
heading = start_heading - (float)rotation_timer.get_msec() / 100;
if (target_heading > heading) {
heading = target_heading;
if (!symmetric) {
body->DestroyFixture(body->GetFixtureList());
b2FixtureDef fixtureDef;
fixtureDef.shape = &mi->GetMesh().getLeftShape();
fixtureDef.density = 1.0f;
body->CreateFixture(&fixtureDef);
}
}
}
update_matrix();
}
/*
* Main loop
*/
int main(void)
{
/* Initialize all peripherals */
open_board();
/* Configure stdout/stderr to be keyboard input to the host */
open_streams();
led_set(3, 1, 1);
for (;;) {
poll_matrix();
update_matrix();
// Handle USB HID reporting
// Generic USB tasks take place in interrupt
USB_USBTask();
USB_HIDTask();
if (GFLAGS & GF_BOOTLOADER)
break;
maybe_sleep();
}
/* Disable stdout/stderr. We can't use it if usb is down */
close_streams();
/* Teardown everything we setup. Nothing can be active before the jump */
close_board();
jump_to_bootloader();
}
void MainWindow::fill_matrix(){
srand( rand() );
searchAct->setEnabled(1);
for(int r = 0; r < rnum; ++r)
for(int c = 0; c < cnum; ++c){
data[r * cnum + c] = rand() % (mod+1) ;
}
update_matrix();
}
/*ARGSUSED1*/
void
motion(int x, int y)
{
if (moving) {
s += 0.1 * (x - bx) / (float)w;
update_matrix();
glutPostRedisplay();
}
}
void write_matrix(int pattern[8][8]){
int i;
int j;
for(i = 0 ; i < 8 ; i++){
for(j = 0 ; j < 8 ; j++){
new_pattern[i][j] = pattern[i][j];
}
}
update_matrix();
}
void release_self(char* Direction){
if(!strcmp(Direction,"N")){
release(1);
update_matrix(train_number, 1, '0');
printf("\nTrain<%d>: Releases North-Lock\n", pid);
}
else if(!strcmp(Direction, "E")){
release(2);
update_matrix(train_number, 2, '0');
printf("\nTrain<%d>: Releases East-Lock\n", pid);
}
else if(!strcmp(Direction, "W")){
release(4);
update_matrix(train_number, 4, '0');
printf("\nTrain<%d>: Releases West-Lock\n", pid);
}
else{
release(3);
update_matrix(train_number, 3, '0');
printf("\nTrain<%d>: Releases South-Lock\n", pid);
}
}
void XfmSetTransform(struct Transform *transform,
int transform_order, int rotate_order,
double tx, double ty, double tz,
double rx, double ry, double rz,
double sx, double sy, double sz)
{
transform->transform_order = transform_order;
transform->rotate_order = rotate_order;
VEC3_SET(transform->translate, tx, ty, tz);
VEC3_SET(transform->rotate, rx, ry, rz);
VEC3_SET(transform->scale, sx, sy, sz);
update_matrix(transform);
}
BSpline::BSpline(const Matrix mat)
{
int N, j, i;
RowVector first(2), last(2);
// specify default parameters in case the matrix
// passed as argument is incorrect
_d = 3;
_k = 0;
_done = false;
draw_init();
// check that the matrix has two rows
if (mat.ncols() != 2) {
cerr << "BSpline::BSpline(): Error: Knot matrix must have two columns" << endl
<< "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
// check that the number of interior knot intervals is a power of two
N = mat.nrows();
j = (int) ceil(log((double)N-2*_d-1)/log(2.0));
if ((N-2*_d-1) != (1<<j)) {
cerr << "BSpline::BSpline(): Error: Incorrect number of interior knots" << endl
<< "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
// check that the first and last d+1 columns are identical
first = mat.Row(1);
last = mat.Row(N);
for (i=1; i<_d; i++) {
if ((first(1) != mat(i+1,1)) ||
(first(2) != mat(i+1,2)) ||
(last(1) != mat(N-i,1)) ||
(last(2) != mat(N-i,2))) {
cerr << "BSpline::BSpline(): Error: Matrix is not an endpoint-interpolating bspline"
<< endl << "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
}
// matrix is in correct format, so create the knots
for (i=1; i<=N; i++)
add_knot((int)rint(mat(i,1)), (int)rint(mat(i,2)));
// update the internal bspline parameters
_j = j;
_done = true;
update_matrix();
}
void MainWindow::delete_row(){
deleteAct->setEnabled(0);
if (data.size() <= cnum ) return;
std::vector< int >::iterator it = data.begin();
int pos = ( ui->tableWidget->currentRow() ) * cnum;
data.erase(it + pos, it + pos + cnum);
//std::cout << data.size() <<std::endl;
--rnum;
std::cout << rnum <<" "<<cnum <<std::endl;
update_matrix();
}
// Move knot
void BSpline::move_knot(int near_row, int near_col, int to_row, int to_col)
{
KnotList::iterator i, bi;
KnotList::reverse_iterator ri;
int dist, bdist, dr, dc, j, bj;
// can only move knots if all knots of bspline have already been specified
if (!endpoints_locked())
return;
//
// search for the knot closest to near_row, near_col
//
dr = _knots.begin()->r() - near_row;
dc = _knots.begin()->c() - near_col;
bdist = dr*dr + dc*dc;
bj = 1;
for (i=_knots.begin(), bi=_knots.begin(), j=1; i!=_knots.end(); i++, j++) {
// compute distance of knot from near_ point
dr = i->r() - near_row;
dc = i->c() - near_col;
dist = dr*dr + dc*dc;
if (bdist > dist) {
bi = i;
bdist = dist;
bj = j;
}
}
// if it is not an endpoint of the spline, we just update a single knot
if (( bj > _d+1) && (bj <= _k)) {
bi->r() = near_row;
bi->c() = near_col;
} else if (bj == 1) {
// closest point is the first endpoint
for (i=_knots.begin(), j=1; j <= _d+1; i++, j++) {
i->r() = near_row;
i->c() = near_col;
}
} else {
// closest point is the second endpoint
for (ri=_knots.rbegin(), j=1; j <= _d+1; j++, ri++) {
ri->r() = near_row;
ri->c() = near_col;
}
}
// update the knot matrix
update_matrix();
}
void QR_decomposition(double **A, double *gamma, int rows, int columns, int *permutation) {
int k;
double t, *norms, *max;
norms = malloc(columns * sizeof(double));
max = malloc(columns * sizeof(double));
for (k = 0; k < columns; k ++) {
update_norms_vector(A, rows, columns, norms, k, max);
permute(A, rows, columns, permutation, norms, k);
t = generating_Q(rows, A, k, gamma, norms);
update_matrix(A, gamma, rows, columns, k);
A[k][k] = -t;
}
free(max);
free(norms);
}
// This function is called after the last knot in the bspline is
// specified
void BSpline::lock_endpoints() {
int i, j;
int new_k;
_done = true;
// In a bspline with N user-specified knots, there are
// N-1 intervals between the knot points (called 'interior
// intervals' in the wavelet tutorial). In order to
// represent it with a wavelet basis, the number of these
// intervals must be a power of 2. If they are not, we must
// 'pad' the bspline with extra knots to make N-1 a power of 2
// In this implementation, N=_k when _done=false;
// In the following we have N=2^(_j), i.e., _j is the exponent
// of 2 in the wavelets tutorial
_j = (int) ceil(log((double)_k-1)/log(2.0));
new_k = 1 << _j;
if (new_k > _k-1) {
// we pad both the beginning and end of the list with half
// the required set of additional knots
for (i=1; i<=(new_k-_k+1)/2; i++)
_knots.push_front(Knot(_knots.begin()->r(),
_knots.begin()->c()));
for (;i<=new_k-_k+1; i++)
_knots.push_back(Knot(_knots.rbegin()->r(),
_knots.rbegin()->c()));
}
// convert into an endpoint-interpolating spline of degree d by
// repeating the first & last control points another d times, so that
// the first and last (d+1) knots are identical
for (i=1; i<=_d; i++) {
_knots.push_front(Knot(_knots.begin()->r(), _knots.begin()->c()));
_knots.push_back(Knot(_knots.rbegin()->r(), _knots.rbegin()->c()));
}
update_matrix();
// print the contents of the matrix
// cout << _knotMat;
}
//*********** thread entry function **********
void* entry_function(void* p_i_thread){
int i_thread=*( (int*)p_i_thread );
char** temp;
// Calculate the array bounds that each thread will process
int bound = height / n_threads;
int begin_row = i_thread * bound;
int end_row = begin_row + bound;
// exclude extern cells
if(i_thread==0) begin_row++;
if(i_thread==n_threads-1) end_row=height;
// Play the game for steps
int i;
for (i=0; i<steps; i++){
update_matrix(begin_row,end_row);
//thread barrier
int bn = pthread_barrier_wait(&barr);
if(bn == PTHREAD_BARRIER_SERIAL_THREAD){
temp=matrix_from;
matrix_from = matrix_to;
matrix_to = temp;
}else if(bn != 0){
printf("Could not wait on barrier\n");
exit(-1);
}
bn = pthread_barrier_wait(&barr);
if(bn != 0 && bn != PTHREAD_BARRIER_SERIAL_THREAD ){
printf("Could not wait on barrier\n");
exit(-1);
}
}
return 0;
}
/*! @param spm Matrix A in Sparse form*/
void SparseMatrix::extractdiagonal(SparseMatrix & spm){
for(int i = 0; i < spm.get_row_size(); i++){
float temp = 1.0/spm.getValue(i,i);
update_matrix(temp,i,i);
}
}
/*============================================================================
* main
*
*==========================================================================*/
int main(int argc, char **argv)
{
int i;
char *infile = NULL;
char *outfile = NULL;
char *line;
int dim=0;
float *M = NULL;
float *M0 = NULL;
float *mean = NULL;
float *eigen_values_r = NULL;
float *eigen_values_i = NULL;
float *eigen_vectors_r = NULL;
float *eigen_vectors_l = NULL;
eigen_t *eigen = NULL;
int num_models = 0;
int models_size = 0;
float **models = NULL;
int num_xs = 0;
int xs_size = 0;
float *xs = NULL;
float start_time=0, end_time=0, total_time=0;
int in_model = 0;
int in_ensem = 0;
int in_input = 0;
int nev = 5;
in = stdin;
out = stdout;
err = stderr;
static struct option long_options[] = {
{"show", optional_argument, 0, 's'},
{"scale", required_argument, 0, 0},
{"sort", required_argument, 0, 0},
{"nev", required_argument, 0, 0},
{"mo", required_argument, 0, 0},
{"proj", optional_argument, 0, 'p'},
{"help", no_argument, 0, 'h'},
{"format", no_argument, 0, 0},
{"header", no_argument, 0, 0},
{"interp", no_argument, 0, 0},
{"add-back-mean", no_argument, 0, 0},
{0, 0, 0, 0}
};
/*========================================================================
* Capture commandline arguments for write_header()
*======================================================================*/
if (argc != 1)
{
int c=0;
for (i=1; i < argc; i++) c += strlen(argv[i]) + 1;
commandline = (char *)calloc(c + 1, sizeof(char));
for (i=1; i < argc; i++)
{
strcat(commandline, argv[i]);
strcat(commandline, " ");
}
}
/*========================================================================
* Process the command line flags
*======================================================================*/
while (1)
{
int option_index = 0;
int c = getopt_long(argc, argv, "p::i:o:v::s::hc",
long_options, &option_index);
if (c == -1) break;
switch (c)
{
case 0:
if (!strcmp("mo", long_options[option_index].name))
{
opt_multi_out = 1;
mo_prefix = optarg;
}
if (!strcmp("interp", long_options[option_index].name))
{
opt_interp_proj = 1;
}
else if (!strcmp("scale", long_options[option_index].name))
{
if (!strcmp("mult", optarg))
opt_scale = SCALE_MULT;
else if (!strcmp("diag", optarg))
opt_scale = SCALE_DIAG;
else if (!strcmp("none", optarg))
opt_scale = SCALE_NONE;
else
error("Unrecognized scale type.");
}
else if (!strcmp("sort", long_options[option_index].name))
{
if (!strcmp("ev", optarg))
opt_sort = SORT_EV;
else if (!strcmp("lc", optarg))
opt_sort = SORT_LC;
else
error("Unrecognized sort type.");
}
else if (!strcmp("header", long_options[option_index].name))
{
write_header(err);
exit(0);
}
else if (!strcmp("add-back-mean", long_options[option_index].name))
{
opt_add_back_mean = 1;
}
else if (!strcmp("format", long_options[option_index].name))
{
fprintf(err,
"\n"
"#BEGIN INPUT\n"
"<PixeLens input text line 0>\n"
"<PixeLens input text line 1>\n"
" ...\n"
"<PixeLens input text line n>\n"
"#END INPUT\n"
"#BEGIN ENSEM \n"
"#BEGIN MODEL\n"
"<point 0>\n"
"<point 1>\n"
" ...\n"
"<point m>\n"
"#END MODEL\n"
" ...\n"
"#END ENSEM\n"
"\n"
"Blank lines are allowed and any line beginning with a '#' that is\n"
"not mentioned above is interpretted as a comment. It may be the case\n"
"that there are no models. \n"
"\n"
);
exit(0);
}
break;
case 's':
opt_show = 0;
if (optarg == NULL)
{
opt_show = SHOW_DEFAULT;
}
else
{
if (strchr(optarg, 'm')) opt_show |= SHOW_MATRIX_FLAT;
if (strchr(optarg, 'M')) opt_show |= SHOW_MATRIX;
if (strchr(optarg, 'e')) opt_show |= SHOW_EIGEN_VALUES;
if (strchr(optarg, 'r')) opt_show |= SHOW_EIGEN_VECTORS_R;
if (strchr(optarg, 'l')) opt_show |= SHOW_EIGEN_VECTORS_L;
}
break;
case 'v':
if (optarg == NULL) verbosity++;
else verbosity = atoi(optarg);
break;
case 'i':
fprintf(err, "%s\n", optarg);
infile = optarg; break;
case 'o': outfile = optarg; break;
case 'p':
fprintf(err, "%s\n", optarg);
opt_proj = 1;
if (optarg != NULL) nev = atoi(optarg);
break;
case 'h': help(); break;
case '?': exit(2);
}
}
if (opt_multi_out && !opt_proj)
{
fprintf(err, "--mo needs -p\n");
exit(2);
}
if (!opt_show && !opt_proj)
opt_show = SHOW_DEFAULT;
if (infile != NULL)
{
in = fopen(infile, "r");
if (in == NULL)
{
fprintf(err, "%s", strerror(errno));
exit(EXIT_FAILURE);
}
}
if (outfile != NULL)
{
out = fopen(outfile, "w");
if (out == NULL)
{
fprintf(err, "%s", strerror(errno));
exit(EXIT_FAILURE);
}
}
/*========================================================================
* Read commands or numerical input from the command line and update
* the matrix accordingly, until there is no more input. The file we
* expect to read has the following structure:
*
* #BEGIN INPUT
* #END INPUT
* #BEGIN ENSEM
* #BEGIN MODEL
* <point 0>
* <point 1>
* ...
* <point dim-1>
* #END MODEL
* ...
* #END ENSEM
*
* Blank lines are allowed and any line beginning with a '#' that is
* not mentioned above is interpretted as a comment. It may be the case
* that there are no models.
*
*======================================================================*/
rl_instream = in;
rl_outstream = out;
while (1)
{
line = readline(NULL);
if (line == NULL) break; /* EOF */
if (strlen(line) == 0) continue; /* Blank line */
if (strstr(line, "#BEGIN INPUT") == line)
{
if (num_input_lines == input_size)
{
input_size += 5;
input = (char **)realloc(input, input_size * sizeof(char *));
}
input[num_input_lines++] = line;
in_input = 1;
line = NULL; // prevent deallocation of line by free()
}
else if (strstr(line, "#END INPUT") == line)
{
in_input = 0;
num_input_lines++;
if (num_input_lines-1 == input_size)
{
input_size += 5;
input = (char **)realloc(input, input_size * sizeof(char *));
}
input[num_input_lines-1] = line;
line = NULL; // prevent deallocation of line by free()
}
else if (strstr(line, "#BEGIN LENS") == line) /*Backward compatibility*/
{
int q;
sscanf(line, "#BEGIN LENS dim %i omega %f lambda %f",
&q, &omega, &lambda);
in_ensem = 1;
}
else if (strstr(line, "#BEGIN ENSEM") == line)
{
in_ensem = 1;
}
else if (strstr(line, "#END LENS") == line
|| strstr(line, "#END ENSEM") == line)
{
in_ensem = 0;
in_model = 0;
}
else if (in_ensem && strstr(line, "#BEGIN MODEL") == line)
{
num_xs = 0;
xs = NULL;
start_time = CPUTIME;
if (in_model)
{
error("File inconsistency. "
"#BEGIN MODEL found before #END MODEL.\n");
}
in_model = 1;
}
else if (in_model && strstr(line, "#END MODEL") == line)
{
/*================================================================
* Now that the x's have been read, update the matrix.
*==============================================================*/
if (!in_model)
{
error("File inconsistency. "
"#END MODEL found before #BEGIN MODEL.\n");
}
if (num_models == models_size)
{
models_size += 100;
models = (float **)realloc(models,
models_size * sizeof(float *));
}
models[num_models++] = xs;
if (verbosity > 1)
{
end_time = CPUTIME;
total_time += end_time - start_time;
fprintf(err, "[%ix%i matrix; %i models; %fs; %fs]\n",
dim, dim, num_models,
end_time - start_time, total_time);
}
in_model = 0;
}
else if (line[0] == '#')
{
continue;
}
else if (in_model)
{
/*================================================================
* Read the input values.
*==============================================================*/
if (num_models == 0)
{
if (num_xs == xs_size)
{
xs_size += 100;
xs = (float *)realloc(xs, xs_size * sizeof(float));
}
dim = num_xs+1;
}
else if (num_xs > dim)
{
error("Model sizes differ!");
}
else if (xs == NULL)
{
xs = (float *)malloc(dim * sizeof(float));
}
xs[num_xs++] = atof(line);
}
else if (in_input)
{
num_input_lines++;
if (num_input_lines-1 == input_size)
{
input_size += 5;
input = (char **)realloc(input, input_size * sizeof(char *));
}
input[num_input_lines-1] = line;
line = NULL; // prevent deallocation of line by free()
}
free(line);
}
if (verbosity > 0)
fprintf(err, "[%ix%i matrix; %i models]\n", dim, dim, num_models);
if (!opt_multi_out)
{
write_header(out);
write_input(out);
}
/*========================================================================
*------------------------------------------------------------------------
*======================================================================*/
if (dim > 0)
{
/*====================================================================
* Center the data points to the origin.
*==================================================================*/
/* Find the average of the models. */
mean = (float *)calloc(dim, sizeof(float));
for (i=0; i < num_models; i++)
vecvecadd(mean, models[i], dim, mean);
vecdiv(mean, num_models, dim, mean);
/* Subtract off the mean from each of the models. */
if (verbosity > 0) fprintf(err, "Normalizing models...\n");
for (i=0; i < num_models; i++)
vecvecsub(models[i], mean, dim, models[i]);
/*====================================================================
* Optionally apply a general multiplicative scaling.
*==================================================================*/
if (opt_scale & SCALE_MULT)
{
if (verbosity > 0) fprintf(err, "Applying multiply scaling...\n");
scale(models[i], dim, NULL);
}
/*====================================================================
* Now with the normalized data, generate the inertia matrix.
*==================================================================*/
if (verbosity > 0) fprintf(err, "Creating matrix...\n");
M = (float *)calloc(dim * dim, sizeof(float));
if (M == NULL) error("Can't allocate memory for matrix.");
for (i=0; i < num_models; i++)
update_matrix(M, models[i], dim);
/*====================================================================
* Optionally apply a diagonalization scaling.
*==================================================================*/
if (opt_scale & SCALE_DIAG)
{
M0 = (float *)malloc(dim * dim * sizeof(float));
float *t;
int i, j;
if (verbosity > 0) fprintf(err, "Applying diagonal scaling...\n");
for (i=0; i < dim; i++)
for (j=0; j < dim; j++)
M0[i*dim+j] = M[i*dim+j] / sqrt(M[i*dim+i] * M[j*dim+j]);
for (i=0; i < num_models; i++)
scale(models[i], dim, M);
t = M;
M = M0;
M0 = t;
}
else
{
M0 = M;
}
/*====================================================================
* Compute the eigen values (and vectors) of the matrix we built.
*==================================================================*/
if (verbosity>0) fprintf(err, "Calculating eigenvalues(/vectors)...\n");
eigen_values_r = (float *)malloc(dim * sizeof(float)); assert(eigen_values_r != NULL);
eigen_values_i = (float *)malloc(dim * sizeof(float)); assert(eigen_values_i != NULL);
if (opt_proj || (opt_show & SHOW_EIGEN_VECTORS_R))
{
eigen_vectors_r = (float *)malloc(dim * dim * sizeof(float)); assert(eigen_vectors_r != NULL);
}
if (opt_show & SHOW_EIGEN_VECTORS_L)
{
eigen_vectors_l = (float *)malloc(dim * dim * sizeof(float)); assert(eigen_vectors_r != NULL);
}
if (get_eigen_values(M, eigen_values_r, eigen_values_i,
eigen_vectors_l, eigen_vectors_r, dim,
0))
{
error("Library call failed for given inputs.");
}
/*====================================================================
* Now place the results in a nice array which we can sort. The array
* is sorted by the real part of the eigenvalues, largest to smallest.
*==================================================================*/
eigen = (eigen_t *)calloc(dim, sizeof(eigen_t));
for (i=0; i < dim; i++)
{
if (eigen_values_r[i] == -0) eigen_values_r[i] = 0;
if (eigen_values_i[i] == -0) eigen_values_i[i] = 0;
eigen[i].re = eigen_values_r[i];
eigen[i].im = eigen_values_i[i];
eigen[i].dim = dim;
if (eigen_vectors_l) eigen[i].lvec = &(eigen_vectors_l[i * dim]);
if (eigen_vectors_r) eigen[i].rvec = &(eigen_vectors_r[i * dim]);
}
/*====================================================================
* Optionally add back the mean model.
*==================================================================*/
if (opt_add_back_mean)
{
if (verbosity>0) fprintf(err, "Adding back mean...\n");
/* add back mean */
if (eigen_vectors_r)
for (i=0; i < dim; i++)
vecvecadd(eigen[i].rvec, mean, dim, eigen[i].rvec);
if (eigen_vectors_l)
for (i=0; i < dim; i++)
vecvecadd(eigen[i].lvec, mean, dim, eigen[i].lvec);
}
/*====================================================================
* Sort.
*==================================================================*/
if (!opt_sort || (opt_sort & SORT_EV))
{
qsort(eigen, dim, sizeof(eigen_t), eigen_compar_rev);
}
else if (opt_sort & SORT_LC)
{
qsort(eigen, dim, sizeof(eigen_t), largest_ev_last_component);
}
/*====================================================================
*--------------------------------------------------------------------
*==================================================================*/
if (opt_proj)
do_eigen_projections(nev, M0, models, mean, num_models, eigen, dim);
}
do_output(M, omega, lambda, eigen, dim);
if (M == M0)
{
free(M);
}
else
{
free(M);
free(M0);
}
free(eigen_values_r);
free(eigen_values_i);
free(eigen_vectors_l);
free(eigen_vectors_r);
free(eigen);
free(mean);
free(commandline);
for (i=0; i < num_input_lines; i++) free(input[i]);
free(input);
for (i=0; i < num_models; i++) free(models[i]);
free(models);
if (in != stdin) fclose(in);
if (out != stdout) fclose(out);
return 0;
}
void
init(void)
{
update_matrix();
}
void XfmSetRotateOrder(struct Transform *transform, int order)
{
assert(is_rotate_order(order));
transform->rotate_order = order;
update_matrix(transform);
}
void XfmSetTransformOrder(struct Transform *transform, int order)
{
assert(is_transform_order(order));
transform->transform_order = order;
update_matrix(transform);
}
void XfmSetScale(struct Transform *transform, double sx, double sy, double sz)
{
VEC3_SET(transform->scale, sx, sy, sz);
update_matrix(transform);
}
void XfmSetRotate(struct Transform *transform, double rx, double ry, double rz)
{
VEC3_SET(transform->rotate, rx, ry, rz);
update_matrix(transform);
}
void XfmSetTranslate(struct Transform *transform, double tx, double ty, double tz)
{
VEC3_SET(transform->translate, tx, ty, tz);
update_matrix(transform);
}
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
rnum = 5;
cnum = 5;
mod = 10;
data.assign(rnum * cnum, 0);
ui->setupUi(this);
update_matrix();
sub_menu = new QMenu(this);
ui->tableWidget->setContextMenuPolicy(Qt::CustomContextMenu);
connect(ui->tableWidget, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(show_menu(QPoint)));
createAct = new QAction(this);
createAct->setIcon(QIcon::fromTheme("window-new"));
createAct->setText("Create new matrix...");
createAct->setStatusTip("CTRL + N");
createAct->setShortcut(Qt::CTRL + Qt::Key_N);
connect(createAct, SIGNAL(triggered()),this, SLOT(create_mat()));
sub_menu->addAction(createAct);
ui->menuFiles->addAction(createAct);
ui->mainToolBar->addAction(createAct);
fillAct = new QAction(this);
fillAct->setIcon(QIcon::fromTheme("view-refresh"));
fillAct->setText("Generate matrix values");
fillAct->setStatusTip("CTRL + G");
fillAct->setShortcut(Qt::CTRL + Qt::Key_G);
connect(fillAct, SIGNAL(triggered()),this, SLOT(fill_matrix()));
sub_menu->addAction(fillAct);
ui->menuFiles->addAction(fillAct);
ui->mainToolBar->addAction(fillAct);
searchAct = new QAction(this);
searchAct->setIcon(QIcon::fromTheme("system-search"));
searchAct->setEnabled(0);
searchAct->setText("Find max");
searchAct->setStatusTip("CTRL + F");
searchAct->setShortcut(Qt::CTRL + Qt::Key_F);
connect(searchAct, SIGNAL(triggered()),this, SLOT(find_max()));
sub_menu->addAction(searchAct);
ui->menuFiles->addAction(searchAct);
ui->mainToolBar->addAction(searchAct);
deleteAct = new QAction(this);
deleteAct->setIcon(QIcon::fromTheme("edit-delete"));
deleteAct->setEnabled(0);
deleteAct->setText("Delete current row");
deleteAct->setStatusTip("CTRL + X");
deleteAct->setShortcut(Qt::CTRL + Qt::Key_X);
connect(deleteAct, SIGNAL(triggered()),this, SLOT(delete_row()));
sub_menu->addAction(deleteAct);
ui->menuFiles->addAction(deleteAct);
ui->mainToolBar->addAction(deleteAct);
ui->menuFiles->addSeparator();
closeAct = new QAction(this);
closeAct->setIcon(QIcon::fromTheme("window-close"));
closeAct->setText("Close file");
closeAct->setStatusTip("CTRL + Q");
closeAct->setShortcut(Qt::CTRL + Qt::Key_Q);
connect(closeAct, SIGNAL(triggered()),this, SLOT(close()));
ui->menuFiles->addAction(closeAct);
ui->mainToolBar->addAction(closeAct);
//QToolBar* pr_bar = this->addToolBar("Main toolbar");
//pr_bar->addAction(closeAct);
}
void
AP_DCM::matrix_update(void)
{
DCM_Matrix update_matrix;
_gyro_vector(0) = gyro_scaled_X(read_adc(0)); // gyro x roll
_gyro_vector(1) = gyro_scaled_Y(read_adc(1)); // gyro y pitch
_gyro_vector(2) = gyro_scaled_Z(read_adc(2)); // gyro Z yaw
//Record when you saturate any of the gyros.
if((abs(_gyro_vector(0)) >= radians(300)) ||
(abs(_gyro_vector(1)) >= radians(300)) ||
(abs(_gyro_vector(2)) >= radians(300)))
gyro_sat_count++;
/*
Serial.print (__adc_in[0]);
Serial.print (" ");
Serial.print (_adc_offset[0]);
Serial.print (" ");
Serial.print (_gyro_vector(0));
Serial.print (" ");
Serial.print (__adc_in[1]);
Serial.print (" ");
Serial.print (_adc_offset[1]);
Serial.print (" ");
Serial.print (_gyro_vector(1));
Serial.print (" ");
Serial.print (__adc_in[2]);
Serial.print (" ");
Serial.print (_adc_offset[2]);
Serial.print (" ");
Serial.println (_gyro_vector(2));
*/
// _accel_vector(0) = read_adc(3); // acc x
// _accel_vector(1) = read_adc(4); // acc y
// _accel_vector(2) = read_adc(5); // acc z
// Low pass filter on accelerometer data (to filter vibrations)
_accel_vector(0) = _accel_vector(0) * 0.6 + (float)read_adc(3) * 0.4; // acc x
_accel_vector(1) = _accel_vector(1) * 0.6 + (float)read_adc(4) * 0.4; // acc y
_accel_vector(2) = _accel_vector(2) * 0.6 + (float)read_adc(5) * 0.4; // acc z
_omega = _gyro_vector + _omega_I; // adding proportional term
_omega_vector = _omega + _omega_P; // adding Integrator term
_accel_adjust(); // Remove centrifugal acceleration.
#if OUTPUTMODE == 1
update_matrix(0, 0) = 0;
update_matrix(0, 1) = -_G_Dt * _omega_vector(2); // -z
update_matrix(0, 2) = _G_Dt * _omega_vector(1); // y
update_matrix(1, 0) = _G_Dt * _omega_vector(2); // z
update_matrix(1, 1) = 0;
update_matrix(1, 2) = -_G_Dt * _omega_vector(0); // -x
update_matrix(2, 0) = -_G_Dt * _omega_vector(1); // -y
update_matrix(2, 1) = _G_Dt * _omega_vector(0); // x
update_matrix(2, 2) = 0;
#else // Uncorrected data (no drift correction)
update_matrix(0, 0) = 0;
update_matrix(0, 1) = -_G_Dt * _gyro_vector(2); // -z
update_matrix(0, 2) = _G_Dt * _gyro_vector(1); // y
update_matrix(1, 0) = _G_Dt * _gyro_vector(2); // z
update_matrix(1, 1) = 0;
update_matrix(1, 2) = -_G_Dt * _gyro_vector(0);
update_matrix(2, 0) = -_G_Dt * _gyro_vector(1);
update_matrix(2, 1) = _G_Dt * _gyro_vector(0);
update_matrix(2, 2) = 0;
#endif
// update
_dcm_matrix += _dcm_matrix * update_matrix;
/*
Serial.print (_G_Dt * 1000);
Serial.print (" ");
Serial.print (dcm_matrix(0, 0));
Serial.print (" ");
Serial.print (dcm_matrix(0, 1));
Serial.print (" ");
Serial.print (dcm_matrix(0, 2));
Serial.print (" ");
Serial.print (dcm_matrix(1, 0));
Serial.print (" ");
Serial.print (dcm_matrix(1, 1));
Serial.print (" ");
Serial.print (dcm_matrix(1, 2));
Serial.print (" ");
Serial.print (dcm_matrix(2, 0));
Serial.print (" ");
Serial.print (dcm_matrix(2, 1));
Serial.print (" ");
Serial.println (dcm_matrix(2, 2));
*/
}