void test11() {
int dim = 5;
Matrix m1 = matrix_create( dim, dim );
Matrix m2;
int i,j;
for ( i=0; i<dim; ++i ) { for ( j=0; j<dim; ++j ) { m1[i][j] = 3*i-j; } }
printf( "m1 =\n" );
matrix_print( m1, dim, dim );
while ( dim > 1 ) {
printf( "delete row %d =\n", dim-1 );
matrix_delete_row( m1, dim-1, dim );
matrix_print( m1, dim-1, dim );
printf( "transpose =\n" );
m2 = matrix_transpose( m1, dim-1, dim );
matrix_print( m2, dim, dim-1 );
matrix_delete( m1, dim-1 );
printf( "delete row %d =\n", dim-1 );
matrix_delete_row( m2, dim-1, dim );
matrix_print( m2, dim-1, dim-1 );
printf( "transpose =\n" );
m1 = matrix_transpose( m2, dim-1, dim-1 );
matrix_print( m1, dim-1, dim-1 );
matrix_delete( m2, dim-1 );
--dim;
}
matrix_delete( m1, dim );
}
void init_select() {
/*
Initialize matrix and array vector for a select_Draw*
*/
GLfloat tmp[16];
gl4es_glGetFloatv(GL_PROJECTION_MATRIX, tmp);
matrix_transpose(tmp, projection);
gl4es_glGetFloatv(GL_MODELVIEW_MATRIX, tmp);
matrix_transpose(tmp, modelview);
}
double accuracy(matrix_list_t* theta, matrix_t* X, matrix_t* y)
{
assert(theta->num == 2);
matrix_t* theta_transpose, *temp, *temp2;
theta_transpose = matrix_transpose(theta->matrix_list[0]);
temp = matrix_prepend_col(X, 1.0);
temp2 = matrix_multiply(temp, theta_transpose);
matrix_t* h1 = matrix_sigmoid(temp2);
free_matrix(theta_transpose);
free_matrix(temp);
free_matrix(temp2);
theta_transpose = matrix_transpose(theta->matrix_list[1]);
temp = matrix_prepend_col(h1, 1.0);
temp2 = matrix_multiply(temp, theta_transpose);
matrix_t* h2 = matrix_sigmoid(temp2);
free_matrix(theta_transpose);
free_matrix(temp);
free_matrix(temp2);
assert(h2->rows == 5000 && h2->cols == 10);
matrix_t* p = matrix_constructor(1, 5000);
int i, j;
for(i = 0; i<h2->rows; i++)
{
double max = 0.0;
unsigned char first = 1;
for(j=0; j<h2->cols; j++)
{
if(matrix_get(h2, i, j) > max || first == 1)
{
vector_set(p, i, j);
max = matrix_get(h2, i, j);
first = 0;
}
}
}
double count = 0;
for(i=0; i<5000; i++)
{
if(vector_get(y, i) == vector_get(p, i))
count = count + 1;
}
free_matrix(p);
free_matrix(h1);
free_matrix(h2);
return count/5000;
}
int
main (void)
{
GLfloat ini[] = {
1, 2, 3, 0,
4, 1, 6, 0,
7, 8, 9, 0,
0, 0, 0, 0
};
GLfloat inv[16];
GLfloat mat[16];
GLfloat tmp[16];
memset(inv, 0, sizeof(inv));
memset(mat, 0, sizeof(mat));
memset(tmp, 0, sizeof(tmp));
if (matrix_invt3(inv, ini)) {
matrix_print(ini);
matrix_transpose(tmp, inv);
matrix_print(tmp);
matrix_mul(mat, ini, tmp);
matrix_print(mat);
matrix_mul(mat, tmp, ini);
matrix_print(mat);
}
return 0;
}
Matrix* matrix_qr_decomposition(Matrix M)
{
assert(is_matrix(M));
int m = M->r;
int n = M->c;
Matrix Q = matrix_new_empty(m, n);
for(int i = 0; i < n; i++) {
double* u = matrix_column_vector(M, i);
double* a = matrix_column_vector(M, i);
for(int k = 0; k < i; k++) {
double* e = matrix_column_vector(Q, k);
double* p = vector_projection(e, a, m);
free(e);
for(int j = 0; j < m; j++)
u[j] -= p[j];
free(p);
}
free(a);
double* e = vector_normalize(u, m);
free(u);
for(int l = 0; l < m; l++)
Q->A[l][i] = e[l];
free(e);
}
Matrix Qt = matrix_transpose(Q);
Matrix R = matrix_multiply(Qt, M);
matrix_free(Qt);
Matrix* QR = calloc(2, sizeof(Matrix));
QR[0] = Q;
QR[1] = R;
return QR;
}
/** Converts a vector in local North, East, Down (NED) coordinates relative to
* a reference point given in WGS84 Earth Centered, Earth Fixed (ECEF) Cartesian
* coordinates to a vector in WGS84 ECEF coordinates.
*
* Note, this function only \e rotates the NED vector into the ECEF frame, as
* would be appropriate for e.g. a velocity vector. To pass an NED position in
* the reference frame of the NED, see \ref wgsned2ecef_d.
*
* \see wgsned2ecef_d.
*
* \param ned The North, East, Down vector is passed as [N, E, D], all in
* meters.
* \param ref_ecef Cartesian coordinates of the reference point, passed as
* [X, Y, Z], all in meters.
* \param ecef Cartesian coordinates of the point written into this array,
* [X, Y, Z], all in meters.
*/
void wgsned2ecef(const double ned[3], const double ref_ecef[3],
double ecef[3]) {
double M[3][3], M_transpose[3][3];
ecef2ned_matrix(ref_ecef, M);
matrix_transpose(3, 3, (double *)M, (double *)M_transpose);
matrix_multiply(3, 3, 1, (double *)M_transpose, ned, ecef);
}
extern void page_rank(size_t size)
{
matrix w;
matrix_init(&w, size);
gen_web_matrix(&w);
matrix g;
matrix_init(&g, size);
gen_google_matrix(&g, &w);
matrix_free(&w);
vector p;
vector_init(&p, size);
matrix_transpose(&g);
matrix_solve(&p, &g);
vector_sort(&p);
vector_save(&p);
matrix_free(&g);
vector_free(&p);
}
/*
This function sets rot_parent_current data member.
Rotation from this bone local coordinate system
to the coordinate system of its parent
*/
void Skeleton::compute_rotation_parent_child(Bone *parent, Bone *child)
{
double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp1[4][4], tmp2[4][4];
if(child != NULL)
{
// The following openGL rotations are pre-calculated and saved in the orientation matrix.
//
// glRotatef(-inboard->axis_x, 1., 0., 0.);
// glRotatef(-inboard->axis_y, 0., 1, 0.);
// glRotatef(-inboard->axis_z, 0., 0., 1.);
// glRotatef(outboard->axis_z, 0., 0., 1.);
// glRotatef(outboard->axis_y, 0., 1, 0.);
// glRotatef(outboard->axis_x, 1., 0., 0.);
rotationZ(Rz, -parent->axis_z);
rotationY(Ry, -parent->axis_y);
rotationX(Rx, -parent->axis_x);
matrix_mult(Rx, Ry, tmp);
matrix_mult(tmp, Rz, tmp1);
rotationZ(Rz, child->axis_z);
rotationY(Ry, child->axis_y);
rotationX(Rx, child->axis_x);
matrix_mult(Rz, Ry, tmp);
matrix_mult(tmp, Rx, tmp2);
matrix_mult(tmp1, tmp2, tmp);
matrix_transpose(tmp, child->rot_parent_current);
}
}
int matrix_square_root_n(int n, double *X, double *I, double *Y) {
/*
This function calculates one of the square roots of the matrix X and stores it in Y:
X = sqrt(Y);
X needs to be a symmetric positive definite matrix of dimension n*n in Fortran vector
format. Y is of course a vector of similar size, and will contain the result on exit.
Y will be used as a workspace variable in the meantime.
The variable I is a vector of length n containing +1 and -1 elements. It can be used
to select one of the 2^n different square roots of X
This function first calculates the eigenvalue decomposition of X: X = U*D*U^T
A new matrix F is then calculated with on the diagonal the square roots of D, with
signs taken from I.
The square root is then obtained by calculating U*F*U^T
*/
if (check_input(X, Y, "matrix_square_root_n")) return 1;
double *eigval, *eigvec, *temp;
int info = 0, i, j;
/* Memory allocation */
eigval=(double*) malloc(n*sizeof(double));
eigvec=(double*) malloc(n*n*sizeof(double));
temp=(double*) malloc(n*n*sizeof(double));
if ((eigval==NULL)||(eigvec==NULL)||(temp==NULL)) {
printf("malloc failed in matrix_square_root_n\n");
return 2;
}
/* Eigen decomposition */
info=eigen_decomposition(n, X, eigvec, eigval);
if (info != 0) return info;
/* Check for positive definitiveness*/
for (i=0; i<n; i++) if (eigval[i]<0) {
fprintf(stderr, "In matrix_square_root_n: Matrix is not positive definite.\n");
return 1;
}
/* Build square rooted diagonal matrix, with sign signature I */
for (i=0; i<n; i++) for (j=0; j<n; j++) Y[i*n+j] = 0.0;
for (i=0; i<n; i++) Y[i*n+i] = I[i]*sqrt(eigval[i]);
/* Multiply the eigenvectors with this diagonal matrix Y. Store back in Y */
matrix_matrix_mult(n, n, n, 1.0, 0, eigvec, Y, temp);
vector_copy(n*n, temp, Y);
/* Transpose eigenvectors. Store in temp */
matrix_transpose(n, n, eigvec, temp);
/* Multiply Y with this temp. Store in eigvec which is no longer required. Copy to Y */
matrix_matrix_mult(n, n, n, 1.0, 0, Y, temp, eigvec);
vector_copy(n*n, eigvec, Y);
/* Cleanup and exit */
free(eigvec); free(eigval); free(temp);
return info;
}
inline void
transformation::operator() (const cnormal& nin, cnormal* nout) const
{
float inv_trans[4][4];
matrix_transpose (inv, inv_trans);
matrix_vector_multiply (inv_trans, nin, *nout);
}
/* Compute Cholesky decomposition of an nxn matrix */
void dpotrf_driver(int n, double *A, double *U) {
double *AT;
char uplo = 'U';
int lda = n;
int info;
/* Transpose A */
AT = (double *)malloc(sizeof(double) * n * n);
matrix_transpose(n, n, A, AT);
/* Call lapack routine */
dpotrf_(&uplo, &n, AT, &lda, &info);
/* Transpose AT */
matrix_transpose(n, n, AT, U);
free(AT);
}
/* Compute singular value decomposition of an m x n matrix A */
int dgesvd_driver(int m, int n, double *A, double *U, double *S, double *VT) {
double *AT, *UT, *V;
char jobu = 'a';
char jobvt = 'a';
int lda = m;
int ldu = m;
int ldvt = n;
int lwork = 10 * MAX(3 * MIN(m, n) + MAX(m, n), 5 * MIN(m, n));
double *work;
int info;
/* Transpose A */
AT = (double *)malloc(sizeof(double) * m * n);
matrix_transpose(m, n, A, AT);
/* Create temporary matrices for output of dgesvd */
UT = (double *)malloc(sizeof(double) * m * m);
V = (double *)malloc(sizeof(double) * n * n);
work = malloc(sizeof(double) * lwork);
dgesvd_(&jobu, &jobvt, &m, &n, AT, &lda, S, UT, &ldu, V, &ldvt, work, &lwork, &info);
if (info != 0) {
printf("[dgesvd_driver] An error occurred\n");
}
matrix_transpose(m, m, UT, U);
matrix_transpose(n, n, V, VT);
free(AT);
free(UT);
free(V);
free(work);
if (info == 0)
return 1;
else
return 0;
}
inline cnormal
transformation::operator() (const cnormal& n) const
{
float inv_trans[4][4];
cnormal res;
matrix_transpose (inv, inv_trans);
matrix_vector_multiply (inv_trans, n, res);
return res;
}
/* Compute epipolar geometry between all matching images */
void BundlerApp::ComputeEpipolarGeometry(bool removeBadMatches,
int new_image_start)
{
unsigned int num_images = GetNumImages();
m_transforms.clear();
std::vector<MatchIndex> remove;
for (unsigned int i = 0; i < num_images; i++) {
MatchAdjList::iterator iter;
for (iter = m_matches.Begin(i); iter != m_matches.End(i); iter++) {
unsigned int j = iter->m_index; // first;
assert(ImagesMatch(i, j));
MatchIndex idx = GetMatchIndex(i, j);
MatchIndex idx_rev = GetMatchIndex(j, i);
bool connect12 =
ComputeEpipolarGeometry(i, j, removeBadMatches);
if (!connect12) {
if (removeBadMatches) {
// RemoveMatch(i, j);
// RemoveMatch(j, i);
remove.push_back(idx);
remove.push_back(idx_rev);
// m_match_lists[idx].clear();
// m_match_lists.erase(idx);
m_transforms.erase(idx);
m_transforms.erase(idx_rev);
}
} else {
matrix_transpose(3, 3,
m_transforms[idx].m_fmatrix,
m_transforms[idx_rev].m_fmatrix);
}
}
}
int num_removed = (int) remove.size();
for (int i = 0; i < num_removed; i++) {
int img1 = remove[i].first;
int img2 = remove[i].second;
// RemoveMatch(img1, img2);
m_matches.RemoveMatch(GetMatchIndex(img1, img2));
}
}
//-------------------------------------------
// マトリックスの転置と積.
// 左集合転置版.
//-------------------------------------------
// c = 結果行列の先頭アドレス.
// a,b = 掛ける行列の先頭アドレス.
// l = a の列数.
// m = a の行数、b の行数.
// n = b の列数.
//-------------------------------------------
void matrix_left_transpose_x(double *c, const double *a, const double *b, const int l, const int m, const int n)
{
double *t = new double[l * m];
matrix_init_zero(t, l, m);
matrix_transpose(a, t, l, m);
matrix_x(c, t, b, l, m, n);
delete[] t;
t = NULL;
}
//-------------------------------------------
// マトリックスの転置と積.
// 右集合転置版.
//-------------------------------------------
// c = 結果行列の先頭アドレス.
// a,b = 掛ける行列の先頭アドレス.
// l = a の行数.
// m = a の列数、b の列数.
// n = b の行数.
//-------------------------------------------
void matrix_right_transpose_x(double *c, const double *a, const double *b, const int l, const int m, const int n)
{
double *t = new double[n * m];
matrix_init_zero(t, n, m);
matrix_transpose(b, t, n, m);
matrix_x(c, a, t, l, m, n);
delete[] t;
t = NULL;
}
// autorelease function
matrix_t* matrix_pseudo_inverse(matrix_t* m)
{
if (m->rows == m->cols) {
return matrix_inverse(m);
} else if (m->rows > m->cols) {
matrix_begin();
// (A^T A)^{-1} A^T
matrix_t* ret = matrix_product(matrix_inverse(matrix_product(matrix_transpose(m), m)),matrix_transpose(m));
matrix_retain(ret);
matrix_end();
return matrix_autorelease(ret);
} else {
matrix_begin();
// A^T (A A^T)^{-1}
matrix_t* ret = matrix_product(matrix_transpose(m), matrix_inverse(matrix_product(m, matrix_transpose(m))));
matrix_retain(ret);
matrix_end();
return matrix_autorelease(ret);
}
}
static void check_matrix_transpose______sent_n_m_matrix_array____returned(void **state){
const int test_arr[2][2] = {{1, 2},{4, 5}};
matrix_t * mt =matrix_new_test(2, 2,test_arr);
matrix_print(mt);
matrix_transpose(mt);
assert_int_equal(matrix_return_element(mt,0,0),1);
assert_int_equal(matrix_return_element(mt,0,1),4);
assert_int_equal(matrix_return_element(mt,1,0),2);
assert_int_equal(matrix_return_element(mt,1,1),5);
matrix_free(mt);
}
// ****************************************************************************
// Same as the previous function, but does not return D and V
// ****************************************************************************
long int find_null_space_decomposition(double *A, long int rows, long int cols, double tolerance)
{
double *Utemp, *Dtemp, *Vtemp, max_val, temp_val;
long int nullity=cols, i, ii, max_loc=0;
Utemp=(double *)malloc(sizeof(double)*rows*cols);
if (Utemp==NULL) quit_error((char*)"Out of memory");
Dtemp=(double *)malloc(sizeof(double)*cols*cols);
if (Dtemp==NULL) quit_error((char*)"Out of memory");
Vtemp=(double *)malloc(sizeof(double)*cols*cols);
if (Vtemp==NULL) quit_error((char*)"Out of memory");
// Calculate the Singular Value Decomposition
svd(A, rows, cols, Utemp, Dtemp, Vtemp);
// Transpose Vtemp
matrix_transpose(Vtemp, cols, cols);
// Sort the singular values into the correct order
for (i=0; i<cols; i++)
{
// Find the maximum singular value in the remainder of the list
max_val=-1.0;
for (ii=i; ii<cols; ii++)
{
if (fabs(Dtemp[ii+cols*ii])>max_val)
{
max_val=fabs(Dtemp[ii+cols*ii]);
max_loc=ii;
}
}
// Update the nullity
if (max_val>fabs(tolerance)) nullity--;
// Swap the singular values
temp_val=Dtemp[max_loc+cols*max_loc];
Dtemp[max_loc+cols*max_loc]=Dtemp[i+cols*i];
Dtemp[i+cols*i]=temp_val;
// Swap the corresponding singular vectors
single_swap_row(i, max_loc, Vtemp, cols);
}
// Return the stuff that is required and throw away the rest
free(Utemp);
free(Dtemp);
free(Vtemp);
return nullity;
}
void main( )
{
Matrix *m, *mT, *eig_vec_J, *eig_vec_T, *L;
Vector *eig_val_J, *eig_val_T;
int i, j, tt_ja, tt_ho;
m = matrix_alloc( 100, 50 );
for( i = 0; i < m->dim_M; i++ )
for( j = 0; j < m->dim_N; j++ )
{
M( m, i, j) = (double) i*i + 120.0;
}
mT = matrix_alloc( 50, 100 );
matrix_transpose( m, mT );
eig_vec_J = matrix_alloc( 100, 100 );
eig_val_J = vector_alloc( 100 );
L = matrix_alloc( 100, 100 );
matrix_prod( m, mT, L );
Start_Clock_once( &tt_ja );
jacobi( L, eig_val_J, eig_vec_J );
End_ms_Clock_once( tt_ja, 1, "jacobi used " );
printf("done with jacobi\n" );
/* for( i = 0; i < 100; i++ ) printf(" %f", V(eig_val_J,i) );
printf("\n");
for( i = 0; i < 100; i++ ) printf(" %f", M(eig_vec_J,i,0) );
printf("\n");
*/
matrix_prod( m, mT, L );
eig_vec_T = matrix_alloc( 100, 100 );
eig_val_T = vector_alloc( 100 );
Start_Clock_once( &tt_ho );
eigen_householder( L, eig_val_T, eig_vec_T );
End_ms_Clock_once( tt_ho, 1, "householder used " );
printf(" done with householder\n" );
/* for( i = 0; i < 100; i++ ) printf(" %f", V(eig_val_T,i) );
printf("\n");
for( i = 0; i < 100; i++ ) printf(" %f", M(eig_vec_T,i,0) );
printf("\n");
*/
}
int main(int argc, char** argv)
{
printf("--------verify--------\n");
verify_matrix();
printf("----------------------\n");
// autorelease関数をbeginとendにはさまないで使うのはNG
// 全てのallocされた関数は
// (1) free
// (2) release
// (3) begin-endの中でautorelease
// のいずれかを行う必要がある。
// 擬似逆行列演算がこれくらい簡単に記述できる。
/* 3*2行列aを確保、成分ごとの代入 */
matrix_t* a = matrix_alloc(3, 2);
ELEMENT(a, 0, 0) = 1; ELEMENT(a, 0, 1) = 4;
ELEMENT(a, 1, 0) = 2; ELEMENT(a, 1, 1) = 5;
ELEMENT(a, 2, 0) = 3; ELEMENT(a, 2, 1) = 6;
/* 擬似逆行列の演算 */
// auto releaseモードに入る
matrix_begin();
// 擬似逆行列を求める。一行だけ！
matrix_t* inva = matrix_product(matrix_inverse(matrix_product(matrix_transpose(a), a)), matrix_transpose(a));
// 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
matrix_print(inva);
// release poolを開放する
matrix_end();
/* ちなみに擬似逆行列を求める関数は内部に組んだので、それを使うこともできる。 */
// auto releaseモードに入る
matrix_begin();
// 擬似逆行列を求める関数を呼ぶ。
matrix_t* inva_simple = matrix_pseudo_inverse(a);
// 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
matrix_print(inva_simple);
// release poolを開放する
matrix_end();
// これはautorelease対象でないので、しっかり自分でfree。
// リテインカウントを実装してあるので、理解できればreleaseの方が高性能。
//matrix_free(a);
matrix_release(a);
return 0;
}
/**
* Get the cameraspace matrix for this camera.
**/
void
camera_from_world(object_t *camera, float mat[16])
{
if (camera->type != OBJ_CAMERA)
errx(1, "camera_from_world must be called on "
"an object of type camera");
/**
* We don't yet cache this. Necessary? Also there's the question of
* whether scaling is a problem here.
**/
object_get_total_transform(camera, mat);
matrix_transpose(mat, mat);
matrix_inverse_trans(mat, mat);
}
END_TEST
START_TEST(test_matrix_transpose) {
u32 i, j, t;
seed_rng();
for (t = 0; t < LINALG_NUM; t++) {
u32 n = sizerand(MSIZE_MAX);
u32 m = sizerand(MSIZE_MAX);
double A[n*m];
double B[m*n];
double C[n*m];
for (i = 0; i < n; i++)
for (j = 0; j < m; j++)
A[m*i + j] = mrand;
matrix_transpose(n, m, A, B);
matrix_transpose(m, n, B, C);
for (i = 0; i < n; i++)
for (j = 0; j < m; j++)
fail_unless(fabs(A[m*i + j] - C[m*i + j]) < LINALG_TOL,
"Matrix element differs from original: %lf, %lf",
A[m*i + j], C[m*i + j]);
}
}
void test2() {
Matrix m1 = matrix_create( 3, 3 );
Matrix m1_t;
int i,j;
for ( i=0; i<3; ++i ) { for ( j=0; j<3; ++j ) { m1[i][j] = i+2*j; } }
printf( "m1 =\n" );
matrix_print( m1, 3, 3 );
printf( "transpose m1 =\n" );
m1_t = matrix_transpose( m1, 3, 3 );
matrix_print( m1_t, 3, 3 );
matrix_delete( m1, 3 );
matrix_delete( m1_t, 3 );
}
void clock_est_update(clock_est_state_t *s, gps_time_t meas_gpst,
double meas_clock_period, double localt, double q,
double r_gpst, double r_clock_period)
{
double temp[2][2];
double phi_t_0[2][2] = {{1, localt}, {0, 1}};
double phi_t_0_tr[2][2];
matrix_transpose(2, 2, (const double *)phi_t_0, (double *)phi_t_0_tr);
double P_[2][2];
memcpy(P_, s->P, sizeof(P_));
P_[0][0] += q;
double y[2];
gps_time_t pred_gpst = s->t0_gps;
pred_gpst.tow += localt * s->clock_period;
pred_gpst = normalize_gps_time(pred_gpst);
y[0] = gpsdifftime(meas_gpst, pred_gpst);
y[1] = meas_clock_period - s->clock_period;
double S[2][2];
matrix_multiply(2, 2, 2, (const double *)phi_t_0, (const double *)P_, (double *)temp);
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)phi_t_0_tr, (double *)S);
S[0][0] += r_gpst;
S[1][1] += r_clock_period;
double Sinv[2][2];
matrix_inverse(2, (const double *)S, (double *)Sinv);
double K[2][2];
matrix_multiply(2, 2, 2, (const double *)P_, (const double *)phi_t_0_tr, (double *)temp);
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)Sinv, (double *)K);
double dx[2];
matrix_multiply(2, 2, 1, (const double *)K, (const double *)y, (double *)dx);
s->t0_gps.tow += dx[0];
s->t0_gps = normalize_gps_time(s->t0_gps);
s->clock_period += dx[1];
matrix_multiply(2, 2, 2, (const double *)K, (const double *)phi_t_0, (double *)temp);
temp[0][0] = 1 - temp[0][0];
temp[0][1] = -temp[0][1];
temp[1][1] = 1 - temp[1][1];
temp[1][0] = -temp[1][0];
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)P_, (double *)s->P);
}
void
matrix_multiplication(float *sq_matrix_1, float *sq_matrix_2, float *sq_matrix_result, unsigned int sq_dimension )
{
float *sq_matrix_2_transposed;
sq_matrix_2_transposed = (float*)malloc(sizeof(float)*sq_dimension*sq_dimension);
matrix_transpose(sq_matrix_2, sq_matrix_2_transposed, sq_dimension);
#pragma omp parallel for
for (unsigned int i = 0; i < sq_dimension; i++) {
for(unsigned int j = 0; j < sq_dimension; j++) {
float local_sum = 0;
for (unsigned int k = 0; k < sq_dimension; k++)
local_sum += sq_matrix_1[i*sq_dimension + k] * sq_matrix_2_transposed[j*sq_dimension + k];
sq_matrix_result[i*sq_dimension + j] = local_sum;
}
}// End of parallel region
}
void test8() {
Matrix m1 = matrix_create( 3, 3 );
Matrix m1_t;
int i,j;
for ( i=0; i<3; ++i ) { for ( j=0; j<3; ++j ) { m1[i][j] = 3*i-j; } }
printf( "m1 =\n" );
matrix_print( m1, 3, 3 );
printf( "delete column 2 =\n" );
matrix_delete_column( m1, 2, 3, 3 );
matrix_print( m1, 3, 2 );
printf( "transpose =\n" );
m1_t = matrix_transpose( m1, 3, 2 );
matrix_print( m1_t, 2, 3 );
matrix_delete( m1, 3 );
matrix_delete( m1_t, 2 );
}
bool PlaneData::SetCorners(double *R, double *t, double f, int w, int h, int ymin, int ymax, int xmin, int xmax)
{
if(xmax==-1) xmax=w;
if(ymax==-1) ymax=h;
if(xmax>w || ymax>h ||xmax<-1 || ymax<-1) printf("Error in bounding box data w=%d h=%d xmin=%d xmax=%d ymin=%d ymax=%d\n",w,h,xmin,xmax,ymin,ymax);
//printf("xmin=%d xmax=%d ymin=%d ymax=%d\n",xmin,xmax,ymin,ymax);
double Rt[9];
matrix_transpose(3, 3, R, Rt);
double center[3];
matrix_product(3, 3, 3, 1, Rt, t, center);
matrix_scale(3, 1, center, -1.0, center);
/* Create rays for the four corners */
//uncomment the follwing code to use the 4 image corners
//double ray1[3] = { -0.5 * w, -0.5 * h, -f };
//double ray2[3] = { 0.5 * w, -0.5 * h, -f };
//double ray3[3] = { 0.5 * w, 0.5 * h, -f };
//double ray4[3] = { -0.5 * w, 0.5 * h, -f };
double ray1[3] = { xmin-0.5 * w, ymin-0.5 * h, -f };
double ray2[3] = { xmax-0.5 * w, ymin-0.5 * h, -f };
double ray3[3] = { xmax -0.5 * w, ymax-0.5 * h, -f };
double ray4[3] = {xmin -0.5 * w, ymax - 0.5 * h, -f };
double ray_world[18];
matrix_product(3, 3, 3, 1, Rt, ray1, ray_world + 0);
matrix_product(3, 3, 3, 1, Rt, ray2, ray_world + 3);
matrix_product(3, 3, 3, 1, Rt, ray3, ray_world + 6);
matrix_product(3, 3, 3, 1, Rt, ray4, ray_world + 9);
double t0 = IntersectRay(center, ray_world + 0, m_corners + 0);
double t1 = IntersectRay(center, ray_world + 3, m_corners + 3);
double t2 = IntersectRay(center, ray_world + 6, m_corners + 6);
double t3 = IntersectRay(center, ray_world + 9, m_corners + 9);
if (t0 > 0.0 && t1 > 0.0 && t2 > 0.0 && t3 > 0.0)
return true;
else
return false;
}
// loop through all bones to calculate local coordinate's direction vector and relative orientation
void Skeleton::ComputeRotationToParentCoordSystem(Bone *bone)
{
int i;
double Rx[4][4], Ry[4][4], Rz[4][4], tmp[4][4], tmp2[4][4];
//Compute rot_parent_current for the root
//Compute tmp2, a matrix containing root
//joint local coordinate system orientation
int root = Skeleton::getRootIndex();
rotationZ(Rz, bone[root].axis_z);
rotationY(Ry, bone[root].axis_y);
rotationX(Rx, bone[root].axis_x);
matrix_mult(Rz, Ry, tmp);
matrix_mult(tmp, Rx, tmp2);
//set bone[root].rot_parent_current to transpose of tmp2
matrix_transpose(tmp2, bone[root].rot_parent_current);
//Compute rot_parent_current for all other bones
int numbones = numBonesInSkel(bone[0]);
for(i=0; i<numbones; i++)
{
if(bone[i].child != NULL)
{
compute_rotation_parent_child(&bone[i], bone[i].child);
// compute parent child siblings...
Bone * tmp = NULL;
if (bone[i].child != NULL) tmp = (bone[i].child)->sibling;
while (tmp != NULL)
{
compute_rotation_parent_child(&bone[i], tmp);
tmp = tmp->sibling;
}
}
}
}
static ERL_NIF_TERM
transpose(ErlNifEnv *env, int32_t argc, const ERL_NIF_TERM *argv) {
ErlNifBinary matrix;
ERL_NIF_TERM result;
float *matrix_data, *result_data;
int32_t data_size;
size_t result_size;
(void)(argc);
if (!enif_inspect_binary(env, argv[0], &matrix)) return enif_make_badarg(env);
matrix_data = (float *) matrix.data;
data_size = (int32_t) (matrix_data[0] * matrix_data[1] + 2);
result_size = sizeof(float) * data_size;
result_data = (float *) enif_make_new_binary(env, result_size, &result);
matrix_transpose(matrix_data, result_data);
return result;
}